上一篇聊了LOAM激光SLAM的开山之作。这篇来看它的改进版——LeGO-LOAM。LeGO-LOAM是2018年纽约城市大学CCNY的Tixiao Shan发表在IROS上的全称是Lightweight and Ground-Optimized LOAM。这篇论文引用量也很高是LOAM改进方案中最有影响力的一个。这个名字就透露了它的两个核心改进轻量化和地面优化。LOAM最大的问题是什么计算量大、对地面利用不够、没有回环检测。LOAM的特征提取虽然精简了但配准部分还是用全部特征点做6DoF优化计算量依然不小。而且LOAM没有利用地面信息对地面车辆来说浪费了很好的约束。LeGO-LOAM就是针对这些痛点来的。面试中经常让你对比LOAM和LeGO-LOAM你得说清楚改进了哪些地方为什么这么改。LeGO-LOAM的核心改进LeGO-LOAM在LOAM的基础上做了三个关键改进地面分割先把点云里的地面点分离出来单独处理。地面点提供了很好的roll、pitch和height约束这对地面车辆特别有用。两步位姿估计先用地面点优化2DoF位姿x, y, yaw再用所有特征点优化4DoF位姿加上z, roll, pitch。这个分步策略减少了优化维度速度更快。而且地面点对水平面约束很强x和y的估计更准。图优化后端加了回环检测和位姿图优化解决了LOAM没有回环检测的问题。用iSAM2做增量优化每次只更新受影响的部分效率高。# LeGO-LOAM的核心流程 class LeGO_LOAM: def process(self, cloud): # 地面分割 ground_pts, non_ground_pts ground_segmentation(cloud) # 特征提取 edge_pts extract_edge(non_ground_pts) plane_pts extract_plane(ground_pts) # 两步位姿估计 pose_2d optimize_2d(ground_pts) # x, y, yaw pose_3d optimize_3d(edge_pts, plane_pts) # 全部6DoF # 回环检测 位姿图优化 if detect_loop_closure(pose_3d): pose_graph.optimize() return pose_3d地面分割的巧妙之处LeGO-LOAM的地面分割用的是Bose等人的方法基于角度和高度阈值。具体做法把点云按照扫描线组织对于每条扫描线上的相邻两个点算它们和传感器连线的角度。如果这个角度小于阈值说明这两个点在同一个平面上大概率是地面。这个方法简单高效不需要复杂的计算实时性很好。地面分割的好处不只是速度快更重要的是它让后续的位姿估计更稳定。地面点提供了明确的垂直方向约束roll和pitch的估计更准了。对于地面车辆来说大部分运动是在水平面上的地面约束正好匹配这个先验。还有一个好处地面分割之后非地面点就只剩下障碍物和建筑边缘了这些点做特征提取更干净。LOAM的特征提取是在全部点云上做的地面点里也有曲率大的点会被误检为边缘点造成干扰。LeGO-LOAM先把地面点去掉特征提取的质量更高。# 地面分割核心逻辑 def ground_segmentation(cloud): for ring in cloud.rings: for i in range(len(ring) - 1): angle compute_angle(ring[i], ring[i1]) if angle threshold: ground_pts.add(ring[i], ring[i1]) else: obstacle_pts.add(ring[i]) return ground_pts, obstacle_pts两步位姿估计策略这是LeGO-LOAM最精华的部分。传统的LOAM一次性优化6个自由度计算量大。LeGO-LOAM拆成两步第一步只用地面点优化x、y、yaw三个自由度。因为地面点在一个平面上对z、roll、pitch的约束很弱所以这一步只优化水平面上的运动。第二步用所有特征点地面点非地面点优化z、roll、pitch三个自由度。x、y、yaw用第一步的结果作为初始值。这个分步策略的好处是每一步优化的维度少收敛更快而且不容易陷入局部最优。实验表明这个策略在精度和LOAM持平的情况下速度快了不少。为什么分两步能更快收敛因为6DoF优化的搜索空间大容易在多个方向上同时震荡。分成2DoF4DoF之后第一步的搜索空间只有3维x, y, yaw收敛很快。第二步虽然也是3维但x, y, yaw已经有了好的初始值所以z, roll, pitch的优化也很快。两步加起来比一步到位快得多。从数学角度看第一步优化的雅可比矩阵只有3列第二步也是3列比直接优化6列的雅可比矩阵计算量小很多。而且雅可比矩阵的条件数也更好数值稳定性更强。回环检测与位姿图优化LeGO-LOAM加了回环检测这是LOAM没有的。回环检测用的是Scan Context方法把每一帧点云编码成一个二维的描述子。当两帧的描述子相似度超过阈值时就认为检测到了回环。检测到回环后用iSAM2做位姿图优化。iSAM2是增量平滑与建图iSAM的第二代它的特点是增量式更新不需要每次都从头求解。每次加入新的因子约束iSAM2只更新受影响的部分效率很高。位姿图里的因子有三种里程计因子相邻帧之间的约束、回环因子检测到回环时的约束、GPS因子如果有的话。iSAM2把这些因子统一优化得到全局一致的位姿估计。面试追问环节面试官LeGO-LOAM和LOAM的主要区别是什么三个区别。一是加了地面分割地面点单独处理。二是位姿估计从一步变两步先2DoF再4DoF。三是加了回环检测和位姿图优化用iSAM2做增量优化。面试官LeGO-LOAM适合什么场景LeGO-LOAM特别适合地面车辆比如自动驾驶汽车、AGV、服务机器人。因为它的地面分割和两步优化都是针对地面运动设计的。如果是无人机或者手持设备地面假设不成立LeGO-LOAM的效果就会打折扣。面试官LeGO-LOAM有什么局限最大的局限是地面假设。如果场景里没有明确的地面比如楼梯、斜坡、崎岖地形地面分割就不好使了。另外LeGO-LOAM的回环检测用的是Scan Context对视角变化比较敏感有时候会漏检。还有LeGO-LOAM没有融合IMU数据纯靠激光雷达在快速运动或者退化场景下容易出问题。面试官Scan Context是怎么工作的Scan Context把一帧点云投影到一个极坐标的二维网格上横轴是角度纵轴是距离。每个格子里存的是这个区域内点的最大高度。这样每一帧点云就变成了一个二维矩阵两帧之间的相似度可以用矩阵的列向量来比较。这个描述子对旋转有一定的不变性计算速度也很快。面试官实际项目中用LeGO-LOAM有什么要注意的地面分割的参数要调不同场景下地面高度阈值和角度阈值不一样。还有LeGO-LOAM的位姿图优化用的是iSAM2这个库对内存管理要求比较高长时间运行要注意内存增长。另外LeGO-LOAM的建图模块和里程计模块是分开运行的两个模块之间的时间同步要做好不然会有延迟问题。LeGO-LOAM是LOAM的一个重要改进它证明了在特定场景下做针对性优化比通用方案更有效。地面分割和两步优化的思路被后来的很多工作借鉴了。Tixiao Shan后来又发表了LIO-SAM在LeGO-LOAM的基础上加了IMU预积分和因子图优化性能更强是目前最流行的激光SLAM方案之一。学LeGO-LOAM建议直接跑官方代码它的ROS包写得很好在KITTI和MulRAN数据集上都能跑。跑通之后对比LOAM的效果你会直观地看到回环检测带来的改善。上一篇第250篇 LOAM深度解析——激光SLAM的开山之作下一篇我们聊CartographerGoogle出品的激光SLAM方案架构思路和LOAM系列完全不同。如果这篇文章对你有帮助欢迎点赞支持一下你的鼓励是我持续更新的动力