【文献分享】NASA JPL团队CoSTAR一大力作：直接激光雷达里程计:利用密集点云快速定位

论文题目：Direct LiDAR Odometry: Fast Localization With Dense Point Clouds

中文题目：直接激光雷达里程计:利用密集点云快速定位

作者：Kenny Chen, Brett T.Lopez, Ali-akbar Agha-mohammadi

论文链接：https://arxiv.org/pdf/2110.00605.pdf

论文开源代码：https://github.com/vectr-ucla/direct_lidar_odometry

0 笔者个人体会

这篇文章的整体算法的大结构还是与LOAM类似，分成两个部分：scan-to-scan(对应于LOAM的laserOdometry部分)，scan-to-map(对应于LOAM的laserMapping部分)，但是对于其中非常多的细节进行了创新，我认为这些创新都很有可取之处。比如文中提到的在判断是否添加关键帧时使用的自适应阈值，根据场景大小调整平移距离阈值，这个就可以用到我们的研究开发中；还有那个数据结构重复利用，节省了数据创建释放过程的时间，这个思想看似小改变，但是对于嵌入式设备中的实时性却是有很重要的意义。

个人认为有一个地方还可以进行小改进，注意到两个部分中都是使用了作者的GICP算法进行点云配准，没有进行解耦，scan-to-map阶段提高精度的作用主要体现在匹配点的数量增加，如果使用了两种不同的配准算法，可能会有更精确的效果。

1 效果展示

2 引言

在大型、具有感知挑战性的环境中，准确的状态估计和映射已经成为自主移动机器人的关键能力。基于激光雷达的SLAM性能的瓶颈之一，就是每次扫描的大量数据点很快就会压倒计算有限的处理器(比如嵌入式)，这可能导致帧下降，最终导致姿态估计不佳。更具体地说，scan-to-scan的对齐需要两个云之间对应点的配准，但这个过程通常涉及最近邻搜索，随着每次扫描点的数量呈指数增长。基于特征的方法试图通过只使用最突出的点来缓解这种情况，但这些方法通常采用计算密集型的特征提取步骤，可能会意外丢弃数据，否则可能有助于提高下游配准的质量。

本文主要贡献：

一种自适应关键帧系统，该系统通过一种新颖的空间度量来有效地捕获重要的环境信息。
提出了一种基于关键帧的快速子映射方法，该方法通过凸优化生成允许的局部子映射来进行全局姿态优化。
NanoGICP，这时一个定制迭代的壁橱点求解器，用于轻量级点云扫描匹配，数据结构回收，消除冗余计算。

3 算法框架

算法框架如下图所示，主要部分就是预处理、scan-to-scan、scan-to-map、地图关键帧管理几个部分。

3.1 预处理

为了最大限度地减少原始传感器数据的信息损失，在预处理过程中只使用了两个滤波器:

首先，我们通过原点周围大小为1 m³的盒子滤波器去除可能来自机器人本身的所有点返回。
然后将生成的云通过分辨率为0.25 m的3D体素网格滤波器发送，以便对后续任务的数据进行轻微采样，同时在周围环境中保持主导结构。

在这项工作中，没有校正运动失真，因为非刚性变换可能会带来计算负担，而是直接使用密集点云，而不是像大多数工作那样提取特征。经过预处理后，每个云平均包含1万个点。

3.2 通过Generalized-ICP进行扫描匹配

这个过程通常分两个阶段进行：

首先提供最佳的瞬时猜测，也就是scan-to-scan的过程
随后将其改进为与之前的关键帧位置更加全局一致，也就是scan-to-map的过程

1.scan-to-scan

在雷达里程计L中，计算相邻帧的相对变换 $\hat X^L_k$ :
$\hat X^L_k = argmin \epsilon (X^L_kP^s_k,P^t_k)$

残差项是由GICP计算得到：
$\epsilon (X^L_kP^s_k,P^t_k)=\sum^N_i d_i^T(C^t_{k,i} + X^L_kC^s_{k,i}X_k^{L^T})^{-1}d_i$
$C^t_{k,i}$ 和 $C^s_{k,i}$ 分别是第k-1时刻和k时刻点云的协方差矩阵， $d_t=p^t_i-X^L_k p^s_i$ 表示对应点的距离残差。

如果有IMU，则使用IMU预积分提供一个先验初始猜测，如果没有IMU则设置先验猜测为单位矩阵I，以减少收敛到次优局部最小值的可能，文章中有一些关于IMU预积分的说明，这里就不多说了。

2.scan-to-map

这部分的目标不是计算两个瞬时点云之间的相对变换，而是通过与局部子图匹配的方式，进一步改进前一步的运动估计，使其更加全局一致。
换句话说，这里的任务是计算当前源云 $P^s_k$ 和某些子图 $S_k$ 之间的最优变换 $X^W_k$ ，使得：

