LOAM SLAM之论文原理解读

1. 概述

简单整理阅读LOAM论文后对LOAM SLAM的理解，以供自己以后参考。

2. 特点

只需要3d激光点云数据，低漂移，低计算量，
SLAM分为两个部分：10Hz的激光里程计和1Hz的建图
激光里程计使用基于特征点进行相邻点云帧数据匹配，
激光建图使用基于特征点进行校正后输入点云数据和地图拼接。
使用的特征点和对应的特征区域有两类：

1. 使用边缘点以及边缘点所在边缘线，
2. 使用平面点以及平面点所在平面。

在KITTI上里程计排行很高。

3. 术语解释

sweep表示一次完整的数据，一帧点云数据，是多次scan之后得到的点云数据，对应于现在的多线激光雷达，一线代表一个scan，16线激光雷达一次获得的数据就是一次sweep，包含16个scan数据．对于3维点云数据，每次处理的都是一次sweep数据，这样才叫三维，否则不就是二维了吗？仅限于此论文的机械结构，如果使用多线激光，一般一帧数据称为scan。这里是因为二维雷达的原因，术语含义发生了变化，但是本质都是一次完整扫描过程称为scan。

评论区有小伙伴提出疑问，这里我再解释一下，首先明确二维激光雷达只是水平方向旋转扫描，此时一次水平扫描完成的数据为一次scan；三维激光雷达（多线雷达，激光束在垂直方向按照时间间隔依次发射）先垂直扫描再水平旋转实现三维扫描，此时一次三维扫描称为一次scan。虽然二维激光和三维激光都有一次scan的术语，但是对应的含义只是说明一次完整的扫描过程，实际表示的数据并不一样。原论文作者使用二维激光实现三维激光，需要添加机械结构实现垂直扫描，这样就会导致多个scan才是我们需要的三维扫描，那么这多个scan称为一次sweep。也就是，这里的一次sweep对应于三维激光中的一次scan，二维激光的一次scan不等于三维激光的一次scan。

评论区有小伙伴对激光束发射提出疑问，此问题针对vlp16激光雷达，具体请转LOAM源代码分析附公式推导，这里我解释一下，激光束是间断发射的16束垂直方向的激光，不是同时发射的，但是间断时间很短，你可以认为是同时发射的，只是认为。可以想一个问题，激光如果是同时发射的，如何根据反射的激光区分他们呢，所以一次只能发射一束激光。根据官方资料，在垂直方向发射激光时，相邻激光束时间差为$2.304\mu s$微秒，16束激光发射完成时会有$18.43\mu s$微秒的空闲时间，一次16束激光完成发射需要花费$16\ast 2.034\mu s+18.43\mu s=55.296\mu s$微秒。当水平旋转频率为600RPM时，对应的水平方向分辨率为$Azimut{h}_{resolution}=10rev/s\ast 360°/rev\ast 55.296\times 10^{-6}s/firingcycle=0.199°/firingcycle$，这下应该都明白了吧。

k表示第几次sweep
$P_k$表示第k次sweep的点云数据
L表示激光雷达坐标系，左上前右手坐标系，$X_{k,i}^L$表示$P_k$中点$i$在激光雷达坐标系L下的坐标
W表示三维世界坐标 ，左上前右手坐标系，$X_{k,i}^W$表示$P_k$中点$\text{i}$在世界坐标系W下的坐标
需要解决的问题
给定点云数据序列$P_k$，计算激光雷达在sweep之间的运动并对历史轨迹的周围环境进行建图。

4. 介绍

作者使用单线激光雷达和电机构成了一个多线激光雷达。
激光雷达具体信息：
型号为UTM-30LX 2维激光雷达 25msec/scan,每秒进行40次扫描。
获得的点云数据
水平范围是180°，分辨率为0.25°
使用电机实现垂直扫描，角速度为180°/s。垂直范围为180°，
一次完整的帧数据(sweep)就是水平范围180°，垂直范围180°。耗时1s。

单纯的激光数据会出现畸变，这是为什么呢？

