VoxelMap++原理解析

一、摘要

代码：github
原文：原文

摘要——本文提出了一种名为 VoxelMap++ 的体素映射方法，利用平面合并技术有效提升了基于激光雷达（或激光雷达-惯性）同步定位与建图（SLAM）的精度和效率。该地图是包含一个平面特征的体素集合，其中每个平面特征采用三自由度（3DOF）表示，并包含相应的协方差估计。考虑到整个地图中将包含大量共面的特征（子平面），这些子平面的 3DOF 估计可以视为更大平面（父平面）的协方差测量。因此，我们设计了基于并查集的平面合并模块，可以节省资源并进一步提高平面拟合的精度。该模块能够区分不同体素中的子平面，并合并这些子平面以估计父平面。合并后，父平面的3DOF表示比子平面更为准确，且不确定性显著降低，从而进一步提升激光雷达（或激光雷达-惯性）里程计的性能。我们在走廊和森林等挑战性环境中的实验表明，本文方法在精度和效率上优于其他先进方法（见附带视频1）。此外，我们的实现 VoxelMap++ 已在 GitHub上开源，适用于非重复扫描激光雷达和传统扫描激光雷达。

二、介绍

介绍：随着3D激光雷达技术的快速发展，激光雷达（惯性）里程计已广泛应用于自动驾驶车辆、无人机和移动机器人等各种应用中，因为它的鲁棒性和准确性。激光雷达（惯性）里程计的一个重要部分是历史地图的表示，几乎所有最先进的激光雷达（惯性）里程计都需要地图能够提供快速准确的注册，并能够在机器人移动过程中高效地增量构建。基于激光雷达的SLAM中最常用的映射方法是KD-Tree点云地图，因为它易于使用。然而，在点云注册过程中，不可避免地需要频繁搜索K个最近邻点来拟合平面，这对于KD-Tree来说是一个耗时的任务。KD-Tree中最近邻查找的时间复杂度是O(log(N))，这对实时性能是不利的。此外，保持点云的不确定性几乎是不可能的。因为每个点都是一个3DOF的随机变量，所以需要使用3×3的协方差矩阵来表示协方差。在协方差估计之后，整个地图的内存使用量将扩展到原始大小的四倍，这在实践中显然是无法承受的。幸运的是，近年来，一些基于空间哈希的体素映射方法已经被学者们提出，例如LIO-Livox、Faster-LIO、Balm、Balm 2.0、VoxelMap。注册的时间复杂度是O(1)，可以保证系统的实时性能。在这些方法中，VoxelMap由于其对平面特征的不确定性估计能力，具有出色的准确性和鲁棒性。然而，缺点是它使用6DOF来表示平面，这浪费了内存，并显著增加了整个系统的时间复杂度。它还没有处理不同体素之间的关系，导致VoxelMap中整个平面（如地面）被分割成许多子平面。如果能够有效地区分共面体素，然后估计由这些小子平面在体素中形成的大平面，整个地图的不确定性将显著降低，这将进一步提高在线激光雷达（惯性）里程计的定位精度。为了解决上述挑战，本文提出了一种新颖的在线可合并体素映射方法，称为VoxelMap++。

具体来说，本文的贡献包括：

• 我们将Voxelmap中的平面拟合和协方差估计方法从6DOF推广到3DOF，使用最小二乘估计。这种改进是从工程实施的角度出发，进一步提-高了协方差估计的效率，并减少了内存使用，使VoxelMap++能够轻松适应各种资源受限的嵌入式平台。
• 我们提出了一种新颖的在线体素合并方法，使用并查集。每个共面特征（子平面）在体素中将被视为一个大型平面（父平面）的测量。合并模块不仅提高了平面拟合的准确性并减少了整个地图的不确定性，而且还减少了地图的内存使用。
  我们在各种场景（结构化、非结构化和退化）中将VoxelMap++与其他最先进的算法进行了比较，证明了算法在准确性和效率方面的优越性。
