激光SLAM--(9) LeGO-LOAM主要代码逻辑

一、地面点提取方法

如下图所示，相邻的两个scan的同一列，打在地面上，形成两个点A和B。
假设这里面的线束是16线的，那可能是地面点的全是下半部分的7条；上图中相邻两个点的垂直高度差为 $\mathrm{h}=\left|{\mathrm{z}}_1-{\mathrm{z}}_2\right|$，水平距离差为$\mathrm{h}=\left|{\mathrm{z}}_1-{\mathrm{z}}_2\right|\mathrm{d}=\sqrt{{\left(x_1-x_2\right)}^2+{\left(y_1-y_2\right)}^2}$。
由此可以算得：$\theta =\mathrm{atan}2(\mathrm{h},\mathrm{d})$，如果为地面点，在理想情况下，这个角点接近0.

但是雷达的安装不会完全水平，并且地面也不是平的，因此这个角度会大于0，LeGO-LOAM设置的是10°。
即小于10°被判断为地面点。

这种地面点的提取算法有些过于简单，还可以结合激光雷达安装高度，等其它信息进行判断。例如下面这种情况，也会被判断为地面点：
所以可以加入高度（雷达中心离地面的高度）判断。。。。
LeGO-LOAM的地面提取的代码在 imageProjection.cpp 中 groundRemoval 函数，如果判断是无效点和地面点，labelMat中的标志会置为-1，即不参与后续线特征和面特征的提取；是地面点的话，groundMat中置为1，不是就置为-1；

二、基于BFS的点云聚类和外点剔除

⼴度优先遍历算法介绍

⼴度优先遍历（BFS）和深度优先遍历（DFS）同属于两种经典的图遍历的算法。 具体到⼴度优先遍历算法来说，⾸先从某个节点出发，⼀层⼀层地遍历，下⼀层必须等到上⼀层节点全 部遍历完成之后才会开始遍历。
通常我们使⽤BFS遍历图结构的时候，会借助⼀个队列std::queue来帮助我们实现，其基本步骤如下 ：

1. 将起始顶点S加⼊队列
2. 访问S，同时把S标记为已访问
3. 循环从队列中取出节点
   1. 如果队列为空，则访问结束
   2. 否则S出队列，将该节点标记为已访问，同时将其全部⼦节点加⼊队列。

在LeGO-LOAM中，BFS算法⽤于实现⼀个简单的点云聚类功能。

基于广度优先遍历的点云聚类算法和外点剔除

思想：点云聚类就是把空间上距离相近的点作为一个集合，打上共同的标签，若集合点数过少，则认为是杂点，需要剔除；
首先是要把一帧点云投影到深度图上；图中顶点是每一个扫描到的点，边是与其上下左右的邻接点。

若每帧点云是360°的话，那左右边界的点和边界另⼀侧也构成邻接，但是上下边界的另一侧就不是了。
具体实现：

1. 遍历每个点，若已被处理则不再处理
2. 如果没有被处理就是一个新聚类（这里没有使用队列，是使用的双指针数组）
3. 判断近邻和自身聚类是否够近angle越大则认为两点越可能在同一个聚类物体上，打上同样的标签。
4. 循环每个点

基于BFS的点云距离分割代码在imageProjection.cpp中

三、两步优化的帧间里程计

就如同LeGO-loam的全称一样，LeGO主要就是轻量级和使用地面优化，其中轻量级是通过两步优化的优化方式（地面优化在第一步）。
和原始loam一样，通过前后两帧点云来估计两帧之间的运动，从而得到里程计输出，只是这里与loam不同的在于是分成两步，每次估计三个自由度。

利用地面点优化

地面点更符合面特征的性质，因此地面点优化问题就使用点到面的约束来构建。但这里对x，y，yaw三个自由度是不能观的，即当这三个自由度发生变化时，点到面的残差不会发生明显的变化。
如上图所示，从下面的帧到上面的帧，明显有平移（x，y）和转向角yaw的运动，但是其点到面的距离是没什么变化的。

利用角点优化

第一步优化完pitch、roll和z之后，剩下还有三个自由度的变量需要进行估计，需要提取角点进行优化，由于多线激光雷达提取的角点通常是垂直的边缘特征，因此对，x、y及yaw有着比较好的能观性，通过角点的优化结合上地面点的结果可以得到六自由度的帧间优化结果。

将上一步已经优化了的z、pitch和roll的三自由度量，和剩下没有估计的三个自由度进行角点的约束优化。
代码实现在帧间里程计部分在FeatureAssociation.cpp中 的runFeatureAssociation的函数。且优化主要在updateTransformation() 函数中。