经典文献阅读之--MAD-ICP(仅由六个参数控制的强大且信息丰富的LiDAR里程计)

1. 笔者感悟与思考

优秀的LO系统很多，开源方案也有很多，但是都有各种各样的经验参数需要调试。换了场景或者传感器，可能之前很好的一组参数也无济于事，懂得都懂，这也是为啥总调侃算法工程师慢慢就变成了调参工程师。

但是这篇文章是笔者见过为数不多的，仅有六个参数控制的一个LO方案，并且这6个参数具有普适性，换了场景和传感器也可以使用。并且这也是一个非常轻量化的LO系统，思路简单但是非常巧妙，是LO领域一个非常好的方案，值得学习和引起大家的思考。

2. 摘要

激光雷达里程计（LiDAR Odometry）是从连续激光扫描中估计传感器自身运动的任务。这个问题已经得到了研究社区二十多年的关注，如今有许多有效的解决方案可供使用。这些系统中的大多数隐含地依赖于对操作环境、所使用的传感器和运动模式的假设。当这些假设被违反时，许多众所周知的系统往往会表现不佳。本文提出了一种激光雷达里程计系统，能够克服这些限制，在不同的操作条件下良好运行，同时实现与特定领域方法相当的性能。我们的算法遵循众所周知的ICP范式，利用基于PCA的kd树实现提取关于正在配准点云的结构信息，并计算对齐的最小化度量。通过管理基于估计的跟踪姿态的不确定性来限制漂移。根据跟踪姿态的估计不确定性管理局部地图来限制漂移。

3. 主要贡献

• 设计了一种多功能且高效的数据结构，能够以单一表示形式对所有必要的数据进行编码。kd树的设计是为了方便每一个所需的操作，例如平面分割、增量数据关联和局部地图管理

• 基于最大化姿态信息设计了自适应局部地图更新的信息感知标准，旨在限制注入局部地图的噪声

• 一个强大的、实时的C++实现，在不同的环境中永远不会丢失跟踪，并且无需参数调整

3.1 pipeline简述

该方案的的主要思想是使用kd树来执行激光雷达里程计中的所有相关操作。对于激光雷达每提供一个新的点云 ，就会构建一个kd树。这个预处理的结果是编码了点云平面分割的数据结构，并允许最近邻查询。每个的叶子将包含点云的一小部分。任务是将所有终端节点与局部地图匹配，直到整个树（代表）被注册。这个过程使用增量式数据关联策略，通过点到平面的ICP完成的。该方案kd树的独特之处在于，整个数据结构可以在不改变搜索能力的情况下进行变换，从而避免了在注册后重建。变换后的kd树被维护在局部地图中，由于信息感知更新准则，始终保持可靠性。最后，估计速度这一状态量，目的是为了点云去畸变和为ICP提供良好的初始预测。整个算法如下：

3.2 MAD-ICP算法核心思想解读

3.2.1 点云表面法线分割

每个输入点云被转换成一个kd树。这里是点的平均值，是表面法线，是最大展开方向。和分别代表节点的左子树和右子树，并对叶子设置为。

构建过程是递归的，流程如下：

• 计算点云的均值和协方差；

• 计算协方差的特征向量；

• 设置最大变化方向为对应于最大特征值的特征向量，法线方向为与最小特征值相关联的特征向量；

• 通过将旋转应用于点云，并沿三个轴扫描最小和最大值，计算定向边界框；

• 如果边界框的最大维度小于阈值米，意味着点在空间中集中；因此递归停止；

• 如果节点不是叶子并且覆盖了较大区域（），则根据谓词将点云分成左右两组，基于此划分平面；算法继续对这两组和进行递归，计算左子树和右子树和。

