[论文笔记|VIO]ICE-BA: Incremental, Consistent and Efficient Bundle Adjustment for Visual-Inertial SLAM

摘要

由于视觉特性和惯性测量的互补性，视觉-惯性SLAM (VI-SLAM)比纯视觉SLAM具有更高的精度和鲁棒性。然而，联合使用视觉和惯性测量来优化SLAM目标函数是一个高计算复杂度的问题。在许多VI-SLAM应用中，传统的优化求解器只能使用非常有限的最新测量数据进行实时姿态估计，而代价是局部定位精度不够理想。在这项工作中，我们更新了VI-SLAM的数值求解器。与传统的求解方法相比，我们的方法提供了一种计算效率显著提高的精确解。我们的求解器允许我们使用大量的测量数据来获得更高的精度和鲁棒性。此外，我们的方法解决了许多先进的SLAM系统无法解决的全局一致性问题:保证回环过程中最小化重投影函数和惯导约束函数。实验结果表明，与其它方法相比，该方法具有较低的定位误差和10倍以上的加速速度。另外，发布了实现的源代码。

1. 介绍

与纯视觉SLAM类似，VI-SLAM提取并建立图像帧间的特征匹配，但它进一步利用惯性测量值(如加速度和角速度)作为运动估计的约束条件。惯性测量对于运动估计，特别是在运动速度快且不规则的情况下，是非常有效的，但这对视觉特征匹配来说是一个挑战。当有足够的计算能力时，OKVIS、VIORB通过使用大量的测量数据，在6自由度(DOF)精度方面显示出优异的结果。

VI-SLAM的计算复杂度值得关注，提高VI-SLAM的计算效率是推广其应用的关键。在VI-SLAM中有两个主要的计算任务:前端任务和求解器(solver)任务。前端任务包括视觉特征提取和匹配。前端任务通常是并行的，因此可以使用现代异构计算体系结构高效地完成。
solver任务的目标是通过最小化给定的一组带有视觉特征和惯性测量的VI-SLAM目标函数来优化位姿参数。solver任务通常是VI-SLAM的速度瓶颈。

以前的大多数VI-SLAM框架只是简单地使用传统的数值求解器来求解目标函数。Bundle adjustment (BA)是只给出视觉测量的求解器任务的一个例子。在这项工作中，我们对BA进行扩展来表示视觉和惯性测量的联合优化。一些传统的解决方案，如Gauss-Newton和Levenberg-Marquardt，其设计目的是在不考虑实时问题的情况下提供精确的数值结果。因此，基于这些求解器的实时VI-SLAM应用只能使用最近的测量来估计最近的位姿(即在局部BA中应用一个非常小的滑动窗口)。理论上，较长时间的测量值能帮助得到较高的估计精度。BA的效率显然是影响VI-SLAM性能的最关键因素之一。

我们改进了VI-SLAM的BA过程，极大地提高了局部和全局优化效率，解决了闭环过程中的不一致性问题。在SLAM问题中，传入的视觉和惯性测量按顺序到达。我们利用这一事实，有效地重用以前优化的中间结果，以避免冗余计算。该算法可显著提高求解器的速度，适用于大多数基于滑动窗口的VI-SLAM。

此外，我们的方法解决了全局一致性问题。如果可以循环可以闭合，并且重投影误差足够小，则全局映射被认为是一致的。对于visual SLAM，可以通过运行全局BA或其位姿图近似来维持全局一致性。然而，对于VI-SLAM，问题要复杂得多，速度和帧间IMU偏置的约束在优化问题中产生了许多局部极小值。当从时间滑动窗口中移除测量值时，初始边缘化会随着时间累积误差，最终会与loop约束冲突。以前的方法要么跳过边缘化(VIORB)，要么在不解决冲突的情况下(VINS-MONO)应用边缘化。

本文提出了一种新颖的视觉惯性SLAM求解算法，其贡献如下:

1. 提出一种新的基于滑动窗口的求解器，利用SLAM测量的增量特性，可得到比现有技术高10倍的效率提升。
2. 提出一种新的相对边缘化算法，可以解决滑动窗口边缘化偏差和全局回环检测约束之间的冲突。
3. 开源了本文算法

