LeGO-LOAM 这篇论文，我前后读了不下三遍，每次在自己做的机器人平台上遇到定位漂移、建图抖动这类问题，都会翻回来重新对照一遍它的设计思路。论文全称是Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain，核心就一句话：让激光里程计和建图算法在算力有限的嵌入式平台上跑得动，同时在起伏地形、退化场景里依然保持可靠的位姿估计。它把点云拆成地面点与非地面点两条线索分别处理，用两步 LM 优化替代一次全量求解，再用回环检测修正全局漂移。不管你是第一次接触激光 SLAM 的初学者，还是已经在工程里调过 LOAM 系方案的开发者，这篇论文都值得当成一份标准教材反复看。
下面我把自己的阅读笔记和实际体会整理出来，尽量把论文里写得隐晦的动机和细节挖出来，也补上一些我实跑过程中踩过的坑。
1. 为什么我需要反复对标“轻量化地面优化”：原版 LOAM 的痛点
LeGO-LOAM 并不是凭空出现的，它本质上是原版 LOAM 的一次针对性重构。要理解它做了什么，先得知道原版 LOAM 在真机上让人头疼的地方在哪里。
1.1 原版 LOAM 的基本流程与它的效率瓶颈
LOAM 的思路很清晰：把一帧激光点云里的特征分成角点和平面点，然后用这些特征去匹配局部地图，分别在 10Hz 的里程计频率和 1Hz 的建图频率下做两次配准，输出位姿。这套方案在当年的公开数据集上效果非常亮眼，尤其是长走廊、结构化环境下面，漂移控制得很稳。
但拿到真机上跑，问题就来了。最明显的是效率：LOAM 在特征提取阶段对每一个点都要计算平滑度，再从中挑出曲率最大和最小的那一批。点云规模一大，这个环节的 CPU 占用率立刻往上蹿。当时我用的是一台性能很普通的嵌入式主机，跑原版 LOAM 每一次扫描要花掉不少时间做全量特征筛选，留给建图和回环检测的余量就非常紧张。论文标题里强调的 lightweight，说白了就是要把这段“奢侈开销”压缩下来。
另一个问题更隐蔽：LOAM 对所有点一视同仁。它不知道当前帧里哪些点属于地面，哪些点属于墙面，哪些点属于一个正在走路的行人。于是真机场景里经常出现这样的现象——机器人停在原地，周围有行人穿行，LOAM 的角点匹配就会被动态物体的边缘点带偏，明明没动，里程计却漂出去十几厘米。
1.2 退化场景下的解法困境
原版 LOAM 还有一个被讨论得很多的问题：退化场景下的六自由度同时优化。所谓退化，就是环境提供的几何约束不够，比如一条非常长的笔直走廊，或者一片空旷的停车场。这种情况下，雷达能看到的有效约束集中在某个方向上，其他方向几乎没有信息，一次性把六个自由度全部丢进优化器，结果就是其中几个自由度“瞎猜”，然后漂移慢慢累积。
LeGO-LOAM 应对这些痛点的思路，可以概括成三个关键词：分割、地面、两步优化。它先用地面检测区分出可靠的地面点，再用点云分割避免动态物体干扰特征关联，最后把位姿优化拆成两步，让不同来源的特征各自负责擅长的自由度。这三个部件在原版里完全没有，却恰好补上了真机落地时最需要的几块拼图。
2. 点和云先分家：基于范围图像的点云分割与地面提取
LeGO-LOAM 的第一个关键模块，是把三维点云组织成一张二维的范围图，然后在这张图上做地面检测和点云分割。这一步做完之后，算法手里的点云不再是散乱的点集，而是带标签的结构化信息。
2.1 范围图像是怎么构建的
雷达点云的坐标是三维 XYZ，但 LeGO-LOAM 并不直接在这堆点里做处理，而是先把它们投影到一张图像上。简单说，将每一个点的水平方位角和垂直俯仰角作为索引，映射到图像的横纵坐标，像素值记录的是该点与传感器之间的距离。
这么做有两个好处。一是检索邻居点变得非常快，不需要建 KD-Tree 再去查最近邻，直接在图像上用相邻像素就能拿到空间相邻的点。二是地面检测和分割算法都可以复用成熟的图像处理思路，效率比纯 3D 操作高很多。
当时我自己实现过一个类似的投影模块，有一点经验想分享：千万不要忽略无效像素的处理。雷达某些角度可能没有回波，或者打到天空、车窗等反射面之后距离异常，这些值要么置零要么标记为无效。如果不处理，后续地面检测时它们会变成噪声源。
2.2 地面点提取的判定逻辑
范围图构建好之后，接下来要回答一个问题：哪些点是地面点？
LeGO-LOAM 采用的办法是基于相邻束之间的夹角判断。想象一束激光从雷达发射出去，打到地面的点连成一条线，同一条垂直扫描线上的相邻点之间，一般会形成一个几何关系。作者通过计算相邻点的深度差和高度差，得到两者连线与水平面的夹角；当这个夹角小于某个阈值时，就认为这一对点构成的局部区域属于地面。
这个阈值很关键。设得太小，陡坡和坑洼都被当成非地面，地面特征不足；设得太大，矮墙的顶部、路肩甚至草地都会被误判成地面，后面用于地面约束的特征里混入大量脏数据。我记得论文里一般推荐 10 度左右，具体取决于雷达的安装角度和线束密度。实跑中，如果你发现俯仰角一直在小范围内抖动，多半是地面阈值过紧导致约束不够；如果发现建图里地面像糊了一层雾，那就是阈值过松，非地面点混进来了。
2.3 点云分割与动态物体剔除
地面点分离之后，剩下的点云还需要进一步分割。这一步的目的是把不同的物体分开，比如行人、车辆、树干、墙面。LeGO-LOAM 在范围图像上做类似区域生长的操作：相邻像素如果距离差小于一个阈值，就归为同一个物体标签；距离差太大，则认为是两个不同物体。
为什么必须做这一步？最直接的原因是：动态物体上的特征点不可信。如果在走廊里有一扇门被打开，这种在上一帧不存在、这一帧突然出现的“角点”如果被拿来配准，就会把位姿拉偏。分割之后，算法可以计算每个被分割物体包含的点数，点数特别少的一般是远处的小物体或者稀疏杂点，大概率可信度低，直接丢弃。同时，运动中的物体会在多次扫描之间造成点云形状不一致，保留它们在分割结果中的标签，后续特征匹配时就能避开。
我自己的体会是，分割的效果直接决定了两步优化能不能稳定工作。分割太细，很多大物体会被拆散，特征匹配不稳定；分割太粗，两个紧挨着的物体被合并，动态干扰又进来了。论文里的参数是经验和实验结合的产物，实际落地时一定要针对自己的场景重新标定。
3. 特征提取的取舍逻辑：为什么先分地面/非地面再计算平滑度
很多人读 LeGO-LOAM 时，会觉得特征提取部分跟 LOAM 没什么区别，都是计算平滑度然后选出角点和平面点。但细看就会注意到，这里有一个非常刻意的顺序变化：先按地面/非地面把点云分成两个集合，再在各自集合里提取特征。
3.1 平滑度的本质是什么
所谓平滑度，其实就是描述一个点和它周围邻居之间的凹凸程度。对点云里的某个点，取它扫描线上前后若干个相邻点，计算这些邻居点到该点连线方向的差异，形成一个曲率特征。曲率大的点，大概率是物体的棱边、墙角、树干边缘，是角点候选；曲率小的点，大概率落在平面内部，比如墙面、地面，是平面点候选。
这里有一个原版 LOAM 里容易被忽略的细节：它只是从全局点云里按曲率排序，然后均匀取点。LeGO-LOAM 则要求先做地面点和非地面点的分离，再分别在这两个集合里做同样的排序。这意味着地面点只会成为平面点候选，而不是角点候选；墙面、树干等非地面点则可能成为角点候选或平面点候选。
3.2 这种取舍带来的直接收益
收益有两方面。第一，特征数量得到了控制，不会出现某一区域特征扎堆、其他区域空荡荡的情况。论文里通过“等分区域取特征”的方式，让角点和平面点在空间上均匀分布。对嵌入式平台来说，特征数量减少意味着匹配时的搜索开销显著下降，这比单纯优化代码更有效。
第二，特征的语义更一致。地面点和非地面点在物理上具有不同的约束能力：地面点能很好地约束高度、横滚、俯仰，但对水平方向的约束较弱；墙面和树干则能约束水平位置和偏航。分开提取之后，后面两步优化时每组特征各司其职，不会互相拖后腿。
我自己曾经试过不做地面分离、直接按 LOAM 方法提取特征，然后在 LeGO 的两步优化框架里跑。结果是第一步优化时，混入的少量墙角点让地面约束变得不稳定，俯仰角偶尔会跳一下。把地面点单独拎出来之后，这个问题立刻消失。这说明论文里的这个顺序不是随意的工程约定，而是整个算法能工作的原因之一。
3.3 特征提取时的工程细节
关于特征提取，论文里还有几个细节值得展开。
一是排序之后选点的数量。角点一般取曲率最大的若干点，平面点取曲率最小的若干点，但为了避免所有特征都集中在某个平面上，论文采用了分区域提取的思路。我当时在实现时做了一个小实验：如果直接从全局曲率排序里取前 N 个，特征会严重集中在墙角附近，墙面上几乎没有特征，这样的分布会让墙面约束形同虚设。按区域划分之后，整个环境的覆盖度明显改善。
二是要控制被分割物体的最大点数。论文里会丢弃那些“点数超过一定阈值”的物体，我理解这是为了排除地面、大墙面这类本身就应该被单独处理的结构，因为这些通常不是角点特征的可靠来源，且提取出的角点大量是重复约束。实际调参时，我一般把阈值设得跟一堵墙的典型点云数量相当，具体数值根据雷达线数来定。
4. “先稳平、再定航向”：两步 LM 优化如何拆解六自由度
前端里程计最核心的部分，是两步 LM 优化。这也是 LeGO-LOAM 区别于其他 LOAM 系方案最明显的设计。
4.1 为什么一次优化被拆成两次
位姿有六个自由度：位置 xyz 和姿态的横滚、俯仰、偏航。原版 LOAM 的做法是把它们放在一起，通过一次非线性优化同时求解。
LeGO-LOAM 的做法是拆开。第一步只用地面点，优先估计高度以及横滚、俯仰，顺带修正一部分水平位移；第二步固定住第一步得到的横滚、俯仰和高度，再用非地面点的角特征、面特征去补齐偏航和剩余水平位移。
这样拆的动机，我理解是两方面的。一方面是数值稳定性：地面点在空间中通常构成一个大平面，对大平面做约束时，高度和横滚、俯仰三个方向的信息非常强，而偏航方向基本没有信息。如果把偏航也强行放进去优化，矩阵中对应方向的约束接近零，数值上容易产生病态解。另一种常见的方法是正则化或在优化中加先验，而 LeGO-LOAM 的选择是干脆把这一步里没有信息的自由度隔离出去。
另一方面是鲁棒性：很多场景里非地面特征并不丰富，比如草地、雪地、水面边的堤岸。这时角点的数量少、质量参差，单独做六自由度优化很容易犯迷糊。但地面点往往是充足的，先锁住横滚、俯仰和高度，即使后面水平定位有些误差，姿态也不会发散。
4.2 两步之间的数据关联如何组织
我在复现这套逻辑时，有个体会很深：两步优化不是简单地重复两次匹配，而是数据关联的组织方式完全不同。
第一步里，地面点的匹配对象是局部地图中的地面平面。LeGO-LOAM 从上一帧或局部地图中提取地面平面模型，把当前帧地面点与该平面之间的距离作为残差。这样做的好处是平面的拟合比逐点匹配更抗噪。第二步里，非地面特征则要靠 KD-Tree 或者直接去局部地图里找最近邻，计算点到线的距离或点到面的距离，作为残差。
这两步对应的残差量纲不同，权重也不同。如果放在同一个优化式里，地面平面距离和点线距离的量级差异会带来权重平衡问题，做起来非常麻烦。拆开之后，各算各的，逻辑干净，调试也方便。
4.3 退化场景中的几类表现
论文的重点实验场景是起伏地形，也就是坡道、山路这类地面高度和朝向都在变化的场景。这类场景对纯 LOAM 而言比较棘手，因为地面并不完全平坦，用全量点匹配时地面点分布不均匀会直接导致高度估计抖动。
LeGO-LOAM 的两步策略在坡道上的表现要稳很多：第一步用地面点快速锁定当前位姿与地面的关系，相当于给优化加了一个强有力的锚点；第二步再慢慢修偏航和水平方向。即使坡道的角度变化很快，只要地面点提取没有断，姿态的收敛都很稳。
当然它也有退化场景。我在一条地下车库的直道上试过，两边是平整的墙壁，地面也平坦，理论上约束很充足。但当车靠近墙壁的一侧时，两步优化里第一步的横滚约束会被墙面的平面点干扰，因为墙面在范围图像中也会被判断成“接近地面角度的连续面”。这时需要靠分割算法把墙面有效地区分出去，否则第一步会出现偏差。
5. 建图与回环检测：LeGO-LOAM 的后端算得上一套完整 SLAM 吗
论文的后半部分把前端里程计输出的位姿交给建图模块，同时维护一个因子图，并在合适的时候检测回环。这部分经常被初学者忽略，但它决定了系统能不能长时间运行。
5.1 建图频率降下来之后为什么精度反而更稳
建图模块的工作频率比里程计低。它拿到的是已经经过两步优化后的位姿，然后把当前帧特征与全局地图做一次更精细的配准，更新地图和位姿。如果只看频率，会觉得低频建图不如高频建图实时性强，但实际效果反而是里程计的累积漂移被持续修正。
原因在于建图匹配使用的是全局地图，而不是上一帧局部信息。全局地图的特征更丰富，约束更强，配准精度比点对点的帧间匹配高很多。它算出来的位姿虽然不如里程计那么“连续”，但更准确；用这个准确位姿去更新局部里程计的基准，相当于每一帧都在被较准。
实际工程中，这部分的频率设置很讲究。太高的频率会让嵌入式平台 CPU 吃紧，地图更新频繁还会引入大量计算量；太低则修正不够及时。我一般在里程计 10Hz 的情况下，把建图频率压到 1Hz 到 2Hz，平台的温度、功耗和内存占用都会改善很多。
5.2 回环检测与全局一致性的修正
LeGO-LOAM 在回环检测上的做法并不复杂，但足够有效。它会在当前帧位置附近搜历史关键帧，对候选帧做一次匹配。如果匹配得分超过某个阈值，就把这个闭环约束塞进因子图里，然后启动一次全局优化。
这意味着什么？在没有回环的时候，系统就是一个里程计加建图系统，漂移会随着时间累积。但一旦机器人回到之前到过的地方，回环检测就能识别出来，全局优化会把之前累积的位姿误差拉回去。这让地图在大范围场景下依然能保持一个相对一致的全局结构。
从工程视角看，很多人会把这段逻辑关掉，因为回环检测对计算资源的占用不低，而且误匹配一旦发生，后果比没有回环更严重。我的建议是，先想清楚自己的应用场景是否真的会回环。如果不会，直接把回环检测这部分停掉，把省下来的算力留给前端，效果往往更好。
5.3 我如何看待论文里的“定位与建图分工”
论文把模块分成四个部分：分割、特征提取、两步 LM 的里程计、以及建图加点图优化的后端。这个分工的最大启示是：定位和建图并不是同一个优化目标。
定位要求快，建图要求准；定位可以牺牲一部分长期精度换取实时性，建图则要尽可能利用所有信息做到全局一致。很多新手把这两件事混在一起，在一个图里同时办，结果实时性和精度两头都不讨好。LeGO-LOAM 用两个频率、两个模块把这个问题理清了，这也是它值得学习的地方。
6. 真机实测与调参记录：把 LeGO-LOAM 跑稳需要避开的地方
这一部分不再是论文阅读笔记，而是我在实际环境里跑 LeGO-LOAM 之后总结的调参和避坑经验，希望能让你少走一些弯路。
6.1 硬件匹配与算力分配
LeGO-LOAM 对雷达的要求是多线激光雷达，16 线也能跑，但地面点的密度会明显偏低。我当时用 16 线雷达时，地面点提取出来的数量少，横滚和俯仰约束偶尔会出现轻微抖动。后来换用 32 线之后，这个问题几乎消失。如果条件允许，还是尽量选线束多的雷达，哪怕牺牲一些探测距离。
算力方面，“轻量化”是相对原版 LOAM 而言的，并不意味着可以跑在非常低端的单片机上。我通常会在类似嵌入式主机这类平台上运行，关闭可视化，再把建图频率降下来，CPU 占用能控制在比较健康的水平。如果你把 rviz 开着跑，会发现 CPU 占用立刻翻倍还多，那不是算法的锅，是可视化的锅。
6.2 地面角度阈值与分割点数量阈值
论文里有两个参数值得反复实验：地面判定的角度阈值和分割物体的最大点数。
角度阈值我前面提过，一般取 10 度上下。但要注意，这个值还跟雷达安装的俯仰角有关。如果雷达前倾了，整个点云的几何关系都会变，需要重新标定。我当时因为固定支架的问题，雷达实际前倾了两三度，地面点提取结果一直很怪，折腾了很久才排查到机械原因。
分割最大点数这个参数，我观察到的现象是：设小了会把大物体的点云切碎，比如一面墙被当成好几个小物体，特征不稳定；设大了会让一些本应被剔出的大物体保留下来，增加计算量。我通常会取一个经验值，让一堵正常的墙面能被完整保留，而远处的车辆能被单独分割出来。这个参数很大程度上依赖场景，换了环境就要重新调。
6.3 动态场景里的表现
LeGO-LOAM 用了分割来剔除动态物体，但我在人群密集的地方测试过，效果并不完美。原因是分割本身依赖相邻点的距离差，而快速移动的人会在点云上拉出一条“拖影”，这些拖影里的点跟背景的深度差大，分割还算容易；但如果人贴着墙走，拖影点与墙面在某些角度下深度差很小，会被合并进墙面标签，动态干扰就漏进去了。
解决思路有两个方向。一个是用高频点云运动补偿，把同一扫描周期内由于传感器运动造成的点云畸变校掉，这样拖影会轻一些；另一个是加 IMU 信息，提高对运动物体的判断能力。这些都属于论文之外的经验补充，但确实能让整体稳定性上一个台阶。
6.4 关于增长方向的一点判断：与 IMU 融合是否必要
最后说说我对“LeGO-LOAM 需不需要 IMU”的看法。论文本身是纯激光方案，不依赖 IMU，这对很多人来说是一个优势，不用做传感器时间同步那一堆麻烦事。但在剧烈运动场景，比如无人机快速转弯、地面机器人急加速急减速，纯激光会面临点云畸变大、特征匹配失效的问题。
后来的很多激光惯性里程计方案，本质上就是把 IMU 预积分塞进优化里，补上空缺约束。如果你要做这类升级，我的建议是不要直接推翻 LeGO-LOAM 的两步结构，改成：第一步用 IMU 的横滚俯仰先校正点云，第二步再用地面点的约束修正 IMU 漂移。这样既保留了地面优化的优势，又解决了高速运动下的失真问题。我自己在后续项目里试过这个思路，效果比单纯用 IMU 或者单纯用激光都稳。
写在后面的一次小迭代
如果你的应用场景里地面点比例不高，比如经常在楼宇间的狭窄通道、立体的工业管廊里跑，那么“地面优化”带来的收益会明显变弱。这时候更应该关注的是分割模块本身的价值，以及第二步优化里的非地面特征。我试过在管廊场景里把第一步的地面约束权重调低，让第二步主导位姿修正，系统依然能工作，只是“轻量化”的红利少了一些。
LeGO-LOAM 给我最大的启发，并不是某一个模块有多惊艳，而是它把“资源受限”和“环境多变”这两个现实约束前置到算法设计里。读论文很容易被公式吸引，但真正落地时，让算法拥有可解释的失败模式和清晰的资源分配策略，往往比多提几个百分点的精度更重要。
