CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure

作者: Pierre Dellenbach, Jean-Emmanuel Deschaud, Bastien Jacquet, François Goulette · ICRA 2022

许可协议

• 开放获取
• 论文: CC BY 4.0

以下内容均依据论文全文，按其 CC BY 4.0 许可协议使用；其中的图已注明作者。

一句话总结

每次扫描带有起始位姿和结束位姿、并允许扫描之间跳变的扫描到地图 ICP，加上基于高程图像的回环；在 KITTI 排行榜上 RTE 为 0.59%，每次扫描 60 ms。

为何重要

该方法只需要点坐标和逐点时间戳，就能在同一个框架内处理车载、Segway 搭载和手持的 LiDAR。其驾驶配置下的里程计在单个 CPU 线程上每次扫描运行 60-80 ms，完整的 SLAM 只使用 LiDAR 数据。

问题

旋转式多线束 LiDAR 在移动中记录每一次扫描，因此必须针对传感器运动校正这些点。大多数里程计方法用恒速模型对一次扫描进行一次去畸变，这在方向或加速度突然变化时会失效；而采用平滑轨迹的连续时间方法则无法跟随真实平台的高频抖动运动。

提出的方法

CT-ICP 用一个起始位姿和一个结束位姿描述每次扫描，根据每个点的时间戳为其插值出一个位姿（旋转用 slerp，平移用线性插值），并求解一个点到平面的扫描到地图 ICP，其中的弱约束使起始位姿保持在上一个结束位姿附近，并限制加速度。局部地图是存储在稀疏体素网格中的稠密点云（每个体素最多 20 个点），回环模块在聚合局部地图的高程图像上匹配 2D 特征，用 ICP 细化匹配，并优化一个 g2o 位姿图。

关键见解

• 允许一次扫描的起始位姿与上一次扫描的结束位姿不同，可以吸收平滑的连续时间轨迹无法表示的高频运动。
• 扫描内的弹性形变在原始数据上很重要：用恒速去畸变替代它，会使 KITTI-raw 上的 RTE 从 0.55% 升至 0.79%，KITTI-360 上从 0.45% 升至 0.60%。
• 法向量和平面度权重从稠密的局部地图而非当前扫描中计算，为点到平面残差提供更丰富的邻域。
• 稀疏体素地图提供常数时间的邻域访问，而不是 kd 树的对数时间访问，这使稠密地图能够实时使用。
• 在经过运动校正的 KITTI 扫描上，所有被比较的方法都很接近；差异出现在原始的、未校正的扫描上。

主要结果

• KITTI 里程计排行榜（测试序列 11-21）：RTE 0.59%，在当时提供公开代码的方法中排名第一。
• 在有真值的序列上的平均 RTE：经运动校正的 KITTI 上为 0.53%（60 ms/扫描），KITTI-raw 上为 0.55%（65 ms），KITTI-360 上为 0.45%（70 ms）；IMLS-SLAM、MULLS 和 pyLiDAR F2M 在 KITTI-raw 上为 0.71-1.61%，在 KITTI-360 上为 0.68-1.55%。
• KITTI-CARLA（仿真）：RTE 0.09%，其他最佳里程计为 0.81%；ParisLuco：1.11%，IMLS-SLAM 为 1.26%。
• 高频运动配置：Newer College 上 RTE 为 0.55%（pyLiDAR F2M 为 2.20%），NCLT 2012-01-08 上为 1.17%（pyLiDAR F2M 为 19.80%），每次扫描分别耗时 430 ms 和 180 ms。
• 回环使 KITTI-raw 00 上的 ATE 从 6.22 m 降至 0.66 m，KITTI-360 00 上从 29.87 m 降至 1.07 m，ParisLuco 上从 29.16 m 降至 9.65 m；在 KITTI-CARLA Town01 上保持在 0.2 m 左右（0.21 m 到 0.26 m）。
• 每次构建并匹配高程网格平均耗时 1.1 s，发现回环时另需 1.2 s 进行位姿图优化。

局限

• 回环要求传感器运动大致在平面内，并要求外参标定使 z 轴与地面法向对齐，这限制了所支持的传感器配置和运动。
• 回环在主线程上运行（每个高程网格 1.1 s，加上每次位姿图优化 1.2 s），因此完整的 SLAM 尚不是实时的；作者表示使用独立线程就能使其实时。
• 局部地图不会在滑动窗口中遗忘点，因此一次糟糕的配准会污染它；防范这一问题的鲁棒配置会增加运行时间（Newer College 上每次扫描 430 ms，NCLT 上 180 ms）。
• 所有实验使用的 Gauss-Newton 求解器采用手工推导的雅可比矩阵，因此其约束无法更改；更灵活的 Ceres 选项需要多个线程才能实时使用。
• 回环候选的数量（10）必须根据预期漂移进行调整，并且在 KITTI-CARLA 上回环后 ATE 没有改善。

对领域的影响

入围 ICRA 2022 杰出论文奖，301 次引用。论文以 C++ 发布了 CT-ICP 里程计（github.com/jedeschaud/ct_icp），并称其为公开且宽松许可的开源代码；回环检测和位姿图后端被放入 pyLiDAR-SLAM（github.com/Kitware/pyLiDAR-SLAM），并按其所述的形式在线提供了所用的数据集。

摘要

多线束 LiDAR 传感器在机器人领域的应用日益增多，尤其是在自动驾驶汽车上用于定位和感知任务，而这两项任务都依赖于构建精确环境地图的能力。为此，我们提出了一种新的实时、仅使用 LiDAR 的里程计方法，称为 CT-ICP（Continuous-Time ICP，连续时间 ICP），并通过一种新颖的回环检测流程将其扩展为完整的 SLAM。该方法的核心是在扫描匹配中引入扫描内的连续性与扫描之间的不连续性的结合。它既允许在配准过程中对扫描进行弹性形变以提高精度，又通过不连续性提高了对高频运动的鲁棒性。

我们在该里程计之上构建了一个完整的 SLAM，使用一种基于高程图像 2D 匹配的快速纯 LiDAR 回环检测，提供带有回环约束的位姿图。为了展示该方法的鲁棒性，我们在七个数据集上对其进行了测试：KITTI、KITTI-raw、KITTI-360、KITTI-CARLA、ParisLuco、Newer College 和 NCLT，涵盖驾驶和高频运动场景。CT-ICP 里程计和回环检测均已在线公开。在 KITTI 里程计排行榜上，CT-ICP 目前在提供公开代码的方法中排名第一，平均相对平移误差（Relative Translation Error，RTE）为 0.59%，在单线程 CPU 上每次扫描的平均时间为 60ms。

链接

• arXiv 2109.12979: https://arxiv.org/abs/2109.12979
• DOI 10.48550/arXiv.2109.12979: https://doi.org/10.48550/arXiv.2109.12979
• PDF: https://arxiv.org/pdf/2109.12979v1
• 代码: https://github.com/jedeschaud/ct_icp