基本信息

题目：KISS-ICP: In Defense of Point-to-Point ICP Simple, Accurate, and Robust Registration  If Done the Right Way

来源：2024 RAL

学校：University of Bonn

是否开源：https://github.com/PRBonn/kiss-icp

摘要：点-点匹配ICP-激光SLAM

        机器人平台的鲁棒和准确的位姿估计，即所谓的基于传感器的里程计，是许多机器人应用中必不可少的一部分。虽然许多传感器里程计系统通过增加自运动估计估计过程的复杂性而取得了进展，但我们却朝着相反的方向发展。通过去除大部分部件并聚焦核心元素，我们获得了一个令人惊讶的有效系统，该系统实现简单，可以使用不同的LiDAR传感器在各种环境条件下操作。我们的里程计估计方法依赖于点到点的ICP结合自适应阈值的对应匹配，采用鲁棒核，简单但广泛适用的运动补偿方法和点云欠采样策略。这使得系统只需要很少的参数，在大多数情况下甚至不需要调整到特定的激光雷达传感器。我们的系统在使用相同参数的不同平台(汽车平台、基于无人机的操作、类似摄位车的车辆或手持激光雷达)的各种操作条件下与最先进的方法相媲美。我们不需要集成IMU数据，仅仅依靠从广泛的3D LiDAR传感器中获得的3D点云，从而实现了不同应用和操作条件的泛化性。

Introduction

        里程计估计对于任何需要在未知环境中自主导航的移动机器人来说都是必不可少的模块。在LiDAR传感领域，目前的里程计管线通常使用某种形式的迭代最近点( ICP )来增量估计姿态[ CT-ICP ]，[ NICP ]，[ 31 ]，[ LOAM ]。尽管在过去的三十年里，激光雷达里程计一直是一个活跃的研究领域，但当前系统的设计通常与机器人运动和环境结构[ LeGO-LOAM ]的假设相耦合以获得准确、稳健的对齐结果。据我们所知，任何现有的3D LiDAR里程计方法都不受参数调整的限制，可以在不同的场景下使用任意的LiDAR传感器，支持不同的运动轮廓，从而支持不同类型的机器人，如地面和空中机器人。

[31] I. Vizzo, X. Chen, N. Chebrolu, J. Behley, and C. Stachniss. Poisson Surface Reconstruction for LiDAR Odometry and Mapping. In Proc. of the IEEE Intl. Conf. on Robotics & Automation (ICRA), 2021.

        我们的设计既不使用复杂的特征提取技术、学习方法，也不使用闭环。相同的参数集工作在各种具有挑战性的场景中，例如具有许多动态对象的机器人汽车的高速公路驱动器、无人机飞行、手持设备、segways等。因此，我们在激光雷达里程计估计的主流研究中退一步，将重点放在将成分减少到其本质上。这使得我们的系统在各种真实世界的场景中表现异常出色，见图1。

        本文的主要贡献是提出了一种简单而高效的方法来构建激光雷达里程计系统，该系统可以在环境中准确地在线计算机器人的位姿。我们确定了核心组件，并适当评估了不同模块对此类系统的影响。除了运动预测、空间扫描下采样和鲁棒核外，我们引入了一种自适应阈值的ICP方法，使我们的方法有效，同时也易于推广。

        我们提出了三个关键的贡献：我们利用点对点ICP的"保持小而简单"的方法是

• 与最先进的里程计系统相媲美；
• 可以在具有相同系统配置的各种环境和运动轮廓中精确地计算机器人的里程计；
• 提供了一种有效的解决运动畸变的方法，而不依赖于IMU或轮式里程计。

        总而言之，"好的老式点对点ICP "是一个令人惊讶的强大工具，如果基本组件做得很好，就几乎不需要移动到更复杂的方法。

