Lego-Laom算法深度解析

参考链接

[1]基于广度优先遍历的点云聚类算法及代码分析
[2]Livox 开源分享：关于激光雷达去畸变的那些事儿
[3]LeGO-LOAM分析之特征提取

系统概述

作者的实验平台是一个移动小车（UGA），挂载了一个Velodyne VLP-16 线激光雷达，还配有一个低精度的 IMU；选用的硬件平台是 Nvidia Jetson TX2（ARM Cortex-A57 CPU）；整体负载是 20Kg；移动速度为：2.0m/s；测试场景为：地面不平（比较颠簸）的草地。

LeGO_LOAM的软件系统输入3D Lidar的点云，输出6DOF的位姿估计。整个软件系统分为5个部分：

• 第一部分：Segmentation： 这一部分的主要操作是分离出地面点云；同时对剩下的点云进行聚类，滤除数量较少的点云簇。
• 第二部分：Feature Extraction： 对分割后的点云（已经分离出地面点云）进行边缘点和面点特征提取，这一步和LOAM里面的操作一样。
• 第三部分：Lidar里程计： 在连续帧之间进行（边缘点和面点）特征匹配找到连续帧之间的位姿变换矩阵。
• 第四部分：Lidar Mapping： 对feature进一步处理，然后在全局的 point cloud map中进行配准。
• 第五部分：Transform Integration： Transform Integration融合了来自Lidar Odometry和Lidar Mapping的pose estimation进行输出最终的pose estimate。

1.点云分割模块

该模块的把点云投影为距离图像，分离出地面点与非地面点，然后过滤掉树叶、杂草等簇较小的点。主要步骤：

• 生成距离图像
• 地面点的初步筛选
• 基于BFS的点云分割
• 噪声过滤

1.1 生成距离图像

文章中采用Velodyne VLP-16激光雷达采集的数据进行实验，首先把原始点云重投影为一个距离图像，分辨率为1800*16（因为VLP-16水平分辨率为0.2°，360/0.2 = 1800，同时垂直方向上位16线数据）。重投影之后，三维点云变为二维图像，以像素点到传感器之间的距离作为像素值。

1.2 地面点的初步筛选

以VLP-16竖直维度的特性来进行标记地面点和非地面点，其在垂直方向的扫描范围为[-15°, 15°]，认为地面点出现在[-15°, -1°]之间的扫描线上，而被标记的地面点可以不用进行后续的分割。而[-1°， 15°]的激光雷达线束一般都指向天空，一定不包含地面点，这里先进行一个简单的筛选。

1.3 基于BFS的点云分割

1.目的 是去除剩余点云中室外环境噪声的影响，如树叶、地面的杂草等，保留原始的地面点和相对较大的静态物体（树干，楼房等）来进行后续的特征提取工作。

2.类别的判断依据：
如图所示，对于参与比较的两个点di和di+1，连接激光雷达和激光点得到两条射线OA和OB，通过计算图中 $\beta$ 是否大于一定阈值判断两个点是否属于同一类别。如果 $\beta$ 大于一定的阈值，说明两者之间没有突变，所以可以认为是同一个聚类，对其进行标记，然后加入到队列当中等待四邻居搜索。

$\beta$ 的值根据OA和OB的长度以及 $\alpha$ 进行计算，这些是已知的信息，对于 $\alpha$ 由于点云在水平方向和垂直方向的分辨率不同，所以对垂直方向上的邻居和水平方向的邻居选择不同的值。

3.BFS搜索过程：
（1）遍历距离图像的每一个像素i
（2）首先，刚开始遍历时队列中只有一个元素，即像素i

（3）然后，对像素i与其上下左右四个邻居进行类别判断，如果某个点被认为与像素i是同一类别，则将该像素点加入到队列，更新开始索引和结束索引

（4）重复上一过程，直到不再检测到新的同一类别的点，结束关于像素i的遍历
（5）对聚类得到的点数进行判断
点数小于30，再做次判断，因为垂直方向，角分辨率较大，如果物体垂直，那么出现的点则会比较少，判断垂直方向上出现的聚类次数，如过大于阈值（代码中阈值为3），认为聚类成功，将该组点作为一个类别进行标记，为其赋予标签，标签的类别如下：

（6）如果聚类失败，即点数小于30,垂直方向的点数小于3,将聚类失败的所有点打上标签999999
（5）对距离图像中的下一个像素点进行BFS聚类，如果遇到被标记的点跳过

