LeGO-LOAM:轻量级地面优化的建图

LEGO-LOAM 是基于LOAM优化的系统，相比之下，有以下优点：
1）LeGO-LOAM是轻量级的，因为可以在嵌入式系统上实现实时姿态估计和建图。
2）去除失真数据，在地面分离之后，执行点云分割以丢弃可能表示不可靠特征的点。
3）LeGO-LOAM引入地面优化，因为我们引入了两步优化姿势估计。从地面提取的平面特征用于在第一步中获得$[t_z,{\theta }_{roll},{\theta }_{pitch}]$；在第二步中，通过匹配从分段点云提取的边缘特征来获得其余部分的变换$[t_x,t_y,{\theta }_{yaw}]$
4）集成了回环检测以校正运动估计漂移的能力。

论文：https://ieeexplore.ieee.org/abstract/document/8594299
代码：https://github.com/facontidavide/LeGO-LOAM-BOR/tree/speed_optimization
下图在KITTI数据集上明显看出定位精度之高。

摘要

本文提出一个轻量级、地面优化的雷达里程计和建图方法，LEGO-LOAM，用来实施估计地面车辆的6DOF位姿估计。该系统是轻便的，可以在低功率嵌入式系统上实现实时位姿估计。该系统是基于地面优化的，在分割和优化步骤中利用了地平面。

首先应用点云分割步骤过滤噪声，
接下来使用特征提取步骤获得边缘和平面特征。
之后，两步LM优化算法使用平面和边缘特征点来解决连续扫描中6DOF变换的不同分量。

使用从地面车辆的可变地形环境中收集的数据集，将LeGO-LOAM的性能与最先进的LOAM进行了比较，结果表明LeGO-LOAM可以在降低计算费用的同时达到相似或更高的精度 。 我们还将LeGO-LOAM集成到SLAM框架中，以消除由漂移引起的姿势估计误差，该误差已使用KITTI数据集进行了测试。

主要贡献

本文的核心在五个部分：点云分割segmentation、特征点提取feature extraction、雷达里程计lidar odometry、雷达建图lidar mapping、变换融合transform fusion。

• 点云分割：使用单次扫描的点云，并将其重投影到距离图像上进行地面分割，非地面点被分割出来。点云分割可以去除不稳定的特征点。
• 特征点提取：基于分割后的点使用LOAM进行边缘点和平面点提取。
• 雷达里程计：根据提取的特征点构建scan-to-scan约束，两次LM优化，估计姿态。第一步，从地面提取的平面特征点用于获得$[t_z,{\theta }_{roll},{\theta }_{pitch}]$；第二步，通过匹配从分段点云提取的边缘特征来获得其余部分的变换$[t_x,t_y,{\theta }_{yaw}]$
• 雷达建图：构建scan-to-map约束，构建全局地图。
• 变换融合：融合Lidar Odometry和 Lidar Mapping进行精确的姿态估计。

一、Segmentation

符号设定：
时间t时获得的点云集$P_t=(p_1,p_2,.....p_n)$，$p_i$是$P_t$中一个单独的点。
$r_i$表示点$p_i$到传感器的欧几里得距离。

目的：将点云分割为边缘点类和平面点类两类。

距离图像range image的分辨率为1800*16.
投影图像的水平方向分辨率为1800，因为激光雷达水平方向角度分辨率为0.2度，所以$\frac{360}{0.2}=1800$，投影图像竖直方向分辨率为16，因为激光雷达竖直方向有16根扫描线。

1）将三维的点云集$P_t$投影到距离图像上，每个点$p_i$现在可以用range
image中的像素表示，像素的值$r_i$表示相应的点$p_i$到传感器的欧几里得距离。
2）完成投影到距离图像上后可以得到一个矩阵，在进行分割之前，对距离图像进行逐列评估，提取地面点。标记为地面点的之后不用进行后续的分割。激光雷达激光扫描束范围是$[-15^o,15^o]$，地面点一定出现在$[-15^o,-1^o]$扫面线上。
3）对点云进行分组聚类，仅保留可表示大对象（例如树干）和地面点的点（图2（b））

使用基于图像的分割方法将距离图像分割为很多个聚类。同一类的点具有相同的一个标签（地面是较为特殊的一类）；
去除以下干扰情况：

• 随风飘动的树叶，草等，在实际的实验中会经常遇，可能会造成出现在前一帧而不出现在后一帧的情况。
• 去除不能聚成类的点云，可以在保留当前帧特征信息的基础上，减少噪音点的干扰，提升特征提取精度。本文将低于30个数据点的类别都当作噪点处理

之后，每个点可以得到以下三个信息：

1. 分割标签（地面点或分割点）
2. 在距离图像中的行和列的索引
3. 和传感器的距离值$r_i$

二、Feature Extraction

特征点提取类似LOAM，但不从原始点云提取，而是从地面点和segmented points提取特征。
基本流程都是先在水平方向上将深度图均分成许多子图像，然后计算每个点的曲率，然后在每个子图（submap）的每一行根据曲率的大小选择若干个角点（edge points）和若干个平面点（planar points）。

二者区别：
1）平滑度计算公式不同
2）点云类型：本文为地面点和分割点；LOAM为原始点云
3）特征点选择标准不同：本文为按照平滑度两次选取，LOAM为一次选取。