Related Works

        在过去的三十年里，点云配准一直是一个活跃的研究领域。ICP算法可以解决寻找一个变换将两个不同的点云带入到一个参考帧下，是摄影测量中绝对定向问题的一个特例（这个对于ICP的描述倒是挺新颖的）。ICP通常由两部分组成。第一种是寻找点云之间的对应关系。第二个方法是计算一个变换，使得从第一步开始定义在对应关系上的目标函数最小化。人们重复这个过程，直到满足一个收敛准则。大多数ICP变体[ 1 ]，[ CT-ICP ]，[ Imls-slam ]，[ NICP ]，[ MULLS ]，[ LOAM ]在数据关联模块中使用最大距离阈值加一个鲁棒核[ 6 ]和最大迭代次数。相比之下，我们通过对系统运动学和数据性质的推理，结合鲁棒核，提出了一种适应变化场景的阈值估计方法。我们避免了控制ICP的迭代次数，以实现更好的泛化性。

[1] J. Behley and C. Stachniss. Efficient Surfel-Based SLAM using 3D Laser Range Data in Urban Environments. In Proc. of Robotics: Science and Systems (RSS), 2018.

[6] N. Chebrolu, T. L ̈abe, O. Vysotska, J. Behley, and C. Stachniss. Adaptive Robust Kernels for Non-Linear Least Squares Problems. IEEE Robotics and Automation Letters (RA-L), 6:2240–2247, 2021.

        ICP可用于从传感器(如RGB - D相机[ KinectFusion ]或LiDAR [ CT-ICP ] )的流数据中获得里程计估计。在本工作中，我们主要关注LiDAR里程计估计问题，尽管所提出的思想可以很容易地扩展到其他距离感知技术中。 

        几乎所有的现代SLAM系统都建立在里程计算法之上。LOAM 提出了激光雷达里程计和建图，通过将平面特征和边缘特征注册到稀疏特征地图来计算机器人的里程计。LOAM启发了许多其他工作[ LIOSAM ]，例如Lego - LOAM，它增加了地面约束以提高精度，最近的F-LOAM [ 33 ]，它用更有效的优化技术修改了原来的方法，使其能够更快地运行。然而，这些方法依赖于手工调优的特征提取，通常需要依赖于传感器分辨率、环境结构等进行繁琐的参数调优。相比之下，我们仅仅依靠点坐标来去除这种依赖于数据的参数自适应。

        Behley和Stachniss [ 1 ]提出了基于surfel的方法SuMa来实现LiDAR里程计的估计和建图。它也被扩展到解释语义[ SuMa++ ]和显式处理动态对象[ 7 ]。与基于表面的建图不同，IMLS - SLAM 选择隐式移动最小二乘曲面作为地图表示。沿着这些思路，Vizzo等人[ 31 ]利用三角网格作为内部映射的表示。以上所有的方法都依赖于一个点到平面的metric来配准连续的激光帧。这就需要进行正态估计，从而引入了额外的数据依赖参数。此外，含噪的3D信息会对正常的计算和随后的配准产生负面影响。我们将证明，通过最小化一个更简单的点对点度量，我们可以获得on-par或更好的里程计性能。此外，这种设计选择使我们能够将内部地图表示为体素化、降采样的点云，简化了实现。

[7] X. Chen, S. Li, B. Mersch, L. Wiesmann, J. Gall, J. Behley, and C. Stachniss. Moving Object Segmentation in 3D LiDAR Data: A Learning-based Approach Exploiting Sequential Data. IEEE Robotics and Automation Letters (RA-L), 6:6529–6536, 2021.

[31] I. Vizzo, X. Chen, N. Chebrolu, J. Behley, and C. Stachniss. Poisson Surface Reconstruction for LiDAR Odometry and Mapping. In Proc. of the IEEE Intl. Conf. on Robotics & Automation (ICRA), 2021.

        最近，一些新的方法[ CT-ICP ]，[ MULLS ]，[ LIOSAM ]被提出来解决里程计估计问题。这些工作大多集中在系统的运行时操作以及准确性上。MULLS提出了一种多指标综合评价法系统，以每次运行调节多个参数为代价，在许多具有挑战性的场景中获得了良好的结果。CT-ICP提出了一种新的方法，将运动不失真引入到配准中显示出巨大的效果，但增加了更多的复杂度。此外，机器人的运动轨迹轮廓必须是先验已知的，例如，一辆汽车将具有不同于segway平台的轮廓。我们对复杂的优化技术的需求提出了挑战，以应对只需要常速度模型的运动变形。此外，我们的系统只依赖于几个参数，并且我们不需要事先知道运动轨迹轮廓。