$\hat X^W_k = argmin \epsilon (X^W_k P^s_k,S_k)$
残差项是与scan-to-scan类似：
$X^W_k=argmin \sum^M_j d_j^T(C^S_{k,j} + X^W_k C^s_{k,j}X_k^{W^T})^{-1}d_j$
这个公式使用前一节从L到W的scan-to-scan的结果进行初始化，即: $X^W_k = X^W_{k−1} * X^L_k$ ，以便该先验运动可以与历史地图数据进行比较，以获得全局一致性。

3.基于关键帧的快速子图

这项工作的一个关键创新在于如何管理地图信息，并在scan-to-map匹配中快速生成局部子地图，以进行全局自运动细化。作者没有直接使用点云并将点存储到典型的八叉树数据结构中，而是保留了要在其中搜索的关键帧的历史记录，其中每个关键帧都以键值对的形式链接到相应的点云帧。然后通过连接关键帧子集的相应点云来生成用于scna-to-map匹配的局部子地图，而不是直接在机器人当前位置的某个半径内检索局部点。

这种设计选择的含义是双重的:首先，通过在“关键帧空间”而不是“点云空间”中搜索，得到了一个计算上更容易处理的问题。

此外，与基于范围的方法相比，基于关键帧的方法构建了更宽松的子地图。也就是说，由于从关键帧点云导出的子图的大小完全依赖于激光雷达传感器的距离，而不是预定的距离，派生的子图可以与当前扫描有更大的重叠。

固定半径r = 20 m的子图(图a)与当前扫描没有充分重叠，并且由于只包含空间附近的点，可能会随着时间的推移而引入漂移;然而，基于关键帧的方法(图b)涵盖了当前扫描的大部分内容，这有助于更好地实现扫描到映射的对齐。

1)通过kNN和凸包选择关键帧:设 $K_k$ 为所有关键帧点云的集合，如下:将子地图 $S_k$ 定义为K个最近邻关键帧扫描 $Q_k$ 和L个最近邻凸包扫描 $H_k$ 的连接，使得 $S_k = Q_k⊕H_k$ 。

2)自适应关键帧:关键帧节点添加通常使用固定阈值(例如，每1米或10◦的平移或旋转变化)，但最佳位置可能高度依赖于周围环境的结构。更具体地说，在大规模环境中，点云扫描捕捉到的特征更加突出，可以在更长的时间内依赖。相反，对于狭窄或小规模的环境，需要更小的阈值来持续捕获子地图中的小规模特征(即紧角)，以获得更好的定位。因此，需要根据瞬时点云扫描中的“空间”来缩放新关键帧的平移阈值，定义为 $m_k = \alpha m_{k−1} + \beta M_k$ ，其中 $M_k$ 为预处理点云中原点到每个点的中值欧式距离，α = 0.95， β = 0.05, $m_k$ 为用于缩放关键帧阈值 $th_k$ 的平滑信号，在k时刻使:

