liv-sam---前世今生

一 发展历史

LOAM 到LeGO-LOAM到LIO-SAM 到LVI-SAM。

LOAM 、LeGO-LOAM、LIO-SAM都是纯激光算法。

LeGO-LOAM、LIO-SAM 和LVI-SAM为Tixiao Shan的最新力作。

slam算法之LOAM系列（一） - 知乎 (zhihu.com)

二 LOAM

       LOAM为比较古老的激光匹配slam方法，可以理解为一个没有图优化的激光里程计，惯导数据只用来进行点云的偏斜矫正和为扫描匹配提供运动先验。

       特征点采用曲率的方式提取，寻找曲率最大，和曲率最小的点，以及曲率次大，和次小点。采用scan-to-scan进行粗定位，scan-to-map进行精匹配并建图。

        mapping输出的1Hz位姿和odometry输出的10Hz位姿的整合过程：当mapping计算出一个位姿Pmap后，就跟与之时间同步的一个odometry位姿Podom计算增量△P = Pmap * Podom-1，然后用△P去修正后续的高频odometry位姿，修正后的10Hz高频位姿就是LOAM最终输出的实时位姿，然后直到mapping算法再计算出一个位姿后重复上述过程。

2.1 特征点提取

        根据点的曲率来计算平面光滑度作为提取当前帧的特征信息的指标。

对整个扫描进行化段，分四段，每段各取两个边缘点和4个平面点。选点三要素：

1不能超过设定的size，每个集合平面点4个，边缘点2个；

2已选取的点周围不能有点，使得点可以分布的更加均匀；

3选取的平面点不能与激光扫描束平行。

2.2 Scan to scan

        当前帧与上一帧匹配，计算两帧之间的位移。

       Kd树找匹配的特征点，然后最小二乘法算位移。

       最小二乘法，点到线的距离，点到面的距离，最小二乘法求最小值（不同的帧相同的点，用R t转换回去，距离应该是零）。

2.3 Scan to map

       当前帧与边长为10米的立方体内的点云匹配。

       LOAM-SLAM的详细讲解请下如下博客。

       LOAM-SLAM原理深度解析 - 知乎 (zhihu.com)

三 LeGO-LOAM

LeGO-LOAM和LOAM的区别与联系 - 知乎 (zhihu.com)

分割，地面和非地面激光线分割。

图优化，利用gtsam优化得到最终的全局地图。

四  LIO-SAM

LIO-SAM是惯导和激光的紧耦合算法。支持回环和图优化，引入了关键帧和滑窗。

为了确保实时的高性能，将旧的激光雷达扫描边缘化以优化姿态，而不是将激光雷达扫描与全局地图匹配。局部尺度的扫描匹配代替了全局尺度的扫描匹配，大大提高了系统的实时性。

数据融合是基于下图中的因子图。除了惯导和激光外，还可以融合其它观测量，如GPS。

       当机器人姿态变化超过用户定义的阈值时，将在图中添加一个新的机器人状态节点x（关键帧）。因子图在插入新节点时使用贝叶斯树的增量平滑和映射进行优化。

       相邻的关键帧之间，通过IMU数据计算预积分，获得位姿变换，构建IMU预积分因子。每个关键帧还有对应的GPS数据参与校正。如果有闭环出现，闭环帧之间可以构建约束。关键帧之间有若干普通帧，这些帧不参与图优化，但是会执行scan-to-map的配准，优化每帧位姿。

4.1 ​​​​​​​框架

    代码架构如下图。整个算法就四个文件。Github里有加了中文注释的源码，网址为：GitHub - smilefacehh/LIO-SAM-DetailedNote: LIO-SAM源码详细注释，3D SLAM融合激光、IMU、GPS

系统架构如，如下图：

4.2 流程

1、激光运动畸变校正。利用当前帧起止时刻之间的IMU数据、IMU里程计数据计算预积分，得到每一时刻的激光点位姿，从而变换到初始时刻激光点坐标系下，实现校正。

2、提取特征。对经过运动畸变校正之后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点特征。

3、scan-to-map匹配。提取局部关键帧map的特征点，与当前帧特征点执行scan-to-map匹配，更新当前帧的位姿。（空间上，时间上的附近帧匹配）

4、因子图优化。添加激光里程计因子、GPS因子、闭环因子，执行因子图优化，更新所有关键帧位姿。

5、闭环检测。在历史关键帧中找候选闭环匹配帧，执行scan-to-map匹配，得到位姿变换，构建闭环因子，加入到因子图中一并优化。