图2展示了最终平面分割的一个示例。这个过程在雷达模式足够密集的情况下可以得到准确的表面法线。但是，在点云稀疏区域，叶子可能由少数噪声点定义，使得它们的法线不可靠。为了解决这个问题，在树构建过程中，当节点足够平坦时，即其最短维度的长度小于参数时，节点的法线会传播并分配给它的子节点。这种技术允许在不增加树构建计算复杂性的情况下，在大多数情况下估计表面法线。在实验中，固定米。对于kd树构建的伪代码，可参考算法2和算法3。

3.2.2 数据关联和ICP估计

对齐过程遵循一个ICP框架，它寻找一个变换，以实现参考模型和当前测量之间的最佳对齐。在系统中，模型由一组在世界坐标系中表示的kd树，而时间的测量由在激光雷达坐标系中中的kd树表达。

ICP交替进行两个步骤：数据匹配和优化。在数据关联期间，的每个叶子根据当前变换估计进行变换，并在其最近的邻居中找到匹配项。让表示在世界坐标系中的时间的传感器姿态，让是中的一个通用叶子。在树中的搜索是一个递归过程，遵循与树构建过程中使用的方程（1）相同的谓词：

搜索从模型中每棵树的根开始，并根据方程（2）指示的子节点迭代继续。递归在到达叶子时停止，因此和形成了一个配对。

此时，需要消除异常值。通过验证和之间的欧几里得距离是否大于搜索半径来完成的，搜索半径计算如下：

在这里，表示叶子的最大大小，表示一个参数，随着查询的范数增长，它增加了搜索大小半径。通常，3D ICP寻求估计一个由旋转矩阵和平移向量组成的变换矩阵。在本文的案例中，估计变换，表示在世界坐标系中时间的传感器姿态。这是通过迭代最小化点到平面误差项使用高斯-牛顿法来计算的。给定一个匹配，点到平面误差计算为：

这显示了投影到法线上和之间的差异。此外，采用了Huber稳健损失，并在实验中将权重参数设置为。

收敛后，可以根据[21]找到解的高斯协方差近似。更具体地说，由于我们的方法在Lie代数se(3)上进行优化，估计的逆协方差是高斯-牛顿算法的系统矩阵。

由于现代激光雷达提供的密度，点到平面误差表现出鲁棒性能，将优化过程引向局部地图中定义的明确表面。

多功能的kd树由分割平面和表面法线组成，可以由当前估计直接变换。具体来说，这在优化过程中发生了多次：在迭代过程中，叶子被变换以进行下一次对应搜索；在最后，当整个kd树及其分割平面通过最终估计的变换到世界坐标系时。树变换不会改变数据关联能力，并且以有限的计算工作量执行。最后，变换后的kd树被推入候选帧队列中，用于局部地图更新。伪代码如下：

3.3.3 局部地图表示和更新

局部地图由一组kd树森林组成，每棵树对应一个关键帧。这种由独立kd树组成的模型，保持了每个点云的准确性和可靠性，不改变表面法线。与增量式kd树或体素网格相比，我们的方法更简单，因为更新局部地图仅通过将新树推入其中来完成。保持对环境的可靠表示对于有效的配准至关重要。许多现有的工作会使用每个新观测来更新局部地图。相反，我们认为仅在需要时更新可以避免不断地向我们的模型中注入噪声。此外，局部地图更新策略是信息感知的，意味着只有不确定性最小的匹配被考虑，避免在优化阶段插入匹配不佳的树。

原则上，局部地图更新可以由姿态变化或空间的变化触发。第一种选择难以调整且缺乏泛化，因为它取决于激光雷达传感器的特定特性。例如，具有全方位视场（FoV）的传感器在旋转时不应触发局部地图更新，因为观察到的空间部分对旋转不变。因此，在本文提出的方案中，当时，触发局部地图更新，其中p是中与匹配的叶子的百分比，参数设置为0.8。

