CT-ICP: 带有闭环的实时且灵活鲁棒的LiDAR里程计-编程知识

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

作者：Pierre Dellenbach, Jean-Emmanuel Deschaud, Bastien Jacquet, and Franc¸ois Goulette

编辑：点云PCL

代码：

https://github.com/jedeschaud/ct_icp

https://github.com/Kitware/pyLiDAR-SLAM

欢迎各位加入知识星球，获取PDF论文，欢迎转发朋友圈。文章仅做学术分享，如有侵权联系删文。

公众号致力于点云处理，SLAM，三维视觉，高精地图等领域相关内容的干货分享，欢迎各位加入，有兴趣的可联系dianyunpcl@163.com。未经作者允许请勿转载，欢迎各位同学积极分享和交流。

摘要

多线LiDAR传感器在机器人技术中的应用越来越广泛，特别是在自动驾驶汽车的定位和感知任务中。然而，感知与定位任务密切相关，机器人能否构建其环境的精细地图也是其中关键，因此，本文提出了一种新的实时LiDAR里程计方法，称为CT-ICP，并提供了一个具备闭环的完整SLAM系统。CT-ICP的原理是使用灵活鲁棒的轨迹表达方式，在扫描数据内部保持姿态的连续性，在扫描之间保持姿态的不连续性，以增强对传感器高频运动的鲁棒性。配准过程基于单帧点云到地图的匹配，使用稀疏体素结构化的密集点云作为地图以实时运行。同时，利用高程图进行快速的闭环检测，并通过图优化来优化姿态，从而实现纯LiDAR的完整SLAM。为了展示该方法的鲁棒性，我们在七个数据集上进行了测试：KITTI、KITTI-raw、KITTI-360、KITTI-CARLA、ParisLuco、Newer College和NCLT，涵盖了行驶和高频运动场景。CT-ICP里程计部分采用C++实现并提供在线代码，闭环检测和姿态图优化部分使用Python编写，并在pyLiDAR-SLAM框架中提供在线代码，CT-ICP目前在KITTI里程计排行榜中排名第一，并具有平均相对平移误差(RTE)为0.59%，在单线程CPU上每次扫描的平均时间为60毫秒。

主要贡献

文提出了一种新的灵活鲁棒的轨迹表达形式，其中单帧点云数据内部的姿态保持连续性，相邻扫描之间的姿态则不连续，在实践中，这是通过点云到地图的配准分辨率来定义的，由每帧扫描数据的两个姿态参数化（用于扫描开始和结束的姿态），并且在前一扫描的结束姿态和当前扫描的开始姿态之间有一个接近性约束。

图1：顶部是彩色的LiDAR扫描，每个点的颜色取决于其时间戳（从最旧的蓝色到最新的红色）。通过在扫描开始和结束处的两个姿态的联合优化和根据时间戳进行插值，将扫描弹性地变形以与地图（白色点）对齐，从而创建连续时间的扫描到地图的里程计，下方展示了我们轨迹的形式，其中扫描内部的姿态保持连续性，相邻扫描之间的姿态则不连续。

图1展示了一个扫描（彩色点）与地图（白色点）的配准结果，颜色从蓝色到红色变化，表示CT-ICP方法使用的每个点的相对时间戳，根据里程计将新的扫描数据对应到建筑物上。文章的主要贡献是提出了：

* 基于扫描内部姿态连续性和扫描之间姿态不连续性的新型灵活的LiDAR里程计。

* 基于稀疏体素结构的稠密点云局部地图，以实现实时处理速度。

* 在行驶和高频运动场景下进行的大规模实验，所有实验都可通过公开和宽松的开源代码进行复现。

* 一种快速的闭环检测方法，与姿态图优化后端集成，构建完整的SLAM，并集成到pyLiDAR-SLAM中。

主要内容

A. 里程计

CT-ICP里程计通过两个姿态对当前扫描点云进行参数化：扫描开始的姿态W_b = (R_b, t_b)（其中R表示旋转，t表示平移，b表示开始）和扫描结束的姿态W_e = (R_e, t_e)（e表示结束）。对于在扫描的第一个时间戳tb和最后一个时间戳te之间捕获的每个传感器测量值（时间t属于[t_b, t_e]），传感器的姿态通过在扫描的两个姿态之间进行插值来估计。这些姿态将LiDAR坐标系L(t)中的点转换到世界坐标系W = L(0)。与其他连续时间轨迹方法不同，新扫描的姿态W_b与上一扫描的结束姿态W_ne−1不匹配。在优化中添加了接近性约束，以强制两帧位姿保持接近，这种形式化方法使得我们的里程计对传感器的高频运动更加鲁棒。

B. 局部地图和鲁棒性配置

局部地图使用之前扫描的点云（类似于IMLS-SLAM），但不同的是世界坐标系中的点存储在稀疏的体素数据结构中，以实现比kd树更快的邻域访问。对于驾驶场景，地图的体素尺寸为1.0米，对于高频运动场景为0.80米，定义地图网格的体素尺寸非常重要，因为它决定了邻域搜索半径以及局部地图的细节级别，每个体素最多存储20个点，以确保任何两个点之间的距离不小于10厘米，以限制由于沿扫描线密度造成的冗余。为了构建点pWi的邻域（用于计算ni和ai），我们从当前点的27个相邻体素中选择k = 20个最近的邻居点。在当前扫描n的CT-ICP解算后，将这些点添加到局部地图中，完全占满的体素中的点将被删除。请注意，与pyLiDAR的F2M相反，局部地图不会按滑动窗口的方式忘记点云，使用这种类型的地图的里程计对于不良配准非常敏感，无法从由于不良扫描点云插入而导致的地图污染中恢复。这在快速定位变化的数据集中特别成问题，针对这些类型的数据集，我们引入了一个鲁棒性配置，用于检测困难情况（快速定位变化）和配准失败（位置不一致或大量新关键点落入空体素），并尝试使用更保守的参数对当前扫描进行新的配准；对于方向变化过大（≥ 5°），我们不将新扫描插入地图，因为这样更有可能导致不对齐，增强的鲁棒性带来了运行时间的增加。

