LOAM：实时的雷达里程计和建图
之前对视觉SLAM主要的开源框架，ORB-SLAM2、SVO等进行了介绍，然后疫情期间对VINS-Mono进行了详细的源码解析，接下来考虑到工作原因需要用到激光雷达、GNSS、IMU等多传感器融合，所以接下来会对最经典的激光SLAM框架LOAM和LeGo-LOAM两个开源系统进行总结。
之前对激光SLAM的印象就是激光雷达采集到点云数据，然后用ICP对准就可获得接收端位姿估计。
第一篇经典的激光SLAM paper是2014年CMU Ji zhang的《LOAM：Lidar Odometry and Mapping in Realtime》
LOAM是基于激光雷达而搭建的在ROS平台下的SLAM系统
论文：https://www.ri.cmu.edu/pub_files/2014/7/Ji_LidarMapping_RSS2014_v8.pdf
代码：https://github.com/HKUST-Aerial-Robotics/A-LOAM/tree/devel/src
对于LOAM的源代码解析分为四部分进行讲解：
LOAM源码解析——scanRegistration
LOAM源码解析2——laserOdometry添加链接描述
LOAM源码解析3——laserMapping添加链接描述
LOAM源码解析4——transformMaintenance
摘要
提出一种实时里程计和建图方法，使用以6自由度运动的2轴雷达的距离测量值。这个问题之所以棘手，是因为不同时间接收到的距离测量值以及运动估计的误差会导致点云的误匹配。迄今为止，通过离线批处理方法可以构建连贯的3D地图，经常使用回环检测纠正随时间的漂移。本文可以达到低漂移和低计算复杂度，而无需高精度测距或惯性测量。获得这样性能的关键是把同时定位和建图问题划分，该方式寻求通过两个算法同时优化大量变量。一种算法以高频率但低保真度执行测距法以估计激光雷达的速度。另一个算法以较低的数量级频率运行，以进行点云的精确匹配和配准。两种算法的组合使该方法可以实时映射。该方法已通过大量实验以及在KITTI里程表基准上进行了评估。结果表明，该方法可以在最先进的离线批处理方法水平上实现准确性。
图左侧为激光雷达里程计，包含点云畸变矫正，右侧为雷达建图。
主要贡献
一、本文主要贡献是把同时定位与建图（SLAM）技术分为两部分，一个是高频率（10HZ）低精度的里程计odometry过程，另一个是低频率（1HZ）高精度的建图mapping过程，二者结合可实现低漂移、低计算量、高精度的SLAM。
二、Lidar Odometry：分为特征点提取Feature Point Extraction和特征点关联 Finding Feature Point Correspondence两部分。
特征点提取在激光雷达每一次sweep中，根据曲率对点进行排序，作为评价特征点局部表面光滑性的标准。曲率最大的为边缘点，曲率最小的为平面点，每个局部提取2个边缘点和4个平面点。
特征点关联使用scan-to-scan方式，分为边缘点匹配和平面点匹配两部分。计算点到直线的距离和点到平面的距离。
姿态解算根据匹配的特征点云估计接收端位姿。
三、Lidar Mapping：
低频率建图，前面获得相邻帧的姿态变换，接下来要和全局地图进行匹配，将其加入到全局地图中。主要分为两步骤：
1、Pk+1点云去畸变，并对齐到世界坐标系中加入点云地图，即为Qk+1
2、Qk+1和Qk配准，优化位姿Tk+1.
一、论文概述和符号设定
- 一个扫描周期内获取到的所有点云记作 P， P k P_{k} Pk表示第k个扫描周期的点云。
- 雷达坐标系设定为 L L L， L k L_{k} Lk表示第k个周期时的雷达观测坐标系，单个点云在 L k L_{k} Lk中表示为 X ( k , i ) L X_{(k,i)}^{L} X(k,i)L。Lidar坐标系：x轴指向左侧，y轴指向上面，z轴指向正前方。
- 全局坐标系设定为 W W W， W k W_{k} Wk表示第k个周期时的全局坐标系，单个点云在 W k W_{k} Wk中表示为 X ( k , i ) W X_{(k,i)}^{W} X(k,i)W。
- 问题为：给定采集点云数据 P k P_{k} Pk，计算前k个周期内雷达位姿以及构建全局地图。
Lidar接收原始点云数据 P ^ \widehat{P} P ，首先进行点云配准Point Cloud Registration（分为特征点提取和关联）配准后点云数据为 P k P_{k} Pk，接下来两个算法：一部分通过Lidar Odometry输出连续两帧之间10HZ的低精度姿态估计，另一部分将畸变矫正后的点云输入Lidar mapping并输出1HZ的高精度点云地图。最后估计的位姿和全局地图进行匹配优化位姿，生成基于地图的10HZ激光雷达姿态数据。
二、Lidar Odometry激光雷达里程计
这里就是传统SLAM前端里程计，根据相邻两帧的特征点估计接收端的姿态。本文在特征点云的特征点提取、特征点关联、最后估计位姿等方面都提出了创新点。
特征点提取：根据点的曲率c来将点划分为不同的类别（边缘或平面），每个局部提取2个边缘点和4个平面点。
特征点关联：相邻两帧点云数据的关联，计算这一时刻对应上一时刻最近的两个点，求点到线的距离，点到面的而距离。
姿态估计：将所有对应到的点求到直线、面的距离之和最短，然后按照Levenberg-Marquardt算法迭代计算，得到两帧之间的变换，最后通过累计计算odom。
A 特征点提取
稀疏SLAM里为了减小计算量经常采用特征点提取，像ORB、SIFT、SURF等特征点，这里对于激光点云主要提取两类：平面点和边缘点。
边缘点：三维空间中尖锐边缘的点，和周围点的差距大，曲率c较高。
平面点：背景上的平面点，曲率c较低。
首先讲述区分二者的重要指标：曲率c。根据点的曲率来计算平面光滑度作为提取特征信息的重要指标。对每一个点通过其周围最近的5个点，计算曲率大小。
首先根据c排序，大于阈值为边缘点，小于阈值为平面点；然后为了使特征点均匀的分布在环境中，将一次扫描划分为4个独立的子区域，每个子区域最多提供2个边缘点和4个平面点。
同时，对于正常流程所取得点也有一定限制：
1、避免周围点已被选择从而保证特征点分布均匀，或者局部平行于激光束的局部平面上的点。如图（a）中的点B
2、避免在被遮挡的边界上的点。如图（b）中的点A被B遮挡住了。
只有在S没有形成与激光束大致平行的表面斑块并且S中没有点在激光束方向上与间隙断开与i的点时，才能选择i点。
最后总结下来选点的三要素：
- 不能超过设定的size，每个集合平面点4个，边缘点2个；
- 已选取的点周围不能有点，使得点可以分布的更加均匀；
- 选取的平面点不能与激光扫描束平行。
结果为下图，红色点为平面点，黄色点为边缘点。但是可以明显看出随着雷达的旋转运动，点云畸变效果。
B 特征点关联
将k时刻的点云和k+1时刻的点云关联起来：首先将第k时刻点云 P k P_{k} Pk重投影到第k+1时刻为 P k ^ \widehat{P_{k}} Pk ，将第k+1时刻扫描的点云 P k + 1 P_{k+1} Pk+1和 P k ^ \widehat{P_{k}} Pk 结合起来估计雷达的位姿。
其实在扫描的过程中激光雷达一直在运动，但是为了简单，这里以每一次扫描作为一个周期，并且把一个周期扫描结束后将所有采集到的点集都投影到下一时刻，表示为 P k ^ \widehat{P_{k}} Pk 。
参数定义：
第k次扫描的点云为 P k P_{k} Pk，提取的边缘点集合为 E k E_{k} Ek，提取的平面点为 H k H_{k} Hk。
第k+1次扫描的点云为 P k + 1 P_{k+1} Pk+1，提取的边缘点集合为 E k + 1 E_{k+1} Ek+1，提取的平面点为 H k + 1 H_{k+1} Hk+1。
我们的目的是找到 P k + 1 P_{k+1} Pk+1与 P k P_{k} Pk之间变换关系。同样分为两部分：边缘点 E k + 1 E_{k+1} Ek+1与 E k E_{k} Ek、平面点 H k + 1 H_{k+1} Hk+1与 H k H_{k} Hk。
已知量：上个周期点云投影到当前时刻重投影点云 P k ^ \widehat{P_{k}} Pk 和当前采集点云 P k + 1 P_{k+1} Pk+1。
结果：我们会从 P k ^ \widehat{P_{k}} Pk 找到 E k + 1 E_{k+1} Ek+1对应的边缘线，以及 H k + 1 H_{k+1} Hk+1对应的平面。
每次迭代过程中，当前时刻采集到的边缘点云和平面点云 E k + 1 E_{k+1} Ek+1和 H k + 1 H_{k+1} Hk+1根据当前估计的位姿重投影到sweep的起始（每一帧初始时刻），表示为 E k + 1 ^ \widehat{E_{k+1}} Ek+1 和 H k + 1 ^ \widehat{H_{k+1}} Hk+1 ，对于 E k + 1 ^ \widehat{E_{k+1}} Ek+1 和 H k + 1 ^ \widehat{H_{k+1}} Hk+1 中的每个点，我们需要在 P k ^ \widehat{P_{k}} Pk 中最接近的临近点。
由于雷达高速运动，一一对应的点云配准是不合适的。而是通过在上一时刻寻找两点确定一条与当前时刻的角点距离最近的直线作为该角点的配准对应；在上一时刻寻找三点确定一个与当前时刻的平面点距离最近的平面作为该平面点的配准对应。
1、边缘点匹配
边缘点和边缘线的对应。对于当前时刻的边缘角点找到上一时刻对应的边缘线，首先通过当前时刻点i对应的上一时刻最近的两个点，然后判断这两个点是不是边缘点，j和l必须是不同线上的点，因为一次一个线在某一段中只能有一个边缘点。
已知：当前时刻边缘点投影到sweep的起始（每一帧初始时刻） E k + 1 ^ \widehat{E_{k+1}} Ek+1 和上一时刻边缘点 E k E_{k} Ek
目的：在 E k E_{k} Ek中找到一条线来求解最近距离。
在当前帧点云 E k + 1 ^ \widehat{E_{k+1}} Ek+1 中选取一个点 i i i,首先在 E k E_{k} Ek中选取与 i i i最近的点 j j j，接下来在 E k E_{k} Ek中选取和点 j j j相邻的扫描线中最近的点 l l l，保证 i , j , k i,j,k i,j,k三点不共线，可以构成三角形。
橘色线表示距离i最近的点j的扫描；蓝色线是两个连续雷达扫描。
最后选取了三个点 i ∈ E k + 1 ^ , j , l ∈ P k ^ {{i\in\widehat{E_{k+1}} ,j,l\in \widehat{P_{k}}}} i∈Ek+1 ,j,l∈Pk 接下来就要求取点i到jl的最短距离。
2、平面点匹配
已知：当前时刻平面点投影到sweep的起始（每一帧初始时刻） H k + 1 ^ \widehat{H_{k+1}} Hk+1 和上一时刻平民啊点 H k H_{k} Hk
目的：在 H k H_{k} Hk中找到一个平面来求解最近距离。
在当前帧点云 H k + 1 ^ \widehat{H_{k+1}} Hk+1 中选取一个点 i i i,首先在 H k H_{k} Hk中选取与 i i i最近的点 j j j，接下来找到与点 j j j在相同激光扫描束的最近点 l l l，最后找到相邻扫描束中与点 j j j最相近的点 m m m。就可以找到一个不共线的，能构成一个平面的三个点。
最后选取了三个点 i ∈ H k + 1 ^ , j , l , m ∈ P k ^ {{i\in\widehat{H_{k+1}} ,j,l,m\in \widehat{P_{k}}}} i∈Hk+1 ,j,l,m∈Pk 接下来就要求取点i到jl的最短距离。
点到平面的距离：
C 姿态估计
激光雷达扫描一圈过程中，假设车辆是匀速运动的，在此过程中，就可以对这一帧数据中的任意点按照其获得时相对于起始点的时间进行内插值。获得每一个点的位姿。首先求解第k帧中第i个点的姿态变换R和T信息，插值公式如下。
其扫描点i时刻的位姿X为：
接下来，就可以使用每个点对应的姿态变换矩阵放进去进行后续求解。通过构建Jacobian矩阵，完成LM求解估计雷达位姿。其实，这个步骤就是把k+1帧的点投到第k帧所在集合中。
罗德里格斯公式：
旋转向量中参数为：
连续两帧，两种特征点的几何关系，以及最后联立非线性方程
LM阻尼高斯牛顿优化求导，计算雅克比矩阵J，用非线性优化的办法最小化d
这里求解的还是局部雷达观测坐标系下的结果，是为了求解相邻帧之间的变换，而为了定位和建图，需要求解的是全局坐标系下的变换。
已知：上一时刻点云集 P k ^ \widehat{P_{k}} Pk ；当前不断扫描的点云 P k + 1 P_{k+1} Pk+1；最新递归的位姿 T k + 1 L T_{k+1}^{L} Tk+1L
整体来说流程为：
扫描刚开始， T k + 1 L T_{k+1}^{L} Tk+1L初始化为0，之后从点云 P k + 1 P_{k+1} Pk+1中提取边缘点集 E k + 1 E_{k+1} Ek+1和平面点集 H k + 1 H_{k+1} Hk+1。针对 E k + 1 E_{k+1} Ek+1和 H k + 1 H_{k+1} Hk+1中的每个点，在 P k ^ \widehat{P_{k}} Pk 找到关联的点。15行：该算法为每个特征点分配双平方权重，与它们关联点具有较大距离的特征点被分配较小的权重，而距离大于阈值的特征点被视为离群点并被分配零权重。 16行：姿势估计将更新一次。 如果找到收敛或达到最大迭代次数，则非线性优化终止。21行：在扫描周期结尾，把当前扫描的一周点云 P k + 1 P_{k+1} Pk+1投影到下一时刻，这里会用到估计的位姿，输出 T k + 1 L T_{k+1}^{L} Tk+1L和 P k + 1 ^ \widehat{P_{k+1}} Pk+1 ；如果不是扫描末尾，只输出位姿 T k + 1 L T_{k+1}^{L} Tk+1L。
三、Lidar Mapping雷达建图
在扫描周期末尾激光雷达里程计输出畸变矫正过的点云 P k + 1 ^ \widehat{P_{k+1}} Pk+1 ，同时还有整个周期内每个点的姿态 T k + 1 L T_{k+1}^{L} Tk+1L。雷达建图过程是为了把 P k + 1 ^ \widehat{P_{k+1}} Pk+1 在世界坐标系下匹配。在得到相邻帧的姿态变换信息后，需要将其和全局地图进行匹配，并将其加入到全局地图中。
主要任务：
- 点云 P k + 1 {P_{k+1}} Pk+1去畸变，对齐到世界坐标系， Q k + 1 ^ \widehat{Q_{k+1}} Qk+1
- Q k + 1 ^ \widehat{Q_{k+1}} Qk+1 和 Q k Q_{k} Qk配准，优化位姿 T k + 1 W T_{k+1}^{W} Tk+1W
参数定义：
- k次扫描累积的地图点云集 Q k Q_{k} Qk
- 第k次扫描结尾，也是k+1帧 t k + 1 t_{k+1} tk+1地图上雷达的初始位姿 T k W T_{k}^{W} TkW
已知：累积的地图 Q k Q_{k} Qk,根据位姿投影的当前帧点云 Q k + 1 ^ \widehat{Q_{k+1}} Qk+1 ，粗略的每个点云位姿 T k + 1 W T_{k+1}^{W} Tk+1W
输出：优化后精密的位姿 T k + 1 W T_{k+1}^{W} Tk+1W
方式：通过不断优化雷达位姿 T k + 1 W T_{k+1}^{W} Tk+1W将 Q k + 1 ^ \widehat{Q_{k+1}} Qk+1 和 Q k Q_{k} Qk匹配上。
利用里程计输出的位姿 T k + 1 L T_{k+1}^{L} Tk+1L，将 T k W T_{k}^{W} TkW不断补充 t k + 1 t_{k+1} tk+1到 t k + 2 t_{k+2} tk+2时间范围内雷达相对于世界坐标系的位姿，最终获得 T k + 1 W T_{k+1}^{W} Tk+1W.
将 P k + 1 ^ \widehat{P_{k+1}} Pk+1 通过粗略估计的位姿 T k + 1 W T_{k+1}^{W} Tk+1W投影到世界坐标系上，表示为 Q k + 1 ^ \widehat{Q_{k+1}} Qk+1 。
map-to-map通过不断匹配 Q k + 1 ^ \widehat{Q_{k+1}} Qk+1 和 Q k Q_{k} Qk优化位姿 T k + 1 W T_{k+1}^{W} Tk+1W，如果用上所有地图数据计算量很大，这里使用一个边长为10m的立方体替代全局地图。且 Q k + 1 ^ \widehat{Q_{k+1}} Qk+1 是10帧雷达扫描的数据，存入到KD-tree中。
在 Q k Q_{k} Qk中选取相邻点集合S，计算S的协方差矩阵M、特征向量E、特征值V。选取边缘线和平面块方式为：
边缘线：V中特征值一大两小，E中大特征值代表的特征向量代表边缘线的方向。
平面块：V中一小两大，E中小特征值对应的特征向量代表平面片的方向。
边缘线或平面块的位置通过穿过S的几何中心来确定。
这样就可以快速找到 Q k + 1 ^ \widehat{Q_{k+1}} Qk+1 中的点 i i i，和 Q k Q_{k} Qk中的边缘点 j , l {j,l} j,l以及平面点 j , l , m {j,l,m} j,l,m然后利用LM算法求解 T k + 1 W T_{k+1}^{W} Tk+1W。
四、实验
作者分别在1）室内、室外环境，2）有无IMU等条件下对算法的精度做了验证，3）并在kitti的城市、乡村、高速公路场景下做了测试。
1、室内室外环境
实验结果如下图：室内要好于室外，因为室内环境是结构化的，特征点提取上比室外自然场景下更多、并且更加稳定。
此外，还对室内走廊和室外果园两个场景的漂移误差做了验证。室内：把起始点和结束点的gap作为漂移误差。室外：搭载高精度GPS/INS作为真值。结果表明室内精确度高。
2、有IMU
添加IMU后，点云经过两种方式进行预处理：1）利用IMU确定方向，旋转一次扫描接收的点云以使其与该扫描中激光雷达的初始方向对齐；2）进行加速度测量，像激光雷达在扫描期间以恒定速度移动一样，可以部分消除失真。通过将来自陀螺仪的角速率和来自加速度计的读数积分在卡尔曼滤波器中，可以得到IMU方向。一个人手持激光雷达在楼梯上行走。红色为单纯IMU，所提方法为绿色，结合IMU为蓝色线。
下图中右上部为结合IMU，右下部为单纯所提算法，明显看出上图中的边缘较锐利。
误差：激光雷达由一个以0.5m / s的速度行走并以0.5m左右的幅度上下移动激光雷达的人握住。地面真相由卷尺手动测量。
3、KITTI数据集
优点：
新颖的边缘点和平面点提取方式。
运动补偿（时间戳）
融合了scan-to-scan和map-to-map两种方式。
缺点：
没有后端优化
不能处理大规模的旋转变量（旋转向量的求解）
参考：
https://zhuanlan.zhihu.com/p/111388877
http://xchu.net/2019/09/29/paper-reading1/
https://blog.csdn.net/qq_21842097/category_7741306.html