4.噪声过滤
将距离图像分割为很多个聚类，同一个聚类的点被标记上唯一的标识。点数较少的聚类（少于30点）被作为噪声去除，这一步可以减少室外环境噪音点的干扰，如随风飘动的树叶，地面上的杂草等不稳定的特征，这样的处理就可以保留原始的地面点和相对较大的静态物体（树干，楼房等）来进行后续的特征提取工作了。如下图中 (a)为原始点云，(b)为进行分割处理后的点云。

2.特征检测

对分割后的点云（已经分离出地面点云）进行边缘点和面点特征提取，提取的特征的目的是进行点云配准，从而得出当前位姿。

2.1 特征提取

• 先对点云进行畸变校正（运动补偿）
• 接着计算点的平滑程度
• 然后按照平滑度排序
  • 如果是不平滑的点，则选为线特征（柱子或者墙壁的棱角）
  • 如果是平滑的点，则选为面特征（地面，墙面等平面）。
• 同时为了避免选择的特征过于集中在同一个地方，会把360°方向切分为6个区域，每个区域平均选择2个线特征和4个面特征。

2.1.1 点云去畸变

这里只对畸变产生的原因以及去畸变的方法进行一个大致的介绍，方便理解Lego-Loam算法，详细内容见参考链接。

1.畸变产生的原因
简单来说，由于激光雷达的帧率较低以及车辆自身的运动，导致对于同一帧激光点处于不同的坐标系，存在了一个运动偏差。

如下图，左图 p1~p3 表示激光雷达依次扫描到的三个位置点，这三点在真实世界中共线。但由于激光雷达自身在一帧时间内存在“剧烈”运动，如中间图所示，雷达自身分别在三个不同的实际姿态下对三个点进行了扫描。因此在最后得到的点云中（最右图），三个点坐标实际处于不同的坐标系，看起来不再共线了。

2.点云去畸变方法
简单来说，把一帧激光雷达数据的每个激光点对应的激光雷达坐标转换到不同时刻的机器人里程计上（近似对应的里程计的位置，达到尽可能去除畸变的目的）
（1）基于匀速运动模型的补偿
激光雷达的频率一般10HZ，即100ms转完一圈，但是平均分配到每个点云上可以认为两个相邻时刻的激光点时间差 $\delta t$ 很小，则可以假设在这个极小的时间里，雷达点云的变化是匀速的。这样可以基于该帧点云的起始时间和 $\delta t$，利用匀速模型对下一时刻的云点进行递推，进而实现运动补偿。经典算法如ICP。

（2）基于辅助传感器的运动补偿
核心思想是可以通过IMU或Odom，进行积分得到某帧数据从起始时刻 $t_k$ 到结束时刻 $t_{k+1}$ 之间的一段相对运动 $\delta T$, 然后根据每个云点到 $t_{k+1}$ 之间的时间间隔，算出每个云点到结束时刻 $t_{k+1}$ 的相对运动，最终把所有的云点都统一到结束时刻 $t_{k+1}$。

2.2 特征提取

Lego-Loam中主要通过计算每个云点的平滑度判断属于线特征还是面特征。

1.平滑度计算
取当前点的左边5个点和右边5个点和当前点的深度差值，然后求平方。
而论文中的计算方法和代码中的不一样，论文中是取深度差的平均值，然后除以自身的模，也就是说论文中的曲率和尺度无关，不管当前点离lidar近还是远，只要曲率超过一定的值就可以，同时在仿真环境也可以解决尺度的问题，而代码中直接取得是绝对大小。

论文中S为点集，$r_j$ 表示S中不等于i的点的深度，$r_i$ 则表示自身的深度。

2.标记遮挡点

图(a)中有2个特征点A和B，但B的方向(斜率)和激光雷达的光束几乎平行，因此移动的过程中很可能丢失，所以尽量不要选择B。
图(b)中有2个特征点A和B，但A后续可能会被B遮挡了，橙色线标识的部分。所以A也尽量要排除。

3.提取特征
一类是曲率比较大的线特征，一类是曲率比较小的面特征，这里的曲率即之前计算的平滑度。

提取的方法是把平面划分为6等分，也就是6个方向（LOAM中为前、后、左、右4个方向），根据上述计算好的曲率进行排序，然后每个方向最多选择2个线特征和4个面特征，如果超过则跳过，在下一个方向上继续选择。如果一个点已经被选择为特征点，那么它的相邻点会被标记，不允许被选为特征了。

最后总结一下，特征点的选择满足以下3个条件:

• 选择的边缘点或平面点的数量不能超过每个方向上的最大值，一共有6个方向，每个方向上最多2个线特征，4个面特征。
• 线特征和面特征周围相邻的点不能被选中。
• 不能是平行于激光雷达光束的点或者被遮挡的点。

