激光slam典型方案总结

未完待续

为什么选择激光雷达？

激光雷达相⽐图像有着对光照、纹理不敏感的优点，激光雷达地图相⽐通⽤的视觉特征点加描述⼦地图有着更好的稳定性，激光雷达⽅案⽐视觉⽅案鲁棒性更优。

激光slam(loam)总述

• LOAM 张绩

LOAM算法是一种的激光匹配slam方法，新的特征提取方式（边缘点和平面点），运动补偿（时间戳），缺点是没有回环检测，后端没有因子图优化，大环境建图会产生漂移，不能处理大规模的旋转变换。

卡内基梅隆大学张绩原版：https://github.com/laboshinl/loam_velodyne

注释版：https://github.com/cuitaixiang/LOAM_NOTED

• A-LOAM 秦通

A-LOAM是港科大秦通对张绩的LOAM框架进行强化的一个激光SLAM框架。这个框架使⽤Eigen以及CeresSolver对原始LOAM进⾏重构，在保证算法原理不变的前提下，对代码框架进⾏优化，使得代码变得⼗分简洁，更加容易被读懂。舍去了IMU接口，同时A-LOAM使用了Eigen、Ceres库完成了LM优化和雅克比矩阵的正逆解，替代了原有LOAM代码中的手动实现。

港科大原版：https://github.com/HKUST-Aerial-Robotics/A-LOAM

注释版：https://github.com/cgbcgb/A-LOAM-NOTED

• LeGO-LOAM TixiaoShan

Le表示轻量级（Lightweight），GO表示基于地面优化（Ground-Optimized）；Lego-LOAM前端增加地⾯点提取；后端增加回环检测和位姿图优化；前后两端的LM优化针对处理运算量做了优化，它的运算速度增加；相较于LOAM并没有牺牲精度；对设备性能要求低，轻量级；在地面点丰富时比较稳定，在地面点缺乏时很容易崩溃；得到的地图比较稀疏。

原版：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM

注释版：https://github.com/wykxwyc/LeGO-LOAM_NOTED

改进版（更好的工程化）：https://github.com/facontidavide/LeGO-LOAM-BOR

• FLOAM

FLOAM是基于LOAM和ALOAM的修改版，计算时间缩小3倍，精度也有一定提高。激光近距离受到到遮挡时的效果比LeGo效果好。

源码：https://github.com/wh200720041/floam

• LIO-SAM TixiaoShan

LIO-SAM与IMU紧耦合作为前端里程计且加入GNSS适配作为全局图优化；号称运行速度快10倍；对传感器要求较高，100hz+的9轴IMU以及带有ring和time通道的激光雷达（velodyne和ouster）；基于因子图构建的激光雷达惯性里程计，可以将大量的相对测量值、绝对测量值、回环等多种不同数据作为因子融入激光雷达惯性里程计系统中；IMU预积分的运动估计被用来去处激光雷达运动畸变，并为激光雷达惯性里程计的优化提供初值；获得的激光惯性里程计的结果反过来用作估计IMU的偏差；为了确保实时性与高性能，进行位姿优化时边缘化掉了一些旧的激光雷达数据，而不是将激光雷达点云与整个地图进行匹配，在局部范围而不是全局范围进行扫描匹配可以有效提高系统的实时性；选择性地引入关键帧和高效的滑动窗口也能提高实时性能；先通过点云特征计算出相对位姿，再利用相对位姿、IMU预积分和GPS做融合，相比于直接一步做紧耦合，大大提高了效率，而且实测性能也很优异。

源码：https://github.com/TixiaoShan/LIO-SAM

注释版本：https://github.com/JokerJohn/opensource_slam_noted

• LVI-SAM TixiaoShan

LVI-SAM增加视觉传感器。

• SC-LEGO-LOAM

在Lego_LOAM的基础上新增了基于Scan_context的回环检测，其他流程完全一致，在回环检测的速度上有些许提升。apt install libparmetis-dev