2. Related Work

① 早期的SLAM多基于EKF (Extended Kalman Filter)。6自由度运动参数和三维路标被概率地表示为单个状态向量。经典EKF的复杂度随着路标数量的增加呈二次增长，这限制了它的可扩展性。
② Visual SLAM只使用视觉特性来解决SLAM问题。通过提取和匹配大量复杂的视觉特征，这些方法能够提供较高的跟踪精度。
③ Visual-Inertial SLAM由于惯性测量(角速度和加速度)提供了额外的约束条件，因此通常不需要大量的图像特征来达到合理的精度。

eg1. 通过从状态向量中排除3D路标来改善早期EKF SLAM。因此，它们能够对状态中的多个帧进行建模。但是作为EKF算法的一种常见行为，它只保持最近的状态，因此对测量误差很敏感，很难从不稳定的跟踪状态中恢复。
eg2. (OKVIS、VINS-MONO、VIORB)使用时间滑动窗口选择最新的视觉和惯性测量值，以优化SLAM目标函数。

经实验表明，在许多情况下，基于滑动窗口的VI-SLAM比基于滤波的方法更鲁棒、更准确。然而，目标函数优化具有较高的计算复杂度。基于滑动窗口的VI-SLAM的性能在很大程度上依赖于计算的可用性，这在移动设备和无人机上受到了严格的限制。我们提出的新方法旨在通过极大地提高优化求解器的效率来解决这一问题。

④ Optimization Solvers通常由各种SLAM实现共享，尽管它们的前端系统和框架非常不同。visual SLAM的BA利用了重投影函数和Hessian的稀疏结构。在这篇论文的工作中，BA是有广义含义的，以表示VI-SLAM的视觉和惯性测量的联合优化。以前的BA方法都存在一个问题，即它的复杂度随着相机的数量呈二次增长。因此，基于这些解算器的SLAM系统只能使用非常有限的最近测量值来进行实时姿态估计。

⑤ 最近，研究人员正在尝试使用增量求解器来尝试利用先前的优化结果来减少新计算量。

Kaess等提出了通过测量矩阵的QR分解来求解优化问题。每个新的优化迭代仅更新分解结果的一小部分，而不是分解整个图。

Ila等人提出逐步恢复估计和协方差，并提出逐步更新BA中的Schur补。

然而，上述方法只适用于解决稀疏相机问题(即大多数关键点只在少数相机中可见)。虽然适用于大型structure-from-motion,但在SLAM问题中，局部滑动窗口中的大多数帧共享大量公共点，这会将增量求解器退化为常规BA求解器。因此，他们并没有表现出比其他先进的SLAM更好的定位精度。

在这项工作中，这篇论文提出了一种新的增量求解器，它更好地利用了SLAM中的特定块矩阵结构，并在速度和精度方面表现出优越的性能。作为对我们早期工作的一个主要扩展，本文进一步讨论了局部BA的加速和相对边缘化对全局一致性的影响。我们还提供了更多的实验结果。据我们所知，本文描述了第一个基于BA的VI-SLAM求解器，实现了前所未有的效率和最先进的精度，同时确保了全局一致性。

3. Framework

我们首先定义约束函数，然后解释我们的局部和全局优化框架。

3.1. Constraint Functions

VI-SLAM的目标是利用在时间点$t$之前的视觉和惯性测量来估计运动状态$C_t$以及一组3D点 { X 1 , ⋅ ⋅ ⋅ , X n t } \{X_1, · · · , X_{nt}\} {X1​,⋅⋅⋅,Xnt​}。

运动状态：

相机姿态：

IMU状态：

将三维点$X_j$投影到对应于二维特征测量的第$i$个图像平面上，有

高斯白噪声：

利用逆深度对3D点进行参数化:

${\rho }_j$：第j个点的逆深度；$s_j$是提取第j个点的源帧,$x¯$就是x的齐次坐标

视觉约束定义为:

在第i帧和第j帧之间得到的IMU测量值$Z_{ij}$提供了相对运动约束。IMU约束定义为:

更多IMU约束的细节在这儿

