＜论文阅读＞LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping
一、介绍
LVI-SAM建立在一个因子图之上，由两个子系统组成：视觉惯性系统(VIS)和激光雷达惯性系统(LIS)。这两个子系统采用紧密耦合的方式设计，其中VIS利用LIS估计来初始化。
利用激光雷达测量来提取视觉特征的深度信息，提高了视觉识别的精度。反过来，LIS利用VIS估计来进行初始估计，进行 Scan-Matching。回环首先由VIS识别，然后由LIS进一步细化。
当两个子系统中的一个出现故障时，LVI-SAM也可以正常工作，在无纹理和无特征环境中增强了它的鲁棒性。
LVI-SAM的系统结构。该系统接收来自三维激光雷达、摄像机和IMU的输入，可分为两个子系统:视觉惯性系统(VIS)和激光雷达惯性系统(LIS)。VIS和LIS可以相互独立运行，同时利用彼此提供的信息，提高系统的准确性和鲁棒性。系统以IMU速率输出姿态估计
该系统利用激光雷达帧进行视觉特征跟踪和选择性提取特征深度。视觉里程计是通过优化视觉重投影和IMU测量的误差来获得的，作为激光 Scan-Matching 的初始猜测，并在因子图中引入约束。用IMU测量的数据消除点云倾斜后，LIS提取激光雷达边缘和平面特征，并将它们与保持在滑动窗口中的特征地图进行匹配，可以将LIS中估计的系统状态发送给VIS，以便于其初始化。
对于回环，候选匹配首先通过VIS辨别，然后通过LIS进一步优化。来自视觉里程计、激光里程计、IMU预积分、回环的限制，在因子图中被一起优化。最后，利用优化的IMU偏差项传播IMU测量值，以IMU速率进行位姿估计。
我们工作的主要贡献总结如下：
- 在因子图的基础上构建紧密耦合的LVIO框架，在重定位的辅助下实现多传感器融合和全局优化；
- 我们的框架通过故障检测绕过故障子系统，使其对传感器退化具有鲁棒性；
- 我们的框架通过不同规模、平台和环境收集的数据进行了广泛的验证。
二、激光雷达视觉惯性里程计平滑和建图
A. System Overview
我们的视觉惯性系统的框架，该系统优化了IMU积分、无深度视觉测量和有深度视觉测量的残差。
视觉里程计是通过最小化视觉和IMU测量的联合残差来获得的。LIS提取激光雷达特征，并将提取的特征与特征地图匹配，实现激光里程计。特征地图以滑动窗口的方式维护，以实现实时性能。最后，将状态估计问题转化为最大后验(MAP)问题，在因子图中共同优化IMU预积分约束、视觉里程计约束、激光雷达里程计约束、以及回环约束。需要注意的是，LIS中采用的多传感器图优化是为了减少数据交换，提高系统效率。
B. Visual-Inertial System
使用角点检测器检测视觉特征，并通过 Kanade–Lucas–Tomasi 算法实现跟踪。在VIS初始化后，我们使用视觉里程计配准激光雷达帧，并获得稀疏深度图像用于特征深度估计。
系统在一个滑动窗口中进行 bundle adjustment ，这里的系统状态 x ∈ X x \in X x∈X可以被写成：
x = [ R , p , v , b ] x=[R,p,v,b] x=[R,p,v,b] R ∈ S O ( 3 ) R \in \mathcal {SO}(3) R∈SO(3)是旋转矩阵， p ∈ R 3 p \in \mathbb R^3 p∈R3是位置向量， v v v是速度， b = [ b a , b w ] b=[b_a,b_w] b=[ba,bw]是IMU的偏置， b a b_a ba和 b w b_w bw分别为加速度和角速度的偏置向量。
变化矩阵 T ∈ S E ( 3 ) T \in \mathcal {SE}(3) T∈SE(3)是从传感器坐标系 B B B 到视觉坐标系 W W W，被表示为 T = [ R ∣ p ] T=[R|p] T=[R∣p]。在接下来的章节中，我们给出了改进VIS初始化和特征深度估计的详细步骤。
详细内容可参考：VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator
1）Initialization
基于优化的VIO通常由于在初始化时解决高度非线性问题而产生分歧。初始化的质量在很大程度上取决于两个因素：传感器的初始运动和IMU参数的准确性。
在实际应用中，我们发现当传感器以较小或恒定的速度运动时，往往无法初始化，这是由于当加速度不够大时，度量尺度是无法被观测的。
IMU参数包括缓慢变化的偏置和白噪声，这两个会同时影响原始加速度和角速度测量。在初始化时对这些参数进行良好的猜测有助于优化更快地收敛。
为了提高我们的VIS初始化的鲁棒性，我们利用了来自LIS的估计系统状态 x x x和IMU偏差 b b b。因为深度可以通过激光雷达直接观测到，因此我们首先初始化LIS，得到 x x x和 b b b。然后根据图像时间戳将它们插值并关联到每个图像关键帧。注意，在两个图像关键帧之间IMU的偏差被假定为是恒定的。最后，利用LIS估计出的 x x x和 b b b作为VIS初始化的初始猜测，这大大提高了初始化速度和鲁棒性。
2) Feature depth association
在初始化VIS时，我们用估计的视觉里程计将激光雷达帧与相机帧进行配准。由于现在的3D激光雷达通常进行稀疏扫描，我们堆叠多个激光雷达帧，以获得一个密集的深度地图。为了将特征与深度值相关联，我们首先在摄像机中心的单位球体上投射视觉特征和激光雷达深度点。深度点然后被向下采样，并在球体上使用它们的极坐标进行恒定密度的存储。
我们利用视觉特征的极坐标搜索二维K-D树来找到球面上最近的三个深度点。最后，特征深度是视觉特征与相机中心 O c O_c Oc所形成的直线的长度，该直线与三个深度点在笛卡尔空间中所形成的平面相交。这个过程的可视化可以在下图(A)中找到，其中特征深度是虚线的长度。
我们通过检查三个最近深度点之间的距离来进一步验证相关的特征深度。这是因为叠加来自不同时间戳的激光雷达帧可能会导致来自不同物体的深度模糊。上图(b)中显示了这种情况的说明，在时间 t i t_i ti时观察到的深度点用绿色表示，当摄像机移动到 t j t_j tj的新位置，观测到新的深度点为灰色。用灰色虚线圈出来的在 t i t_i ti时刻观测到的深度点，由于激光帧的叠加，在 t j t_j tj处仍可观察到。利用不同目标的深度点关联特征深度会导致估计不准确。我们通过检查特征深度点之间的最大距离来拒绝这种估计。如果最大距离大于2m，则特征点没有深度关联。
配准深度图和视觉特征的演示如上图所示，配准深度地图和视觉特征。(a)和(c）中，深度图的颜色变化表示深度变化。(b)和
(d)绿点是与深度成功关联的视觉特征。深度关联过程失败的特征显示为红色。
在上图(a)和(c）中，将视觉里程计配准的深度点投影到摄像机图像上，在上图 (b)和(d)中，与深度成功关联的视觉特征被着色为绿色。尽管深度图覆盖了上图(a)中的大部分图像，但由于验证检查失败，(b)中位于窗口角落的许多特征缺乏深度关联。
3) Failure detection
由于剧烈的运动、照明变化和无纹理的环境，导致VIS故障。当机器人进行攻击性运动或进入无纹理环境时，跟踪特征的数量大大减少。特征不足可能导致优化分散。我们还注意到，当VIS失败时，有很大的IMU偏差估计。
因此，当跟踪的特征数低于一个阈值时，或估计的IMU偏差超过阈值时，我们报告VIS故障。主动的故障检测对于我们的系统是必要的，这样它的故障就不会破坏LIS的功能，一旦检测到故障，VIS重新初始化并通知LIS。
4) Loop closure detection
我们利用DBoW2进行闭环检测。对于每个新的图像关键帧，我们提取BRIEF描述符，并将它们与之前提取的描述符匹配。DBoW2返回的回环候选对象的图像时间戳被发送到LIS进行进一步验证。
C. Lidar-Inertial System
激光雷达惯性系统的框架。系统维护的因子图有四种类型的约束：IMU预积分约束、视觉里程计约束、激光雷达里程计约束、闭环约束。激光雷达里程计约束由 Scan-Matching 得到，我们将当前的激光雷达关键帧与全局特征地图相匹配。闭环约束的候选帧首先由VIS提供，然后通过 Scan-Matching 进一步优化。
我们为特征地图维护了一个激光雷达关键帧的滑动窗口，这保证了有限的计算复杂度。当机器人姿态变化超过阈值时，被选择为新的激光雷达关键帧，在关键帧对之间的激光雷达帧被丢弃。在选择一个新的激光雷达关键帧后，就把一个新的机器人状态 x x x作为节点添加到因子图中。以这种方式添加关键帧不仅实现了内存消耗和地图密度之间的平衡，而且也有助于维护一个相对稀疏的因子图，用于实时优化。
详细内容可参考：LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing andMapping
1) Initial guess
我们发现初始猜测对 Scan-Matching 的成功与否起着至关重要的作用，特别是当传感器经历攻击性运动时。初始猜测的来源在LIS初始化前后是不同的。
在LIS初始化之前，我们假设机器人从零速度的静态位置开始。然后我们对原始IMU测量值进行积分，假设偏差和噪声是零值。积分得到两个激光雷达关键帧之间的平移和旋转变化，产生扫描匹配的初始猜测。我们发现这种方法可以成功地初始化系统在特殊条件下，即当初始线速度低于10m/s，角速度低于180°/s。一旦LIS被初始化，我们就可以在因子图中估计IMU偏置，机器人位姿和速度。然后我们将它们发送到VIS以帮助其初始化。
在LIS初始化后，我们可以从两个来源获得初始的猜测：伴随矫正偏差的IMU测量值积分，和VIS。当视觉惯性里程计可用时，我们使用它作为最初的猜测，如果VIS报告故障，我们就切换到IMU测量来进行初始猜测。这些程序提高了初始猜测的准确性和鲁棒性，无论是在纹理丰富和无纹理环境。
2) Failure detection
虽然激光雷达可以在远距离捕捉环境的细节，它仍然会遇到扫描匹配受限的情况，例如激光雷达处于一个平坦、开放、无结构的环境中。Scan-Matching 的非线性优化问题可以表述为迭代求解线性问题：
m i n T ∣ ∣ A T − b ∣ ∣ 2 \underset{T}{min}||AT-b||^2 Tmin∣∣AT−b∣∣2 A A A和 b b b是在 T T T处线性化得到的，在优化的第一次迭代中，当 A T A A^TA ATA的最小特征值小于某一阈值时，LIS报告失败。故障发生时，激光雷达里程计约束不添加到因子图中。
关于这些假设所基于的详细分析，请参阅：On Degeneracy of Optimizationbased State Estimation Problems