源码：https://github.com/irapkaist/SC-LeGO-LOAM.git

注释：https://github.com/Black-Chocolate/sc_lego_loam_noted

改进情况：​​​​​​室外3D建图定位（一）Lego-Loam几个版本测试篇_yuanguobin01的博客-CSDN博客

SC-LEGO-LOAM 扩展以及深度解析（一）
SC-LEGO-LOAM 扩展以及深度解析（二）
SC-LEGO-LOAM 扩展以及深度解析（三）

小FOV固态雷达loam总述

• LIVOX-LOAM

针对livox lidar开发的loam算法，livox是固态激光雷达，区别于传统velodyne等雷达，它的视野很小，很容易发生帧间特征点的漂移，所以需要定制loam算法。在一个非常有限的视场下的特征提取和选择，鲁棒的异常值抑制，运动目标滤波和运动失真补偿。还集成了其他功能，如并行管道、使用单元格和地图的点云管理、回环闭合、地图保存和重新加载实用程序等。

loam_livox代码结构简介_东风小火的博客-CSDN博客从数据流视角解析 loam_livox_东风小火的博客-CSDN博客

源码：https://github.com/hku-mars/loam_livox

• livox_mapping

livox公司根据LOAM_NOTED开发的算法，无回环功能。

源码：https://github.com/Livox-SDK/livox_mapping

非线性优化方法总述

• 渐进式（滤波）

efk

ukf

• 最小二乘

ceres：Aloam前端、Aloam后端

cv-solve：LegoLOAM前端、LegoLOAM后端scan2mapLM

• 批量式（BA与图优化）

g2o：如orb后端

gtsam：如lio-sam后端、LegoLOAM后端

ALoam

好文参看：：https://www.cnblogs.com/wellp/p/8877990.html

特征提取

• 订阅：初始的激光雷达数据
• 发布：
  • /velodyne_cloud_2          有序点云（设置索引，去除NaN值，删除Lidar坐标系原点过近点）
  • /laser_cloud_sharp          极大边线点集合
  • /laser_cloud_less_sharp  次极大边线点集合（含极大边线点）
  • /laser_cloud_flat               极大平面点集合
  • /laser_cloud_less_flat       次极大平面点集合（含极大平面点）

• 计算点云点的水平角度，使点云有序：为每个点找到它所对应的扫描线（SCAN）；为每条扫描线上的点分配时间戳。后面的激光框架基本都不需要了，后面雷达的驱动将每个点的线号、角度、时间戳都发出来了。
• 特征点提取方式，根据点的曲率来提取特征点。即把特别尖锐的边线点与特别平坦的平面点作为特征点。
• 曲率是求取做法是同一条扫描线上取目标点左右两侧各5个点，分别与目标点的坐标作差，得到的结果就是目标点的曲率。当目标点处在棱角位置，与周围点的差值较大，曲率较大，属于线特征；反之当目标点在平面上时，周围点与目标点的坐标相近，这时属于面特征。
• 曲率计算完后进行特征分类提取：每条扫描线分成6个扇区，在每个扇区内，找曲率最大的20个点，作为次极大边线点，其中最大的2个点，同时作为极大边线点；寻找曲率最小的4个点，作为极大平面点，剩下未被标记的点，全部作为次极大平面点
• 为了防止特征点过多聚堆，每提一个（极大边线点/次极大边线点/极大平面点），将这个点和它附近的点全都标记为“已选中”，在下次提取特征点时，将会跳过这些点；对于次极大平面点，采取降采样的方法避免过多聚堆。
• 次极大边线点中包含两个极大边线点，次级大边线点就是边线点集合，极大边线点就是边线点集合中最好的两个。

前端odometry

• 订阅：有序点云velodyne_cloud_2、极大边线点、次极大边线点、极大平面点、次极大平面点。
• 发布：
  • /laser_cloud_corner_last   上一帧的边线点
  • /laser_cloud_surf_last       上一帧的平面点
  • velodyne_cloud_3              有序点云（从velodyne_cloud_2订阅来的点云未经其他处理）
  • /laser_odom_to_init            帧间的位姿粗估计
  • /laser_odom_path               帧间的平移运动 for rviz
