LeGO-LOAM学习笔记

核心内容 ：基于地平面的优化

正如标题中Lightweight and Ground-Optimized所表达的，这篇论文的核心是基于地面信息对LOAM进行了改进。对地面信息的利用有两个方面：1、通过提取出地面点云并赋予语义标签，对平面特征的匹配可以使用语义信息，从而避免非地面点云中的平面与地平面点云的误匹配。2、在点云配准时，非线性迭代优化过程中每次迭代对6自由度位姿分两步进行估计。stage1: 首先基于地面点中的平面特征匹配估计\(t_z\), \(\theta_{roll}\), \(\theta_{pitch}\)，因为使用地平面匹配估计出来的值在几何上是稳定的，而其他三个量是不稳定的。换句话说，由于代价函数中这一部分的误差是两个平面间的欧式距离，\(t_z\), \(\theta_{roll}\), \(\theta_{pitch}\)三个维度上的误差会被惩罚，而另外三个维度就不会有明显的惩罚（朝x或y方向平移平面或绕z轴旋转平面，两个平面之间的距离是不变的，具体可见《Geometrically Stable Sampling for the ICP Algorithm》中的分析）。stage2: 然后，将第一步估计出的三个量当作常量，利用非平面点中的edge特征匹配构建误差估计剩余的\(t_x\), \(t_y\), \(\theta_{yaw}\)三个量。

LeGO-LOAM基本流程

lego-loam的基本流程大致分为点云分割->特征提取->点云配准->点云地图更新几个阶段，可见论文中的流程图。实际代码实现中，在点云分割前还会通过IMU对点云的运动畸变进行矫正。

点云分割

点云分割主要有两个目的：1、将地平面点云分割出来，区别对待地平面点云和非地平面点云。2、过滤非地平面点云中一些杂点。 这些杂点一般对应环境中的树叶、草等小物体，从这样的点云提取的特征是很不稳定的。点云分割采用的是range image的方法，也就是将点云根据激光雷达的模型和参数投影生成一个图像，这个图像的索引对应激光光束的水平和竖直方向的角度，而像素的值就是该束激光测量到的距离（range）。提取地平面点以及非平面点的聚类分割都是基于range image的，直接follow两篇论文中的方法。

特征提取

LOAM能够高效实时运行的一个原因是点云配准时是基于特征的，这样就大大缩减了匹配时计算的规模。为了从所有方向均匀地提取特征，会将range image水平地划分成多个sub-image，每个sub-image选择最明显的那些特征，最后得到的特征总量是固定的。本文与LOAM类似，通过计算每个点的roughness来提取边缘edge特征和平面planar特征，如果roughness较大，说明该点为edge特征点，否则认为是planar特征点。roughness计算公式如下：

\[c = \frac{1}{|S|*||r_i||}|| \sum_{j\in{S}, j\neq i}(r_i-r_j)|| \]

与LOAM中不同的是，lego-loam会有区别地从地平面点云和非地平面点云中提取edge和planar特征。具体而言，地平面点云中只会提取平面特征，而非地平面点云两种都会提取。

点云配准

点云配准也就是文中的lidar odometry模块：“The lidar odometry module estimates the sensor motion between two consecutive scans. The transformation between two scans is found by performing point-to-edge and point-to-plane scan matching”.
这个模块也与LOAM中的方法类似，改进之处在于：1）标签（语义）匹配。由于点云被划分成了地面点和非地面点，因此只有标签相同的特征才会进行匹配。2）优化过程分为两步。也就是上文核心内容中所描述的，先基于地平面的平面特征匹配估计\(t_z\), \(\theta_{roll}\), \(\theta_{pitch}\)，再将这三个量作为已知量，利用非地平面的边缘特征匹配估计剩余状态分量。这种两步迭代优化的方式加速了收敛的过程。

点云地图更新

连续两帧点云配准后，新的点云数据会被融合到点云地图中。融合时的最小单位不是单个激光点，而是表达边缘或平面特征的一组点。融合时，当前点云与一个局部点云地图进行匹配，可以得到更加精细的位姿。当然，由于局部点云更大，这个过程更加耗时，因此执行频率更低。局部点云的确定有两种方式，或者选择点云地图中再当前估计位姿100米范围内的局部点云，或者利用pose graph，选择K个最近的结点所关联的那些点云。

以上就是基本的学习笔记，更加深入的话可以看下两篇基于range image进行地平面提取和点云分割的方法以及点云对齐时几何稳定性的分析那篇论文，有时间还可以具体读下lego-loam代码，因为具体实现会跟论文有些差异。