论文地址：在公众号「计算机视觉工坊」，后台回复「LVI-SAM」，即可直接下载。
我相信很多人对激光视觉惯导融合的系统都是这样设计的，但是最难的是把自己的想法保质保量的实现出来。我们做不到但是大佬可以！
摘要
本文提出了一个紧耦合的雷达视觉惯导SLAM系统，可以实时高精度鲁棒的进行状态估计和建图。LVI-SAM构建在因子图之上，并且由两个子系统组成：一个视觉惯导系统和一个激光惯导系统。这两个子系统利用紧耦合的方法，视觉惯导的系统利用激光惯导的估计来做初始化。视觉惯导系统利用激光雷达的测量给视觉特征提供深度来提升精度。同时激光惯导系统利用视觉惯导的估计值作为初值来做帧的匹配。利用视觉做闭环检测，把检测的结果给激光惯导系统来做优化。当激光惯导和视觉惯导子系统的一个失败的时候LVI-SAM仍然可以工作，这极大的提升了系统在缺少纹理或者特征场景中的鲁棒性。LVI-SAM系统在数据集上进行了测试，取得了很好的效果。
介绍
基于雷达的SLAM系统可以获取很长范围的环境的细节，但是在缺少结构信息的场景中很容易失效。例如长走廊或者是开阔的广场。基于视觉的方法在纹理信息丰富的场景中表现良好，而且很容易做场景重识别。但是他们对光照的变化，快速运动和初始化很敏感。因此，激光和视觉经常和惯导融合来提升他们的精度和鲁棒性。惯导可以帮助去除点云的运动畸变，而且在缺少特征的环境中撑一小段时间，同时可以帮助视觉系统恢复尺度信息。
本文提供了一个紧耦合雷达视觉惯导里程计，LIS系统和VIS系统可以独立的运行（当一个系统失败的时候），也可以耦合在一起（当检测到很多特征点的时候）。视觉惯导系统执行利用雷达帧恢复深度的视觉特征的跟踪，通过优化视觉重投影误差和imu测量误差的视觉里程计为激光雷达的scan-matching提供初值，并且把约束加入到因子图中。当利用imu把点云的畸变去除后，激光惯导系统检测点云的边缘和平面特征，并和保存在划窗中的features map对齐。LIS系统估计得到的系统状态可以传到VIS系统中做初始化。对于闭环，候选的匹配帧通过视觉的词袋模型得到，在LIS系统中做优化。来自视觉里程计，激光里程计，imu预积分和闭环的约束都会放到因子图中，最后，优化得到的IMU的bias用来递推给出IMU频率的位姿。本文的主要贡献有：
- 基于因子图的紧耦合的LVIO系统，实现了多传感器融合与基于场景重识别的全局优化；
- 通过故障检测来绕过失败的子系统，使其对传感器性能下降具有鲁棒性；
- 利用不同的数据集进行了完善的验证。
从系统的角度来看，作者的工作是新颖的，代表了VIO和LIO中最新技术的集成，实现了高精度的LVI系统，作者希望大家可以以此为基础，推动激光惯导视觉融合系统的发展。
完整的激光视觉惯导SLAM系统
A. 系统概述
系统接受3D激光点云、单目图像和IMU信息作为输入。VIS系统接受图像和IMU信息，雷达点云是可选的输入。通过最小化IMU和视觉的测量残差来得到视觉里程计。激光里程计通过最小化检测到的线面特征到特征图的距离得到。特征图保存在一个滑窗中来实时的执行。最后状态估计问题，可以公式化为一个MAP问题。利用iSAM2来优化因子图中IMU预积分，视觉里程计，激光里程计和闭环的约束的残差。需要注意的是，LIS中采用的多传感器图优化旨在减少数据交换并提高系统效率。
B. 视觉惯导系统
视觉惯导的pipeline如图2所示。
视觉特征利用角点检测器，利用KLT算法进行跟踪。我们利用视觉里程计对齐激光帧的点云得到一个稀疏的深度图来得到特征的深度完成视觉惯导初始化。系统在滑窗中执行BA，其中状态X可以写成：
R是SO3表示旋转矩阵，p表示位置，v表示速度，b=[b_a, b_w]是imu的偏置。变换矩阵T是SE3表示从机体坐标系到世界坐标系的变换 。下边将详细的介绍VIS的初始化和特征深度估计。读者们可以读VINS-Mono的原文来了解细节。
1）初始化：基于优化的VIO系统由于很强的非线性导致初始化发散。初始化的质量取决于两个主要因素：初始化的传感器运动和IMU参数的精度。在实际的实验中，我们发现VINS很容易在系统速度很小或者匀速的时候初始化失败。这是由于加速度激励不够大导致尺度不可观。IMU的参数包含渐变的bias和高斯白噪声。他影响这加速度和角速度的测量，在初始化的时候好的初值可以帮助系统快速的优化。
为了改善VIS初始化的鲁棒性，我们利用LIS系统来估计系统的状态X和IMU的偏置b。因为直接可以通过雷达的观测得到深度信息。我们首先初始化LIS系统获得x和b。然后我们通过插值来把激光帧和视觉帧通过时间戳对齐。假设IMU的偏置在两个图象关键帧之间是常数。最后把LIS系统初始化得到的x和b作为VIS初始值，这很有效的改善初始化的速度和鲁棒性。
2）深度特征关联：根据VIS初始化的结果，我们利用视觉里程计对齐视觉帧和雷达帧。由于当前的3D lidar扫描的是稀疏的点，我们把多帧激光点云组合起来得到一个稠密的深度图。为了把特征和深度值关联起来，我们把视觉特征和雷达点投影到以相机为圆心的单位圆上。然后对深度点进行降采样并利用极坐标进行保存保证点的密度是常数的。我们利用二维的KD-tree（极坐标）来找视觉特征点周围最近的三个点，最后特征点的深度计算的是相机中心和特征点的连线的长度，这条线和三个深度点得到的平面在笛卡尔坐标系中有交点。可视化如下图a所示，特征深度是虚线的长度。
我们通过检查和特征点最近的三个深度点之间的距离来进一步验证关联的特征深度。这是因为堆叠来自不同时间戳的激光雷达帧可能会导致来自不同对象的深度模糊。这种情况的图示在图3（b）中。在时间t_i处观察到的深度点以绿色表示。相机在t_j移至新位置并观测到了灰色的深度点。但是在t_i观测到的被灰色线框起来的点，由于点云堆叠在t_j时刻可能还是能看到。所以使用来自不同对象的深度点来关联特征深度会导致估算不准确。与类似Laser-Visual-Inertial Odometry and Mapping with High Robustness and Low Drift论文中的做法一样，我们通过检查特征周围深度点之间的最大距离，如果最大距离大于2m，就不对该特征点进行数据关联。
图4显示了对齐的深度图和视觉特征。图4的a和c，利用视觉里程计对齐点云得到深度图投影到了图像上。在图像b和d上，通过深度图恢复视觉特征的被标记成了绿色。需要注意的是，虽然图4a中深度点覆盖了图像的大部分区域，但是由于很多特征落在窗户的角上导致深度关联检验失败。
3）失败检测：如果运动变化剧烈，光照变化或者环境缺少纹理就会导致VIS系统失败。这是由于在这种场景中跟踪的特征点会显著减少，而特征太少会导致优化失败。当VIS系统失败的时候会导致IMU的bias很大，所以当VIS系统跟踪的特征点少于一个阈值或者IMU估计出来的bias大于一个阈值的时候我们认为VIS系统失败。主动失效检测对于本文的系统是必需的，以便其故障不会影响LIS系统。一旦检测到故障，VIS将重新初始化并通知LIS。
4）闭环检测：本文利用DBoW2来做闭环检测，对于每一个新来的图像关键帧，检测出BRIEF描述子并把他和原来检测的描述子匹配。通过DBoW2检测到的闭环候选帧的时间戳会给到LIS系统来做进一步的验证。
C.雷达惯导系统
如图5所示，提出的雷达惯导系统是基于LIO-SAM的，主要是利用因子图来做全局的位姿优化。
主要有四种约束，IMU的预积分约束，视觉里程计的约束，雷达里程计的约束和闭环检测的约束加入到因子图中参与优化。激光雷达里程计约束来自扫描匹配，在这里，我们将当前的激光雷达关键帧与全局特征图进行匹配。闭环检测的约束候选帧首先由VIS提供，然后通过扫描匹配进一步优化。我们为特征图维护了一个激光雷达关键帧的滑动窗口，这保证了有限的计算复杂性。当机器人位姿变化超过阈值时，将选择一个新的激光雷达关键帧。丢弃位于关键帧之间的普通激光雷达帧。选择新的激光雷达关键帧后，新的机器人状态x将作为节点添加到因子图中。以这种方式添加关键帧，不仅可以在内存消耗和地图密度之间取得平衡，而且还有助于维护相对稀疏的因子图以进行实时优化。可以通过LIO-SAM的论文了解细节。在以下各节中，我们重点介绍提高系统鲁棒性的新策略。
1）初始值估计：通过实验发现初始值对于连续的扫描匹配有着重要的作用，尤其是在剧烈运动的情况下。在LIS系统初始化之前和之后初值的来源是不同的。在LIS系统初始化之前，我们假设机器人在初始位置静止，然后假设IMU的bias和噪声都为0，对IMU原始值积分得到两个雷达关键帧的位置和姿态信息作为scan-match的初值。经过实验发现这种方法能在有挑战的场景（初始速度小于10m/s，角速度小于180°/s）中初始化系统。一旦LIS系统初始化完成，我们估计因子图中的IMU的bias，机器人的位姿，速度。然后把这些量传到VIS系统中完成其初始化。
在LIS系统初始化完成之后，我们可以通过两个途径得到Scan-match的初值：IMU的积分和VIS系统。当VIS系统的里程计可以输出位姿时我们以他为初值，如果VIS系统报告失效的时候，我们利用IMU的积分作为初值。这个过程在纹理丰富和缺乏纹理的环境中都增加了初值的精度和鲁棒性。
2）失败检测：尽管激光雷达可以得到场景中很远范围内的细节，但是也会在一些场景中导致扫描匹配失败，如图6所示。我们利用On Degeneracy of Optimization-based State Estimation Problems论文中介绍的方法来检测LIS系统是不是失败了。
scan-match的非线性优化可以写成一个线性最小二乘的形式：
其中A和b是从T处的线性化获得的。当$A^TA$的最小特征值小于第一次优化迭代的阈值时，LIS报告失败，此时不会将激光雷达里程计约束添加到因子图中。可以从On Degeneracy of Optimization-based State Estimation Problems看到这些假设所基于的详细分析。
实验
设备：十六线的Velodyne雷达、FLIR BFS-U3-04S2M-CS的单目相机、MicroStrain 3DM-GX5-25的IMU、Reach RS GPS、Intel i7-10710U in Ubuntu Linux
作者在三个自己录制的数据集上进行了测试都取得了很不错的效果。