• 去除因运动产生的畸变，没有IMU，使用匀速模型假设，使⽤上⼀个帧间⾥程记的结果作为当前两帧之间的运动，假设当前帧也是匀速运动，可以估计出每个点相对起始时刻的位姿。
• 边线点匹配方法：基于最近邻原理建立边线点之间的关联，每一个极大边线点去上一帧的次极大边线点中找匹配，假如在第k+1帧中发现了边线点i，通过KD-tree查询在第k帧中的最近邻点j，查询j的附近扫描线上的最近邻点l，j与l相连形成一条直线l-j，让点i与这条直线的距离最短，构建一个非线性优化问题，以点i与直线lj的距离为代价函数，以位姿变换7个量(四元数表示旋转+位移)为优化变量。
• 平面点匹配方法：假如在第k+1帧中发现了平面点i，通过KD-tree查询在第k帧（上一帧）中的最近邻点j，查询j的附近扫描线上的最近邻点l和同一条扫描线的最近邻点m，这三点确定一个平面，让点i与这个平面的距离最短，构建一个非线性优化问题，以点i与平面lmj的距离为代价函数，以位姿变换7个量(四元数表示旋转+位移)为优化变量。
• 匹配完要更新次级大线点和面点的KD-tree，留作下一帧的匹配。
• ceres求解，两次最小二乘即迭代两次ICP，第一次初值是R和T为（0,0,0,1）和（0,0,0），第二次为第一次的结果。
• 最小二乘求的是残差的平方和，因为有平方，所以就是非线性。
• ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);定义一下ceres的核函数，使用Huber核函数来减少外点的影响，即去除outliers。

后端mapping

• 订阅：当前帧全部点云(经过一次降采样)、上一帧的边线点集合，上一帧的平面点集合，当前帧的位姿粗估计。
• 发布：
  • /laser_cloud_surround           附近5帧组成的降采样子地图 for rviz
  • /laser_cloud_map                  所有帧组成的点云地图
  • /aft_mapped_to_init               经过Map to Map精估计优化后当前帧位姿精估计
  • /velodyne_cloud_registered              当前帧原始点云（从velodyne_cloud_3订阅来的点云未经其他处理）
  • /aft_mapped_to_init_high_frec         里程计坐标系位姿转化到世界坐标系位姿（地图坐标系），mapping输出的1Hz位姿，odometry输出的10Hz位姿，整合成10hz作为最终结果
  • /aft_mapped_path经过Map to Map  精估计优化后的当前帧平移
• “周围5帧组成的子地图submap与其他所有帧组成的全部地图进行匹配“这个是错误的！应该是当前帧的特征点和submap进行匹配。
• 根据前端位姿粗估计，将里程计位姿转化为地图位姿，得到后端优化的一个初始估计值，将已经消除畸变的当前帧点云先转换到全局坐标系变成Qk+1，然后与局部地图或称为submap做特征匹配，将配准好的点云成Qk+1添加局部地图Qk中。
• 如果完全使用所有区域的点云进行匹配，这样的效率会很低，而且内存空间可能会爆掉。LOAM采用的是栅格（cube）地图的方法，将整个地图分成21×21×11个珊格，每个珊格是⼀个边⻓50m的正⽅体，当地图逐渐累加时，珊格之外的部分就被舍弃，这样可以保证内存空间不会随着程序的运⾏⽽爆掉，同时保证效率。
• 对submap的管理并不是基于时序的滑动窗口方式，而是空间范围划分方式，当下的frame对应的submap可以包含很早期的frames，这种submap方式比基于时序的滑动窗口方式会包含更多的历史帧信息，对于点云配准更友好，当然这也就导致内存占用增加。
• 然后就是匹配，即在局部地图Qk中找特征点的关联，在odometry算法中，关联的确定是为了最快的计算速度，使用的是基于最近邻的思路来找对应线以及对应面；而mapping算法是通过对特征点周围的点云簇（最近邻的5个点作为点云簇）进行PCA主成分分析（求点云簇的协方差矩阵的特征值和特征向量），来确定找到边的方向向量以及面的法向量。
  • 找到全部地图特征点中，当前特征点的5个最近邻点。
  • 如果是边线点，则以这五个点的质心，以5个点的主方向向量（类似于PCA方法）为方向，作一条直线，令该边线点与直线距离最短，构建非线性优化问题
  • 如果是平面点，则寻找五个点的法方向（反向的PCA方法），令这个平面点在法方向上与五个近邻点的距离最小，构建非线性优化问题。
  • ceres求解两次ICP，kdtree搜索都和前端优化一样。

