论文阅读及复现——《CT_ICP: Real-time Elastic LiDAR Odometry with Loop Closure》

论文阅读之------《CT_ICP: Real-time Elastic LiDAR Odometry with Loop Closure》带闭环的实时弹性激光雷达里程计

• [1. 主要贡献](#1. 主要贡献)
• [2. 相关说明](#2. 相关说明)
• [3. 激光里程计](#3. 激光里程计)
• • [3.1 里程计公式构建](#3.1 里程计公式构建)
  • [3.2 局部地图与健壮性](#3.2 局部地图与健壮性)
• [4. 回环检测与后端](#4. 回环检测与后端)
• [5. 实验结果](#5. 实验结果)
• • [5.1 里程计实验结果](#5.1 里程计实验结果)
  • [5.2 回环检测实验结果](#5.2 回环检测实验结果)
• [6. 总结](#6. 总结)

论文网址：https://arxiv.org/abs/2109.12979

源码网址：https://github.com/jedeschaud/ct_icp

复现过程：https://blog.csdn.net/qq_44164791/article/details/132188049?spm=1001.2014.3001.5502

1. 主要贡献

1. 提出了一个具有扫描内姿态连续性 和扫描间不连续性的新型弹性激光雷达里程计
2. 使用稀疏体素结构存储稠密点云的局部地图，可以达到实时处理的速度。
3. 做了大量实验，在7个高频运动驾驶场景数据集上进行了实验，所有的代码均开源
4. 提出了一个回环检测的方法，还实现了位姿后端，构成了一个完整的SLAM。

2. 相关说明

CT-ICP 定义的轨迹在扫描内连续，在扫描间间断。在一个扫描过程中，轨迹可以使用开始位姿和结束位姿表示。CT-ICP 的最终轨迹是不连续的，因为一次扫描的开始位姿不等于上一次扫描的结束位姿。这种方法考虑到插值不适用于不规则运动。

激光雷达里程计在开发环境中依然有累积误差，这会导致轨迹偏移。回环闭合可以全局矫正轨迹，但回环检测仍然是一个开放性的问题。现在大多数解决方法都主要依赖配准法（ registration methods）直接闭合回环。这样只能处理小规模轨迹和较小偏移。很多位置识别方法都在每一个扫描中识别位置，这对环境变化很敏感，更适合驾驶场景。最近和多基于深度学习的方法被提出，但由于训练样本，这些方法不适用于新环境。CT-ICP 的回环闭合算法作用于投影在一个高程图像上的汇聚点云（aggregated point clouds）。该方法需要传感器的运动接近 2 维，且需要估计重力矢量，该方法可以用于任何满足这些条件的激光雷达里程计。CT-ICP 在局部地图中检测回环。因此 CT-ICP 不需要检查每次扫描是否与之前的位置一样。这在在线 SLAM 场景中更高效。

3. 激光里程计

3.1 里程计公式构建

在每一帧，里程计具有两个位姿，分别是帧起始位姿和帧结束位姿, 时间也一样:

与其他里程计不同的是，当前帧的起始位姿不等于上一帧的结束位姿，这两个位姿之间还存在一个邻近约束。

定义求解问题如下:

1. 局部地图和鲁棒的配置文件

这部分论文里写的有点像是实验报告，两段话，大致讲了体素格参数配置和雷达点云帧插入标准的大致准则(判断角度变化是否大于阈值)。
2. 回环检测和后段

思路和SC有点类似，但是实现方式不一样

将地图中的每个点插入到2D高程网格，使每个像素点保持在最大高度，在2D网格中，通过Zmin和Zmax的差值获取高程图像，然后提取旋转不变的2D特征并且和高程网格一起保存。提取到匹配对后，通过ICP和RANSAC约束位姿...

总体而言，限制很多，首先要求是运动尽量是平面运动，然后要求将Z轴和地平面法向量对齐，这个回环不如用SC来代替。

3.2 局部地图与健壮性

我们使用过往的扫描作为局部地图。体素用于存储世界坐标系中的点，为了快速访问临近体素，体素使用稀疏的数据结构实现，而不是使用 kd 树。

所以与kd树相比，其访问时间复杂度是线性的，而不是对数。对于驾驶场景来说，高频运动场景下，局部地图的体素尺寸是1米和0.8米。

体素的尺寸就是地图的栅格大小，它决定邻近搜索半径和局部地图的详细程度。每个体素存储的点达到 20 个，为了避免冗余，任意两个点的距离都不小于10厘米。为了构造点 p i W p^{W}_i piW 的近邻（用于计算法向量 n i n^i ni 和平面权重 a i a^i ai ），我们在地图中离该点最近的 27 个（边长为 3 的立方体）体素中找出k=20个最近点。处理完当前扫描后，点被加入局部地图。体素被占满时，其中的点将被删除（为什么）。我们的局部地图不像 pyLiDAR的F2M一样，局部地图不会使用滑动窗口丢弃点云。

如果向地图中添加错误的数据，或者方向变化较快，使用滑动窗口丢弃点云的地图很容易受影响。对于方向变化快的数据，我们提出一个健壮性的方案：检测困难情况（快速的方向变化）和失败的注册（位置不一致或者大量新关键点落入空体素），使用更保守的参数重新注册；如果方向变化大于5度，我们就不会把新的扫描结果插入地图，否则很可能偏离方向。健壮性的提高会增加一定的时间复杂度。

4. 回环检测与后端

CT-ICP 的回环闭合算法在内存中维持一个窗口，窗口内是里程计最近添加进来的扫描。当窗口大小等于 N m a p N m a p NmapN_{map} NmapNmap个扫描时，点就会被加入位于窗口中心位置的那个扫描的点云中。解释：假如窗口大小是 9，如果窗口内有 9 个扫描，就把窗口内的点加入第 5 个扫描的点云中。这是因为一般情况下，第 5 个扫描与其它 8 个扫描的重合最多。

地图的每个点都会被插入一个 2 维的高程栅格，让每个像素的点保持最大高度。在这个 2 维栅格内，以点的最小z坐标 z m i n z_{min} zminz和最大 z 坐标 z m a x z_{max} zmax作为 z 轴的范围，得到高程图像。然后提取 2 维旋转不变特征，与高程栅格一块保存在内存中。除了最后 N o v e r l a p N_{overlap} Noverlap个扫描，其它都被移出窗口。

每一个新创建的高程图像（包含 N m a p − N o v e r l a p N_{map} - N_{overlap} Nmap−Noverlap个扫描），都会与内存中的高程图像匹配。使用 2 维刚体变换和 RANSAC 算法，把新创建的高程图像变换到与内存中保存的高程图像一致。如果能匹配，就对高程栅格的点云，使用 ICP 改善算法（Open3D 的 ICP 算法）提高 2 维刚体变换，得到一个精确的 6 自由度闭环约束。为了减少候选数量，使用阈值 n c a n d i d a t e s = 10 n_{candidates} = 10 ncandidates=10筛选与当前高程栅格最近的 10 个高程栅格。这个阈值与误差有关，阈值过小导致闭环的漏检。

CT-ICP 的后端使用 g2o 实现标准的位姿图。位姿图定期添加新位姿，但只有检测到闭环时，才对整个轨迹作全局优化。

CT-ICP 的算法需要传感器在平面上运动（上下运动幅度小），还需要外参标定以对齐 z 坐标与实际平面。这限制了传感器和运动。如果重力矢量或者局部平面已知，高程图像就能正确地投影，这个限制也就没有了。但我们在第 Ⅴ 部分说了，在室外场景中，使用向上的传感器，使我们的闭环检测成功检测到一些闭环。

5. 实验结果

5.1 里程计实验结果

表中数据在 5 个驾驶场景数据集和2个高频运动场景数据集上的相对变换误差 RTE。AVG 是所有 sequences 上 RTE 的平均值，ΔT是每个扫描的平均耗时。KITTI-corrected 是唯一一个被校正过的数据集，其它数据集的点云都是原始未校正的。

从表中可以看出，所有里程计方法在经过运动校正的 KITTI-corrected 数据集上表现接近。处理未校正的数据集 KITTI-raw 和 KITTI-360 时，其它方法的表现明显变差，而 CT-ICP 在未校正的数据集上的表现接近其在校正过的数据集上的表现。在校正过的数据集上，CT-ICP 的弹性策略不起作用，这表明了CT-ICP的地图和scan-to-map算法的有效性。在 KITTI 的在线评测上，CT-ICP 以 0.59% 的 RTE 排名第一。

表中KITTI-corrected 数据集上，CT-ICP 的结果与 MULLS 不同，这是因为 CT-ICP 把一套参数用于所有 sequence，而 MULLS 针对城市、高速公路和村庄场景，使用三套不同的参数。

KITTI-CARLA 数据集的速度变化比 KITTI 快。在该数据集上，CT-ICP 的表现比其它方法都好：CT-ICP 的 RTE 仅为 0.09%，其它方法的最好结果是 0.81%。这证明 CT-ICP 的弹性策略和扫描间位姿不连续策略可以有效弥补位姿误差。

不同于 KITTI 的 64 线激光雷达，ParisLuco 数据集的雷达是 32 线的。由于获取于市中心，ParisLuco 是低惯性的（low inertia）。在该数据集上，CT-ICP 能超过使用稠密地图的IMLS-SLAM。

NCD 和 NCLT 数据集使用棍子和平衡车采集，所以不稳定。而且它们的每个 sequence 包含更多扫描，环境也多样（植被、道路、室内、室外），因此更具挑战性。此外这两个数据集还有很多边界情况，比如快速地从室内转移到室外、急转弯、运动的高频间断引起的扫描大变形。一般的雷达里程计很难处理这些情况。表 1 表明 CT-ICP 在 NCD 数据集上表现优秀。在 NCLT 数据集上的实验也证明了 CT-ICP 的健壮性，因为 CT-ICP 在处理边界情况时，RTE 比 pyLiDAR F2M 还低。

为了专门验证 CT-ICP 的主要贡献，即其弹性策略，只使用匀速运动模型扭曲（distort）扫描，以测试里程计。在 KITTI-raw 上，RTE 从 0.55% 增加到 0.79%，在 KITTI-360 上，RTE 从 0.45% 增加到 0.60%。

5.2 回环检测实验结果

在所有数据集上，CT-ICP 使用的回环闭合模型都是 CT-ICP 加回环闭合，其中
N m a p = 100 N N_{map} = 100N Nmap=100N， N o v e r l a p = 30 N N_{overlap} = 30N Noverlap=30N。 z m i n z_{min} zmin略低于标注值（使用传感器获取一个近似的外部标定）， z m a x z_{max} zmax比 z m i n z_{min} zmin大 10 米。对于 NCLT，使用刚体变换把向下的 z 轴变换成向上。每个高程珊格的计算时间加上与前一个高程珊格的匹配时间，平均需要 1.1 秒。位姿图优化平均需要 1.2 秒。当前的回环闭合由主线程负责，利用多线程可以实现实时。

CT-ICP 里程计估计出的轨迹和回环闭合校正效果。第 1 幅图是在 KITTI-raw 数据集 sequence 00 的 4541 个扫描上的实验结果，第 2 幅图是在 KITTI-360 数据集 sequence 06 的 9698 个扫描上的实验结果，第 3 幅图是在 KITTI-CARLA 数据集 Town 01 的 5000 个扫描上的实验结果，第 4 幅图是在 NCD 数据集 0 1 s h o r t e x p e r i m e n t 01_short_experiment 01shortexperiment的 15301 个扫描上的实验结果，第 5 幅图是在 NCLT 数据集 2012-01-08 的 42764 个扫描上的实验结果。

上图图是在 KITTI-360 数据集 sequence 00 的 11501 个扫描上，回环闭合的量化结果。左上部是投影局部地图得到的一个高程图像，右上部是 CT-ICP 里程计轨迹和回环闭合（CT-ICP 加回环闭合）校正效果，下部是在左上图所示的局部地图中发现的多个闭环约束（使用绿色表示）。

表是回环闭合在每个数据集的一个 sequence 上的实验结果。ATE 是固定了估计轨迹于实际轨迹间的刚体变换后的平均绝对轨迹误差，单位是米。 N l o o p N_{loop} Nloop是检测到的回环数量。LO 代表激光雷达里程计，LC 代表回环闭合。

在各数据集的其中一个 sequence 上的实验结果。首先，记录 CT-ICP 在 KITTI-raw、KITTI-360 和 ParisLuco 数据集上，回环闭合的最好结果。KITTI-360 的 ATE 在闭合前后变化很大，这是因为它的 sequence 最长。KITTI-CARLA 的 ATE 在回环闭合前后变化不大，这是因为该数据集包含的路径几何形状简单，也就是说平面大且平整。所以扫描匹配的挑战性主要受传感器采集数据时的运动情况影响。CT-ICP 的对齐效果接近完美，达到了 ATE 为 21 厘米的精度，如图 3 所示，其估计的轨迹与标注值几乎重合。

最后，在每个 sequence 上检测多个回环。回环的数量取决于高程栅格的构造频率，也就是和重叠大小 N o v e r l a p N N_{overlap}N NoverlapN以及地图大小 N m a p N N_{map}N NmapN有关。每到交叉路口，就计算回环约束。重叠大小 N o v e r l a p N N_{overlap}N NoverlapN越大，越能检测出更多回环（包括连续的局部地图间的约束）。

6. 总结

CT-ICP 提出了一个新的实时里程计。在包含驾驶场景和高频运动场景的 7 个数据集上，进行了多方面的评比，CT-ICP 都超过现有方法。

核心是连续的扫描匹配算法，它在优化时，弹性地变换一个新扫描，以弥补采集数据时的运动。

CT-ICP 提供所有代码和数据集以供验证所有实验结果。

在IMLS的基础上，建模帧内和帧间约束

展望： CT-ICP 会着重于后端，进一步延伸扫描外的连续策略，充分利用提出的回环闭合算法。