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

　　通过在每一帧扫描的开始和结束时刻联合优化两个姿势，并根据时间戳进行插值，使扫描进行弹性变形以与地图(白点)对齐，从而创建连续时间扫描到地图的里程计。图片最下面下面说明轨迹具有扫描内姿势的连续性和扫描之间的不连续性。这里也推荐「3D视觉工坊」新课程《三维点云处理：算法与实战汇总》。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

　　本文提出了一种新的实时激光雷达里程计方法，该方法在七个不同数据集(从驾驶到高频运动场景)上超越了目前的技术水平。该方法的核心是匹配CT-ICP的连续扫描，它在优化过程中弹性地扭曲新的扫描，以补偿采集过程中的运动。

　　未来的工作将专注于改进后端，将所提出的连续公式扩展到扫描匹配之外，并充分利用循环关闭程序。

　　*博客内容为网友个人发布，仅代表博主个人观点，如有侵权请联系工作人员删除。