Voxelmap++
相关文件/Github/视频： ArXiv Github VoxelMap++: Mergeable Voxel Mapping Method for Online LiDAR(-inertial) Odometry 是电子科大近期挂在ArXiv上的一篇论文。我个人觉得这篇论文解决了很多在VoxelMap使用过程中的问题，因此写个笔记。 背景 由于VoxelMap能够估计平面的不确定性，使得VoxelMap具有极佳的精度及鲁棒性。但是VoxelMap同样存在若干不足： VoxelMap使用6自由度表示平面，造成了时间和空间复杂度的增长； VoxelMap没有处理不同voxel之间的关系，导致在VoxelMap中一个平面会有大量重复的voxel； 为了解决这些问题，作者基于VoxelMap提出了VoxelMap++，一种在线的可合并体素建图方法，并以三自由度表示平面。 作者认为VoxelMap++的主要贡献有如下几点： 使用最小二乘估计（LSE）将平面拟合和方差估计方法从6自由度降低到3自由度； 提出了一种基于并查集的在线体素合并方法； 在大量不同场景下对比了VoxelMap++和其他SOTA算法，展现了VoxelMap++在精度和效率上的优势； 使得VoxelMap++能够适应不同类型的激光雷达（包括多线旋转式激光雷达和固态雷达），并开源了VoxelMap++的代码； 方法 算法流程 VoxelMap++的流程同VoxelMap基本一致，仅修改了体素拟合模块并新增了体素合并模块。 3自由度平面表示 由VoxelMap中的推导，可以计算局部坐标系下的点$^L \boldsymbol p_i$的协方差并将其转换到世界坐标系下，如下所示： $$ \textbf{A}_i = \begin{bmatrix}\omega_i & -d_i \lfloor\omega_i\rfloor_{\times}\textbf{N}(\omega_i)\end{bmatrix}\tag{1} $$ $$ \Sigma_{^L\textbf{p}_i} = \textbf{A}_i \begin{bmatrix}\Sigma_{d_i} & \textbf{0}_{1\times 2} \\ \textbf{0}_{2\times 1} & \Sigma_{ \omega_i}\end{bmatrix}\textbf{A}_i^T \tag{2} $$ $$ {}^W\textbf{p}_i ={}_{L}^{W}\textbf{R}{}^L\textbf{p}_i + {}^W_L\textbf{t} \tag{3} $$ 作者指出，对于一组方差为$\Sigma_{W_{\textbf p_i}}$的共面点云$W_{\textbf p_i},(i=1,…,N)$，平面可以参数化为以下形式： $$ ax+by+z+d=0 \tag{4} $$ 由于所有的点$W_{\textbf p_i}$都满足上式，则可以构建一个最小二乘优化函数，如(5)所示，其中 $\textbf n^T=[a,b,d]$ ....