LOAM: Lidar Odometry and Mapping in Real-time

一、概述

Loam主要是一种激光匹配slam的方法，也就是一种定位与建图的方法，相比于其他的制图方法，loam主要解决的是雷达快速移动的过程中的畸变问题，如果雷达的扫描速度和雷达的移动速度相差太大，会导致扫描过程中偏差太大，利用loam就可以一定程度上改善这个问题。

对于这篇论文，一个很关键的点就是搞明白一些量的定义，首先对于雷达的扫描过程需要有一个大体的了解。雷达是分线的，一般有十六线、三十二线等等，线代表的就是竖直方向上发送激光的数目，这堆线是在同一列上的，十六线就代表一个时刻扫描的一列上发出了16条射线，下一个时刻扫描方向水平移动一定角度，再扫描下一列的点，如此重复，就得到了全部的点云数据。可以看出，线越多，扫描的内容也就越多。在这篇论文中，将一个sweep定义为对空间全部扫描一次，也就是取到了范围内的所有列，而一个scan代表同一线上扫描到的所有点，换个角度来看，如果将所有的点放到一个矩阵里面，对于一个十六线的雷达，每次移动0.5°，扫描范围是180°，那么矩阵有十六行，每一行都对应一线，一共有360列，每一列代表每次移动扫描到的列，一个scan就代表一行的全部数据，一个sweep就代表整个矩阵。

了解了雷达的工作过程，就不难发现，一帧点云的构建并不是一瞬间就完成的，而是一点一点拼出来了，在转向速度和雷达移动速度偏差不太大的情况下，扫描也不会出现太大的偏差，一旦偏差过大，甚至极限一点，移动速度远大于转向速度，那么对于一个sweep来说，可能现在这一列是在原点扫描的结果，到了下一列扫描的时候已经到终点了，那构建的结果必然也偏差很大。这篇论文提供的解决方法就是一方面执行高频率的里程计但是低精度的运动估计用于定位；另一方面在比定位低一个数量级的频率执行匹配和注册点云信息，实现建图和校正里程计。

由于loam主要的目的在于推进偏移的最小化，所以在这篇论文里面并没有加入回环检测的部分，这个算法主要的结构如下图所示：

这篇论文将复杂的问题划分为了两部分：高频率的里程计和低频率的制图，里程计用于提取特征点来进行相邻帧之间点云数据的匹配，也就是找出变换关系，用这个变换关系去消除畸变，制图则是用校正后的数据进行点云的拼接。可以看见雷达的数据送入odometry之后产出的结果一部分送入了mapping，之后再变换整合的部分又将这两部分的变换整合了一下，从而达到了一个很好的效果。

二、Lidar Odometry

在里程计的部分中，主要有三部分：特征点提取、建立匹配关系和运动估计。
特征点提取主要是提取两类点：边点和平面点。从名字就可以看出，边点其实是原来三维空间上边上的点，而平面点则是原来三维空间里面平面上的点。这里的特征点提取是针对scan来操作的，也就是说是在一次scan上提取特征点。
提取特征点本质上是根据点的曲率来计算平面光滑度作为提取当前帧的特征，平面点是在三维空间中处于平滑平面上的点，其和周围点的大小差距不大，曲率较低，平滑度较低。相反边缘点是在三维空间中处于尖锐边缘上的点，其和周围点的大小差距较大，曲率较高，平滑度较高。提取时按照下面的公式计算c值，c值最大的为边点，c值最小的称为平面点。

为了保证特征点分布的均匀，提取时将一个scan分为了四个区域，对每个区域上限制了特征点的数目：一个区域只能有2个边点和4个平面点。同时选的点周围不能有点已经被选中。并且，为了避免选到一些可靠性比较低的点，选点的时候还排除了下面的两种情况：

左图的情况是指不希望出现点B这样物体面本身与激光的射线方向接近平行的平面上的点，右图的情况是指不希望出现这种存在间隔的情况，AB可能在一条线上，但是因为间隔的存在，会存在距离的偏差。
所以，候选的点必须满足三个条件：子区域数量要求、周围没有别的被选点、位置要求。满足要求的候选点送入计算c值的模块，通过计算c值，找出最大的两个和最小的四个点，从而得到了2个边点和4个平面点。

建立匹配关系则是找出前后两帧特征点的对应关系，从这部分开始，最头痛的就是里面的时间戳关系，也就是下面这张图：

tk时间开始，到tk+1时刻，也就是蓝色的线，由于点云数据是一点一点生成的，所以蓝色线表现为一个不断上升的过程，这一个时间段生成的原始点云数据记作pk，前面也提到过了，在这篇论文的背景下蓝色的pk是存在偏差的，对这部分偏差进行校正，得到的点云会投射到结束时刻也就是tk+1时刻，这部分无偏差的点云数据称为pk-（横线画在上面），算法实际上是从这个时刻开始的，也就是已知上一帧的变换矩阵和无偏点云数据pk-，对未校正的pk+1进行处理得到下一个时刻的转换矩阵。