• 我们使VoxelMap++适应不同类型的激光雷达（多旋转激光雷达和非传统固态激光雷达），并在GitHub上以可读性和模块化的方式开源，以分享我们的发现，并为社区做出贡献。

三、3DOF 平面拟合与协方差估计

A. 点与平面不确定性

局部激光雷达框架中的激光雷达点的不确定性由两部分组成：测距不确定性和方位角不确定性（见图1(a)）。设${\omega }_i\in S^2$为测量的方位角，${\delta }_{{\omega }_i}\sim \mathcal{N}\left(0_{2\times 1},{\Sigma }_{{\omega }_i}\right)$为${\omega }_i$的切平面中的方位角噪声，$d_i$为深度测量值，${\delta }_{d_i}\sim \mathcal{N}\left(0,{\Sigma }_{d_i}\right)$为测距噪声。那么，测量点${}^L{p}_i$的噪声${\delta }_{L_{p_i}}$及其协方差${\Sigma }_{L_{p_i}}$为

$$\begin{array}{rl}{\delta }_{L_{p_i}}& =\underset{A_i}{\underbrace{\left[{\omega }_i-d_i\lfloor {\omega }_i\rfloor \wedge N\left({\omega }_i\right)\right]}}\left[\begin{array}{c}{\delta }_{d_i}\\ {\delta }_{{\omega }_i}\end{array}\right]\sim \mathcal{N}\left(0,{\Sigma }_{L_{p_i}}\right),\\ {\Sigma }_{L_{p_i}}& =A_i\left[\begin{array}{cc}{\Sigma }_{d_i}& 0_{1\times 2}\\ 0_{2\times 1}& {\Sigma }_{{\omega }_i}\end{array}\right]A_i^T.\end{array}(1)$$

其中$N\left({\omega }_i\right)=\left[\begin{array}{ll}{N}_1& N_2\end{array}\right]\in R^{3\times 2}$是${\omega }_i$处切平面的正交基，$\lfloor \rfloor \wedge$表示斜对称矩阵，映射叉积。方程(1)的详细推导可在[27]中找到。

解释：这段内容描述了激光雷达点在局部坐标系中的不确定性是如何由测距和方位角的不确定性组合而成的。这种不确定性是通过一个线性变换$A_i$和噪声向量来表示的，其中包含了测距和方位角的噪声。这个模型考虑了激光雷达测量中的随机误差，并且这种误差会传播到后续的点云处理和分析中。

考虑到激光雷达点${}^L{p}_i$将通过估计的姿态${}_L^W\overline{T}=\left({}_L^{W}R,{}_L^{W}t\right)\in SE(3)$进一步投影到世界框架中，其估计不确定性为$\left({\Sigma }_R,{\Sigma }_t\right)$，如图1(a)所示，通过以下刚体变换

$${}^W{p}_i={}_L^W{R}^L{p}_i+{}_L^{W}t(2)$$

因此，激光雷达点${}^W{p}_i$的不确定性为

$${\Sigma }_{W_{p_i}}={}_L^{W}R{\Sigma }_{L_{p_i}L}{}^W{R}^T+{}_L^{W}R{\lfloor {}^L{p}_i\rfloor }_{\wedge }{\Sigma }_R{\lfloor {}^L{p}_i\rfloor }_{\wedge L}^{TW}{R}^T+{\Sigma }_t$$

其中${\Sigma }_R$是${}_L^{W}R$在切空间中的不确定性，${\Sigma }_t$是${}_L^{W}t$的不确定性。考虑到由于激光雷达测量和姿态估计的不确定性，不同位置的点云的协方差有很大的差异：近距离的点的噪声主要由测距噪声主导，而远距离的点的噪声主要由方位角噪声主导。单个激光雷达点的不确定性分析也是平面特征不确定性模型的基础。

