从零理解SLAM的LOAM系列算法

1. LOAM算法原理

LOAM 的全称是 Lidar Odometry and Mapping（激光雷达里程计与建图），是由Ji Zhang博士于2014年提出的一种开创性的、仅使用3D激光雷达进行实时里程计估计和地图构建的算法。

在LOAM出现之前，激光SLAM要么计算量巨大难以实时，要么精度不足，LOAM的核心贡献在于它创造性地将复杂的SLAM问题解耦为两个并行的优化线程：一个高频低精度的里程计线程和一个低频高精度的建图线程。

核心思想：解耦与分治 将6自由度（6-DoF）的位姿估计这一复杂问题，分解为两个更简单的问题：

• 运动估计的高频部分：快速但相对粗糙地估计传感器运动（里程计），高频率输出以保证实时性。
• 运动估计的低频部分：精细地校正运动估计和构建地图，低频率运行以保证精度。

这种设计巧妙地平衡了计算效率和估计精度。

关键流程 LOAM的工作流程主要分为四个部分：

1.1 点云特征提取

LOAM不会使用原始激光点云中的所有点（数据量太大，且大部分是冗余的），而是提取具有代表性的特征点，并将特征点分为两类：

• 边缘点：位于物体尖锐边缘上的点，即在曲率计算中，这些点在一个主方向上的曲率会很大，可以想象成一堵墙和地面的交界处。
• 平面点：位于平滑表面上的点，在曲率计算中，这些点的曲率很小，可以想象一堵墙或者地面的中间部分。

通过计算每个点与其相邻点集合的曲率（粗糙度）来筛选出曲率大的点作为边缘点，曲率小的点作为平面点。同时，会剔除不可靠的点（如被遮挡的边缘点或近似平行的平面点）。

1.2 特征点关联与里程计计算（高频线程）

目的： 在两个连续的激光扫描（比如S_k和S_{k+1}）之间，以高频（例如10Hz）计算激光雷达的运动变换（位姿T_{k+1}）。

• a. 对当前帧S_{k+1}提取特征点（边缘点和平面点）。
• b. 将这些特征点投影到上一帧S_k的结束时刻（使用上一次估计的运动进行去畸变），试图在上一帧S_k对应的点云地图中为它们找到对应的特征。
• c. 寻找关联： 当前边缘点 -> 在上一帧中找相邻的两个边缘点构成的线。 当前平面点 -> 在上一帧中找相邻的三个平面点构成的面。
• d. 构建优化问题：通过当前点到对应的“线”或“面”的距离，构建一个点到线和点到面的距离误差函数。
• e. 求解位姿变换：使用迭代优化算法（如LM算法） 最小化所有特征点的距离误差，最终求解出最优的位姿变换T_{k+1}。这个位姿是相对上一帧的，所以是里程计信息。

1.3 建图与位姿优化（低频线程）

**目的：**以较低频率（例如1Hz）将特征点注册到全局地图中，并利用更丰富的地图信息对里程计线程估计出的位姿进行精细化校正，消除累积误差。

• 特征累积：将过去多帧（例如最近几秒）的特征点累积成一个局部子地图（Sub-map），这个地图比单帧点云稠密得多。
• 全局投影：将当前帧的特征点投影到全局地图的坐标系下。
• 寻找关联：为当前帧的每个特征点在局部子地图中寻找对应的“线”或“面”（搜索范围更大，计算更耗时，但更精确）。
• 构建同样的距离误差函数：这次是优化当前帧相对于全局地图的位姿，这个优化过程能有效地校正里程计线程积累的漂移。

1.4 点云畸变处理

由于激光雷达在扫描过程中本身是运动的，一帧点云中的不同点是在不同位姿下采集的，这会导致点云“畸变”，LOAM通过在特征关联和优化过程中，线性插值相邻位姿变换的方式来对点云进行运动畸变校正，这是其实现高精度的重要一环。

2. 常见的LOAM落地方案

原始的LOAM算法是用C++实现的，但代码风格较为学术化，不易理解和移植。因此，社区中出现了许多基于LOAM思想改进、优化和重写的流行方案。