（翻来覆去，其实也只对比了四五篇ICP的方法）

III. KISS-ICP – KEEP IT SMALL AND SIMPLE

        KISS-ICP 是一种轻量级、有效且鲁棒的LiDAR里程计方法，目标是通过最小化的组件实现高精度、简单易用的点云配准系统。系统通过对连续的LiDAR点云进行序列化配准，估计传感器的全局位姿  。这里的 SE(3) 表示三维空间中的刚体变换，包括旋转（SO(3)）和位移（ ）。

整个算法分为四个主要步骤：

1. 运动预测与点云去畸变（Deskewing）：利用运动预测（如恒定速度模型）对点云进行去畸变处理，消除传感器运动引起的点云扭曲。
2. 点云降采样：通过体素网格（Voxel Grid）对点云进行降采样，减少计算量并提高配准效率。
3. 对应点估计：通过自适应阈值方法，在当前点云与局部地图（Local Map）之间寻找对应点，剔除异常点。
4. 点云配准：使用鲁棒的点到点ICP（Iterative Closest Point）算法，将当前点云配准到局部地图，更新全局位姿估计。

A. Step 1: Motion Prediction and Scan Deskewing

        我们主张重新思考在连续记录数据的移动机器人背景下的点云配准。人们不应该将其视为配准任意点对的三维点云。取而代之的是通过对连续扫描进行配准来估计机器人的实际运动偏离其预期运动的程度。

        在考虑LiDAR数据之前，可以使用不同的方法来计算机器人的期望运动。最受欢迎的三种选择是恒速模型、通过编码器获得的轮式里程计和基于IMU的运动估计。常速模型假设机器人以与上一时间步相同的平移和旋转速度运动。它不需要额外的传感器因此(没有轮式编码器,就没有IMU)是应用最广泛的选项。

        我们的方法使用恒速模型有两个原因：首先，它是普遍适用的，不需要额外的传感器，并且避免了传感器之间时间同步的需要。其次，正如我们将在实验评估中所展示的那样，它的效果足够好，可以在搜索数据关联和进行3D扫描时提供坚实的初始猜测。这是由于机器人LiDAR传感器一般以10 Hz ~ 20 Hz，即每0.05 s ~ 0.1 s记录一次流式点云。在大多数情况下，加速或减速，即在如此短的时间间隔内发生的与等速模型的偏差是相当小的。如果机器人加速或减速，则机器人位姿的常速估计会略微偏离，因此，我们需要通过配准来校正这个估计。这些加速度决定了(静态) 3D点的可能位移。

总结：

目标：LiDAR传感器在扫描过程中会因自身运动导致点云畸变（例如，传感器在移动时记录的点云会“拉伸”或“扭曲”）。这一步骤通过预测传感器的运动并对点云进行去畸变处理，生成一个“静止”状态的点云。

方法：KISS-ICP 使用恒定速度模型（Constant Velocity Model）作为运动预测的基础。假设机器人在短时间内（通常为 0.05s 至 0.1s 的LiDAR扫描周期）保持恒定的平移速度 vt 和角速度 ωt。这种假设的优点是：

• 不需要额外的传感器（如轮式编码器或IMU）。
• 不需要高精度的时间同步。
• 对于大多数机器人应用，短时间内的加速度变化较小，恒定速度模型足以提供合理的初值。

        注意，也可以使用轮式里程计或基于IMU的运动预测方法来计算每个时间步的vt和ωt。这不会改变我们的做法。然而，我们使用恒定速度作为普遍适用的方法。这种形式的去畸变，特别是在常速模型下，不需要额外的传感器，传感器之间的高精度时间同步，也不需要估计IMU的bias。这种方法往往比更复杂的补偿系统表现得更好[ CTICP ]，至少在扫描开始和结束之间的运动很小的情况下，就像大多数机器人应用一样。