3.2. Local and Global Optimization

在求解一个长时间的VI-SLAM问题时，只进行全局优化是不可行的。我们的框架(图2)包含了两个局部优化(局部BA)和全局优化(全局BA)。

1. 局部BA在一个只包含最新帧和点的临时滑动窗口中优化状态。local BA的目标是减少累积误差，并尽可能快地扩展地图。局部BA的代价函数是最小化：

$t_0=t-n+1$是滑动窗口的第一帧，n是滑动窗口的大小。$V_i$表示在第i帧中跟踪的点的集合。作为我们的主要贡献之一，第4节解释了如何有效地求解Eq.(3)。

2. 全局BA以相对较低的频率与本地BA并行运行。Global BA优化了从局部滑动窗口移除但在Global map中被选择为关键帧的帧。在全局BA中，如果它携带超过N个（实验中为20）从所有其他帧中看不到的特征，则该帧在全局BA中被选择作为关键帧。全局BA的代价函数为：
   其中$L_i$为第i个相对位姿约束所涉及的关键帧集合。Loop closure触发全局BA，该BA应考虑映射一致性。对于一个典型的循环约束，$|L_i$| = 2。

3. Relative Marginalization
   相对边缘化在局部BA的最后一个关键帧和从局部BA中删除的最后一个关键帧之间产生相对位姿约束,(如图2中$C_{k_{m-1}}$与$C_{k_m}$之间的约束关系)，以便从全局BA获得约束(比如闭环)可以帮助将相机位姿固定在局部BA中，防止因累积误差引起的漂移。(见后续5)

4. Efficient Solver for VI-SLAM

4.1. General Incremental BA Solver

在VI-SLAM的全局优化中，每个代价函数fk仅涉及非常小的变量子集。利用这种稀疏模式和块结构可以得到（6）的有效构造。此外，由于SLAM问题的性质，新状态和测量总是递增地到达，在每次迭代中只有一小部分变量发生变化，即只有一小部分fk需要重新线性化。可以利用这一事实来显著加速（6）的构建。早期工作不是在每次迭代中从头计算（6），而是逐步更新[A|b]：


其实就是marg掉点后对应的信息矩阵，用舒尔补表示。

增量更新$[S_{i_1,i_2}|s_i]$：

其中P是代价函数中涉及的点集，需要重新线性化。

共轭梯度法的思想就是找到 n 个两两共轭的共轭方向，每次沿着一个方向优化得到该方向上的极小值，后面再沿其它方向求极小值的时候，不会影响前面已经得到的沿哪些方向上的极小值，所以理论上对 n 个方向都求出极小值就得到了 n 维问题的极小值。在 n 维的优化问题中，共轭梯度法最多 n 次迭代就能找到最优解（是找到，不是接近）。
虽然 CG 方法可以减小求解时间和内存占用，但是缺点是矩阵 A 的条件数会很大程度上影响收敛速度。为了减小条件数，需要对 A 进行预处理，即得到预处理共轭梯度法 PCG。

【PCG算法改进——I-PCG】见4.3部分

PCG算法1
PCG算法2

4.2. Improvement for Local BA

第4.1节中介绍的增量BA (IBA)可以显著加速全局BA，其中大多数关键帧不共享公共点。然而，在局部BA中，大多数点可以被滑动窗口中的大多数帧观察到。因此，(10)中定义的$[S_{i_1{i}_2}^j|s_i^j]$的很大一部分必须重新评估，并且Schur补的增量更新则退化为标准过程。图3显示了此IBA流程(原始IBA或O-IBA)的运行时间。Schur补的更新在整个运行时中占主导地位。

图3:MH 01 easy sequence中每一帧的总运行时间和Schur补充时间。O-IBA是第4.1节中介绍的原始IBA;ST-IBA是第4.2节中介绍的基于子轨道的IBA。在400到900帧之间，O-IBA时间显著增加，因为在这段时间内更多的帧共享相同的特征点。ST-IBA并没有受到预期的影响。

我们提出了一种改进的增量BA求解器来解决局部BA中的Schur补问题——基于子轨道的IBA (ST-IBA)。其核心思想是将原始长特征轨迹$X_j$分割为几个短的重叠子轨迹$X_{j_1}、X_{j_2}$、···，如图4所示