$th_k =\left\{ \begin{array}{l} 10m,\ \ \ \ if\ m_k>20m\\ 5m, \ \ \ \ \ \ if\ m_k>10m\&m_k≤20m\\ 1m, \ \ \ \ \ \ if \ m_k>5m\&m_k≤10m\\ 0.5m, \ \ \ if \ m_k≤5m\\ \end{array} \right.$

旋转阈值固定在30°。下图展示了关键帧添加以及自适应关键帧的效果，这个操作有助于增强对环境维度变化的鲁棒性。

3.3 算法优化

1.扫描拼接子地图法线

广义icp涉及最小化两个云之间的平面距离，其中这些平面通过扫描中每个点的计算协方差来建模。文章不是在每次迭代中计算子地图中每个点的法线，而是假设子地图协方差 $C^S_k$ 可以通过将构成子地图的n个关键帧中的法线 $C^S_{k,n}$ 拼接起来来近似，即 $C^S_k≈\sum^N_n C^S_{k,n}$ 。因此，不需要显式地计算每个子地图的法线集，而只需将先前计算的法线集拼接在一起即可重建。

2数据结构回收利用

在上述基础上，当前LiDAR里程计的几个算法步骤可以从数据结构共享和重用中受益，通过消除不必要和冗余的操作，大大降低了整体系统开销。如下表所示，该系统总共需要8个元素才能成功执行scna-to-scan和scna-to-map的匹配。

在四个必需的kdtree数据结构中，只有两个需要显式构建。也就是说，源(输入)云 $T^{source}_k$ 的树可以在每次扫描采集时只构建一次，并在两个模块之间共享(因为两个源都使用相同的扫描)。对于scan-to-scan的目标树 $T^{target}_k$ ，这仅仅是前一次迭代的扫描到扫描的源树 $T^{source}_{k-1}$ ，因此可以传播。scna-to-map的目标树需要显式地构建，但由于子地图是从一组关键帧派生的，因此只有当通过我们的kNN和凸包策略选择的关键帧集从一次迭代到下一次迭代发生变化时，才需要执行此构建，例如 $S_k≉= S_{k−1}$ 。否则，可以再次重用数据结构以节省额外的计算量。另一方面，GICP所需的点协方差 $C_k$ 只需在每次扫描采集中计算一次，其数据可以直接在其他三个实例中共享。

3.双NanoGICP

为了促进扫描匹配模块之间的交叉对话，作者开发了NanoGICP，这是一种自定义迭代最近点求解器，它结合了FastGICP和NanoFLANN开源软件包，并对数据结构共享进行了额外的修改。
NanoGICP使用NanoFLANN高效构建KD-tree，用FastGICP进行点云对应匹配。

4 实验与结果

4.1 组件评估

1.基于关键帧的子图创建

比较了三种子映射方案的绝对姿态误差(APE)、处理时间和CPU负载，包括:基于半径(r = 10 m)、基于1m静态阈值的关键帧，以及基于自适应阈值的关键帧。

在图中可以看出：关键帧空间中的子地图可以通过考虑更远的点来显著减少位置误差，否则这些点将超出基于半径的方法的范围。处理时间和CPU负载显示出类似的趋势:基于半径的处理每次扫描明显慢于每次扫描74.2 ms，平均CPU负载为37.5%，而静态和自适应方案分别为21.6 ms / 10.2%和19.1 ms / 9.1%。

2.数据结构回收利用

测量和比较不同回收方案之间的处理时间和CPU使用率通过箱线图和下降的百分比扫描数据集。

回收kdtrees而不回收协方差可以略微改善处理时间和CPU百分比，而回收协方差而不回收kdtrees可以提供更显著的性能提升;这是合理的，因为文中的协方差回收方案比kdtree重用更具主动性。最后，使用表中详细说明的完整方案显着降低了这两个指标，平均处理时间为21.9 ms, CPU负载为9.5%，这可以防止任何LiDAR帧丢失。

3.NanoGICP

该基准测试测量了在100次运行中对齐两个LiDAR扫描的平均收敛时间，并与PCL的GICP实现以及FastGICP的多线程实现进行了比较。可以观察到，与FastGICP (72.88 ms)和PCL的GICP (178.24 ms)相比，NanoGICP的平均收敛速度(42.53 ms)更快。

4.2 基准测试结果

为DLO的测程精度和CPU负载与几种LiDAR和LiDAR-imu测程方法(包括BLAM， Cartographer，LIO-Mapping，LOAM和LOCUS)进行了比较，使用来自地下挑战赛城市电路的Alpha和Beta课程数据集，结果如下表所示。

结果所示：提出的方法的CPU负载被测量为远低于任何其他算法，平均和峰值使用不到一个核心。除了内部数据结构的广泛重用之外，这可能是系统生成子地图的结果。这一观察结果也可以解释DLO的绝对姿势误差(APE)和平均误差(ME)要低得多，相对姿势误差也有类似的趋势。有了这个更快的处理时间，文中的方法在Alpha和Beta过程中都优于所有其他方法，即使没有运动失真校正，在Beta过程中对于max, mean和standard deviation的精度也是两倍以上。除了更宽松的子地图方法之外，所提出的方法比其他方法更不可能掉帧，并且有处理资本以更高的分辨率匹配密集的点云。

4.3 田野实验

使用Ouster OS1将DLO集成到飞行器(上图a)上，使用Velodyne VLP-16将DLO集成到Boston Dynamics Spot(上图b)上，在两个具有感知挑战性的环境中进行了手动和自动遍历:在肯塔基州列克星敦的地下石灰岩洞穴和加利福尼亚州洛杉矶的废弃地铁中(下图)。

这两个地点都包含经常挑战感知系统的环境属性，包括光线条件差，没有特色的走廊，以及灰尘或雾等微粒的存在。尽管在废弃的地铁中穿越了超过850米的三个不同的楼层，文中的系统仅报告了10厘米的端到端漂移，这主要归功于DLO强大的关键帧方案，该方案适用于大小空间。在地下矿井的试验表明：虽然洞穴深处的环境没有任何外部照明，但DLO仍然可以在348米的自主飞行中可靠地跟踪飞行器。这些结果证明了文中的方法在现实世界中的可靠性。

5 总结

这项工作提出了Direct LiDAR Odometry (DLO)，这是一种轻量级和精确的前端定位解决方案，在极端环境下长期遍历的计算开销最小。一个关键创新是我们如何有效地使用关键帧-点云对数据库派生出用于全局姿态优化的局部子地图。这反过来又允许大量的求解器数据结构在系统模块之间共享和重用，所有这些都可以使用我们定制的NanoGICP云注册包来实现。

未来将会将研究重点放在与IMU的紧耦合上。