后端mapping：整合前端输出结果

• mapping输出的1Hz位姿，odometry输出的10Hz位姿，需要整合成10hz作为最终结果
• 过程：当mapping计算出一个位姿Pmap后，就跟与之同步的一个odometry位姿Podom计算odom增量△P = Pmap * Podom-1，然后用△P去修正后续的高频odometry位姿，修正后的10Hz高频位姿就是LOAM最终输出的实时位姿，然后直到mapping算法再计算出一个位姿后重复上述过程。
• △P = Pmap * Podom-1 ：-1是逆，即 Podom*△P=Pmap ，因此△P 代表未发生当前帧Podom时mapping值

LeGO-LOAM

好文参看：https://zhuanlan.zhihu.com/p/382460472；blog.csdn.net/m0_50610065/article/details/123834057

点云预处理：投影+分割Projection+Segmentation

• 订阅：初始的激光雷达数据
• 发布：
  • /full_cloud_projected                      有序点云（intensity是行号列号，即整数部分是线束值，小数部分是水平方向角度0~1799）
  • /full_cloud_info                               有序点云（intensity是距离range）
  • /ground_cloud                                地面的点云
  • /segmented_cloud                         分割的点云（含地面）
  • /segmented_cloud_pure                分割的点云（不含地面）
  • /segmented_cloud_info                 分割的点云标签（自定义消息）
  • /outlier_cloud                                 分割失败的点云，即非聚类的点
• copyPointCloud；copy点云到pcl标记Ring index，如果不是velodyne的lidar后面会通过计算得出点云属于哪个线束，为投影做准备
• findStartEndAngle()；找到开始时刻和结束时刻的方向角度，后面去畸变有用;
• projectPointCloud()；计算距离（Range），把点云投影到RangeImage上，也就是说得到的原始点云是一个球形，我们把球形投影到一个圆柱体上然后把它展开。以16线激光为例，转换的range图，高为16，长为旋转一周的采样次数，这里为1800，因此最后得到的是16*1800的图像。这个图像可以用数组表示，其中的坐标为x,y，而z会转换为深度信息，最后转换好的rangeImage效果如下图，类似一个深度图。 此处对激光点云的处理是 LEGO-LOAM 和 LOAM 的一个区别。点云的坐标信息存储在 intensity 中，采用的方法是：整数部分存储的是激光线束值，小数部分存储的水平航向值（0~1799）。​
• groundRemoval()；进行地面提取，对rangeImage中行Ring小于7的点进行地面检测，如果是32线雷达，则是Ring小于20的；判断rangeImage中A和相邻点B之间与水平方向的夹角ɑ小于10°则为地面。
  ​
• cloudSegmentation()；在去除地面之后对剩下的点云进行分割，对rangeImage从[0,0]点开始，通过BFS（深度优先）递归进行查找，遍历它前、后、左、右的4个点，分别进行对比；A和B是Lidar扫描的连续2个点云，如果A和B的夹角β小于60°，红线这2个点不属于同一个物体，反之绿线可认为是同一个物体，通过这个聚类的方法进行分割；为了提高效率和可靠性，分割出来的点云数量大于30个的集群则认为有效；5个点以上且横向线数大于3的集群也认为有效。
  
