LVI-SAM源码解析（一）：论文阅读

本文主要是介绍LVI-SAM源码解析（一）：论文阅读，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

1. 摘要

LVI-SAM是一种基于平滑化和映射实现的激光雷达-视觉-IMU紧耦合SLAM方法，能够实现高精度、高鲁棒性地实时状态估计和地图构建。

LVI-SAM建立于包含两个子系统的因子图上：视觉-IMU子系统（VIS）和激光-IMU子系统（LIS），两个子系统采用紧耦合设计方式。其中VIS利用LIS进行初始化，利用LiDAR测量的深度信息提高精度，LIS利用VIS的估计结果作为扫描匹配初始值。回环检测首先由VIS识别，再由LIS进一步确认。

两个子系统中任意一个失效了，LVI-SAM仍可以正常工作，这说明它在无纹理和无特征环境下仍然具有高鲁棒性。

2. 代码地址

https://github.com/TixiaoShan/LVI-SAM

3. 引言

主要贡献：

LVI-SAM是基于因子图实现的紧耦合LVIO框架，并且在位置识别的帮助下实现了多传感器融合和全局优化。
LVI-SAM可以通过故障检测绕过失效的子系统，因此对传感器退化具有鲁棒性。
不同尺度、平台和环境下，LVI-SAM均能够有效运行。

4. LVI-SAM方法介绍

4.1. 系统概述

VIS处理图像、IMU的测量结果和激光雷达测量结果（可选），最小化视觉和IMU测量结果的联合残差实现视觉里程计。LIS通过特征与特征地图的匹配来提取特征并执行激光雷达里程计，特征地图以滑动窗口的方式进行维护，从而提高实时性。最后利用ISAM2，联合优化因子图中的IMU预积分约束、视觉里程计约束、激光雷达里程计约束、和回环约束，解决状态估计问题（上述过程也可以表示为最大后验概率问题）。LIS中的多传感器图优化的目的是减少数据交换并提高系统效率。

4.2. 视觉-惯性子系统（VIS）

VIS模块基于VINS-MONO，视觉特征点采用角点检测器提取，并采用KLT光流法追踪。VIS初始化以后，我们利用视觉里程计对激光雷达帧进行配准，并且获取稀疏的深度图用于特征点深度估计。系统在滑动窗口中执行光束法平差，系统状态x可以表示为：

$x=[R,p,v,b]$

其中R是旋转矩阵，p为平移向量，v为速度，b是包含加速度计零偏和陀螺仪零偏的IMU的零偏向量。从传感器到世界坐标系的位姿转换用 $T \in [R | p]$ 表示。

4.2.1. 初始化

基于优化的VIO初始化阶段总是会发散，其原因是初始化是一个高度非线性的总是。初始化的质量主要依赖于两个因素：初始化化时传感器的运行和IMU的参数精度。在实际测试中，当传感器运动很小或者常速运动时就会初始化失败。这是因为加速度激励较小的时候尺度是不可观的。IMU含有缓慢变化的偏置和白噪声，这同时影响加速度和角速度的测量。在初始化时，更好的参数有益于更快收敛。

为了提高VIS初始化的鲁棒性，首先初始化LIS获得系统状态x和IMU的零偏，然后基于图像帧的时间戳将它们插值并关联到图像帧（注意到此处假定两个相邻的图像关键帧之间的零偏值是不变的），最后用关联值初始化VIS，从而提高VIS初始化的速度和鲁棒性。

4.2.2. 特征深度关联

在初始化的基础上，利用已经估计好的视觉里程计把激光雷达帧配准到视觉帧。一般激光雷达单帧是比较稀疏的，所以叠加多帧以获得稠密的深度图。

为了把视觉特征和深度值关联起来，首先将视觉特征点和点云点投影到以相机为球心的单位球上，用极坐标形式表示点的位置，进行下采样。在极坐标空间用二维kd-tree搜索每个视觉特征点附近的三个临近点云点，认为从相机光心到特征点（归一化平面上）的直线在笛卡尔空间与三个激光点确定的平面之间的交点的深度值就是特征点的深度值。

