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

论文网址：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$ 的近邻（用于计算法向量 $n^i$ 和平面权重 $a^i$ ），我们在地图中离该点最近的 27 个（边长为 3 的立方体）体素中找出k=20个最近点。处理完当前扫描后，点被加入局部地图。体素被占满时，其中的点将被删除（为什么）。我们的局部地图不像 pyLiDAR的F2M一样，局部地图不会使用滑动窗口丢弃点云。

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

4. 回环检测与后端

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

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

每一个新创建的高程图像（包含$N_{map}-N_{overlap}$个扫描），都会与内存中的高程图像匹配。使用 2 维刚体变换和 RANSAC 算法，把新创建的高程图像变换到与内存中保存的高程图像一致。如果能匹配，就对高程栅格的点云，使用 ICP 改善算法（Open3D 的 ICP 算法）提高 2 维刚体变换，得到一个精确的 6 自由度闭环约束。为了减少候选数量，使用阈值$n_{candidates}=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_{map}=100N$，$N_{overlap}=30N$。$z_{min}$略低于标注值（使用传感器获取一个近似的外部标定），$z_{max}$比$z_{min}$大 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 数据集$01_{s}hor{t}_{e}xperiment$的 15301 个扫描上的实验结果，第 5 幅图是在 NCLT 数据集 2012-01-08 的 42764 个扫描上的实验结果。

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

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

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

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

6. 总结

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

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

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

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

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