• publishCloud()；最后将点云有序化进行ros发布：列号 + 行号 * 1800 即points[columnIdn + rowIdn * Horizon_SCAN]
• 分割的点云标签（自定义消息）：
  • int32[N_SCAN] startRingIndex
  • int32[N_SCAN] endRingIndex
  • float32 startOrientation
  • float32 endOrientation
  • float32 orientationDiff
  • bool[N_SCAN*Horizon_SCAN]   segmentedCloudGroundFlag   是否为地面点
  • uint32[N_SCAN*Horizon_SCAN]  segmentedCloudColInd          column index（0~1799）
  • float32[N_SCAN*Horizon_SCAN] segmentedCloudRange          距离信息

前端Odometry：Feature Extraction+Feature Association

• 订阅：非聚类的点、分割的点云、分割的点云标签（自定义消息）、imu原始信息
• 发布：
  • /laser_cloud_sharp          极大边线点集合用于rviz
  • /laser_cloud_less_sharp  次极大边线点集合用于rviz
  • /laser_cloud_flat               极大平面点集合用于rviz
  • /laser_cloud_less_flat       次极大平面点集合用于rviz
  • /laser_cloud_corner_last   边线点集合
  • /laser_cloud_surf_last       平面点集合
  • /outlier_cloud_last             远点和离散点集合
  • /laser_odom_to_init           雷达里程计粗估计
• 拿分割好的点云进行特征提取，提取的特征的目的是进行点云配准，从而得出当前位姿。
• imu的处理：IMU角度转换；对位置、速度和角度做积分，计算出当前的位置、速度和角度。
• Feature Extraction：和ALoam特征提取部分大同小异
  • 利用imu积分去除运动畸变 adjustDistortion()；
  • 计算曲率平滑度确定平面点和边线点 calculateSmoothness()；
  • 标记闭塞点 markOccludedPoints()；ALoam中没有这部分
    
    第一类闭塞点：如(a)中的B点所示，虽然它处于一个平面上而且可以被辨识为平面点，但是那个平面和激光束基本处于平行的关系，很有可能在下一次的扫描中不可见故需被排除掉。用相邻两点之间的距离差判断，若距离大于2cm的平面点则将其排除，越平行激光光两点距离越远。
    第二类闭塞点：如(b)中的A点所示，该类点会被辨识为边线点，但是此类点有可能是平面点，因为遮挡造成橙色的部分没有被扫描到而被误判为边线点。用相邻两点之间的距离差判断，距离大于30cm的边线点排除。
  • 提取线特征和面特征 extractFeatures()；
  • 发布特征点云用于rviz publishCloud()；
• Feature Association：
  • 利用imu更新初始猜测位置 updateInitialGuess()，下面的最小二乘法求解非线性问题要有一个初值；
  • 计算两帧之间的相对位姿变换 updateTransformation()；
    地面点和边线点的位姿推理，分别估计三自由度的变量，cv::solve求解非线性问题，偏导为0处求极值，问题本质也是ICP：
    • 利用地面点优化pitch，roll，z：地面点更符合面特征的性质，因此地面点的优化问题就使用点到面的约束来构建，但地面点之间的约束对x，y和yaw这三个自由度作用不大，就是当这三个自由度的值发生变化时，点到面的残差不会发生显著变化，所以，地面点之间的优化只会对pitch，roll以及z进行约束和优化，详细计算过程LEGO-LOAM源码解析 --- FeatureAssociation节点(4) - 知乎
    • 利用角点优化x，y，yaw：由于多线激光雷达提取的角点通常是垂直的边缘特征，比如电线杆，这些特征对x、y以及yaw有着比较好的能观性，详细计算过程LEGO-LOAM源码解析 --- FeatureAssociation节点(3) - 知乎
  • 积分相对位姿，更新位姿 integrateTransformation()；
  • 发布雷达里程计消息 publishOdometry()；
  • 发布用于mapOptimization的点云  publishCloudsLast()；

