上一篇聊了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[i+1]) if angle < threshold: ground_pts.add(ring[i], ring[i+1]) 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优化的搜索空间大，容易在多个方向上同时震荡。分成2DoF+4DoF之后，第一步的搜索空间只有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的开山之作
下一篇我们聊Cartographer，Google出品的激光SLAM方案，架构思路和LOAM系列完全不同。
如果这篇文章对你有帮助，欢迎点赞支持一下，你的鼓励是我持续更新的动力！
)