4.3 gtsam

        图优化，维护的是一个由节点和边组成的连接图。当前端新加入一个观察或者关键帧时，向图中添加一个边、或一个边和一个节点。此时图中需要判断是否产生了新的回环，如果产生，则构建最小二乘法，优化此回环。但此回环中的节点可能也处在别的环路中，因此需要将图中所有回环都进行一次优化。计算量可观。

        gtsam维护的一颗贝叶斯树。当前端新加入一个观察或者关键帧时，产生一个新的因子f(xi, xj)，首先找到所有受影响的团，为起点是包含xi或 xj的团，终点是根团。含xi或 xj的团以下的支树，以及任何不包含含xi或 xj的支树不受影响。

        然后更新贝叶斯树，将树中受影响的部分转换回因子图，将新的因子添加进去。再将这些临时因子图通过消元算法重新消元，生产成新的贝叶斯树，且之前没有受影响的支树也可以重新添加回去。

         里程计添加关键帧时，回环添加新的观测时，不区分，一样进行上面的处理找到贝叶斯树中需要更新的部分。然后重新因子化，添加新的因子，再消元。消元的过程产生平方根信息矩阵，求解更新量。但不是每添加一个因子都需要进行上述过程，在新加因子少时，可以用R矩阵的增量分解方法直接求解更新量。固定滞后平滑滤波，感觉对里程计添加顺序节点很有用。

        求解出的更新量，如要更新到每个节点的位姿吗？在每一个团上，我们都可以检测变量估计之间的差值，解的更新向下传播，直到差值小于一定的阈值，传播才停止。

gtsam 原理学习-CSDN博客

五 LVI-SAM

麻省理工&史蒂文斯理工学院Tixiao Shan开源的激光和视觉紧耦合算法。

5.1 ​​​​​​​算法框架

        框架由两个关键的子系统组成：视觉惯性系统(VIS)和激光雷达惯性系统(LIS)。VIS系统接受图像和IMU信息，雷达点云是可选的输入。通过最小化IMU和视觉的测量残差来得到视觉里程计。激光里程计通过最小化检测到的线面特征到特征图的距离得到。特征图保存在一个滑窗中来实时的执行。最后状态估计问题，可以公式化为一个MAP问题。

5.2 ​​​​​​​视觉惯导系统vis

特征点提取与跟踪采用与vins相同的方法。

初始化，利用视觉里程计对齐激光帧的点云得到一个稀疏的深度图来得到特征的深度完成视觉惯导初始化。

深度特征关联：根据VIS初始化的结果，我们利用视觉里程计对齐视觉帧和雷达帧。由于当前的3D lidar扫描的是稀疏的点，我们把多帧激光点云组合起来得到一个稠密的深度图。为了把特征和深度值关联起来，我们把视觉特征和雷达点投影到以相机为圆心的单位圆上。然后对深度点进行降采样并利用极坐标进行保存,保证点的密度是常数的。我们利用二维的KD-tree（极坐标）来找视觉特征点周围最近的三个点，最后特征点的深度计算的是相机中心和特征点的连线的长度，这条线和三个深度点得到的平面在笛卡尔坐标系中有交点。可视化如下图3(a)所示，特征深度是虚线的长度。

      我们通过检查和特征点最近的三个深度点之间的距离来进一步验证关联的特征深度。这是因为堆叠来自不同时间戳的激光雷达帧可能会导致来自不同对象的深度模糊。这种情况的图示在图3(b)中。在时间ti处观察到的深度点以绿色表示。相机在tj移至新位置并观测到了灰色的深度点。但是在ti观测到的被灰色线框起来的点，由于点云堆叠在tj时刻可能还是能看到。所以使用来自不同对象的深度点来关联特征深度会导致估算不准确。与类似[17]中的做法一样，我们通过检查特征周围深度点之间的最大距离，如果最大距离大于2m，就不对该特征点进行数据关联。

失败检测：如果运动变化剧烈，光照变化或者环境缺少纹理就会导致VIS系统失败。这是由于在这种场景中跟踪的特征点会显著减少，而特征太少会导致优化失败。当VIS系统失败的时候会导致IMU的bias很大，所以当VIS系统跟踪的特征点少于一个阈值或者IMU估计出来的bias大于一个阈值的时候我们认为VIS系统失败。主动失效检测对于本文的系统是必需的，以便其故障不会影响LIS系统。一旦检测到故障，VIS将重新初始化并通知LIS。

闭环检测：本文利用DBoW2来做闭环检测，对于每一个新来的图像关键帧，检测出BRIEF描述子并把他和原来检测的描述子匹配。通过DBoW2检测到的闭环候选帧的时间戳会给到LIS系统来做进一步的验证。

5.3 ​​​​​​​激光惯导系统lis

雷达惯导系统是基于LIO-SAM的，主要是利用因子图来做全局的位姿优化。

Scan-match的初值：IMU的积分和VIS系统。当VIS系统的里程计可以输出位姿时我们以他为初值，如果VIS系统报告失效的时候，我们利用IMU的积分作为初值。

失败检测：尽管激光雷达可以得到场景中很远范围内的细节，但是也会在一些场景中导致扫描匹配失败，如下图所示。

       扫描匹配中的非线性优化问题可以表示为迭代求解一个线性问题。

        其中A和b是由T处的线性化得到的。当A TA的最小特征值小于第一次优化迭代时的阈值时，LIS报告失败。当发生故障时，激光雷达里程计约束不会添加到因子图中。