CT-ICP：实时弹性激光雷达里程计与回环检测-电子发烧友网

本文主要是提出了一种新的实时激光雷达里程测量方法，称为CT-ICP(连续时间ICP)，通过新颖的环路检测程序完成完整的SLAM。该方法的核心是在扫描匹配中引入连续性和扫描间不连续的组合。它允许在配准过程中扫描的弹性变形，以提高精度，并增加了对不连续的高频运动的鲁棒性。在七个数据集上进行了测试:KITTI、KITTI-raw、KITTI-360、KITTICARLA、ParisLuco、Newer College和NCLT，分别用于驾驶和高频运动场景。

1 引言

本文提出的目的：

使用匀速运动假设矫正点云畸变的方法无法适应大幅度的方向变化或者剧烈速度变化

即使目前有工作考虑了连续时间内的运动，但是也无法适应高频的剧烈运动情况，或者以精度损失为代价。

本文主要贡献：

提出了一种基于位姿扫描内连续性和扫描间不连续的弹性激光雷达里程计。

本文次要贡献：

提出了一种基于密集点云的局部地图，存储在稀疏体素结构中，以获得实时处理速度。

在驾驶和高频运动场景的7个数据集上进行大规模实验，所有实验都有开源代码可以复现。

结合姿态图后端构建了完整SLAM的快速回环检测方法，为pyLiDAR-SLAM(开源)。

2 算法概述

算法主要流程如下图所示：

图中的彩色部分为激光雷达扫描。点云的颜色对应每个点的时间戳(蓝色表示时间戳更远的点云，红色表示时间戳更近的点云)。

通过在每一帧扫描的开始和结束时刻联合优化两个姿势，并根据时间戳进行插值，使扫描进行弹性变形以与地图(白点)对齐，从而创建连续时间扫描到地图的里程计。图片最下面下面说明轨迹具有扫描内姿势的连续性和扫描之间的不连续性。

3 算法框架

3.1 里程计公式

在激光雷达的每一帧中，里程计有两个位姿用于参数化插值：

起始位姿

结束位姿

对于在每一帧扫描的第一个时间戳和最后一个时间戳之间的时间∈[,]捕获的每个传感器的测量，通过在扫描的起始和结束两个姿态之间插值来估计传感器的姿态。

与其他里程计不同的是，当前帧的起始位姿不等于上一帧的结束位姿，这两个位姿之间加入了一个邻近约束，迫使两个姿态保持接近，这使里程计对传感器的高频运动更加稳健。

本文的优化函数如下：

其中，优化变量,为扫描到地图的连续时间ICP(其实就是插值和ICP的过程)。

是从每一帧雷达数据中提取的一个特征点序号的集合，对于每个i有：

是样本点与其在地图中最近的邻居之间点到平面距离的残差；

为世界坐标系中表示的点，为在局部地图中邻域的法线，为传感器测量值(在LiDAR坐标系中)；

是雷达坐标系在时间到世界W的变换。通过定义，在和之间进行插值估计。对于旋转插值，使用标准球面线性插值(slerp)。

还引入了有利于平面邻域的权重:，是邻域的平面度，其中是邻域协方差特征值的平方根。

优化函数还引入了两个约束(位置一致性约束)和(等速约束)，其权重分别为和，定义如下:

迫使传感器的开始和结束位置保持一致(限制不连续性)，而限制过快的加速。

CT-ICP执行迭代，直到满足参数步长范数的阈值(通常为平移0.1 cm和旋转0.01°)或达到多次迭代(5次以确保实时性)。

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

作为局部地图，世界坐标系(W)中的点存储在体素的稀疏数据结构中，以便比kd-trees(常数时间访问而不是对数访问)更快地进行邻域访问。地图的体素大小控制着邻域搜索的半径，以及存储点云的详细程度。

每个体素最多存储20个点，这样两个点之间的距离就不会超过10厘米，以限制由于沿着扫描线的测量密度而造成的冗余。一旦一个体素被填满，就不会再插入点了。

为了构建点的邻域(用于计算ni和ai)，从当前点的27个相邻体素中选择地图中k=20个最近邻。在对当前扫描n运行CT-ICP后，这些点被添加到局部地图中。

使用这些类型地图的里程计对错误配准高度敏感，并且无法从错误扫描插入的地图污染中恢复，为了处理这个问题，引入了一个鲁棒的配置文件，可以检测硬情况(快速方向变化)和注册失败(位置不一致或大量新关键点落在空体素中)，并尝试使用一组更保守的参数(最明显的是大量采样关键点和更大的邻域搜索)对当前扫描进行新的注册;对于重要的方向修改(≥5◦)，不会在地图中插入新的扫描，因为这有更高的不对齐概率。

3.3 回环检测和后端

回环检测算法在其内存中保留了一个由里程计记录的最后扫描的窗口。当窗口达到扫描的大小时，这些点被聚合成一个点云，放置在窗口中心的坐标框架中。

然后将该地图的每个点插入到二维高程网格中，使每个像素点保持最大高程。从这个二维网格中，通过在和之间剪切每个像素的z坐标来获得高程图像。

然后提取旋转不变的2D特征，并将其与高程网格一起保存在内存中。除了最后一次之外的所有扫描都将从窗口中删除。

每次建立新的高程图像时，它都会与保存在其内存中的高程图像进行匹配。使用RANSAC鲁棒拟合两个特征集之间的二维刚性变换，并使用内层数的阈值来验证对应关系。当匹配被验证后，对初始2D变换在高程网格的点云上进行ICP细化，产生精确的6自由度闭环约束。

后端使用位姿图，当添加新的里程约束时，会向图中添加新的姿态，但只有当检测到新的环路约束时，轨迹才会进行全局优化，此时闭环模块的轨迹也会更新。

4 实验与结果

在KITTI, KITTI-raw, KITTI-360, KITTI- carla, ParisLuco, Newer College Dataset (NCD)和NCLT数据集上进行了实验。

4.1 里程计实验

使用KITTI相对平移误差(RTE)进行评估。并且在提供的数据集上与其他三种LiDAR里程计进行比较:MULLS、IMLSSLAM和pyLIDAR-SLAM F2M。实验结果如下：

4.2 回环检测实验

为了定量评价其质量，RTE不太适合，并且在环路关闭后趋于恶化。为了证明全局轨迹改进，使用标准的绝对轨迹误差(ATE)来进行评估。然而，为了将质量评估与初始姿态的方向误差分离开，在计算ATE之前首先估计地面真值与估计轨迹之间的最佳刚性变换。这里也推荐「3D视觉工坊」新课程《三维点云处理：算法与实战汇总》。

下图给出了实验定性结果：