后端Mapping ：图优化+LM

• 有2种后端方式：
  • 第一种方式：获取传感器视野范围内的特征集，通过查找离当前位置100米以内的点云地图。把当前帧扫描到的点云和点云地图做匹配。
  • 第二种方式：通过建立姿态图(Pose-Graph)，每帧特征集的pose都可以当作姿态图中的一个节点，特征可以视为该节点的传感器测量。
    由于激光制图的姿态漂移非常低，假设短时间内没有漂移，选取最近的一组特征集合，新节点和选取最近的节点之间的空间约束可以使用isam优化，得到位姿变换。
    顶点表示优化变量，边表示误差项；顶点表示位姿，边表示约束。
• LOAM采用第一种方式，LeGO-LOAM采用两种方式结合。
• LOAM保存的是点云地图，LeGO-LOAM保存的是每帧的特征点云集合。
• LeGO-LOAM把当前帧扫描到的点云线面特征和周围关键帧的点云地图做匹配，进一步优化姿势变换，较低的频率运行。过程：
  • 1. 转换到map坐标系 transformAssociateToMap()。
  • 2. 提取周围关键帧并对关键帧特征降采样 extractSurroundingKeyFrames()。
    如果使用了回环检测进行图优化，则提取上一时刻的 50个关键帧；如果没有使用回环检测，则添加离当前欧式距离最近的50个关键帧拼接成点云。
    对关键帧降采样，主要的作用就是方便后续的图优化过程，提高图优化的速度。
  • 3. 对当前帧特征进行降采样 downsampleCurrentScan()。
  • 4. 当前帧到map的优化 scan2MapOptimization()
    以第一种方式进行优化，类似ICP，最多进行10次优化，重复下面过程：
    a.根据周围关键帧点云创建kdtree（角特征点云、面特征点云）；
    b.从kdtree中搜索当前特征点的5个最近邻点；
    c.如果是边线点，则以这五个点的质心，以5个点的主方向向量（类似于PCA方法）为方向，作一条直线，令该边线点与直线距离最短，构建非线性优化问题；
       如果是平面点，则寻找五个点的法方向（反向的PCA方法），令这个平面点在法方向上与五个近邻点的距离最小，构建非线性优化问题；
    d.用 cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR)进行LM优化，QR分解（cv::DECOMP_QR）或奇异值分解（cv::DECOMP_SVD）可以求解超定（奇异矩阵）线性系统。
  • 5. 关键帧图优化 saveKeyFramesAndFactor()
    以第二种方式进行优化，如果有新的关键帧加入，需要计算出当前帧与与上一帧之间的约束情况，这种约束可以认为是小回环，加入到后端中去优化，优化后的结果作为关键帧的位姿与点云，然后同步到 scan2Map过程中：
    a.把当前关键帧pose加入gtsam graph；
    b.更新iSAM
    c.图优化isam->calculateEstimate()并保存关键poses；
    d.保存更新的transform
    是否关键帧：当前帧和之前帧的距离大于0.3米，即0.3m一次小回环图优化。
  • 6. 校正位姿：独立线程回环检测如果成功，才会进行这一步，将回环图优化后的位姿替换之前的位姿。
  • 7. 发布坐标转换、关键帧、姿态。

后端Mapping ：回环检测

• 通过回环检测来进一步消除累积误差，回环检测作为后端的一个线程loopClosureThread()独立执行：
  • detectLoopClosure判断是否进行回环：首尾之间的距离小于7米，并且时间相差30s以上
  • 如果进行回环：
    a.通过ICP算法对比当前帧和之前帧是否匹配；
    b.如果匹配则添加新约束到isam；
    c.然后通过iSAM2来优化姿态图。
• 作者同时提醒回环检测的ICP算法当里程计漂移太大时经常失败，对于更高级的闭环方法，建议采用SC-LeGO-LOAM ，它的特征采用的是点云描述符。