每个子轨道$X_{j_k}$跨越$l$个相邻的框架，$l<|X_j|$。我们在实验中设$l$ = 5。子轨道还包括局部BA中的关键帧。相应的逆深度${\rho }_j$成为几个相同的重复值${\rho }_{j_1}{\rho }_{j_2}$,···。因此，只要有一个|$X_j$ |达到滑动窗口的大小n,S就从一个密集的全矩阵变成一个对角带状矩阵。进一步，$[S_{i_1,i_2}|s_i]$(11)的增量更新变为:

其中P¯就是成本函数中需要重新线性化的子轨迹点集合， $V{¯}_{i_1{i}_2}^j$表示帧$(i_1,i_2)$中与第j个点对应的公共子轨迹点集合。与(11)相比，(13)的效率更高，不仅是因为S变得更稀疏，而且由于短子轨道参与再线性化的概率很低，所以$P{¯}_{i_1{i}_2}$通常比$P_{i_1{i}_2}$小很多。

(13)中定义的$[Q{¯}_{i_1{i}_2}^j|q{¯}_i^j]$可以进行增量更新以进一步加速:

表1:整个EuRoC数据集的平均RMSE和建议方法的运行时。固定线性化点和ST-IBA可以在不牺牲精度的前提下显著提高效率。

4.3. Incremental PCG for IBA

为了求解(9)，我们对原PCG算法进行了改进。在标准PCG中$\delta {\phi }_c$初始化为0,然后迭代更新向最优值。在IBA的情况下,如果$\delta {\phi }_{c_i}$不够大,最小值$\delta {\phi }_{c_i}$不会更新相机$i$的状态(Sec. 4.1)。

对于这样的摄像机$i$，下一个迭代的结果$\delta {\phi }_{c_i}^{+}$将非常接近前一个$\delta {\phi }_{c_i}^{-}$，因为两个结果都是通过将相同的$\delta {\phi }_{c_i}^{-}$更新为类似的最佳值而获得的。 这一观察有助于我们更好地初始化$\delta {\phi }_{c_i}$​并加速PCG的收敛。

对于状态在最后一次迭代中没有改变的摄像机$i$:初始化$\delta {\phi }_{c_i}^{+}=\delta {\phi }_{c_i}^{-}$，其余的仍然使$\delta {\phi }_{c_i}^{+}=0$

我们将此算法命名为增量PCG（I-PCG），因为它还利用了SLAM测量的增量特性。 如表1所示。由于更好的收敛, I-PCG提高精度约 20％。

5. Relative Marginalization

如果局部BA的滑动窗口帧数超过阈值(如我们的实验中为50)，则需要消除滑动窗口中最早的帧$t_0$。边缘化并没有忽略这一被移除的帧所承载的信息，反而将其转化为线性先验信息应用于其余变量。 边缘化通常用于VIO(没有全局映射) 然而，在VI-SLAM的情况下，误差累积将逐渐破坏边缘化产生的先验。从滑动窗口生成的不好的先验最终将与全局映射和闭环约束产生冲突，进而降低整体精度。

我们的主要贡献之一是维持边缘化先验和具有相对边缘化的全局BA之间的一致性，关键思想是相对于参考关键帧坐标系建立先验，而不是全局坐标系。 它类似于视觉SLAM的相对BA，其中所有参数都以相对坐标表示，以避免在闭环时调整所有参数。 相比之下，我们使用边缘化的相对表示。 此外，由于重力方向变得可观察，因此VI-SLAM的相对表示更复杂。





注【 Marginalization 中的𝑒𝑝′证明】

𝑒𝑝等式左边的推导：

我们用图5中的详细描述来说明相对边缘化过程。