解释：这段内容描述了激光雷达点从局部坐标系到世界坐标系的转换过程中，如何考虑传感器的测量误差和传感器姿态估计的不确定性。这涉及到一个刚体变换，其中包括旋转和平移，以及这些变换的不确定性。这个模型有助于理解在不同距离下，激光雷达点的不确定性如何变化，这对于后续的SLAM（同时定位与地图构建）算法和点云处理非常重要。

B. 3DOF平面拟合和协方差估计

像其他激光雷达（惯性）里程计的实现一样，由于平面特征的广泛可用性，我们只使用环境中的平面特征。在每个体素中，我们维护一个3DOF平面拟合结果 $\mathbf{n}=\left[\begin{array}{c}a\\ b\\ d\end{array}\right]$ 及其协方差 ${\Sigma }_n$。在本小节中，我们将阐述如何在3DOF而不是其他冗余表示中拟合平面并估计其协方差。

首先，根据对激光雷达传感器测量噪声的分析，我们可以计算出局部激光雷达框架中每个激光雷达点 ${\mathbf{p}}_i^L$ 的协方差 ${\Sigma }_{L_{p_i}}$，然后使用姿态估计结果 $({\mathbf{R}}_L^W,{\mathbf{t}}_L^W)$ 将其转换到世界框架。因此，我们可以进一步估计世界框架中激光雷达点的协方差 ${\Sigma }_{W_{p_i}}$。详细推导可以在文献[2]中找到。

$$\begin{array}{rl}& A_i=\left[\begin{array}{ll}{\omega }_i& -d_i{\lfloor {\omega }_i\rfloor }_{\times }N({\omega }_i)\end{array}\right]\\ & {\Sigma }_{L_{p_i}}=A_i\left[\begin{array}{cc}{\Sigma }_{d_i}& 0_{1\times 2}\\ 0_{2\times 1}& {\Sigma }_{{\omega }_i}\end{array}\right]A_i^T(1)\end{array}$$

$${\mathbf{p}}_i^W={\mathbf{R}}_L^W{\mathbf{p}}_i^L+{\mathbf{t}}_L^W(2)$$

$${\Sigma }_{W_{p_i}}={\mathbf{R}}_L^W({\Sigma }_{L_{p_i}}+\lfloor {\mathbf{p}}_i^L\rfloor {\Sigma }_{L_R}\lfloor {\mathbf{p}}_i^L{\rfloor }^T){\mathbf{R}}_L^T+{\Sigma }_{L_L^{W_t}}(3)$$

假设一组共面的激光雷达点 ${\mathbf{p}}_i^W$，其中 $i=1,\cdots ,N$ 与协方差 ${\Sigma }_{w_{p_i}}$ 已经使用(3)确定。然后，我们利用线性最小二乘法来计算平面的3DOF表示。考虑一个平面可以从这个点云中提取出来，参数化为(4)，通过沿z分量（主轴）归一化。当平面法线的z分量接近零时，这个表达式可能是奇异的。然而，这个问题可以通过计算点云对每个坐标轴的投影来轻松解决，方差最小的投影是主轴。

$$ax+by+z+d=0(4)$$

由于所有 ${\mathbf{p}}_i^W$ 都是共面的满足(4)，所以最小二乘优化函数可以构建为(6)。经过一系列相同的变形，可以得到 $\mathbf{n}$ 的闭式解(7)。

$$\left[\begin{array}{ccc}{x}_1& y_1& 1\\ x_2& y_2& 1\\ ⋮& ⋮& ⋮\\ x_n& y_n& 1\end{array}\right]\mathbf{n}=\left[\begin{array}{c}-z_1\\ -z_2\\ ⋮\\ -z_n\end{array}\right]$$

$$\mathbf{n}=\frac{A^{\ast }}{|A|}\mathbf{e}(6)$$

其中 $A^{\ast }$ 是 $A$ 的伴随矩阵，$A$ 和 $\mathbf{e}$ 的表达式如下：

$$A=\left[\begin{array}{ccc}\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{array}\right]$$

