KISS-ICP: In Defense of Point-to-Point ICP Simple, Accurate, and Robust Registration If Done the Right Way
论文地址：https://ieeexplore.ieee.org/document/10015694/
代码：https://github.com/PRBonn/kiss-icp

一、文章概述

1.问题导向

SLAM系统经过长久的发展，系统变得越来越复杂，拥有许多参数需要调整，同时新型的激光雷达使得很多里程计不再适用。

2.目标

我们的设计既不使用复杂的特征提取技术、学习方法，也不使用循环闭合。相同的参数集适用于各种具有挑战性的场景。专注于激光里程计的本质。

3.摘要

机器人平台的稳健和准确的位姿估计，即所谓的基于传感器的里程计，是许多机器人应用的重要组成部分。虽然许多传感器里程计系统通过增加运动估计过程的复杂性而取得了进展，但我们却朝着相反的方向前进。通过去掉大部分部件并聚焦于核心元件，我们获得了一个令人惊讶的有效系统，该系统易于实现，并且可以使用不同的激光雷达传感器在各种环境条件下运行。我们的里程计估计方法依赖于点对点ICP结合自适应阈值进行对应匹配、稳健的核、简单但广泛适用的运动补偿方法以及点云分采样策略。这产生了一个只有几个参数的系统，在大多数情况下，这些参数甚至不需要调整到特定的激光雷达传感器。我们的系统使用相同的参数，在使用不同平台的各种操作条件下与最先进的方法性能相当：汽车平台、基于无人机的操作、赛格威等车辆或手持激光雷达。我们不需要集成IMU信息，仅依赖从各种3D LiDAR传感器获得的3D点云数据，从而实现了不同应用和操作条件的广泛范围。我们的开源系统在所有提供的数据集中的运行速度都快于传感器帧速率，并且是为真实世界场景而设计的。

4.贡献

提出一种简单而高效的方法来构建激光雷达里程计系统，该系统可以在机器人通过环境导航时准确地在线计算机器人的姿态。
我们表明，通过正确使用基于系统物理和传感器数据性质的基本推理的ICP，我们可以获得有竞争力的里程计。
除了运动预测、空间扫描下采样和鲁棒的核之外，我们在机器人运动估计的背景下引入了用于ICP的自适应阈值方法，这使得我们的方法有效，同时易于推广。

5.不足

不能够自适应下采样，不适用空间尺度多变的场景
轨迹不够鲁棒，不够平滑

二、方法解析

2.1 系统pipeline

输入：点云
输出：位姿与点云地图