这里的匹配关系主要是选取特征点间的匹配关系，我们现在得到了校正后的且投影到tk+1时刻的点云pk-，这里面是有上一个时刻的特征点的，之后在未校正的点云pk+1中，利用粗略的转换矩阵，将点云前投影回tk+1时刻，也就是这一帧点云的开始时刻，这时我们就得到了tk+1时刻的两个点云，我们点的选择也是在这两个点云中进行的。对于边点，首先选择前投影点云中的一个边点i，再在pk-中选择距离i最近的点j，最后选择pk-中与i最近且不在同一个scan的点，这样做主要是为了防止在构建特征区域的时候出现三点共线的情况。

对于平面点，选择前投影的点云中的一个平面点i，之后在pk-的平面点中选择与i距离最近的点j，再在j同一个scan找一个最近点l和与j不在一个scan的另一个最近点m，这样相当于在pk-的平面点中找了三个平面点。

找这些点最基本的出发点是想建立一个优化的方向，前面提到过，前投影本身是粗略的，也就是存在偏差的，在没有偏差的情况下，前投影的特征点应该是和pk-的特征点重合的，但因为偏差存在，特征点之间也是存在距离的，所以我们优化的方向应该是让这个距离尽可能小，为了增加算法的鲁棒性，这里没有选择特征点之间的距离最小，而是让点到特征区域的距离最小，这个距离对于边点就是点i到直线jl的距离：

对于平面点就是点i到平面jlm的距离：

对于这两个式子，只有前投影的i的坐标是不知道的，这里就涉及到前面提到的粗略的转换矩阵，这个矩阵其实就对应着第三部分也就是运动估计。
对这篇论文而言，它假设雷达的运动是匀速的，所以变换矩阵理论上是可以根据时间关系，进行线性插入的，也就是下面的式子：

这个式子最难看懂的地方就是那些该死的时间戳下标，首先对这个式子来说，时间轴的关系式tk——tk+1——ti——t，其中tk+1——t正好对应这一个sweep的时间，所以右边分数的部分实际上就是扫描的i的时候用的时间在整个sweep耗时中所占的比例。再看两个变换矩阵，变换矩阵上标L表示的是这个变换矩阵是在雷达坐标系下的，这个上标倒还好理解，主要是在后面制图的时候区分坐标系的，下标就离谱，不带括号的实际上是带括号的一种缩写，左边的部分表示的是tk+1时刻与ti时刻的坐标转换，右边不加括号的，表示的是tk+1与t时刻的坐标转换，也可以看成是帧间的变换矩阵，也就是我们最终的目标。
也就是说，利用上面的式子我们可以利用粗略的帧间转换矩阵通过时间关系实现ti时刻与tk+1时刻的坐标转换，而这个坐标转换被用在前面的前投影中，从而和距离挂上了钩，我们要优化的距离，最终变成优化一个最优的帧间变换矩阵，让特征点到达特征区域的距离最小。
这里就省略一下了，大量的数学运算过后，我们得到了对于边点和平面点的两个优化式子：

让这些式子最优化，可以联合写成下面的式子：

之后利用列文伯格-马夸特法(LM)法去进行非线性优化，最终得到一个最优的帧间变换矩阵TLk+1。这个矩阵也就是我们要求的运动估计。

三、Lidar Mapping

前面的帧间优化的执行频率是高于这部分制图的执行频率的，也就是说大量的优化少量的制图，累计一定的帧数目的点云数据才会进行建图，虽然频率较低，但是精度很高。
本质上来说建图的目的就是将点云数据融入到世界地图里面去，精确估计点云数据在世界坐标系下面的位姿。
就过程而言，制图和前面的里程计实际上是很像的，区别在于制图多了一个坐标系的转换。

上图是制图的一个过程的模拟，行驶方向是从右向左，已经知道的量有：k+1帧之前的扫描的点云在全局坐标系下面的投影QK，可以理解为目前为止前面全部的已经校正的点云数据；第k次扫描的末尾也就是k+1帧起始的时候的位姿变换信息TKW，注意这里的转换矩阵是在世界坐标系下的，而在里程计中已知的是雷达坐标系下的；利用里程计得到的输出TK+1L，也就是在雷达坐标系下将第K+1帧映射到k+2时刻的变换矩阵。
利用这三个量，我们可以首先将TKW通过TK+1L向后推演一个时刻，也就是得到TK+1W，再利用这个转换关系，将k+1帧点云投影到全局坐标系下，即为QK+1-（这里加横杠变成了前投影）。这样问题转换为对TK+1W的优化，优化用到的工具是QK和QK+1-两个点云数据。问题和里程计很像，同样都是利用两个投影到同一个时刻的点云进行距离最优化，最后变成位姿的最优化，从而得到前后两帧的转换关系。转换关系得到后进行点云的匹配拼接，就可以扩大地图的范围。

因为针对的对象换成了制图，所以需要做一定的改变，一方面使用的QK+1-是十帧里程计输出的数据，而QK是之前的地图数据。也就是说QK+1-是攒了十次sweep的数据。对于QK，如果采用之前全部的地图来计算，必然会导致开销很大，所以这里变为使用边长为10m的立方体，用来代替全局地图进行TK+1W的计算。
由于实时性的缘故，找对应的特征点更换了方法，制图其他的算法步骤和里程计的基本一致：

参考链接：
https://zhuanlan.zhihu.com/p/111388877
https://www.cnblogs.com/zhjblogs/p/14221647.html
https://blog.csdn.net/i_robots/article/details/108331306