C.回环闭合与后端

本文的回环闭合算法在内存中维护了一个由里程计配准的最近扫描点云的窗口，当窗口大小达到Nmap扫描时，将点聚合成一个点云，并放置在窗口中心的坐标框架中，然后，将该地图的每个点插入到2D高程网格中，对于每个像素，保留其最大高程的点，通过将每个像素的z坐标限制在zmin和zmax之间，可以获得一个高程图像。然后，提取旋转不变的2D特征并将其与高程网格一起保存在内存中，除了最后Noverlap个扫描外，将从窗口中删除所有扫描，每当构建新的高程图像，它将与内存中保存的高程图像进行匹配。使用RANSAC对两个特征集之间进行鲁棒的2D刚性变换拟合，并使用内点数的阈值来验证对应关系，当验证匹配时，对高程网格的点云执行初始2D变换的ICP优化（使用Open3D的ICP），从而产生精确的6自由度循环闭合约束。

图4：在KITTI-360序列上进行回环闭合的定性结果，左上方是通过投影局部地图构建的高程图像，右上方显示了CT-ICP里程计的轨迹以及使用计算出的回环闭合约束进行校正的轨迹（CT-ICP+LC），底部显示了与左上方局部地图相同转弯处找到的不同回环闭合约束（绿色）。

图4总结了我们的回环闭合过程，目前该过程要求传感器的运动在地面上大部分是平面的，并且外部标定要将z轴与地平面的法线对齐，这限制了支持的传感器设置以及允许的运动。请注意，如果已知重力矢量或局部地平面，则可以解决此问题，这种情况下可以正确投影高程图像。然而，在传感器大部分指向上方的室外场景中，我们的回环闭合检测成功检测到许多回环，从而使我们的后端能够成功校正里程计中的漂移误差。

实验

从表I可以看出，在使用经校正的扫描数据集（KITTI-corrected）时，所有方法的结果非常接近，然而，当我们使用未校正的扫描数据（KITTI-raw和KITTI-360）时，所有其他方法的性能大幅下降，另一方面，CT-ICP能够获得非常接近KITTI-corrected的结果，我们还在KITTI-corrected的测试序列（从11到21）上运行了我们的里程计，并将结果在线提交到KITTI基准测试：我们在那些公开代码的人中排名第一，相对平均位移误差（RTE）为0.59%。

表II显示了每个数据集序列的结果，首先注意到在KITTI-raw、KITTI-360和ParisLuco上，环路闭合后ATE有了显著改善；尤其是在KITTI-360上，因为该序列更长，然而，在KITTI-CARLA上，ATE在环路闭合前后几乎相同。该数据集具有非常简单的几何形状，具有大型且完美的平面，因此对于扫描匹配而言，挑战主要在于传感器在采集过程中的运动。CT-ICP能够实现近乎完美的对齐，已经能够在绝对轨迹估计上达到21厘米的精度；

这在图3中可视化，地面真实轨迹和估计轨迹几乎无法区分，最后每个序列检测到的回环闭合约束相对较多，这个数量取决于高度图构建的频率，由重叠大小Noverlap和地图大小Nmap来控制。当到达交叉口时，可以计算多个环路约束，如图4所示，通过增加重叠区域，我们的方法可以检测到更多的环路约束（包括相邻局部地图之间的约束）。

图3：使用CT-ICP里程计和我们的回环闭合校正（CT-ICP+LC）估计的序列00的轨迹：KITTI-raw（4541帧）、KITTI-360的序列06（9698帧）、KITTI-CARLA的Town01序列（5000帧）、NCD的01短实验（15301帧）和NCLT的2012-01-08序列（42764帧）。

总结

本文提出了一种新的实时里程计方法，超越了现有技术在七个具有不同特点的数据集上的表现，从驾驶到高频运动场景都适用，我们方法的核心是连续的扫描匹配CT-ICP，它在优化过程中弹性地扭曲新的扫描以补偿采集过程中的运动，发布了代码和数据集，以便复现以上所有结果，在未来的工作中将专注于改进我们的后端，以扩展所提出的连续形式，超越扫描匹配方式并充分利用回环闭合过程。

资源

自动驾驶及定位相关分享

【点云论文速读】基于激光雷达的里程计及3D点云地图中的定位方法

自动驾驶中基于光流的运动物体检测

基于语义分割的相机外参标定

综述：用于自动驾驶的全景鱼眼相机的理论模型和感知介绍

高速场景下自动驾驶车辆定位方法综述

Patchwork++：基于点云的快速、稳健的地面分割方法

PaGO-LOAM:基于地面优化的激光雷达里程计

多模态路沿检测与滤波方法

多个激光雷达同时校准、定位和建图的框架

动态的城市环境中杆状物的提取建图与长期定位