CT-ICP解析

总结一下贡献：

Tracking —— A new elastic LIDAR odometry based on the continuity of poses intra-scan and discontinuity between scans

Mapping —— A local map based on a dense point cloud stored in  a sparse voxel structure to obtain real-time processing  speed .  

Loop closure—— A new loop closure procedure that operates on aggregated point clouds projected onto an elevation image

System—— A fast method of loop detection integrated with a pose graph back-end to build a complete SLAM, integrated into pyLiDAR -SLAM

关键变量的定义

图2 ICP配准部分

图2 是本文内容的关键，左边是基于point-to-plane的ICP示意图，蓝色的线对应本文中的局部地图，红色的线对应当前的scan。接下来逐个解释图2中的每个公式

（2）为ICP的优化的代价函数

 （3）中是局部地图对应点的法向量，是地图中对应的点，是雷达坐标系下的点云转换到地图的世界坐标系下之后的激光点云

（5）（6）求出scan的起始点和结束点的位姿变化之后，中间的点云通过插值获得其位姿，旋转上采用球形插值，平移上采用线性插值，比例系数根据时间戳得到

（3）中的是平面系数，参考文献[2]中给出了原理，其中是特征值的开方，,    当和比较接近时，就说明点云分布在两个方向上，就说明是接近平面的分布，当远远大于和时就说明是接近线分布，从这可以看出，代价函数更喜欢环境中的平面点

Local Map，

这部分没有什么理论可讲，都是一些trick，局部地图是体素化的，总结如下 图

这种配准算法一旦发生错误以后很难恢复，所以，为了避免加速度过大带来的配准错误，当一帧数据估计的角度超过5°时，就认为是错误配准，不加到局部地图里面。

Loop Closure

把点云地图转换成高程图，并根据图像中的特征进行粗匹配，然后再用ICP进行精确匹配，构建全局约束，最后进行位姿图优化，流程如下：

 总体而言，主要是在于去畸变这一块儿做的更加细致了，旋转上用球形插值代替了传统的线性插值，然后回环检测上也是一贯的corse-to-fine，只是把点云图转成了高程图，然后投影成一个二维图像进行处理，可以用的方法也多一些，可能这里换成SIFT也是一样的效果。

参考文献：

[1] Low K L. Linear least-squares optimization for point-to-plane icp surface registration[J]. Chapel Hill, University of North Carolina, 2004, 4(10): 1-3.

[2] Demantké J, Mallet C, David N, et al. Dimensionality based scale selection in 3D lidar point clouds[C]//Laserscanning. 2011.

[3]Dellenbach P, Deschaud J E, Jacquet B, et al. CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure[J]. arXiv preprint arXiv:2109.12979, 2021.