通常的边缘化方法边缘化状态时参考坐标系是世界坐标系，提供的先验相当于告诉优化器我当前在世界坐标系的哪里，虽然保留了边缘化的状态的信息，但由于累计误差的存在，提供的先验可能是不正确的，会与闭环约束产生冲突，因此ICE-BA提出了相对边缘化的思路，实现也很简单，其实就是将边缘化状态的参考坐标系变为和要边缘化的这帧最近的关键帧，而不是世界坐标系，这样提供的先验信息就是相对于这个关键帧的信息，不再是全局的了，就不会存在上面提到的冲突的问题。边缘化的是窗口中的最老帧，利用相对边缘化，这将对GBA中与最老帧有共视关系的所有keyframe有产生一个线性约束。边缘化位姿（通常还需要再边缘化路标点，否则hession矩阵会变得很稠密，因为相关的路标点之间也多了一些先验，这里只考虑对位姿的先验约束，相当于也边缘化了路标点），如下图所示，这个相对边缘化约束是加载位姿之间的，注意，这个约束是对GBA的约束。

通过上述分析，可以得到如下边缘化过程，下面有详细的解释，(a)就是$g_0$为参考帧，边缘化$M_0$，在$T_1、M_1、g_0$之间生成一个先验约束，如果下面要边缘化的$T_1、M_1$不是关键帧，则直接边缘化掉即可，产生新的先验约束，如（b）图所示。如果要边缘化的是新的关键帧$T_{t_0}$的话，此时参考帧是$g_{k_0}$,对于全局优化，边缘化状态$M_{t_0}$和先验因子,会在GBA中产生关于${[}^{k_i}{T}_{t_0},g_{k_0}{,}^{k_i}{T}_{t_i}]$的相对约束，即相对边缘化因子。对于局部优化来说，需要换一下参考帧，由$g_{k_0}$变为$T_{t_0}$,分为两步，先边缘化$[g_{k_0}{,}^{k_0}{T}_{t_0}{,}^{k_0}{T}_{t_i}]$，生成$M_{t_0}$的先验，然后将参考系变为$g_{t_0}$,然后类似(a)中边缘化掉$M_{t_0}$即可，这样就完成了参考坐标系的变更。

ICE-BA整体优化流程：

GBA中包含所有的关键帧，优化之后，会将优化结果传给LBA，LBA得到更新的关键帧位姿后，固定住这些关键帧位姿进行局部窗口优化，因为GBA采用增量求解方式，相对于batchBA快很多，所以可以这样操作。

6. 评估

为了评估我们提出的求解器，我们构建了一个SLAM系统，该系统由提出的求解器，视觉测量的前端和闭环检测器组成。 前端检测Harris特征，使用光流跟踪法建立帧间特征轨迹，并使用直接匹配法匹配双目帧上的特征。 我们的闭环检测器存储关键帧的词袋特征，用于闭环检测。 一旦检测到闭环，我们使用匹配帧之间的相对位姿和协方差作为全局BA中的相对约束。

我们使用EuRoC数据集进行定量评估，并在许多具有挑战性的环境中进行定性比较。在所有实验中，滑动窗口大小设置为50。

表1：整个EuRoC数据集的建议方法的平均RMSE和运行时间。 固定线性化点和ST-IBA可在不牺牲精度的情况下显着提高效率。 I-PCG由于更好的收敛而降低了RMSE，但由于我们设置了最小的迭代次数，因此没有计算时间。 相对边缘化提高了预期的准确性和效率，因为额外的约束加速了收敛。

表3：使用Intel i7 CPU的局部/全局BA（LBA / GBA）与EuRoC数据集的运行时间（ms）的比较。

Google tango是一款商用设备，经过高度优化，可实现强大而准确的运动跟踪。 我们将立体声SLAM系统与Tango Phab 2进行比较，如图6所示。没有闭环，我们的系统显示比Tango的比较轨迹和更准确的比例。 通过闭环，我们的系统始终优于Tango。

图6：我们系统和Google tango的轨迹。 理想情况下，轨迹的最终位置应与初始位置相同。

7、结论

在本文中，我们提出了一种新的VI-SLAM优化算法，利用稀疏性和独特的矩阵结构优化基于滑动窗口的BA。 此外，提出了一种新的相对边缘化来提高全局一致性。 实验证明，我们的方法不仅可以显着加速优化过程，还可以提供比其他最先进的SLAM方法以及商业系统更低的姿态估计误差。