B. Step 2: Point Cloud Subsampling

        识别点云中的一组关键点是扫描配准的常用方法[ 14 ]。它通常是为了在数据关联中实现更快的收敛和/或更高的鲁棒性。然而，点云的复杂滤波通常伴随着额外的一层复杂度和参数，往往需要进行调整。

[14] T. Guadagnino, X. Chen, M. Sodano, J. Behley, G. Grisetti, and C. Stachniss. Fast Sparse LiDAR Odometry Using Self-Supervised Feature Selection on Intensity Images. IEEE Robotics and Automation Letters (RA-L), 7(3):7597–7604, 2022.

（好像就是Voxel 降采样，没看到什么特殊的处理）

C. Step 3: Local Map and Correspondence Estimation

（描述的并不是很详细，如何获取匹配点的？还是最近邻匹配，作为局部匹配，很有可能有错误匹配，并没有看到针对错误匹配的外点做的工作。个人感觉，其实不如以Quatro为代表的全局匹配，这个可实打实的在保证错误匹配的基础上保证了鲁棒的点云配准论文review 点云配准 Quatro_quatro算法-CSDN博客论文Review Registration Quatro++ | Hyungtae Lim 出品！必属精品！长文！| KAIST | IJRR | 可用于回环检测-CSDN博客） 

D. Adaptive Threshold for Data Association

（原来在这里才开始说匹配的，上一小节只是讲了如何和local map对应）

        ICP通常执行最近邻数据关联来寻找两片点云之间的对应点。在搜索关联时，通常在对应点之间施加最大距离，通常使用1 m或2 m的值。这个最大距离阈值可以看作是一个异常值剔除方案，因为所有距离大于这个阈值的对应关系都被认为是异常值而被忽略。

        该阈值τ所需的值取决于期望的初始位姿误差，场景中动态物体的数量和类型，以及一定程度上的传感器噪声。它通常是启发式选择的。基于Sec .中关于匀速运动预测的考虑。而III - A，我们可以通过分析里程计随时间偏离运动预测的程度，从数据中估计一个可能的极限。该偏差T恰好对应于将局部ICP校正应用于预测位姿(但事先并不知道)。直观上，我们可以观察到机器人的加速度在ΔT的量级上，如果机器人没有加速，那么ΔT会有一个很小的量级，往往在零附近，这意味着恒速假设成立，ICP不必做任何修正。

 E. Step 4: Alignment Through Robust Optimization

        我们的配准基于经典的点对点ICP。这种选择的优点是我们不需要计算数据依赖的特征，如法线、曲率或其他描述符，这些特征可能依赖于扫描仪或环境。此外，对于噪声或稀疏的LiDAR扫描仪，法线等特征往往不是十分可靠。因此，在对准过程中忽略法向量等参数是一个明确的设计决策，使我们的系统能够很好地推广到不同的传感器分辨率。

实验

硬件平台

数据集

• KITTI 里程计数据集：用于与先进LiDAR里程计方法的性能比较，包含城市、高速公路等场景，采用 Velodyne HDL-64E 传感器。
• MulRan 数据集：测试不同传感器（Ouster OS1-64）在城市环境中的性能，包含多条相似轨迹。
• NCLT 数据集：基于 Segway 平台的校园场景数据集，测试在非汽车场景中的表现。
• Newer College 数据集：手持设备采集的数据（Ouster OS0-128 和 OS1-64 传感器），包含短时和长时间实验，测试在复杂运动模式下的鲁棒性。

benchmark

• CT-ICP：一个完整的SLAM系统，包含闭环检测，性能优异但复杂度较高。
• MULLS、SuMa、F-LOAM：其他开源的LiDAR里程计方法，通常依赖IMU或特定参数配置。

 B. Performance on the KITTI-Odometry Benchmark

C. Comparison to State-of-the-Art Systems on Other Datasets

初体验

TODO