2.1 A-LOAM

A-LOAM (Advanced LOAM)这可以说是最著名、最广泛使用的LOAM学习版本，由香港大学Shen Shanjie博士开源，它并不是一个算法上的重大创新，而是一个代码层面的重大优化和重构。

• 代码清晰：使用Eigen库和Ceres Solver优化库重写了整个优化过程，代码结构清晰，注释详细，非常适合初学者学习和理解LOAM的原理。
• 去除依赖：移除了原始LOAM中复杂的里程计积分和ROS相关代码，更纯粹。
• 性能良好：保持了与原始LOAM相当甚至更好的性能。

适用场景：学术研究、教育学习、快速原型开发。几乎所有人学习LOAM都是从A-LOAM入手。

2.2 LeGO-LOAM

LeGO-LOAM(Lightweight and Ground-Optimized LOAM) 由Tixiao Shan博士（也是LIO-SAM的作者）提出，专门为地面车辆（如UGV）设计。

• 轻量化：首先对点云进行分割，滤除不可靠的点（如噪声和数量较少的点云簇）。
• 地面优化：显式地分离地面点云和非地面点云。
• 使用平面点特征主要用来优化roll, pitch和z（与地面约束强相关）。
• 使用边缘点特征主要用来优化x, y和yaw。
• 这种分步优化进一步降低了计算量，提高了在平坦环境中的鲁棒性和精度。
• 闭环检测：引入了ICP和Scan Context等方法进行闭环检测，有效消除累积误差。

适用场景：室外地面机器人，尤其是在有明显地面结构的场景中表现优异。

2.3 LIO-SAM

LIO-SAM (Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping)，同样是Tixiao Shan博士的工作，它将LOAM推向了新的高度。

• 紧耦合IMU融合：不再是简单的松耦合（例如用IMU做点云去畸变），而是将IMU预积分因子作为一个约束项，与激光里程计因子、GPS因子等一起，放入一个因子图（Factor Graph）优化框架中。
• 多传感器融合：可以灵活地融入GPS等绝对观测信息，以及在IMU失效（如剧烈运动）时依赖激光里程计。
• 滑动窗口优化：使用图优化而不是单纯的帧到地图匹配，能更好地利用历史信息，精度更高，鲁棒性更强。

适用场景：对精度和鲁棒性要求极高的应用，如无人机、自动驾驶汽车。需要高质量的IMU。

2.4 F-LOAM

F-LOAM (Fast LOAM)旨在进一步提升LOAM的计算效率。

• 无需特征提取：它不使用边缘点和平面点，而是直接使用所有点，但采用了一种高效的平面-面元（Plane-Patch）特征模型。
• 更快的最近邻搜索：使用高效的数据结构和匹配方法，减少了最耗时的KNN搜索过程。
• 精度与速度的平衡：在保持与LOAM相当精度的同时，显著降低了计算开销。

适用场景：计算资源受限的平台（如嵌入式设备、车载计算单元），需要更高的运行频率。

2.5 LOAM-Livox

这是专门为大疆的Livox品牌研制的非重复扫描式固态激光雷达（如Mid-40, Horizon）适配的版本。

• 特征提取适配：Livox雷达的扫描模式（花瓣状或玫瑰状）与传统旋转式雷达（线式扫描）完全不同，原始LOAM的特征提取方法会失效，LOAM-Livox改进了特征提取和选择策略，以适应其独特的FOV和点云分布。
• 运动畸变处理：由于Livox雷达的高扫描频率，其运动畸变模型也需要特殊处理。

适用场景：使用Livox系列雷达的所有项目。

3. 总结

选择建议：

学生或研究者理解原理：从 A-LOAM 开始读代码 机器人是地面车辆，且计算资源一般：优先尝试 LeGO-LOAM。 有高质量的IMU，并且追求极致精度：推荐LIO-SAM 计算平台性能较弱：可以尝试 F-LOAM。 使用的是Livox雷达：直接使用 LOAM-Livox 或其衍生版本。