相关文件/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++的主要贡献有如下几点：

  1. 使用最小二乘估计（LSE）将平面拟合和方差估计方法从6自由度降低到3自由度；
  2. 提出了一种基于并查集的在线体素合并方法；
  3. 在大量不同场景下对比了VoxelMap++和其他SOTA算法，展现了VoxelMap++在精度和效率上的优势；
  4. 使得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]$ .$\textbf n$的闭式解如(6)(7)所示：

$$ \begin{gather} \tag{5} \begin{bmatrix} x_1 & y_1 & 1 \\ x_2 & y_2 & 1 \\ \vdots & \vdots & \vdots \\ x_n & y_n & 1 \end{bmatrix} \textbf{n} = \begin{bmatrix} -z_1 \\ -z_2 \\ \vdots \\ -z_n \end{bmatrix} \end{gather} $$ $$ \begin{gather} \tag{6} \textbf{n}= \frac{\mathbf{A}^{*}}{\left|\mathbf{A}\right|}\textbf{e} \end{gather} $$

其中 $\mathbf{A}^{*}$ 是 $\textbf{A}$的邻接矩阵，$\textbf{A}$ 和$\textbf{e}$ 分别为：

$$ \tag{7} \begin{aligned} &\mathbf{A} = \begin{bmatrix} \sum x_{i}x_{i} & \sum x_{i}y_{i} & \sum x_{i} \\\ \sum x_{i}y_{i} & \sum y_{i}y_{i} & \sum y_{i} \\\ \sum x_{i} & \sum y_{i} & N \end{bmatrix} \\ &\textbf{e} = \begin{bmatrix}-\sum x_{i}z_{i} & -\sum y_{i}z_{i} & -\sum z_{i}\end{bmatrix}^{T} \end{aligned} $$

以上推导可参见Efficient multi-sensor aided inertial navigation with online calibration.

基于$\textbf{n}$的闭式解, 可以得到平面的不确定性：

$$ \tag{8} \mathbf{\Sigma}_{\textbf{n}} = \sum_{i}^{N}\frac{\partial \textbf{n}}{\partial {}^W\textbf{p}_{i}} \mathbf{\Sigma}_{{}^W\textbf{p}_{i}} \frac{\partial \textbf{n}}{\partial {}^W\textbf{p}_{i}} ^{T} $$

建图与体素合并

作者指出，找出体素之间的关系（如共面关系），能够提升voxelmap的精度并进一步提升激光雷达（-IMU）里程计的表现。

作者在原版VoxelMap的基础上引入了并查集（wiki，oi-wiki）。利用并查集，可以区分不同体素中的平面$\mathcal P^k$，并将共面的平面合并为一个大的平面$\mathcal P^f$.对组成大平面$\mathcal P^f$的各个小平面$\mathcal P^k$的估计结果将被视为对$\mathcal P^f$的协方差的测量值。具体如下：

利用哈希表维护voxelmap，并将三位空间离散化为一个个体素。哈希表的key是体素的id，而哈希表的值则为并查集的节点，代表着该体素内部的平面特征。由于在哈希表内使用了并查集节点替代八叉树，因此VoxelMap++的体素边长相较于VoxelMap应当更小，以达到相同的分辨率。作者在论文中使用了0.5m的边长，而VoxelMap论文中使用的边长是3m。

在获得第一帧激光雷达点云后，首先找出哪些体素含有足够数量的点，再利用PCA来判断哪些体素中的点可以构成一个平面。之后再基于(6)，(8)计算平面参数$\textbf n=[a,b,d]^T$以及不确定性$\Sigma_\textbf n$。通过状态估计可以随后接收到的点云帧的全局姿态，然后再将点云中的点记录到全局点云地图中：

  • 如果点的位置已经存在一个voxel且该voxel没有收敛，该点将被用来增量更新这个voxel；
  • 如果点的位置不存在voxel,则在该位置新增一个voxel；

基于voxel的LIO的里程计的一个问题是随着点数的增加，计算时间会逐渐增大。本文认为在单一voxel中的点数超过50时，该voxel的参数就收敛了。因此在一个voxel中存在超过50个点时，就不对这个voxel进行更新了。当一个voxel停止更新时，其中所有的点信息都会被丢弃，只保留平面参数$\textbf n=[a,b,d]^T$以节省内存。

本文提出的基于并查集的平面合并方法将一个平面使用一个集合表示。当合并平面时，首先计算两个平面的相似度，当相似度超过卡方检验确定的阈值时，则将两个平面进行合并，并更新平面的参数信息。具体过程很琐碎，可以查阅论文。

基于并查集的平面合并

基于IESKF的状态估计

本文所使用的状态估计方法类似FAST-LIO以及VoxelMap中采用的方法。如下：

状态估计

Comments

经过尝试，个人认为基于voxel的LIO在户外结构化场景下尚可，但是在非结构化或狭窄场景下就表现感人（需要降低voxel尺寸，但是计算时间大大增加，且飘飞的可能性很大）。整体而言，体素泛用性不如类似FAST-LIO系列的点-点云距离的表示方法。