1、计算每个点$p_i$的平滑度公式：
令S作为range image同一行的连续点$p_i$的点集，S中有一半点位于$p_i$的两侧，本文中S=10.

2、为了所有方向均匀提取特征，将图像水平划分为几个相等的子图像。对于算出来的平滑度排序，根据阈值区分边缘点（$c>c_{th}$）和平面点（$c<c_{th}$）。所有的边缘点$F_{me}$和平面点集$F_{mp}$

• 从每一行中选取不属于地面点，且具有最大$c$值的边缘点，组成集合$F_{me}$;
• 从每一行中选取最小$c$值的平面点（可以标记为地面或边缘点），组成集合$F_{mp}$
  
  
  3、再进行一次筛选，得到精炼的边缘点$F_e$和平面点集$F_p$
• 从子图每一行提取最大$c$的边缘特征，他们不属于地面
• 从子图每一行提取最小$c$的平面特征，他们一定属于地面。
  
  
  
  上面对特征点和特征平面各进行了2次抽取。将360°范围图像分为6个子图像。1800*16.的图像划分为6个300 * 16的子图像，每个子图像设定：$F_{me}=40，F_{mp}=80,F_e=2，F_e=4$

三、Lidar Odometry

特征提取部分获得四种点云类型 ：$F_{me}=40，F_{mp}=80,F_e=2，F_e=4$

为了获得$t$时刻和$t-1$时刻姿态变换关系，选取$t$时刻精简的特征点$F_e^t$和$F_p^t$，以及$t-1$时刻丰富的特征点$F_{me}^{t-1}$和$F_{mp}^{t-1}$，构建约束关系为：

构建$（F_e^t，F_{me}^{t-1}）$，点到边缘线的对应关系。
构建$（F_p^t，F_{mp}^{t-1}）$，点到平面的对应关系。

改进以提高匹配的准确性和效率：
1、标签匹配
2、两步LM优化

1、标签匹配

Segmentation模块，将点云划分为边缘点和平面点两类，接下来要利用上这些标签信息。

• 对于边缘点，在$F_{me}^{t-1}$中具有边缘点标签的点云中寻找$F_e^t$关联点。
• 对于平面点，在$F_{mp}^{t-1}$中具有平面点标签的点云中寻找$F_p^t$关联点。

这样的操作可以保证相邻帧中地面信息保持不变，以及聚类后点云被分为若干块，提高匹配准确性，缩小了潜在对应特征的数量。

2、两步LM优化

和LOAM直接计算6DOF的一次LM优化不同的是，这里采用两次LM优化。

• 优化计算平面点 （$F_p^t，F_{mp}^{t-1}$）的对应约束，得到$[t_z,{\theta }_{roll},{\theta }_{pitch}]$

• 在上述三个变量约束条件下，优化计算边缘点 （$F_e^t，F_{me}^{t-1}$）的对应约束，得到$[t_x,t_y,{\theta }_{yaw}]$
  
  这样做的优点在于：

• 由于地面在相邻帧基本保持不变，所以用点到面的约束关系，可以计算出竖直维度的变动$[t_z,{\theta }_{roll},{\theta }_{pitch}]$。

• 当算出竖直维度的变动时，可以以此为初值输入到第二步的优化中，减少迭代次数，算出水平维度的变动$[t_x,t_y,{\theta }_{yaw}]$
  最终，两步LM优化过后，可以得到6-DOF的姿态变换（$t_z,{\theta }_{roll},{\theta }_{pitch}，t_x,t_y,{\theta }_{yaw}$），两步L-M优化得到相同的精度，计算时间可以减少约35%

四、Lidar Mapping

Lidar Mapping模块中，将特征点与周围点云图进行配准，以进一步优化姿势变换，但以较低频率运行。具体细节与LOAM相似。LeGO-LOAM的主要不同之处在于如何存储最终点云图。LeGO-LOAM按照标签存储每一个特征点（$F_{me}^t，F_{mp}^t$），而不是单一的点云图。
问题是图和把$t$时刻的点云（$F_{me}^t，F_{mp}^t$）匹配到全局地图点云$Q^{t-1}$，有两种方法：

• 基于传感器视域
• 基于图优化

1、基于传感器视域

第一种和Zhang Ji论文类似，选择在传感器视野里面的特征点集获得$Q^{t-1}$。选择距离当前传感器位置100m以内的特征集合。选择的特征集合然后变换和融合到单个周围地图。

2、基于图优化

1、图的节点：每个特征集合（$F_{me}^t，F_{mp}^t$）的传感器位姿。
2、雷达建图模型的位姿估计drift很低，假设在短时间内没有drift。通过选择一组相近的特征集合来构建全局地图特征点云$Q^{t-1}=（F_{me}^{t-k}，F_{mp}^{t-k}）,....,（F_{me}^{t-1}，F_{mp}^{t-1}）$
3、使用在L-M优化之后获得的变换来添加新节点（$F_{me}^t，F_{mp}^t$）与$Q^{t-1}$中已有节点加上空间约束。
4、用loop closure进一步消除雷达建图的drift。如果用ICP发现当前特征集和先前特征集之间有匹配，则添加新约束。

参考：
https://zhuanlan.zhihu.com/p/115986186
https://wykxwyc.github.io/2019/04/26/LeGO-LOAM-Paper-Traslation-and-Summary/
https://blog.csdn.net/orange_littlegirl/article/details/93851775