LeGO-LOAM算法详解
整体框架
LeGO-LOAM算法的总体框架如下图所示：
图中新增加了绿框中的Segmentation环节，同时对后续的特征提取、Odometry以及Mapping部分均有一定的修改，主要包括：
- 增加Segmentation操作，把点云投影为距离图像，分离出地面点与非地面点（分割点，segmented point）。
- 平滑度计算公式不同，原始LOAM中为使用点集中的坐标相减，而LeGO-LOAM中为使用点集中的欧式距离作差。
- 特征点选取中原始LOAM为按照平滑度的值及已有特征点数量分为平面点/边缘点两类，而LeGO -LOAM中不仅考虑到平滑度的值还同时考虑到点的类型为“地面点/分割点”，分别进行提取不同的特征点集。
- Lidar Odometry模块相邻帧之间特征点的对应关系也分别按照“地面点/分割点” 进行寻找，使得寻找匹配特征点的效率、精度更高。
- Lidar Mapping模块中LeGO-LOAM提供了基于传感器视野范围与基于图优化获取的两种获取特征对应点方法。此外，还加入了iSAM2进行后端优化（闭环检测）。
综上，LeGO-LOAM的总体思路与LOAM基本相同，1）把原始点云投影为距离图像，并在进行特征提取之前区分为“地面点/分割点”。2）对具有相同类别的特征点进行匹配。3）加入iSAM2进行闭环检测。这三点是其主要创新。
1 Segmentation模块
文章中采用Velodyne VLP-16激光雷达采集的数据进行实验，首先把原始点云重投影为一个距离图像，分辨率为1800*16（因为VLP-16水平分辨率为0.2°，360/0.2 = 1800，同时垂直方向上位16线数据）。重投影之后，三维点云变为二维图像，以像素点到传感器之间的距离作为像素值。
以VLP-16竖直维度的特性来进行标记地面点和非地面点，其在垂直方向的扫描范围为[-15°, 15°]，认为地面点出现在[-15°, -1°]之间的扫描线上，而被标记的地面点可以不用进行后续的分割。
随后，将距离图像分割为很多个聚类，同一个聚类的点被标记上唯一的标识。点数较少的聚类（少于30点）被作为噪声去除，这一步可以减少室外环境噪音点的干扰，如随风飘动的树叶，地面上的杂草等不稳定的特征，这样的处理就可以保留原始的地面点和相对较大的静态物体（树干，楼房等）来进行后续的特征提取工作了。如下图中 (a)为原始点云，(b)为进行分割处理后的点云。
2 Feature Extraction模块
这一模块在计算平滑度时与经典的LOAM相似，考虑到特征点在各个方向分布均匀，把距离图像水平均分为若干个子图像（360°划分为六等分）。随后对t时刻点云 P t P_t Pt中的每一个点 P i P_i Pi左右各选取5个点组合成点集 S S S，通过下式计算平滑度。
这里采用了距离图像中激光点到传感器之间的距离，与原始LOAM中直接使用两点坐标不同。
A \mathbb{A} A
随后，从子图像中的地面点中选取 n F p n_{F_p} nFp个平面点 F p F_p Fp，从分割点中选取 n F e n_{F_e} nFe个边缘点 F e F_e Fe。
此外，从子图像中的地面点与分割点中选取 n F p n_{\mathbb{F}_p} nFp个平面点 F p \mathbb{F}_p Fp, 从子图像中的非地面点中选取 n F e n_{\mathbb{F}_e} nFe个边缘点 F e \mathbb{F}_e Fe，即存在以下关系， F p F_p Fp⊂ F p \mathbb{F}_p Fp以及 F e F_e Fe⊂ F e \mathbb{F}_e Fe。
如下图中所示，( c ) 中为有类别约束的平面点 F p F_p Fp及边缘点 F e F_e Fe，(d) 中为子图像中所有的平面点 F p \mathbb{F}_p Fp与边缘点 F e \mathbb{F}_e Fe。
3 LiDAR Odometry
特征提取时，我们得到了四种特征点集{
F p F_p Fp, F e F_e Fe, F p \mathbb{F}_p Fp, F e \mathbb{F}_e Fe}。为了更好的寻找相邻两帧点云数据之间的对应特征点对，采用如下图所示的思路进行优化：
上图中的流程可以总结为：
- 对于平面点：在 F p ( t − 1 ) \mathbb{F}_p^{(t-1)} Fp(t−1)中具有分割点标签的点云中寻找 F p t F_p^t Fpt的对应的关联点。
- 对于边缘点：在 F e ( t − 1 ) \mathbb{F}_e^{(t-1)} Fe(t−1)中具有分割点标签的点云中寻找 F e t F_e^t Fet的对应的关联点。
这可以使缩小对应点的候选范围，同时提高匹配的精度和效率。有了对应的特征点对之后，需要对其进行优化求解六个姿态变换参数，LeGO-LOAM使用两步LM优化方法进行处理。
首先，采用平面点 F p ( t − 1 ) \mathbb{F}_p^{(t-1)} Fp(t−1)与 F p t F_p^t Fpt对应的约束，优化计算得到了{ t z t_z tz, t r o l l t_{roll} troll, t p i t c h t_{pitch} tpitch}。随后，基于 F e ( t − 1 ) \mathbb{F}_e^{(t-1)} Fe(t−1)与 F e t F_e^t Fet对应的约束，以及之前优化得到的{ t z t_z tz, t r o l l t_{roll} troll, t p i t c h t_{pitch} tpitch}，进行优化得到{ t x t_x tx, t y t_y ty, t y a w t_{yaw} tyaw}。最终，两次优化结果融合，得到最终的变换参数{ t x t_x tx, t y t_y ty, t z t_z tz, t r o l l t_{roll} troll, t p i t c h t_{pitch} tpitch , t y a w t_{yaw} tyaw}。
4 LiDAR Mapping
作者在这里介绍了两种方法：1）基于传感器视域。 2）基于图优化。分别来把上一步输出的点云与局部位姿变换到全局地图中。
其中，第一种方法将全局点云地图分割为很多cube，然后根据传感器的有效探测距离（VLP-16为100m）选取一定数量的cube，组合成前 t − 1 t-1 t−1 时刻获取的点云地图 Q ( t − 1 ) Q^{(t-1)} Q(t−1)，然后对当前帧 Q t Q^t Qt与其相交的部分进行匹配，与经典的LOAM算法基本相同。
第二种方法加入了位姿图和回环检测 (iSAM2)，提高了建图效率与精度，但是目前还不太理解，这里就先不做详细的介绍了。