Ref:
https://www.cnblogs.com/wellp/p/8877990.html
https://blog.csdn.net/weixin_41690997/article/details/105563830?depth_1-

介绍

LOAM[1]是Ji Zhang于2014年提出的使用激光雷达完成定位与三维建图的算法，即Lidar Odometry and Mapping。之后许多激光SLAM算法借鉴了LOAM中的一些思想（改进版的LOAM），可以说学习LOAM对学习3D激光SLAM很有帮助。
　　整个LOAM本质就是一个激光里程计，没有闭环检测，也就没有图优化框架在里面，该算法把SLAM问题分为两个算法同时运行，实现实时建图，一个odometry算法，10Hz；另一个mapping算法，1Hz。（LOAM核心思想是将定位和建图的分割，通过两个算法：一个是执行高频率的里程计但是低精度的运动估计（定位），另一个算法在比定位低一个数量级的频率执行匹配和配准点云信息（建图和校正里程计）。将这两个算法结合就获得了高精度、实时性的激光里程计。）两个算法都是从点云中提取尖锐的边（sharp edges）和平整表面（planar surface）特征点，然后进行特征匹配，来估计lidar的运动以及fine match with local map，匹配的过程核心思想还是基于距离的判断，LOAM里面的匹配（correspondence）并不是简单的feature point到feature point的，而是edge point 到 edge line以及planar point 到 planar patch，在后续运动估计中用到的dis也是点到线的距离以及点到面的距离，所以我感觉匹配的过程就像是一个基于feature的点到面（线）的ICP算法；当然odometry和mapping算法的edge line 和 planar patch的确定略有不同。
　　LOAM通过点云特征提取和点云匹配来解决低漂移和低计算的问题。我们知道匹配的问题分为scan-to-scan、scan-to-map、map-to-map。而三者各有特点，首先说scan-to-scan匹配：优点是计算量小速度快，缺点是误差累计大，长距离误差累计后地图就无法看了。map-to-map的匹配：优点是精度高，误差累计小；缺点就是计算量大，实时性压力大。scan-to-map的匹配居中。LOAM的优点就是很好地利用力匹配的优缺点，首先，虽然scan-to-scan匹配精度差，但是我们可以只是使用它做一个获取粗的里程计，用获取的结果用于去除匀速运动造成的运动畸变，由于scan-to-scan的计算量较小因此我们可以高频执行。其次，有了里程计校正后的点云数据，接下来我们就可以做一个map-to-map的匹配了。但是map-to-map存在计算量大的问题，因此 我们可以让其执行的频率降低。这样的高低频率结合就保证了计算量的同时又兼具了精度。
　　LOAM算法由四个节点构成。分别完成特征提取，高频低精度odom，低频高精度odom，双频odom融合的功能，每个节点以rosnode形式存在，也就是独立的进程
LOAM的整体思想就是将复杂的SLAM问题分为：1. 高频的运动估计； 2. 低频的环境建图。

1.1 节点简述

  • a. ScanRegistration： 　　
    　　根据VLP16的激光扫描模型, 对单帧点云（paper中称为一个Sweep）进行分线束（分为16束）, 每束称为一个Scan， 并记录每个点所属线束和每个点在此帧电云内的相对扫描时间（相对于本帧第一个点）。 针对单个Scan提取特征点,因为角分辨率越大, 代表越远的物体, 反射的两点距离越大, 中间丢失的信息越多。因此, LOAM没有针对Scan和Scan之间的点的关联性提取和描述特征, 而是直接针对单个Scan提取特征. 而相对时间会在laserOdometry中用于运动补偿.所有Scan的特征点,拼到两个点云中(因为是corner和surface两种特征点,所以是两个点云).至此,每帧点云,输出两帧对应的特征点云, 给下一个节点laserOdometry。

  • b. laserOdometry:
    　　实现运动补偿和帧间配准.每帧激光都会参与（所以帧率同VLP16的扫描帧率，10hz）.通过对每一帧激光的配准,可以得到一个精度较差的ODOM,帧与帧配准的初始POSE可以由IMU得到，或者在没有IMU的时候由匀速运动模型(简单的说就是假设这一帧运动量与上一帧一致)得到。
    本节点输出的ODOM频率为10hz.此ODOM的作用:
    （a） 在laserMapping中用于位姿的预测。
    （b） 在transformMaintenance中为laserMapping输出的低频ODOM提供插值,以获得高频(10HZ)的ODOM.
    在实际应用中,帧间匹配实现的ODOM,可以由IMU, 视觉里程计,底盘编码器等替代.而运动补偿在高速场景是不可或缺的

  • C. laserMapping:
    　　本节点使用实现一个较为完整的SLAM过程，主要工作：
    （a）通过多帧的激光特征点云基于POSE拼接，形成特征点云地图。由于包含conrner和surface两种，所以要建立两种地图。
    （b）将新入的帧与地图做配准，得到更精确的POSE，然后按照ａ进行建图。
    实际过程中由于单帧计算量大，所以采取每隔几帧配准一次（比如没５帧配一次，也就是２Hz）。如果一帧配一次，transformMaintenance也就没有存在的意义。
    这个地图可以作为离线地图。用作定位使用。
    当然这个地图功能还不完善，比如没有回环检测，这时候可以参考LeGO-LOAM，此文运用iSAM和ICP实现回环检测和全局优化建图。