检查三个最近邻激光点之间的距离以进一步验证关联信息的有效性。这是因为使用不同时刻的激光帧叠加到一起会导致源自不同物体的深度模糊。如下图所示， $t_i$ 时刻观测到的点用绿色表示， $t_j$ 时刻相机移动到了新的位置，观测到的新的点用灰色表示，但是图中用灰色虚线框框起来的点，因为激光点云叠加的原因，却会被误认为是 $t_j$ 时刻也能观测到的点。所以需要使用距离阈值来剔除掉这种情况。

视觉特征和深度图关联的一个证明如下图所示。图中a和c是利用视觉里程计把激光点云投影到图像中，在b和d中成功关联的结果用绿色表示。

4.2.3. 失效检测

遇到剧烈运动、光照变化和无纹理环境时，VIS跟踪的特征点急剧减少，进而导致优化发散，VIS系统失效。VIS失效时，估计的IMU零偏会变大。因此在特征点数量小于一定的阈值时，或估计的IMU零偏超过一定阈值时，则报告VIS子系统失效。

失效检测是必要的，这样可以保证VIS失效不会影响LIS的功能，一旦检测到VIS失效，VIS将重新初始化并通知LIS。

4.2.4. 回环检测

采用词袋模型DBoW2进行回环检测，对于每个新图像关键帧，提取BRIEF描述子，与之前的描述子进行词袋匹配。把DBoW2返回的回环帧时间戳发送给LIS，由LIS进行进一步的回环验证。

4.3. 激光-惯性子系统（LIS）

LIS部分是由LIO-SAM改造而来，维护一个全局位姿优化的因子图。因子图中包含四种约束：IMU预积分约束，视觉里程计约束，激光里程计约束，回环约束，因子图会对这几种约束进行联合优化。

激光里程计约束源于激光扫描匹配，该匹配是把当前帧和全局特征地图匹配起来。候选的回环约束首先由VIS检测到，然后再进行激光扫描匹配优化。系统维护一个激光雷达关键帧的滑窗用以限制计算复杂度。当位姿变化超过一定阈值时才会产生新的关键帧。夹杂在关键帧之间的帧会变抛弃。一个新关键帧的产生时，相应的一个新的状态变量也会被添加到因子图中。以此种方式添加关键帧不仅实现了地图密度和内存占用的平衡，也有助于维护一个相对稀疏的因子图以便于实时优化。

4.3.1. 初值预测

剧烈运动时，激光雷达帧匹配选取的初值（预测值）十分重要。LIS初始化之前与之后预测来源是不同对的。

LIS初始化之前，假设机器人从某位置启动，初速度为0。假设零偏和噪声均为0，对IMU测量值积分，计算两个激光雷达关键帧之间的平移和旋转以作为预测。在速度小于10m/s且角速度小于180°/s时，此方法可以成功初始化LIS。LIS初始化以后，在因子图优化IMU零偏、位姿、速度，并发送给VIS帮助VIS初始化。

LIS初始化以后，预测的来源有两个：校正后的IMU预积分值和VIS的估计结果。VIS结果可用时，优先采用VIS的估计结果。如果VIS失效，则利用IMU获取预测。这种双来源的预测方法提高了多纹理环境和无纹理环境的预测准确性和鲁棒性。

4.3.2. 失效检测

激光雷达遇到像上面一样的几何特征丢失的场景下会产生退化。

在扫描匹配过程中非线性优化问题可以表达为迭代地求解线性问题 $\min_{T} \left \| AT -b \right \|$ 。当 $A^TA$ 的最小特征值小于第一次优化迭代是的阈值时，认为LIS失效。（可以这样想，A的奇异值特别小的时候， $A^TA$ 接近于病态矩阵或奇异矩阵，导致得到的解不可信）LIS失效后，就不在因子图中添加激光里程计约束了。