【荐闻】MAD-ICP：一种基于激光雷达里程计（LO）的新型方法

论文：MAD-ICP: It Is All About Matching Data -- Robust and Informed LiDAR Odometry

作者：Simone Ferrari, Luca Di Giammarino, Leonardo Brizi, Giorgio Grisetti

论文地址：https://arxiv.org/abs/2405.05828

代码地址：https://github.com/rvp-group/mad-icp

方法介绍

本文提出了MAD-ICP，这是一种基于激光雷达里程计（LO）的新型方法。MAD-ICP利用了一种高效且通用的kd-tree数据结构，并结合估计的姿态不确定性动态维护一个稳健的环境模型。

1.1 kd-tree的构建

对于激光雷达提供的每个新云Ck，构建一个kd-tree Tk。这个预处理的结果是一个数据结构，它对点云进行了平面分割（Sec. III-A），并允许进行最近邻查询。每个Tk的叶节点将包含点云的一个小子集。任务是将所有终端节点与局部地图进行匹配，直到整个树（表示Ck）都进行了注册。这个过程是通过点-平面ICP（Iterative Closest Point）和增量数据关联策略（在Sec. III-B中描述）来完成的。

1.2 数据关联和ICP估计

ICP交替进行两个步骤：数据匹配和优化。在数据关联期间，基于当前变换估计，将Tk的每个叶节点转换到世界坐标系w中，并在模型M中找到其最近邻。搜索是一个递归过程，遵循与树构建过程中使用的相同谓词。

1.3 局部地图表示和更新

局部地图由一组kd-tree组成，每个树对应一个关键帧。这个模型由独立kd-tree组成，保留了每个点云的准确性和可靠性，同时保持了表面法线的不变性。我们的方法比增量kd-tree或体素栅格更简单，因为更新局部地图仅通过简单地将新树推入它来完成。

1.4 速度估计和初始猜测优化

速度估计有两个目的：首先，通过去失真当前点云来补偿运动畸变；其次，预测下一个姿态，为ICP算法提供良好的初始猜测。我们使用高斯牛顿法计算速度，并使用最后n个姿态来平滑平移和旋转速度。

总结

MAD-ICP利用kd-tree数据结构进行平面分割、增量数据关联、局部地图管理和姿态估计。它利用PCA对点云进行分割，形成代表小平面区域的叶节点。这种分割有助于平面分割和数据关联。

在树构建过程中，通过将节点的法线传播到其子节点，可以在大多数情况下估计表面法线，而不增加树构建的计算复杂性。在数据关联和ICP估计阶段，使用点-平面误差度量进行ICP，并采用增量数据关联策略，以减少错误匹配的影响。在局部地图表示和更新阶段，使用kd-tree森林作为局部地图，并通过最大化姿态信息来选择关键帧进行更新。这样可以避免不断注入噪声。

最后，计算速度以补偿运动畸变，并预测下一姿态，为ICP算法提供良好的初始猜测。