2 算法

2.1 系统整体架构

LOAM主要包含两个模块，一个是Lidar Odometry，即使用激光雷达做里程计计算两次扫描之间的位姿变换；另一个是Lidar Mapping，利用多次扫描的结果构建地图，细化位姿轨迹。由于Mapping部分计算量较大，所以计算频率较低（1Hz），由Mapping校准细化Odometry过程中计算出来的轨迹。

2.2 Lidar Odometry算法

Lidar Odometry是通过Lidar的两次扫描匹配，计算这两次扫描之间Lidar的位姿变换，从而用作里程计Odometry。既然提到了两次扫描的匹配，自然而然想到了经典的ICP算法。然而LOAM并没有采用全部的激光点进行匹配，而是筛选出了两类特征点，分别是角点和平面点。
　　所谓角点，是当前激光扫描线束上曲率较大的点；而平面点，即曲率较小的点。在匹配时，首先提取当前扫描中的角点和平面点，对于角点，可以认为是物理世界中直线元素的采样，所以计算到上一次扫描中对应直线的距离；而对于平面点，认为是物理世界平面元素的采样，所以计算到上一次扫描中对应平面的距离。通过不断优化，使距离最小，从而得到最优的位姿变换参数。

2.2.1 角点到直线距离的计算

计算角点到对应直线距离时，需要确定对应直线的方程，才能够计算距离。作者认为，可以有如下假设：角点是雷达某个线束与物理世界两个平面夹角相交时的采样，那么这个线束相邻的雷达扫描线束也会与这个平面夹角相交，所以这个平面夹角的直线可以用前后两次扫描的角点进行标识。
图1：两种特征点提取示意

具体而言，当获得当前某一个角点时，首先根据假设的运动参数（上一次运动参数作为这一次优化的初始），计算这个角点在上一次扫描时的坐标，之后查找上一次扫描中最近邻的角点，并在相邻的雷达扫描线束中搜索最近的角点，从而得到了直线方程，便能够计算点到之间距离。

2.2.2 平面点到平面距离的计算

计算平面点到对应平面距离时，也需要首先拿到平面的方程，即平面上一点与平面的法向量。与角点对应直线搜索方式类似，首先找上一次扫描中最近邻的平面点，之后在同一个扫描线数和不同的线束上各提取一个平面点，这样共得到了3个不共线的平面点，唯一确定了平面，从而计算平面点到平面的距离。

2.2.3 一些其他细节

  1. 在角点和平面点的选择上，为了使分布更加均匀，通常将激光雷达一圈的扫描均匀分成几个部分，分别在每个部分中提取曲率最大（角点）和最小（平面点）的几个点。
  2. 在激光雷达的一次扫描过程中，由于运动，采集到的点云会有运动畸变，所以在LOAM中点云会通过预估的运动参数去畸变进行对齐
  3. 雷达扫描到的一些数据点是不稳定的，作者认为有两种不稳定数据点，如下图所示。一种是采集面与扫描面基本平行，另一种是被遮挡。在获得一次激光雷达扫描的点云后，这两种不稳定点会被首先去除。

图2：两种不稳定点示意

2.3 Lidar Mapping算法

Mapping部分的意义是，通过与多次扫描构成的地图进行匹配，得到Lidar Odometry中利用两帧之间计算位姿产生的漂移，从而对轨迹进行细化。如下图所示：

图3：扫描的轨迹和地图

图中，分别表示前k次扫描的轨迹（蓝色曲线）和地图（黑色直线），而最新的轨迹为。将k+1次雷达扫描到通过Odometry得到的映射到地图坐标系中，得到了未经校准的地图，可以看出存在一定的误差，并没有与原有地图对齐。此时通过一次Lidar Mapping，可以消掉这部分的误差，从而校准位姿参数。

