作者 | yachen zhang 编辑 | 3D视觉之心
原文链接：https://zhuanlan.zhihu.com/p/115986186
点击下方卡片，关注“自动驾驶之心”公众号
戳我-> 领取自动驾驶近15个方向学习路线
本文只做学术分享，如有侵权，联系删文
LeGO-LOAM是以LOAM为框架而衍生出来的新的框架。其与LOAM相比，更改了特征点的提取形式，添加了后端优化，因此，构建出来的地图就更加的完善。
本文将先针对LeGO-LOAM进行原理的深度解析，然后再对比LOAM和LeGO-LOAM的区别和联系。如果有同学对LOAM框架不是很熟悉的话，希望可以先阅读这篇文章：
yc zhang：LOAM-SLAM原理深度解析872 赞同 · 101 评论文章
论文：https://ieeexplore.ieee.org/abstract/document/8594299
代码：LeGO-LOAM-BOR 和LeGO-LOAM NOTED
文章较长，创作不易，希望大家可以点赞关注收藏支持一波哈~
1、论文概括
LeGO-LOAM是发表于IROS 2018的文章，全称为：LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain .
从文章名称，可以看出，LeGO-LOAM相对于LOAM的提升主要在于轻量级和地面优化。接下来，我将从论文的整体框架入手，详解介绍LeGO-LOAM和LOAM的区别和联系。
本文的核心在于四个部分：分割，特征提取，雷达里程计，雷达建图。
分割模块通过对一帧的点云重投影到图像中，进行地面分割，非地面点被分割出来；
特征提取模块基于分割后的点使用和LOAM一样的方法提取边缘点和平面点；
雷达里程计模块基于提取的特征点构建 scan-to-scan 约束关系，使用两次LM优化，得到姿态变换矩阵；
雷达建图模块将得到的特征点进一步处理，构建 scan-to-map 的约束关系，构建全局地图。
下面将针对这四个部分进行详细的阐述。
2、Segmentation
首先设定在 时刻获取的点云 。 是 时刻的点云 中的一个点。将该点云投影到一个距离图像中，其分辨率为 。
1800 ：由于VLP-16的水平方向的角度分辨率为 ，所以，投影图像的水平方向分辨率为 。
16 ：由于VLP-16的竖直方向是16根扫描线，所以，投影图像的竖直方向的分辨率就变成了 16 。
在将点云投影到图像上后，每个三维空间的点就变为二维空间的像素点了，我们获取点 对应的像素点到传感器的欧几里得距离 。
再进行分割之前，可以对距离图像进行列式评估。进行地面点的提取。
由于距离的竖直方向为 16 ，这个也代表了原始三维空间中的竖直维度的特性。所以，通过判断其竖直维度的特性，可以很好的标记出地面点和非地面点。
通过VLP-16的激光扫描束的范围为 ，地面点必然出现在 扫描线上。
在这一个过程中，被标记的地面点可以不用进行后续的分割。
然后，使用基于图像的分割方法将距离图像分割为很多个聚类。同一个聚类的点被标记上唯一的标识。
这一步，可以将一些微小物体点当作噪音点去除，减少相邻帧之间微小物体不重复出现的造成的干扰：
随风飘动的树叶，草等，在实际的实验中会经常遇，可能会造成出现在前一帧而不出现在后一帧的情况。
去除不能聚成类的点云，可以在保留当前帧特征信息的基础上，减少噪音点的干扰，提升特征提取精度。
为了提高处理效率，本文将低于30个数据点的类别都当作噪点处理，这样保存下来的就是一些相对来说比较静态的物体了（例如，树干，楼房等）。这样的话，整个距离图像就可以被分成若干个比较大的类别。加上前面设定的一个类别——地面点，这样就可以进行后续的处理了。
基于这一步的处理，一帧点云中的点可以被小区很多噪音点，结果如下所示：
从图中可以看到，大部分的散乱点都被干掉了，这样需要处理的数据就变得很干净了。
基于这一步之后，每个点可以得到以下三个信息：
分割标签（地面点或分割点）
在距离图像中的行和列的索引
和传感器的距离值
我们这些分割处理后剩余的点都传入到特征提取模块，进行后续的特征提取处理。
这一步的处理在代码中体现如下：
投影图像的处理为：
点云投影到图像的函数
行，列和距离值的处理：
聚类的处理如下：
通过对于竖直角度的计算和阈值的判定，来决定到底是属不属于地面点，这样就可以分离地面点和非地面点了。
对于分离的非地面点使用BFS进行聚类处理：：
设定判定条件，保留相应的聚类：
3、Feature Extraction
在获取了地面点和分割点后，需要进行后续的特征提取操作，这里的操作基本和LOAM的操作一致，如果有同学对LOAM的特征提取不太了解，建议先看完LOAM的解析：
yc zhang：LOAM-SLAM原理深度解析872 赞同 · 101 评论文章
在本文中，为了能够从所有方向( 360∘ )提取特征点，将距离图像水平均分为若干个子图像（sub-image）。对于每一个子图像，都进行下面的处理。
选取 时刻的点云 中的一个点 ，在点 同一竖直方向上，左右各找5个点，构建一个集合 。
算每个点的平滑度：
这样就可以算出每个点在集合 中的平滑度了。
对于算出来的平滑度排序，进行特征点的选取。
设定平滑度阈值 ：
: 边缘点
：平面点
根据上面的评判标准，划分出了边缘点和平面点。
从每一行中选取不属于地面点，且具有最大 值的 个边缘点，组成集合 .
从每一行中选取最小 值的 个平面点，属于地面点或分割点都行，组成集合 .
这一步选取了大部分的数据点，为了方便计算，我们再进行一次筛选：
从集合 中选取不属于地面点，且具有最大 值的 个边缘点，组成集合 .
从集合 中选取属于地面点，且具有最小 值的 个平面点，组成集合 .
这一步操作后，就得到如下关系：
他们之间的关系如图所示：
在LeGO LOAM的代码中，将 的距离图像划分为了6个 的子图像。
在每个子图像内设定： 。
这样，就可以得到一帧点云中的四个特征点集合： 。
纵观整个过程，LeGO-LOAM的特征提取和LOAM的特征提取区别在于：
使用的点云类型不同
计算公式不同
平面点和边缘点的选择标准不同
这里虽然特征点集合看上去比LOAM多，但是其实和LOAM的是一样的，不过LOAM在论文中没有点明的。
下面依次进行对比：
具体实现，平滑度计算：
其他的在代码逻辑上基本和LOAM一致，这里不再赘述。
4、Lidar Odometry
在前面特征提取部分，获取了四种点云类型：。设定当前时刻为 ，则上一时刻为 。
为了更好的获取 时刻和 时刻的姿态变换关系，我们选取 时刻的特征点 和 ，以及 时刻的 和 。
由于 ，所以，肯定可以从 和 中找到 和 的关联点。
剩下的步骤就和LOAM里面构建约束关系的方式相同了。
构建 的点到线的对应关系
构建 的点到面的对应关系
但是，LeGO-LOAM还做了些许的改进来提高匹配的准确性和效率，主要在以下两个方面：
标签匹配
两步LM优化
4.1 标签匹配
在第二部分分割模块，我们将点云划分为地面点和分割点，使用这些标签来进行对应关联点的搜寻。
对于边缘点：在 中具有分割点标签的点云中寻找 的对应关联点
对于平面点：在 中具有地面点标签的点云中寻找 的对应关联点
这一步的操作其实非常巧妙，有以下两个优点：
在相邻帧中地面信息基本保持不变
聚类后点云被分为若干块，缩小了对应点的候选范围
不过，这一步的处理也不是没有问题的，在实际的实验中，如果地面不是特别的平坦，也就是说相邻帧间的地面有了一定的变动，这个时候，LeGO-LOAM就不能很好的运行。
再获取了点到线和点到面的对应关系后，需要使用列文伯格-马夸特法（LM）优化方法来进行优化求解。
如果对这一块不是很熟悉的话，可以阅读这篇文章：
yc zhang：最小二乘问题的四种解法——牛顿法，梯度下降法，高斯牛顿法和列文伯格-马夸特法的区别和联系690 赞同 · 44 评论文章
4.2 两步LM优化
在这一步，和LOAM相比，将针对6-DOF( )的一次LM优化，更改为了两次LM优化方法：
优化计算 的对应点约束，得到
优化计算 的对应点约束，基于第一步得到的 ，得到
两步计算有着一定的合理性：
由于地面在相邻帧间基本保持不变，所以，使用点到面的约束关系，可以计算出竖直维度的变动 。
当算出竖直维度的变动时，可以以此为初值输入到第二步的优化中，减少迭代次数，算出水平维度的变动 ，提升计算效率。
最终，基于两步LM优化，得到6-DOF的姿态变换矩阵 。
具体的实现如下所示：
两次优化:
LM优化：
这里需要注意的是LeGO-LOAM沿袭了LOAM_Velodyne的opencv的写法，并没有使用ceres solver库求解。
实际是求解高斯牛顿法：
关于高斯牛顿法和LM法的区别和联系，我之前的文章有详细描述过，有兴趣的同学可以去看一下。
5、Lidar Mapping
前面雷达里程计模块获取了 scan-to-scan 的对应关系后，下面要考虑的问题是，如何将 时刻的 匹配到中 。其中是对应的全局地图特征点云。
我们设定 时刻的全局地图为： 。
就是从 中找到的和 相关联的点云。
为了获取这个对应的全局地图特征点云 ，有两种方法：
基于传感器视域
基于图优化
下面将详细介绍这两种方法：
5.1 基于传感器视域
根据传感器的有效探测距离，选取一个cube，将里面的点云作为对应的全局地图特征点云。
VLP-16的有效探测距离是100m，所以，本文就选用100m，来获取 。
此方法和LOAM的方法基本一致。
5.2 基于图优化
首先获取每个节点的观测数据： 。
由于Lidar Mapping的位置估计误差较低，所以，可以假设在一个较短的周期内，几乎没有误差。因此，可以选择一组相近的特征集合来构建对应的全局地图特征点云 。其中， 表示的是 的大小。
通过LM优化方法，构建 和 之间的姿态约束
使用回环检测的方法，利用gtsam优化得到最终的全局地图。
这一部分和LOAM的最大区别在于加入了位姿图和回环检测，解决了LOAM没有后端优化的弊端，提升了建图的效率。
这里主要说明下回环检测的使用情况：
设定回环检测线程：
进行回环检测操作：
检测回环：
回环检测中匹配用的是icp：
使用gtsam优化位姿关系：
6、总结
我们已经知道了LeGO-LOAM的算法流程，这里将总结一下LOAM和LeGO-LOAM的区别和联系：
通过框架图的对比，可以看到，LeGO-LOAM多了分割模块，其他几个模块基本一致，就是频率发生了变化。
各模块的对比如下：
上面的图详细的整理了LeGO-LOAM和LOAM的区别与联系，再实际的使用中，大家可以根据具体情况，来选择到底使用哪个算法。
关于LOAM和LeGO-LOAM算法的对比已经整理完毕了，虽然LOAM系还有一个loam_livox算法，不过由于我不咋玩dji的雷达，所以，这一个就随缘更新吧。
后续，我将整理HDL算法框架，整理不易，希望路过的同学们可以点赞支持一下哈~
投稿作者为『自动驾驶之心知识星球』特邀嘉宾，欢迎加入交流！
① 全网独家视频课程
BEV感知、毫米波雷达视觉融合、多传感器标定、多传感器融合、多模态3D目标检测、车道线检测、轨迹预测、在线高精地图、世界模型、点云3D目标检测、目标跟踪、Occupancy、cuda与TensorRT模型部署、大模型与自动驾驶、Nerf、语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频（扫码即可学习）
② 国内首个自动驾驶学习社区
国内最大最专业，近2700人的交流社区，已得到大多数自动驾驶公司的认可！涉及30+自动驾驶技术栈学习路线，从0到一带你入门自动驾驶感知（2D/3D检测、语义分割、车道线、BEV感知、Occupancy、多传感器融合、多传感器标定、目标跟踪）、自动驾驶定位建图（SLAM、高精地图、局部在线地图）、自动驾驶规划控制/轨迹预测等领域技术方案、大模型、端到端等，更有行业动态和岗位发布！欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频
③【自动驾驶之心】技术交流群
自动驾驶之心是首个自动驾驶开发者社区，聚焦2D/3D目标检测、语义分割、车道线检测、目标跟踪、BEV感知、多模态感知、Occupancy、多传感器融合、transformer、大模型、在线地图、点云处理、端到端自动驾驶、SLAM与高精地图、深度估计、轨迹预测、NeRF、Gaussian Splatting、规划控制、模型部署落地、cuda加速、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向。扫码添加汽车人助理微信邀请入群，备注：学校/公司+方向+昵称（快速入群方式）
④【自动驾驶之心】平台矩阵，欢迎联系我们！
扫一扫
792
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