• 激光的点云数据不是同一时间获取的，当sweep开始时激光处于A位置，当sweep结束时激光处于B位置，因为激光扫描时间较长，如果是一堵平行的墙面，理想状况下激光处于A或者B位置测量的距离应该一样，但是实际中激光处于A位置测量的距离和处于B位置的测量的距离一定会出现差异，失去了原始平行墙面的特征，进行激光SLAM时，AB位置的差距必须要考虑，或者说激光在这期间的运动必须要考虑，对应到点云数据会发现，点云数据出现较大的偏移。因此在数据处理时需要构造激光运动模型来消除点云畸变带来的影响。
  
  如下图所示，由于机器人的运动，激光里程计直接获得的数据与实际数据点位置存在差异，因此需要进行修正。原始点云数据经过激光里程计修正之后畸变效果明显降低。
  
  整个LOAM的流程：点云注册，激光里程计（高频低精度），激光建图（低频高精度），坐标发布。
  

5. 特征点提取

特征点包括边缘点和平面点

$S$是第$k$帧点云数据中点$i$对应$X_{ki}^L$附近的一个连续点集合，下式表示的含义：
根据点$i$和$S$中点的归一化矢量和的模来判断点i的类型，如果模大，则为边缘点，模小则为平面点。$|S|$表示$S$中点的数量。

如下图所示，边缘点的矢量和的模一般较大，矢量和不为零向量，而对应平面点的矢量和的模一般较小，矢量和为零向量。

将每次scan扫描得到的点云数据均匀分为四个区域，每个区域最多包含2个边缘点，4个平面点。
需要丢弃不可靠点，例如下面a中的B点，所在平面和激光束平行；下面b中的A点，实际中处于平面特征，由于被遮挡的原因可能被视为边缘点，因此不是真正的边缘点，在相邻帧数据中可能不会再次观测到。这两类点需要进行排除。
还有离群点：某点与左右相邻两个点的距离过大。

因此选择特征点时需要满足条件：

1. 特征点数量满足子区域最大值，每个子区域的特征点数目是一定的，防止过多的特征点。
2. 特征点周围点不能再次被选择，本来每个子区域的特征点数量就比较少了，进一步防止特征点集中分布。
3. 特征点非平行激光束的平面点，非被遮挡的平面点，非离群点。这些都是不可靠的点云数据，不能在连续两帧点云数据中得到，不能作为特征点。

下图就是找到的边缘点（黄色），平面点（红色），由于激光雷达处于运动中，这个时候还存在点云畸变，未进行校正，暂时也不能进行校正。

6. 寻找特征点关系

对于边缘点，寻找边缘点和边缘点所在直线的关系，对于平面点，寻找平面点和平面的关系。
$P_k$开始的时间戳为$t_k$，结束的时间戳为$t_{k+1}$，经过畸变校正之后将$P_k$重投影到$t_{k+1}$得到${\bar{P}}_k$。
估计激光雷达运动时，使用的是
重投影点云数据(经过畸变校正)${\bar{P}}_k$和新的点云数据（未经过畸变校正）$P_{k+1}$。

$E_{k+1}$和$H_{k+1}$分别表示在$P_{k+1}$（未经过畸变校正）中找到的边缘点和平面点集合。对应的，我们需要在${\bar{P}}_k$中找到与$E_{k+1}$中边缘点对应的边缘线和与$H_{k+1}$中平面点对应的平面。

• 这里简单说一下，为什么？理想情况下，没有畸变，激光雷达在短时间里获取的相邻帧数据是含有大量同一特征点的，就是$P_{k+1}$和$P_k$中很多特征点只是雷达在不同位置下观察的同一个点。（前面已经去除了不可靠的特征点），那么现在只要找到一个变换关系$T$使$P_{k+1}$和$P_k$中对应的点重合，那这个$T$实际上表示了激光雷达的帧间运动。为了减少计算量，增加鲁棒性，不直接使用点与点的匹配，而是使用边缘点和对应的线特征，平面点和对应的面特征，这样也可以得到变换关系$T$。然而现在的点云数据是含有畸变的，并且将矫正的$P_k$也重投影到了$t_{k+1}$时刻，那么现在就是一边进行矫正，一边进行匹配。直到所有点和对应特征区域距离和最小。这就是一个迭代的过程，迭代时，变换关系$T$一直在发生变化，$T$的变换使$所有点和对应特征区域距离和$一直朝着最小的方向前进。

每次迭代计算进行时都会将$E_{k+1}$和$H_{k+1}$中的特征点重投影到$k+1$次sweep开始$t_{k+1}$（这是因为我们可以参考的只有 上一帧 矫正后 重投影到$t_{k+1}$的数据${\bar{P}}_k$），使用${\tilde{E}}_{k+1}$和${\tilde{H}}_{k+1}$表示迭代过程中重投影到$k+1$次sweep开始$t_{k+1}$的点集合。对于$\tilde{E}$