$$\mathbf{e}={\left[\begin{array}{c}-\sum x_i{z}_i\\ -\sum y_i{z}_i\\ -\sum z_i\end{array}\right]}^T$$

基于 $\mathbf{n}$ 的闭式解，平面的不确定性因此是

$${\Sigma }_n=\sum _i^N\frac{\partial \mathbf{n}}{\partial {\mathbf{p}}_i^W}{\Sigma }_{w{p}_i}{\left(\frac{\partial \mathbf{n}}{\partial {\mathbf{p}}_i^W}\right)}^T$$

其中协方差矩阵 ${\Sigma }_n$ 是平面3DOF表示的不确定性，它是从平面上的点的协方差矩阵 ${\Sigma }_{w_{p_i}}$ 计算出来的。

注意 A、A* 和 e 中的所有元素都是求和结果，因此可以很容易地使用动态规划来递增地更新3DOF平面表示，这将有效地降低更新中拟合平面的复杂性。

B. 可合并体素映射方法

1. 动机：在基于体素的映射方法中，学者们总是忽略体素之间的关系，不管体素地图是如何分割的或者体素有多小。不可否认，将所有体素简单地视为独立个体对于工程实践来说非常容易，并且具有强大的鲁棒性。然而，如果我们能够有效地识别体素之间的关系（例如，它们是否属于同一平面），那么利用这种关系可以合理地提高体素地图的准确性，并进一步促进激光雷达（惯性）里程计的性能。同时，合并具有统一属性的体素（例如共面）也可以有效地节省内存使用，这对于商业低成本机器人无疑是有意义的。

基于上述想法，我们在先前研究的基础上修改了体素地图的构建和更新过程。然后创造性地提出了一种基于并查集的体素合并方法。这种方法可以区分不同体素中的平面，称为 ${\mathcal{P}}^k$，并合并 ${\mathcal{P}}^f$ 以估计由这些小平面组成的大平面，大平面称为 ${\mathcal{P}}^f$。这些 ${\mathcal{P}}^k$ 的估计结果被视为 ${\mathcal{P}}^f$ 的协方差测量。之后，我们使用最小二乘法估计 ${\mathcal{P}}^f$ 的平面3DOF表示和协方差，这比 ${\mathcal{P}}^k$ 的估计结果要精确得多，从而提高了激光雷达（惯性）里程计的准确性。

2. 体素地图的构建和更新：我们的方法基于空间哈希，以保证系统的实时性能。精确地说，我们的体素地图是由一个哈希表维护的，3维空间被离散化为每个小体素，哈希表的键是每个体素的标识，而哈希表的值是并查集的节点，代表这个体素中的平面特征。

我们将每个体素的边长设置为 $0.5m$，而不是VoxelMap中的默认3.0m。这是合理的，因为我们用并查集的节点替换了八叉树用于平面合并，这将在III-C3中介绍。由于缺乏体素分割的特性，如果我们想要实现相同的分辨率，就必须减小体素的边长。这主要影响哈希表的内存使用。然而，通过合并不同体素中的 ${\mathcal{P}}^k$，我们可以释放大部分体素以减少内存使用。在IV-C中显示的实验中，我们发现VoxelMap++的内存使用量甚至比VoxelMap还要低，因为我们不需要在每个体素中维护平面表示。

在第一帧激光雷达点到达后，我们检查所有包含足够点的体素，然后使用PCA来判断每个体素中的点是否可以形成一个平面。之后，我们根据(6)(8)计算并存储平面参数 $\mathbf{n}=\left[\begin{array}{c}a\\ b\\ d\end{array}\right]$ 及其不确定性 ${\Sigma }_n$。对于第一次扫描后到达的激光雷达点，它们在世界框架中的姿态将通过状态估计获得。然后这些点被注册到地图中。如果该位置已经存在一个体素，并且这个体素尚未收敛，那么这个点将被添加到其中，并且体素的参数将基于(6)(8)递增地更新。

C 基于Union-Find的平面合并

