【论文阅读】CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure

一、激光里程计

CT-ICP本身属于一个对ICP的改进，其主要的改进其实体现在前端的部分。在里程计的部分，CT-ICP很独特地用两个位姿来参数化一次扫描，首先划定一个时间段，记为τb和τe，对这两个时间点，各自取一个位姿，所以两个位姿可以构成四个参数：

论文里统一用加粗的字体表示当前帧的待优化参量，所以我们最重要得到的就是这个大写的X量。这里使用scan来表示一个时间段内获得点云的集合，按照一般的SLAM流程，这里我们就把一次scan叫做一帧点云，那么我们的目标就是获得一帧点云开始时以及结束时的两个位姿，而一帧点云生成过程中的位姿则是通过插值的方法一点一点生成的。

有了一帧的初始位姿和结束位姿，这个时间段内的其它位姿就可以用插值的方法计算出来，具体来说平移t使用线性插值，而旋转R使用的则是球形插值的方法，利用插值我们可以得到其它时刻的位姿，从而将所有时刻的点云投影到世界坐标系下。除了使用两个位姿来做插值，CT-ICP还认为上一帧的结束和当前这一帧的开始是不同的，也就是说t-1帧的结束位姿和t帧的开始位姿实际上是有差别的，这个差别会在传感器运动速度极大的时候被放大，而在CT-ICP中作者用一个约束的方法来建立两个位姿之间的联系，这一点属于是这篇论文一个比较难理解的地方，论文称之为帧之间的不连续性。

具体来说，对于一个新的帧，首先使用网格抽样的方法对点云做一个取样，得到一个关键点集合：

i表示样本点的编号，pi表示提取的特征点，sn表示第n帧点云。我们的目标是把这些点加入到局部地图中，也就是计算他们与局部地图之间的配准关系，总的目标函数为：

我们只要得到初始和结束时的位姿，就可以利用插值得到这个时间段内任何一个，所以寻找的量就是加粗的X，目标值有三部分，主要是第一部分也就是类似ICP的计算。

这个值实际上是一个平均值，对当前帧的每个关键点，都计算一个被鲁棒核函数套起来的距离值，这个距离值采用的是点到面的ICP距离计算方法：

这里的αi可以看作一个比值，表示i所处的时间戳在这个时间段内的比例。

对于每个特征点，都会根据其采样时的时间戳，根据线性插值的方法得到这个特征点采样时刻时的位姿，利用这个位姿将这个特征点投影到世界坐标系下，利用点到面的ICP从局部地图中找出与之对应的点并得到点到面的距离，套上鲁棒核函数后求和取平均。这部分将作为目标函数中ICP的部分参与后续的优化。并且在计算位姿时，CT-ICP使用的是局部地图和当前帧，并非当前帧和上一帧，所以一定程度上增加了匹配临近点的数量，能够让计算更加准确。

关于点到面的ICP，其主要是由于Point-to-Point的方法不允许平面之间的滑动，它是求源点云中的点到目标点云中点组成曲面的距离，它要求点云能够提供法向量，具体可以参考链接。

关于目标函数中另外的两项，他们表示的是位置的一致性和速度的一致性，计算公式如下：

对于位置一致性约束，主要是让当前这帧的开始时位姿的t与上一帧结束时位姿的t尽可能接近，而速度约束这是要求这两帧生成过程中平移的变化量或者说位置的移动量尽可能保持一致。在此基础上还加了两个系数在目标函数中，从而让CT-ICP更加具有弹性。

在激光里程计这一部分，区别于LOAM先畸变校正再点云配准，CT-ICP是通过一帧的起始位姿和结束位姿的插值来实现位姿的推理，通过插值将点投影到世界坐标系下，与局部地图做scan-to-map的点云配准，以此来得到帧的起始和结束位姿，进而得到整个帧里面每个点的位姿。个人感觉这样做的主要是因为传感器的限制，LOAM由于有IMU，所以可以得到每个位置下的位姿，而CT-ICP是一个纯激光雷达的框架，所以用位姿作为变量来推理出每个位置下的位姿，再结合目标函数优化出最优的两个位姿。

另外一个问题在于，为什么要将上一帧的结尾的位姿和当前帧的起始的位姿视为两个不同的位姿，这一点应该属于论文提出的激光SLAM框架的核心部分，他们的框架是一个扫描内姿态连续性和扫描间不连续性的框架，扫描内姿态连续性指的是一次扫描内的所有位姿都可以通过起始的位姿插值得到，而扫描间不连续性则对应扫描首尾位姿不同这一点，这一点也是他们和其它里程计十分不同的一点，按照他的这个理论来说，点云必须不是同时生成的，我们必须要将一帧点云看作是一点一点产生的，在一帧点云的最后和下一帧点云的开始必须是两个独立的时刻，不仅如此，点云生成的时刻必须要很严格，如果一帧点云生成需要1秒，那么0.99秒的时候产生的点云和1.01秒时产生的点云的位姿必须视为两个位姿，甚至于说从正负两个方向无限趋向于1的时候的位姿，我们也不能认为是一样的，只有这样才能解释什么叫扫描间的不连续性。

二、局部建图

局部建图这部分看的不是很懂，论文说局部地图使用体素网格的方式来存储世界坐标系的点，这种方法要比一般的kdtree更快，每个网格存储最多20个点，网格满了就不再向其中加入新的点，同时为了保证网格内部点的稀疏性，任何两个点之间的距离不能超过十厘米。并且CT-ICP没有使用滑动窗口机制，所以不会随意丢弃网格，保持网格简洁性的方法是利用网格的距离和当前的距离，当距离超过阈值的时候就不再对这个网格做修改。

此外，考虑到配准算法一旦出错之后，后续的错误很难修正，所以在建图时增加了一个约束，如果一次扫描的角度偏差超过5°时，就认为是配准错误，不向局部地图中加入新的内容。

也就是说，按照论文叙述的，通过里程计我们可以得到一个扫描的起始位姿，从而插值得到内部的所有位姿，通过这些位姿我们可以将点转换到世界坐标系下，从而与局部地图拼接产生新的地图。地图采用的是体素的存储方法，个人理解就是一个一个立体块，把新的点向地图块里面放，放满了就不放了，而且计算出来的位姿会有一个筛选，如果角度偏差太大就不往里面扔。

三、回环检测与后端

回环检测这里使用的方法有一部分有点像之前看过的SCAN CONTEXT，首先地图都会保存一个高程图的格式，个人理解就是一个能够表示高度的二维图像，有点像地理上的海拔图，回环检测过程会维护一个窗口，窗口满的时候，就会对窗口内的扫描点云，生成一个高程图，用这两个高程图做一个2D的图像匹配，如果能够匹配上，就用OPEN3D的ICP来计算一个刚体变换，如果可以计算出来就认为产生回环。

后端使用g2o的位姿图优化，当检测到回环的时候，对整个轨迹做一个全局优化。

个人感觉这里使用的高程图，和SCAN CONTEXT的编码方式实际上就是一个东西，都是将3D的点云转换为2D的图像，从而将点云的回环检测转换为一个2D图像匹配的方法。

参考链接：
论文翻译
论文解析
论文解析