1. 运动预测与点云去畸变
2. 点云下采样
3. 在局部地图中寻找最邻近对应点
4. 自适应ICP迭代优化
   这项工作旨在通过顺序注册扫描仪记录的点云来增量计算移动的激光雷达传感器的轨迹。我们将元件减少到构建有效、精确、稳定且相当简单的激光雷达里程计系统所需的最少数量。对于以局部的、以自我为中心的点云的形式的每个3D扫描，我们执行以下四个步骤来获得在时间t的全局姿态估计TT∈SE（3）。首先，我们应用传感器运动预测和运动补偿（通常称为去扭斜）来消除扫描期间由传感器运动引起的3D数据失真。其次，我们对当前扫描进行二次采样。第三，我们估计输入点云和参考点云之间的对应关系，我们称之为局部地图。我们使用自适应阈值方案进行对应估计，限制可能的数据关联并过滤掉潜在的异常值。第四，我们使用鲁棒的点到点ICP算法将输入点云配准到局部地图。最后，我们用配准扫描的下采样版本更新局部地图。在以下部分中，我们将详细描述这些组件。

   2.2 Motion Prediction and Scan Deskewing

   在考虑激光雷达数据之前，可以使用恒速模型的方法来计算机器人的预期运动。恒速模型假设机器人以与前一时间步相同的平移和旋转速度移动。它不需要额外的传感器(没有车轮编码器，没有IMU)，因此是最广泛适用的选择。我们的方法使用恒速模型有两个原因:首先，它是普遍适用的，不需要额外的传感器，并且避免了传感器之间时间同步的需要。第二，正如我们将在实验评估中展示的那样，当搜索数据关联和消除3D扫描的偏斜时，它可以很好地提供可靠的初始猜测。这是由于机器人激光雷达传感器通常以10赫兹至20赫兹（即每0.05秒至0.1秒）记录和传输点云。在大多数情况下，加速度或减速度（即在如此短的时间间隔内与恒速模型的偏差）相当小。根据先前姿势估计 和 ，在当前时刻t的平移速度和角速度，如下获得:
   
   
   
   
   每个点的校正点可以通过当前时间t的平移速度和旋转速度获得，从而获得去失真的点云:
   
   
   
   这种形式的扫描去扭斜，尤其是恒速模型，易于实现，通常适用，并且不需要额外的传感器、传感器之间的高精度时间同步或IMU偏差估计。

   2.3 Point Cloud Subsampling

   识别点云中的一组关键点是扫描配准的常用方法。这样做通常是为了在数据关联中实现更快的收敛和/或更高的鲁棒性。使用第一次下采样的点云存储局部点云，用第二次下采样的点云存储关键点。然而，大多数体素化方法选择每个被占用体素的中心对点云进行下采样。相反，我们发现保持原始点坐标是有利的，对于单次扫描，每个体素仅选择一个点，并保持其坐标以避免离散化错误。
   
   

   2.4 Local Map and Correspondence Estimation

   我们利用体素网格来存储3D点的子集。我们使用体素大小为v ×v ×v的网格，每个体素最多存储N个点。在配准之后，我们通过使用全局姿态估计Tt添加来自新扫描的点来更新体素网格。不更新已经包含Nmax点的体素。此外，给定当前姿态估计，我们移除最大传感器范围rmax之外的体素。因此，地图的大小将保持有限。我们使用哈希表而不是3D数组来存储体素，从而实现内存高效的表示和快速最近邻搜索

   2.5 Adaptive Threshold for Data Association

   ICP通常执行最近邻数据关联，以找到两个点云之间的对应点。当搜索关联时，通常在对应点之间施加最大距离，通常使用1m或2m的值。该最大距离阈值可以被视为异常值拒绝方案，距离大于该阈值的所有对应都被视为异常值并被忽略。KISS-ICP通过模型误差来确定距离阈值。在存在潜在加速度的情况下，我们可以估计连续扫描中对应点之间的可能点位移，表示为
   
   
   
   偏差的平移分量，计算原理如下图，位姿偏差影响最大位移的示意图
   
   
   
   
   
   
   搜寻的边界上限是
   
   
   
   它的标准差是
   
   
   
   其中M_t是模型误差值高于阈值的数量
   
   
   
   设置搜寻边界阈值为τ_t=3σ_tτ_t=3σ_t

   2.6 Alignment Through Robust Optimization

   我们的注册基于经典的点对点ICP。这种选择的优点是，我们不需要计算依赖于数据的特征，如法线、曲率或其他描述符，这些特征可能取决于扫描仪或环境。
   在局部地图搜寻最邻近点
   
   
   
   我们执行稳健的优化，最小化点对点残差的总和
   
   
   
   设计一个M-estimator用来计算每个点残差的权重
   
   
   
   采取位姿最小变化和最大迭代次数来控制优化，最后得到位姿
   
   

   2.7 Parameters

   我们的实现依赖于七个参数。所有都显示在表中。我们在所有实验中使用相同的参数。请注意，激光扫描仪的最大范围是一个取决于传感器技术的值，因此我们不认为它是一个参数。大多数其他方法使用更大的参数集:MULLS 有107个参数，SuMa 有49个参数，CT-ICP 在它们各自的配置文件中有30个参数。相比之下，我们的方法只有两个参数用于对应搜索，四个参数用于图表示和扫描子采样，一个参数用于ICP终止。