在体素地图更新后，汇聚的平面 ${\mathcal{P}}^k$ 将与其邻近平面合并。平面合并算法基于并查集（Union-Find）设计，如算法1所示。

算法1：平面合并

在步骤1-3中，初始化并查集中的新节点，即汇聚的平面。然后在步骤4-17中，这些新节点将与并查集中的其他节点合并。在步骤5-16中，基于哈希键查询每个邻近平面 ${\mathcal{P}}_i^k$ 和 ${\mathcal{P}}^n$。在步骤6-8中，我们将根据(9)所示的马氏距离计算 ${\mathcal{P}}_i^k$ 与 ${\mathcal{P}}^n$ 的相似性。当马氏距离小于 ${\chi }^2$ 分布的95%给定的阈值时，这两个平面将被合并。在步骤9中，我们将 ${\mathcal{P}}_i^k.f$ 和 ${\mathcal{P}}^n.f$ 视为具有协方差 ${\mathcal{P}}_i^k.f$ 的大平面的测量，并根据(10)(11)估计其协方差和表示，这是一个基于最小化迹 ${\Sigma }_{n_{N{\mathcal{P}}^k}}$ 的简单加权平均算法。在步骤10-12中，这是基于并查集的节点合并的两个简单情况，如图3(a,b)所示。在步骤13-15中，这是关于 ${\mathcal{P}}_i^k.f$ 和 ${\mathcal{P}}^n.f$ 的平面合并的另一个复杂情况，如图3©所示。在步骤16中，我们在合并后执行修剪，以保持并查集的最大深度为2，避免额外的时间消耗，如图3(d)所示。在步骤17中，我们将大平面 ${\mathcal{P}}_i^k.f$ 的结果分配给 ${\mathcal{P}}_i^k.f$，然后 ${\mathcal{P}}^n.f$ 和 ${\mathcal{P}}_i^k.f$ 已成功合并。最后，在步骤18中，我们释放 ${\mathcal{P}}_i^k$ 和 ${\mathcal{P}}^n$ 中的资源，即3DOF表示和 $3\times 3$ 协方差。合并后，数百个体素将共享 ${\mathcal{P}}^k.f$ 中的相同平面表示，这将有效减少内存使用。

$$\gamma =(n_{{\mathcal{P}}_i^k}-n_{{\mathcal{P}}^n})({\Sigma }_{n_{{\mathcal{P}}_i^k}}+{\Sigma }_{n_{{\mathcal{P}}^n}}{)}^{-1}(n_{{\mathcal{P}}_i^k}-n_{{\mathcal{P}}^n}{)}^T(9)$$

$$\begin{array}{rl}{n}_{N{\mathcal{P}}_i^k}& =\frac{trace({\Sigma }_{n_{{\mathcal{P}}^n}})n_{{\mathcal{P}}_i^k}+trace({\Sigma }_{n_{{\mathcal{P}}_i^k}})n_{{\mathcal{P}}^n}}{trace({\Sigma }_{n_{{\mathcal{P}}_i^k}})+trace({\Sigma }_{n_{{\mathcal{P}}^n}})}\\ {\Sigma }_{n_{N{\mathcal{P}}_i^k}}& =\frac{trace({\Sigma }_{n_{{\mathcal{P}}^n}}{)}^2{\Sigma }_{n_{{\mathcal{P}}_i^k}}+trace({\Sigma }_{n_{{\mathcal{P}}_i^k}}{)}^2{\Sigma }_{n_{{\mathcal{P}}^n}}}{(trace({\Sigma }_{n_{{\mathcal{P}}_i^k}})+trace({\Sigma }_{n_{{\mathcal{P}}^n}}){)}^2}\end{array}(10)$$