特征提取输出：
（1）从非地面点中提取非平面点
将非平面点按照曲率从高到低进行排序，在6个方向上分别选择曲率最高的点

• cornerPointsSharp： 每个方向曲率最高的2个点，最多2x6=12个
• cornerPointsLessSharp： 每个方向曲率最高的20个点，最多20x6=120个

（2）从地面点中提取平面点
将平面点按照曲率从低到高进行排序，在6个方向上分别选择曲率最低的点

• surfPointsFlat： 每个方向曲率最低的4个点，最多4x6=24个
• surfPointsLessFlat： 每个方向曲率最低的40个点，最多40x6=240个
  

3雷达里程计

主要包括特征匹配、和位姿估计两部分。

3.1 特征匹配

特征匹配主要是根据上述提取出的面特征和线特征进行匹配，然后根据最小二乘法，得出前后2帧的坐标转换。

在Lego-Loam中，采用两部优化来估计相邻帧的位姿：

• 先对面特征进行匹配，估计和优化得到位姿中的$t_z$、${\theta }_{roll}$、${\theta }_{pitch}$部分
• 根据优化结果，再对线特征进行匹配，估计和优化得到位姿中的$t_x$、$t_y$、${\theta }_{yaw}$

之所以对位姿分开计算，是因为Lego-Loam做了地面分割，面特征全部都是地面这同一个平面的点，因此在两个面匹配后只能保证$t_z$、${\theta }_{roll}$、${\theta }_{pitch}$是准确的。基于面特征匹配估计出$t_z$、${\theta }_{roll}$、${\theta }_{pitch}$信息后将其固定，然后基于估计结果再对先特征匹配估计其余的位姿量，可以提高点云整体的估计结果。

在Lego-Loam中，基于标签信息为特征寻找匹配
之前已经对点云进行了分割，得到了地面点、以及非地面点的多个类别，在寻找匹配关系时只对属于同一类别的点进行匹配搜索，减少了匹配搜索的时间。

1.线特征匹配
根据点到直线的距离最短寻找匹配关系。存在k+1时刻点云中的一个点i,在第k帧通过KD-Tree搜索得到与点i最近的点j，以及和点j相邻扫描线中最近的点l

他们的几何关系如下:

点i到直线jl的距离De为：

通过最小化De可以得到最佳匹配。

2.面特征匹配
根据点到平面的距离最短寻找匹配关系。存在k+1时刻点云中的一个点i,在第k帧通过KD-Tree搜索得到与点i最近的点j，以及和点j同一描线中最近的点l、不同扫描线的最近点m,jlm构成不共线的平面

他们的几何关系如下：

点i到平面jlm的距离Dp为:

通过最小化Dh可以得到最佳匹配。

3.2 位姿估计

核心思想：求解一个R和t，使得上述两个距离公式中等式右边和等式左边的差值最小。

上述等式中的k+1时刻的坐标i和k时刻的坐标jlm等是在不同时刻下的坐标表示，构建位姿约束方程就是通过$X_{k+1}=R{X}_k+t$，将k+1时刻的位姿转换到k时刻。

有了关于位姿的残差放方程，剩下的就是SLAM中常规的优化求解问题了。

4.地图构建

4.1 全局地图

即SLAM最终的输出结果，在Lego-Loam中对每一帧的点云和位姿进行关联并单独保存，在代码中表现为一个集合。即在t时刻下，所创建的关于前t-1时刻的地图为：
$$M^{t-1}=\left\{\left\{F_e^1,F_p^1\right\},\left\{F_e^2,F_p^2\right\},...,\left\{F_e^{t-1},F_p^{t-1}\right\}\right\}$$

4.2 局部地图

局部地图主要为帧间运动估计的进一步优化提供服务，由当前帧附近的点云组成，点云的选择方式如下：

• 回环检测：则添加过去50个关键帧然后拼接成点云，相当于滑窗
• 位姿优化：则添加离当前欧式距离最近的50个关键帧，然后拼接成点云。

基于局部地图的匹配优化scan-2-map本质上和激光雷达里程计中scan-2-scan一样，只不过和当前帧的匹配对象由上一帧点云换成了局部地图点云

5.性能评价-对标LOAM算法

5.1特征点数量对比

LeGO-LOAM特征点整体下降幅度超过：29%，40%，68%，72%。

5.2迭代次数对比

里程计的迭代次数降低了34%，48%。

5.3 运行时间对比

降低了60%

5.4 位姿误差对比

Lego-LOAM可以用更少的计算时间实现可比或更好的位置估计精度。