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.

一、论文概述和符号设定

1. 一个扫描周期内获取到的所有点云记作 P，$P_k$表示第k个扫描周期的点云。
2. 雷达坐标系设定为$L$，$L_k$表示第k个周期时的雷达观测坐标系，单个点云在$L_k$中表示为$X_{(k,i)}^L$。Lidar坐标系：x轴指向左侧，y轴指向上面，z轴指向正前方。
3. 全局坐标系设定为$W$，$W_k$表示第k个周期时的全局坐标系，单个点云在$W_k$中表示为$X_{(k,i)}^W$。
4. 问题为：给定采集点云数据$P_k$，计算前k个周期内雷达位姿以及构建全局地图。

Lidar接收原始点云数据$\hat{P}$，首先进行点云配准Point Cloud Registration（分为特征点提取和关联）配准后点云数据为$P_k$，接下来两个算法：一部分通过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点。

最后总结下来选点的三要素：

1. 不能超过设定的size，每个集合平面点4个，边缘点2个；
2. 已选取的点周围不能有点，使得点可以分布的更加均匀；
3. 选取的平面点不能与激光扫描束平行。

结果为下图，红色点为平面点，黄色点为边缘点。但是可以明显看出随着雷达的旋转运动，点云畸变效果。

B 特征点关联

将k时刻的点云和k+1时刻的点云关联起来：首先将第k时刻点云$P_k$重投影到第k+1时刻为$\widehat{P_k}$，将第k+1时刻扫描的点云$P_{k+1}$和$\widehat{P_k}$结合起来估计雷达的位姿。

其实在扫描的过程中激光雷达一直在运动，但是为了简单，这里以每一次扫描作为一个周期，并且把一个周期扫描结束后将所有采集到的点集都投影到下一时刻，表示为$\widehat{P_k}$。

参数定义：
第k次扫描的点云为$P_k$，提取的边缘点集合为$E_k$，提取的平面点为$H_k$。
第k+1次扫描的点云为$P_{k+1}$，提取的边缘点集合为$E_{k+1}$，提取的平面点为$H_{k+1}$。

我们的目的是找到$P_{k+1}$与