$${\Sigma }_{n_{N{\mathcal{P}}_i^k}}=\frac{trace({\Sigma }_{n_{{\mathcal{P}}^n}}{)}^2{\Sigma }_{n_{{\mathcal{P}}_i^k}}+trace({\Sigma }_{n_{{\mathcal{P}}_i^k}}{)}^2{\Sigma }_{n_{{\mathcal{P}}^n}}}{(trace({\Sigma }_{n_{{\mathcal{P}}_i^k}})+trace({\Sigma }_{n_{{\mathcal{P}}^n}}){)}^2}(11)$$

图2(b)是没有合并的平面图，图2©是合并后的平面图，相同颜色的平面属于同一个平面，并共享拟合结果 n 和协方差 ${\Sigma }_n$。

D. 基于IESKF的状态估计

状态估计是基于迭代扩展卡尔曼滤波器的，类似于FAST-LIO[1]和VoxelMap[2]。假设我们有一个基于IMU传播的状态估计先验 ${\hat{x}}_k$ 和协方差 $P_k$。这个先验将通过与点到平面距离匹配来更新，以形成一个最大后验概率（MAP）估计。具体来说，第i个有效的点到平面匹配导致观测方程如下所示：

$$\begin{array}{rl}{z}_i& =h_i(x_i,n_i)=0\end{array}$$

$$\approx h_i({\hat{x}}_i^{\kappa },0)+H_i^{\kappa }(x_i\boxminus {\hat{x}}_i^{\kappa })+v_i$$

$$h_i({\hat{x}}_i^{\kappa },0)=\frac{{\Omega }^T({}_L^W{R}^L{p}_i+{}_L^{W}t)+d}{\Vert \Omega \Vert }$$

其中 $h_i({\hat{x}}_i^{\kappa },0)$ 是第 $\kappa$ 次迭代中的点到平面距离，$\Omega$ 是注册平面 ${\mathcal{P}}_i^k\cdot f$ 的法向量 $[a,b,1{]}^T$。${\mathcal{P}}_i^k\cdot f$ 可以通过首先从体素地图中通过空间哈希查询 ${\mathcal{P}}^k$，然后基于并查集找到 ${\mathcal{P}}^k$ 的根节点来确定。$v_j\sim (0,R_j)$ 是由 ${}^L{p}_i$ 和 $n_{{\mathcal{P}}_i^k\cdot f}$ 传播的观测噪声，如(14)式所示。

$${\Sigma }_{n_{{\mathcal{P}}_i^k,f},L{p}_i}=\left[\begin{array}{cc}{\Sigma }_{n_{{\mathcal{P}}_i^k,f}}& 0_{3\times 3}\\ 0_{3\times 3}& {\Sigma }_{L{p}_i}\end{array}\right]$$

$$R_i=J_{v_i}{\Sigma }_{n_{{\mathcal{P}}_i^k,f},{}^L{p}_i}{J}_{v_i}^T$$

$$J_{v_i}=\left[J_{n_i},J_{L{p}_i}\right],J_{L{p}_i}=\frac{1}{\Vert \Omega \Vert }{\Omega }^T\times {}_L^{W}R$$

$$J_{n_i}=\frac{1}{\Vert \Omega \Vert }{\left[\begin{array}{c}{x}_i\left(1-\frac{1}{\Vert \Omega {\Vert }^2}{h}_i(x_i,0)\right)\\ y_i\left(1-\frac{1}{\Vert \Omega {\Vert }^2}{h}_i(x_i,0)\right)\\ 1\end{array}\right]}^T$$

最后，结合状态先验和所有有效的测量，我们可以得到MAP估计：

$$\underset{{\hat{x}}_k^{\kappa }}{\min }\left\{{\Vert {\hat{x}}_k^{\kappa }\boxminus {\hat{x}}_k\Vert }_{{\hat{P}}_k}^2+\sum _{i=1}^N{\Vert h_i({\hat{x}}_k^{\kappa },0)+H_i^{\kappa }(x_k\boxminus {\hat{x}}_k^{\kappa })\Vert }_{R_i}\right\}$$

其中第一部分是状态先验，第二部分是测量观测。详细的解决方案基于迭代扩展卡尔曼滤波器，可以参考[22]。