　　如上图所示，在Mapping时，LOAM维护了一个以当前位置为中心，具有多个block的子地图，论文中指出这个子地图是10m的立方体。当Lidar采集到的点落入到这个子地图的某些block时，将从这个子地图提取出去对应的block，然后在这些block中像特征点提取那样，提取当前扫描中角点和平面点近邻的对应点，但有不同之处，并不像第2小节那样提取2个近邻角点确定直线或者3个平面点确定平面，而是提取更多的点进行拟合，因为子地图是多次扫描构成的结果。
　　对于当前扫描的一个角点，提取对应子地图中多个近邻角点，通过奇异值分解求出这些角点的主方向，从而的到直线方程，从而计算点到直线距离；对于平面点，通过寻找最小特征值对应的向量得到拟合平面的法向量，从而计算点到平面距离。再进行优化，得到对齐扫描与子地图的位姿纠正参数，之后将原有的轨迹进行纠正，便得到了细化的轨迹。
　　mapping算法的match跟odometry算法的match过程略有不同（主要体现在correspondence确定方式的不同），在 中提取特征点的方式跟之前一样，也就是根据c值判断一个点是edge or planar point，但是特征点的数量是odometry算法的10倍，然后在Qk中找特征点的correspondence，在odometry算法中，correspondence的确定是为了最快的计算速度（基于最近邻的思路找对应线以及对应面），而mapping算法是通过对特征点周围的点云簇进行PCA主成分分析（求点云簇的协方差矩阵的特征值和特征向量），来找到对应边和对应面。correspondence确定后的过程就跟之前一样了，实现将 配准到local map.

2.4 补充说明

LOAM一文中提出，可以通过IMU进行辅助。所谓“辅助”就是在激光雷达两次扫描之间，利用IMU得到较为准确的变化轨迹，从而进行点云畸变的去除，从而不需要靠上次扫描获得的运动参数插值对这次的畸变进行去除，能够提升精度。但IMU的数据并没有参与到优化当中，所以IMU在LOAM算法中只起到了辅助作用，而后续一些其他3D激光雷达算法，例如LINS[3]，Lio-mapping[4]和LIO-SAM[5]等，利用IMU进行了紧耦合的优化，取得了更好的效果。

3 代码

对简化版的开源代码A-LOAM进行简单介绍。
　　LOAM的作者曾经开源了LOAM的代码，但由于某些原因又取消了开源，现在网上也流传着一些当时的片段或者牛人的实现。而A-LOAM是LOAM的一个简化版本，去掉了IMU以及一些其他细节，采用了Eigen，ceres等替代了原有LOAM代码中的手动实现，非常适合学习LOAM思想，也适合新手入门3D激光SLAM。A-LOAM的链接是：https://github.com/HKUST-Aerial-Robotics/A-LOAM

3.1 代码概览

A-LOAM代码通过ROS运行，主要有3个节点，分别是：ascanResigtration, alaserOdometry和alaserMapping，对应特征点提取、里程计计算和mapping细化三部分。整体的结构如下：

图5 A-LOAM代码结构

  • ascanResigtration：处理Lidar输入的点云，提取并发布角点、平面点以及简单处理后的完整点云；
  • alaserOdometry：接收特征点，计算位姿参数（轨迹）并发布
  • alaserMapping：接收Lidar的点云与Odometry计算的轨迹，进行细化，得到精确的轨迹和地图并发布

3.2 代码学习资料

注意有些注释的是LOAM，有些是A-LOAM，基本上LOAM的注释涵盖了A-LOAM

  1. LOAM中文注释版
  2. LOAM笔记及A-LOAM源码阅读
  3. LOAM代码解析
  4. 试跑及程序注释
  5. 文章，代码解读

3.3 其他

  1. SLAM笔记

4 补充

Low-drift and real-time lidar odometry and mapping

即LOAM，实现了低漂移运动估计在6自由度和低计算复杂度。
使这种性能水平成为可能的关键思想是将寻求同时优化大量变量的典型复杂问题（如图1所示）划分为两个算法。一种算法在高频率但低保真度下执行里程计，以估计激光扫描仪在环境中移动的速度。虽然没有必要，但如果有一个IMU，它可以提供一个运动先验，并有助于解释总的，高频的运动。第二种算法以一个数量级的低频率运行，用于点云的精细匹配和配准。具体来说，这两种算法都提取出位于边缘和平面上的特征点，并将特征点分别与边缘线段和平面面片相匹配。在里程算法中，特征点的对应关系是通过保证计算速度来实现的，而在映射算法中则是通过保证精度来实现的。

该方法首先解决了一个比较容易实现的在线速度估计问题，然后通过批量优化映射，得到高精度的运动估计和地图。并行算法结构保证了实时求解问题的可行性。此外，由于运动估计是在更高的频率下进行的，所以映射有足够的时间来提高精度。当交错运行比里程算法慢一个数量级时，映射算法包含大量特征点并使用足够多的迭代来收敛。
论文贡献：

  • 提出了一个采用双层优化的软件系统，在线估计自我运动并建立地图；
  • 我们精心实现了几何特征的检测和匹配，以满足系统的要求：里程计算法中的特征匹配粗糙且快速，以保证高频率，并且在映射算法中是精确而缓慢的，以确保低漂移；
  • 我们使用涵盖各种环境类型的大量数据集对该方法进行了彻底测试；
  • 我们真诚地尝试将我们的工作呈现到更详细的层次，以使读者能够重新实现该方法。