当发生这种情况时，从帧候选列表中提取不确定性最小的帧。为了比较多个候选者相关联的协方差矩阵，已经提出了几种函数——称为最优性准则[22]；在我们的算法中，采用矩阵行列式，也称为D-最优性准则[23]。这个候选者成为我们的新关键帧，并被推入局部地图中。在[24]中也考虑了姿势不确定性的行列式，但其目的是触发新的关键帧选择。相反，我们通过最小化来选择新的的关键帧，根据当前观测来触发这个事件。需要指出的是，在重新访问已经短期探索过的区域时，算法的行为退化为定位过程，锚定在以前创建的关键帧上，无需新的更新。

3.3.4 速度估计和初始值的精细化

速度估计有两个目的：第一，通过去畸变当前点云来补偿运动失真；第二，预测下一个姿态，为ICP算法提供良好的初始猜测。

激光雷达的完整点云采集实际上是连续的。因此，在数据采集期间的运动反映在点云畸变中，导致点对齐不良，考虑到传感器的扫描频率。运动补偿对于纠正这种效果至关重要，确保在3D环境中准确的空间表示[25]。如[26]所述，鉴于每个点云片段的采集速率很高，我们采用经典的恒定运动模型进行去畸变。

不同地，考虑到每个点云被完全测量的速率较低，采用平滑技术，以便为里程计估计过程提供良好的初始猜测。解决方案涉及使用高斯-牛顿法计算速度，如ICP部分所述。使用最后n个姿态对当前时间k的平移和旋转速度和进行平滑。在实验中，我们设置n = 10。具体来说，给定从当前时间瞬间k到先前一个时间i（）的变换，按如下方式计算误差：

其中运算符是旋转矩阵的对数映射。

4. 实验评估

在本节中，我们报告了我们的方法在几个公共数据集上的广泛测试结果。为了定量比较我们的算法与其他最新技术(SOTA)方法，我们采用了KITTI基准测试[2]和VBR基准测试[27]来分别评估里程计估计的局部精度和鲁棒性，利用相对姿态误差(RPE)。我们在一台配备有i9-13900kf CPU和16个物理核心的机器上进行了所有实验。我们的系统由一个最小且鲁棒的LO组成，仅使用一组参数)。定量结果表明，我们的LO在性能上与其他SOTA方法相当或更好。实验场景包括高速公路、狭窄建筑、楼梯、静态和动态设置在内的广泛异构环境。我们还考虑了各种激光雷达传感器和任意运动轮廓（包括不同类型的机器人、汽车和手持设备）。

我们在以下数据集上进行了广泛的实验活动：KITTI[2]、Mulran[28]、Newer College[29]、Hilti 2021[30]。本实验评估包括与四种其他已作为开源发布的激光雷达里程计的比较：F-LOAM[31]、MULLS[4]、CT-ICP[13]和KISS-ICP[5]。

4.1 参数

该方案仅依赖于六个参数，在不同 LiDAR 传感器或环境之间转换时不需要调整。具体如下：

4.2 算法对比结果

图3：每种比较方法的累积误差曲线，使用所有序列RPE [%] 进行计算。对于水平轴上的给定误差值，垂直轴显示一种方法实现较低误差的序列数量

图4：所用数据集的某些序列中每种方法的定性绝对一致性

表2：使用 KITTI 基准测试在不同的公共数据集上获得的结果。该表显示了相对姿态误差 (RPE)。对于每个数据集，显示平均值，最后显示总平均值

5. 总结与展望

本方案提出的激光雷达里程计（LO）的关键是在多个阶段巧妙地重用先前计算的信息：用于划分空间和构建搜索结构的主成分分析（PCA）也用于对表面进行分类和分配法线。同样，由配准得出的费舍尔信息矩阵用于自适应本地地图更新，以限制漂移。大多数计算都由输入数据通过沿不同阶段计算的隐含统计数据控制。由此产生了一个鲁棒、简单和自适应的LO，它解决了大多数现有解决方案的局限性，这些解决方案过于适合特定的数据领域。我们通过一系列广泛的比较实验支持我们的方案。作为未来的工作，以一种连续的方案融合惯性传感器，以使系统更加鲁棒。