【文献翻译】ICE-BA: Incremental, Consistent and Efficient Bundle Adjustment for Visual-Inertial SLAM

前言

  2018年CVPR出现了一篇论文，该篇论文较新，引起了博主的注意。博主译文水平不好，为了学习，所以进行论文翻译和研究，博文以译文为主。如果有翻译不对之处，还请各位指出，便于我修改。

文献信息

引用格式：Haomin Liu, Mingyu Chen, Guofeng Zhang, Hujun Bao and Yingze Bao. ICE-BA: Incremental, Consistent and Efficient Bundle Adjustment for Visual-Inertial SLAM. IEEE Conference on Computer Vision and Pattern Recognition (CVPR), 2018.
所属级别：CCF A类
发表期刊：CVPR
发表年份：2018
原文链接：ICE-BA: Incremental, Consistent and Efficient Bundle Adjustment for
Visual-Inertial SLAM

github代码：https://github.com/ZJUCVG/ICE-BA

原始版本代码：https://github.com/ZJUCVG/EIBA

---

【文献翻译】CVPR2018论文：ICE-BA：视觉惯性SLAM的增量，一致和高效的束调整

摘要

  由于视觉特征和惯性测量的互补性，现代视觉惯性SLAM（VI-SLAM）比纯视觉SLAM具有更高的精度和鲁棒性。然而，联合使用视觉和实际测量来优化SLAM目标函数是高计算复杂性的问题。在许多VI-SLAM应用中，传统的优化求解器只能使用非常有限数量的近期测量来进行实时姿态估计，代价是次优的定位精度。在这项工作中，我们翻新了VI-SLAM的数值求解器。与传统解算器相比，我们的提议提供了具有显着更高计算效率的精确解决方案。我们的求解器允许我们使用非常多的测量值来实现更高的精度和鲁棒性。此外，我们的方法解决了许多最先进的SLAM系统未解决的全局一致性问题：保证在回环检测期间最小化重投影函数和惯性约束函数。实验证明，与其他方式相比，我们的新方法可以降低定位误差并提高10倍以上的速度。我们发布实施的源代码以使社区受益。

1、引言

  同步定位和建图（SLAM）是许多应用中经典但持续的研究问题。 近年来，由于成像和惯性传感器的可用性大大增强，视觉惯性SLAM（VI-SLAM）越来越多地应用于移动增强现实，无人机，自动驾驶，机器人等产品。类似于纯视觉SLAM，VI -SLAM提取并建立跨图像帧的特征对应关系。 但它进一步利用惯性测量（例如加速度和角速度读数）作为运动估计中的约束。 惯性测量对于运动估计非常有效，尤其是当运动快速且不规则时，这对于视觉特征匹配而言是众所周知的挑战。 在给定足够的计算能力的情况下，通过使用大量测量数据，现有技术的VI-SLAM [22,26]在6自由度（DOF）精度方面表现出优异的结果。

  由于SLAM的大多数应用都是可移动的和注重时间效率的，因此VI-SLAM的计算复杂性也值得关注。 只有少数VI-SLAM应用[24,28,11]可以部署到嵌入式设备上。 提高VI-SLAM计算的效率不可避免地是普及其应用的关键。 VI-SLAM中有两个主要的计算任务：前端任务和求解器任务。 前端任务包括视觉特征提取和匹配。 前端任务通常是可并行化的，因此可以使用现代异构计算架构有效地完成。 求解器任务的目标是通过在给定一组视觉特征和惯性测量的情况下最小化VI-SLAM目标函数来优化位姿参数。 求解器任务通常是VI-SLAM的速度瓶颈。

  大多数以前的VI-SLAM框架只是应用传统的数值求解器来解决目标函数。 捆绑调整（BA）是仅给出视觉测量的求解器任务的示例。 在这项工作中，我们将术语BA推广为表示视觉和惯性测量的联合优化。 这些传统求解器，如Gauss-Newton和Levenberg-Marquardt，旨在提供数字上准确的结果，而无需考虑实时问题。 因此，基于这些求解器的实时VI-SLAM应用[24,7,22]仅能够使用最近的测量来估计最新的设备位姿（即，在局部BA中应用非常短的滑动窗口）。 从理论上讲，较长的测量历史可以提高估算精度。 BA的效率显然是VI-SLAM性能最关键的因素之一。

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

  此外，我们的方法解决了全局一致性问题，这对AR等应用程序至关重要。 如果可以闭环关闭并且重新投影误差被充分最小化，则认为全局地图是一致的。 对于视觉SLAM，可以通过运行全局BA或其位姿图近似来维持全局一致性[9,25]。 然而，对于VI-SLAM来说问题更加复杂，其中帧之间的速度和IMU偏差的约束在优化问题中产生许多局部最小值。 当从时间滑动窗口中移除测量值时，边缘化误差会随着时间累积，这最终会与循环约束发生冲突。 以前的方法要么跳过边缘化[26]，要么在不解决冲突的情况下应用边缘化[28]。

  本文提出了一种新的可视化SLAM求解器算法，其贡献如下：一种新的基于滑动窗口的求解器，利用SLAM测量的增量特性，与现有技术相比，效率提高了10倍以上; 一种新的相对边缘化算法，可以解决滑动窗口边缘化偏差和全局闭环约束之间的冲突; 我们经过实验验证的实现将是开源的。

---

2、相关工作

  早期的SLAM主要是基于EKF（扩展卡尔曼滤波器）[6,8]。 6个DOF运动参数和3D地标概率地表示为单个状态向量。 经典EKF的复杂性随着地标的数量呈二次方式增长，限制了它的可扩展性。

  Visual SLAM [20,25,9]仅使用视觉特征解决了SLAM问题。 通过仔细提取和匹配大量复杂的视觉特征，这些方法能够提供高跟踪精度。

  视觉惯性SLAM通常不需要大量的图像特征来实现合理的精度，因为惯性测量（角速度和加速度）提供了额外的约束。 [24,32]通过从状态向量中排除3D地标来改进早期的EKF SLAM。因此，它们能够在该状态下对多个帧建模。然而，作为EKF算法的常见行为，它们仅维持最近的状态，因此它们对测量误差敏感并且难以从不稳定的跟踪状态恢复。 [7,22,28,26]使用时间滑动窗口来选择最近的视觉和惯性测量以优化SLAM目标函数。他们表明，在许多情况下，基于滑动窗口的VI-SLAM比基于过滤器的方法更加稳健和准确。然而，目标函数优化具有高计算复杂度。基于滑动窗口的VI-SLAM的性能高度依赖于计算可用性，其严格限制在移动设备和无人机上。我们提出的新方法旨在通过大大提高优化求解器的效率来解决这个问题。

  优化求解器通常由各种SLAM实现共享，尽管它们的前端系统和框架非常不同。 视觉SLAM的BA [30]利用了重投影函数和Hessian的稀疏结构。 在这项工作中，术语BA被概括为表示VI-SLAM的视觉和惯性测量的联合优化。 [2,4,31]提高了BA在大规模设置中的效率。 [17]表明，基于块的预处理共轭梯度（PCG）可用于求解Schur补集以提高效率。 BA [1,21]也有出色的设计实现，这些实现通常由最先进的SLAM系统使用。 然而，所有这些方法都受到以下事实的影响：其复杂性随着摄像机数量的增加而呈二次方式增长。 因此，建立在这些求解器上的SLAM系统只能使用非常有限数量的近期测量来进行实时姿态估计。

  最近，研究人员正在尝试使用增量求解器来尝试利用先前的优化结果来减少新计算量。 Kaess等人 [19,18]提出通过测量矩阵的QR分解来解决优化问题。每个新的优化迭代仅更新分解结果的一小部分，而不是分解整个图。同样，Ila等人[16]建议逐步恢复估计和协方差，并最近建议在BA中逐步更新Schur补集[15]。然而，上述方法仅适用于解决“稀疏”相机问题（即，大多数关键点仅在少数相机中可观察到）。虽然这对于大规模的运动结构是正确的，但在SLAM问题中，局部滑动窗口中的大多数帧共享大量公共点，这将这些增量求解器退化为常规BA求解器。因此，它们没有比其他状态SLAM更好的定位精度。在这项工作中，我们提出了一种新颖的增量求解器，它可以更好地利用SLAM中的特定块矩阵结构，并在速度和精度方面表现出卓越的性能。作为我们早期工作的主要延伸[23]，本文进一步讨论了局部BA的加速和全局一致性的相对边缘化。我们还提供了更多的实验结果。据我们所知，本文描述了第一个基于BA的VI-SLAM求解器，以最先进的精度实现了前所未有的效率，同时确保了全局一致性。

3、框架

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

3.1约束函数

  视觉惯性SLAM的目标被定义为使用直到时间点$t$的视觉和惯性测量来估计运动状态$C_t$，以及一组3D点 { X 1 ， ⋅ ⋅ ⋅ ， X n t } \{{\rm X}_1，···，{\rm X}_{nt}\} {X1​，⋅⋅⋅，Xnt​}。 ${\mathrm{C}}_t=({\mathrm{T}}_t，{\mathrm{M}}_t)$，其中${\mathrm{T}}_t=({\mathrm{R}}_t，{\mathrm{p}}_t)$是相机姿态，并且${\mathrm{M}}_t=({\mathrm{v}}_t，{\mathrm{b}}_t)$是包括速度${\mathrm{v}}_t$和传感器偏差${\mathrm{b}}_t$的IMU状态。 将3D点${\mathrm{X}}_j$投影到对应于2D特征测量${\mathrm{x}}_{ij}=\pi ({\mathrm{T}}_i\circ {\mathrm{X}}_j)+{\mathrm{n}}_{ij}$的第$i$个图像平面上，其中${\mathrm{n}}_{ij}$是高斯噪声${\mathrm{n}}_{ij}\mathcal{N}(0,{\Sigma }_{ij}^{vis})$。 使用逆深度[5]参数化3D点为${\mathrm{X}}_j={\mathrm{T}}_{s_j}^{-1}\circ \frac{1}{{\rho }_j}{\bar{\mathrm{x}}}_{s_{j}j}$，其中${\rho }_j$是第$j$个点的逆深度，$s_j$是从中提取第$j$个点的源帧。 $\bar{\mathrm{x}}$是$\mathrm{x}$的齐次坐标。 视觉约束定义为

$$f_{ij}^{vis}({\mathrm{T}}_i,{\mathrm{T}}_{s_j},{\rho }_j)=\pi ({\mathrm{T}}_i\circ {\mathrm{T}}_{s_j}^{-1}\circ \frac{1}{{\rho }_j}{\bar{\mathrm{x}}}_{s_{j}j})-{\mathrm{x}}_{ij}\sim \mathcal{N}(0,\sum _{ij}^{vis})\text{\hspace{1em}(1)}$$

  在帧$i$和$j$之间获得的IMU测量${\mathcal{Z}}_{ij}$提供相对运动约束。 IMU约束定义为

  $\Delta$和协方差矩阵${\Sigma }_{ij}^{imu}$仅依赖于${\mathcal{Z}}_{ij}$，并且可以在优化之前预先集成。 ${\hat{\mathrm{b}}}_i$是预积分时的偏差估计。 有关详细信息，请参阅[10]。

  在VI-SLAM [14]中，绝对位置和绕重力的偏航角是不可观测的。 对第一相机${\mathrm{C}}_0$施加先验，表示为$f_0^{prior}({\mathrm{C}}_0)\sim \mathcal{N}(0,{\Sigma }_0^{prior})$。

3.2 局部和全局优化

  仅在解决长期VI-SLAM问题时执行全局优化是不可行的。 与[28,26]类似，我们的框架（图2）包括局部优化（局部BA）和全局优化（全局BA）。

图2：局部和全局优化框架

  局部BA优化临时滑动窗口内的状态，该窗口仅包含最新的帧和点。 局部BA的目标是减少累积误差并尽可能快地扩展地图。 局部BA的成本函数是最小化：

  其中$t_0=t-n+1$是滑动窗口中的第一帧，$n$是滑动窗口的大小。 ${\mathcal{V}}_i$表示在帧$i$中跟踪的点集。 作为我们的主要贡献之一，第四部分解释了如何有效地解决方程（3）。

  全局BA以相对较低的频率与局部BA并行运行。 全局BA优化从局部滑动窗口移除但在全局地图中选择为关键帧的帧。 如果它携带超过N个（例如在我们的实验中为20）从所有其他帧中看不到的特征，则该帧在全局BA中被选择作为关键帧。 全局BA的成本函数是

  其中${\mathcal{L}}_i$是涉及第$i$个相对位姿约束的关键帧集合。 闭环会触发应考虑地图一致性的全局BA。 对于典型的循环约束，$|{\mathcal{L}}_i|=2$。作为我们的主要贡献之一，第四部分解释了如何有效地解决方程（4）。

  相对边缘化在局部BA中的最后一个关键帧与从局部BA中移除的最新帧（例如，图2中的$C_{km-1}$和$C_{km}$之间的约束）之间产生相对位姿约束，以便从全局BA获得约束（例如，闭环）可以帮助将相机位姿固定在局部BA中，防止因累积误差引起的漂移。 更多细节将在第五部分中讨论。

4、VI-SLAM的高效求解器

  高效地解决方程式 (3）和（4）是VI-SLAM速度的关键。 最小化这些公式可以概括为$\arg {\min }_{\varphi }{\sum }_k||f_k(\varphi )|{|}^2$。 在典型的高斯牛顿求解器中，$\varphi$最优值是通过优化迭代${\varphi }^{+}={\varphi }^{-}\oplus \delta \varphi$得到, 其中下标 $-/+$表示迭代之前$/$之后的状态，而$\oplus$是流形上的广义加法[10]。 在每次迭代中，成本函数$f_k$以当前估计值${\varphi }^{-}$线性化

  其中${\mathrm{J}}_k=\frac{\partial f_k({\varphi }^{-}\oplus \delta \varphi )}{\partial \delta \varphi }$和${\mathrm{e}}_k=f_k({\varphi }^{-})$分别是雅可比矩阵和误差向量。$\delta \varphi$通过正规方程求解

4.1 一般增量BA解决器

  在VI-SLAM的全局优化中，每个成本函数$f_k$仅涉及非常小的变量子集。 例如，（1）中的$f_{ij}^{vis}$仅涉及3种类型的变量$({\mathrm{T}}_i,{\mathrm{T}}_{s_j},{\rho }_j)$。 然后相应的${\mathrm{A}}_k$和${\mathrm{b}}_k$分别仅具有9和3个非零元素。 利用这种稀疏模式[30]和块结构[17]可以得到（6）的有效构造。 此外，由于SLAM问题的性质，新状态和测量总是递增地到达。 结果，在每次迭代中只有一小部分变量发生变化，即只有一小部分$f_k$需要重新线性化。 可以利用这一事实来显着加速（6）的构建。 在我们的早期工作[23]中，我们不是在每次迭代中从头开始计算（6），而是逐步更新[A | b]。

  其中$\mathcal{L}$是需要重新线性化的成本函数集（即涉及至少一个$|\delta {\varphi }_i|$超过预设阈值），$[\delta {\mathrm{A}}_k|\delta {\mathrm{b}}_k]\triangleq [{\mathrm{A}}_k|{\mathrm{b}}_k{]}^{+}-[{\mathrm{A}}_k|{\mathrm{b}}_k{]}^{-}$。

  对于BA问题，有效求解（（6））的常见策略是边缘化点以获得仅涉及相机的简化线性方程。$\varphi$被重新排序为$\varphi =({\varphi }_c^T,{\varphi }_p^T)$，第一个是摄像头，第二个是点参数。 因此，[A | b]可以写成

  消除第二行以获得仅涉及$\Delta {\varphi }_c$的Schur补集

  对应于$\mathrm{S}$中的$(i_1,i_2)$相机对和在$s$中的第$i$个相机的块可以被有效地计算为

  正如[23]中所介绍的那样，通过逐步更新$[{\mathrm{S}}_{i_1{i}_2}|s_i]$，可以利用SLAM测量的增量到达（the incremental arrival）来加速（10）的构造。

  其中$\mathcal{P}$是成本函数中涉及的点集，需要重新线性化。

  当且仅当$(i_1,i_2)$共享公共点或它们之间具有约束时，${\mathrm{S}}_{i_1{i}_2}$才非零。 这种特殊的稀疏结构可以通过预处理的共轭梯度（PCG）专门用于有效地求解（（9））[2,4,17]。 求解$\Delta {\varphi }_c$后，点变量$\Delta {\varphi }_c$可以通过将$\Delta {\varphi }_c$反向代入（8）的第二行来解决，对于每个点$j$分别

其中${\mathcal{X}}_j$表示看到点$j$的摄像机组。

4.2 局部BA的改进

  第4.1节中引入的增量BA（IBA）可以显着加速全局BA，其中大多数关键帧不共享共同点。 但是，在局部BA中，滑动窗口中的大多数帧可以观察到大多数点。 结果，必须重新评估（10）中定义的大部分$[{\mathrm{S}}_{i_1{i}_2}^j|{\mathrm{s}}_i^j]$，并且Schur补的增量更新降级到标准过程。 图3显示了此IBA过程的运行时（原始IBA或O-IBA）。 Schur补的更新在总运行时间中占主导地位。

图3：MH 01 easy序列中每个帧的总运行时间和Schur补时间[3]。 O-IBA是4.1部分中引入的原始IBA; ST-IBA是在Sec4.2中引入的基于Sub-Track的IBA。 在帧400到900之间，O-IBA时间显着增加，因为在这段时间内更高数量的帧共享相同的特征点。 ST-IBA没有像预期的那样受到影响。

  我们提出了一种改进的增量BA求解器来解决局部BA中的Schur补问题。我们将其称为基于Sub-Track的IBA（ST-IBA）。 关键思想是将原点长特征追踪${\mathcal{X}}_j$分成几个短的重叠子追踪${\mathcal{X}}_{j_1}，{\mathcal{X}}_{j_2}，...$，如图4所示。

图4：我们将（a）中的原始特征轨道${\mathcal{X}}_j$分成（b）中的3个重叠sub-track${\mathcal{X}}_{j_1}，{\mathcal{X}}_{j_2}$和${\mathcal{X}}_{j_3}$，每个跨越$l=3$个相邻帧和关键帧$K$

  每个sub-track${\mathcal{X}}_{jk}$跨越$l$个相邻的帧，其中$l<|{\mathcal{X}}_j|$。 我们在实验中设置$l=5$。 子Sub-tracks还包括局部BA中的关键帧。 相应的逆深度${\rho }_i$变为几个相同的重复值${\rho }_{j_1},{\rho }_{j_2}$。 对于每对$(i_1,i_2)\in {\mathcal{X}}_j\times {\mathcal{X}}_j$,引入非零块Si1i2而不是边缘化？j，我们边缘化${\rho }_{j_k}$，它引入${\mathrm{S}}_{i_1{i}_2}$用于更小的一对对$(i_1,i_2)\in {\mathcal{X}}_{j_k}\times {\mathcal{X}}_{j_k}$。 因此，$\mathrm{S}$变为密集的全矩阵(dense full matrix) - 只要有一个$|{\mathcal{X}}_j|$达到滑动窗口$n$的大小 - 到对角带矩阵。 此外，$[{\mathrm{S}}_{i_1{i}_2}|{\mathrm{s}}_i]$（11）的增量更新变为

  其中$\bar{\mathcal{P}}$是需要重新线性化的成本函数中涉及的sub-track点的集合，并且${\overline{\mathcal{V}}}_{i_1{i}_2}^j$表示对应于第$j$个点的帧$（i_1，i_2）$的公共sub-track点的集合。 与（11）相比，（13）更有效，不仅因为$\mathrm{S}$变得更稀疏，而且因为${\bar{\mathcal{P}}}_{i_1{i}_2}$通常远小于${\mathcal{P}}_{i_1{i}_2}$，因为重新线性化中涉及的短sub-track的概率非常低。 （13）中定义的$[{\overline{Q}}_{i_1{i}_2}^j|{\overline{\mathrm{q}}}_i^j]$也可以逐步更新以进一步加速：

  请注意，sub-track过程仅用于更新Schur补。 在求解Schur补之后，我们针对每个原始点$j$而不是sub-track点$j_k$更新（12）的3D点。 与传统方法相比，由于目标函数完全相同，特别是点替换仍然使用原始正规方程而没有任何近似，再多几次迭代可以使解收敛，最终精度不会降低。 如图3和表一所示。 如图1所示，所提出的ST-IBA比原始IBA快2~10次，没有任何明显的精度损失。

4.3 IBA的增量PCG

  为了解决（9），我们翻新了原始的PCG算法[17]。 在标准PCG中，$\delta {\varphi }_c$被初始化为零，然后迭代地更新为最佳值。 在IBA的情况下，如果不$\delta {\varphi }_{c_i}$够大，则最小化器$\delta {\varphi }_{c_i}$将不会实际更新相机$i$的状态（第4.1节）。 对于这样的摄像机$i$，下一个迭代的结果$\delta {\varphi }_c^{+}$将非常接近前一个$\delta {\varphi }_c^{-}$，因为两个结果都是通过将相同的${\varphi }_{c_i}^{-}$更新为类似的最佳值而获得的。 这一观察有助于我们更好地初始化$\delta {\varphi }_c$并加速PCG的收敛。 具体来说，我们初始$\delta {\varphi }_{c_i}^{+}=\delta {\varphi }_{c_i}^{-}$用于那些状态在最后一次迭代中没有改变的摄像机$i$，并且$\delta {\varphi }_{c_i}^{+}=0$。 我们将此算法命名为增量PCG（I-PCG），因为它还利用了SLAM测量的增量特性。 如表1所示。由于更好的收敛, I-PCG提高精度约 20％。

5、相对边缘化

  如果局部BA的滑动窗口中的帧数超过阈值（例如在我们的实验中为50），则需要消除滑动窗口中的最早帧$t_0$。 边缘化不是忽略这个消除帧中携带的信息，而是将其转换为线性先验应用于剩余变量。 边缘化通常用于视觉惯性测距法（VIO）[24,22,11]，它不维护全局图。 然而，在VI-SLAM的情况下，误差累积将逐渐破坏边缘化产生的先验。 从滑动窗口生成的损坏的先验最终将与全局地图和闭环约束冲突，并降低整体精度。

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

  在解释细节之前，我们首先回顾一下这些符号。 $C_i$是帧$i$的运动状态，其包括位姿${\mathrm{T}}_i=({\mathrm{R}}_i，{\mathrm{p}}_i)$和IMU状态${\mathrm{M}}_i=(\mathrm{v}i，\mathrm{b}i)$。 我们可以参考帧$i$最接近的关键帧$k_0$来表示全局位姿${\mathrm{T}}_i$和重力方向，如下：${}^{k_0}{\mathrm{T}}_i={\mathrm{T}}_i\circ {\mathrm{T}}_{k_0}^{-1}$和$g_{k_0}={\mathrm{R}}_{k_0}g$。 速度${\mathrm{v}}_i$在其自己的参考系中表示为${}^i{\mathrm{v}}_i={\mathrm{R}}_i{\mathrm{v}}_i$。 运动状态可以在局部表示为${\mathrm{C}}_i^{\prime }={(}^{k_0}{\mathrm{T}}_i，{\mathrm{M}}_i^{\prime })$和${\mathrm{M}}_i^{\prime }={(}^i{\mathrm{v}}_i，{\mathrm{b}}_i)$。 因此，$f_{ij}^{vis}({\mathrm{T}}_i,{\mathrm{T}}_{s_j},{\rho }_i)$成为

  边缘化${}^{k_0}{\mathrm{T}}_i$将导致 { ρ j ∣ j ∈ V i } \{\rho_j|j\in \mathcal{V}_i\} {ρj​∣j∈Vi​}之间的完全相关，使BA的稀疏性无效。 受[27]的启发，我们通过将每个${\rho }_j$复制为${\rho }_j^{\prime }={\rho }_j$来维持稀疏性，并丢弃除${\mathrm{x}}_{ij}$之外的所有测量值。 然后将重复的点边缘化，产生高斯因子

  类似地，$f_{ij}^{imu}({\mathrm{C}}_i,{\mathrm{C}}_j)$变为

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

图5：相对边缘化。让$h^{vis}$，$h^{imu}$，$h^{prior}$分别表示视觉，惯性和先验因素。 （a）对于第一帧$t_0=0$，我们加一个弱先验因子$h_0^{prior}(g_0,{\mathrm{M}}_0^{\prime })\sim \mathcal{N}(0,{\sum }_0^{prior})$。连接到先验因子$h_0^{prior}(g_0,{\mathrm{M}}_0^{\prime })$和惯性因子$h_{01}^{imu}({\mathrm{g}}_0,{\mathrm{M}}_0^{\prime },{\mathrm{C}}_1^{\prime })$的状态${\mathrm{M}}_0^{\prime }$被边缘化，这导致先验因子$h_1^{prior}(g_0,{\mathrm{C}}_1^{\prime })\sim \mathcal{N}(0,{\sum }_1^{prior})$。 （b）对于下一帧$t_0=1$，除了涉及视觉因子$h_{{\mathcal{V}}_1}^{vis}{(}^0{\mathrm{T}}_1)$，并且${}^0{\mathrm{T}}_1$和${\mathrm{M}}_1^{\prime }$都被边缘化之外，该过程是类似的。 （c）通常，视觉因子 h V 1 h i s ( k 0 T t 0 , { k 0 T s j ≠ k 0 ∣ j ∈ V t 0 } ) h_{\mathcal{V}_1}^{his}(^{k_0}{\rm T}_{t_0},\{^{k_0}{T_{s_j\neq k_0|j\in \mathcal{V}_{t_0}}}\}) hV1​his​(k0​Tt0​​,{k0​Tsj​̸​=k0​∣j∈Vt0​​​})中涉及$k_0$以外的更多关键帧。边缘化这样的因素将引入所有涉及的关键帧（黄色圆圈）之间的相关性。重复此过程，直到边缘化帧$t_0$是（d）中的新关键帧。然后，局部和全局BA的过程以不同的方式进行。 （e）对于全局BA，我们将先验因子 h t 0 p r i o r ( g k 0 , M t 0 ′ , { k 0 T k ∈ K t 0 } ) h_{t_0}^{prior}({\rm g}_{k_0},{\rm M}_{t_0}^\prime,\{^{k_0}{\rm T}_{k\in \mathcal{K}_{t_0}}\}) ht0​prior​(gk0​​,Mt0​′​,{k0​Tk∈Kt0​​​})和IMU状态${\mathrm{M}}_{t_0}^{\prime }$边缘化。相对约束被提交给全局BA，如图2所示。（f）对于局部BA，我们首先将先验因子 h t 0 p r i o r ( g k 0 , M t 0 ′ , { k 0 T k ∈ K t 0 } ) h_{t_0}^{prior}({\rm g}_{k_0},{\rm M}_{t_0}^\prime,\{^{k_0}{\rm T}_{k\in \mathcal{K}_{t_0}}\}) ht0​prior​(gk0​​,Mt0​′​,{k0​Tk∈Kt0​​​})边缘化。除${\mathrm{M}}_{t_0}^{\prime }$之外的所有相关状态都被边缘化，在${\mathrm{M}}_{t_0}^{\prime }$产生先验。此时，$t_0$成为新的参考关键帧。出现新状态${\mathrm{g}}_{t_0}$以及其上的弱先验，并且在帧$t_0$的参考中表示下一帧$t_0+1$的位姿，即${}^{t_0}{\mathrm{T}}_{t_0+1}$。然后我们将先验因子和惯性因子边缘化为$h_{t_0,t_0+1}^{imu}({\mathrm{g}}_{k_0},{\mathrm{M}}_{k_0}^{\prime },{\mathrm{C}}_{t_0+1}^{\prime }\})$。 ${\mathrm{M}}_{t_0}^{\prime }$被边缘化，在${\mathrm{g}}_{t_0}$和${\mathrm{C}}_{t_0+1}^{\prime }$上产生先验。在（e）和（f）完成之后，系统返回到类似于（b）的状态。

  在最早的帧$t_0$边缘化之后，该过程将在下一帧$t_0+1$处产生先验，表示为

  其中${\mathcal{K}}_{t_0}$是一组所涉及的关键帧，它们展开为 K t 0 = K t 0 − 1 ∪ { s j ∣ j ∈ V t 0 } \mathcal{K}_{t_0}=\mathcal{K}_{t_0-1}\cup \{s_j|j\in \mathcal{V}_{t_0}\} Kt0​​=Kt0​−1​∪{sj​∣j∈Vt0​​}。 请注意，这些状态的相对表示仅用于边缘化。 在优化期间，需要将状态和先验转换到全局坐标系（the global frame）。 我们将先验因子（18）转换到全局坐标系，表示为

  请注意，关键帧位姿仅在全局BA中调整，因此从当前BA的先前因子中消除。 如果边缘化帧$t_0$是新的关键帧，则边缘化过程将向全局BA提交相对约束（图5e），表示为

  其中 K t 0 ′ = K t 0 − 1 ∪ { t 0 } \mathcal{K}_{t_0}^\prime=\mathcal{K}_{t_0-1}\cup \{t_0\} Kt0​′​=Kt0​−1​∪{t0​}。 类似地，相对约束从参考帧$k_0$转换到全局坐标系，表示为

  其中 L t 0 = K t 0 ′ ∪ { k 0 } {\mathcal L}_{t_0}=\mathcal{K}_{t_0}^\prime\cup \{k_0\} Lt0​​=Kt0​′​∪{k0​}。

6、评估

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

  我们使用EuRoC [3]数据集进行定量评估，并在许多具有挑战性的环境中对Google Tango进行定性比较。 在所有实验中，滑动窗口大小设置为50。 较大的滑动窗口不会提高精度但会降低效率。

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

表2：使用EuRoC数据集转换RMSE（m）。 注意，估计和地面实况轨迹的空间对准是在没有立体算法的比例调整的情况下执行的。 其他方法的结果是根据他们发布的代码从我们自己的实验中生成的，这些代码与他们论文中报告的数字略有不同。

6.1 算法验证

  我们验证了每个子部分中引入的算法的每个步骤。 表1显示了整个系统的性能，以及分别禁用线性化点，ST-IBA，I-PCG和相对边缘化的定点。 所有测试均在具有i7 CPU@3.6GHz的台式PC上运行。

6.2 局部准确性

  我们比较表2中不同立体SLAM系统的端到端精度。 OKVIS [22]和SVO [11]都是视觉惯性测距法（VIO）。 我们通过提供与我们相同的特征追踪来运行iSAM2 [18]，而不提供闭环约束，因此它作为VIO运行。 为了公平比较，我们在有和没有闭环的情况下显示我们的结果。 由于我们在局部滑动窗口中使用了50帧，因此在没有闭环的情况下，我们的系统已经实现了比最先进的替代方案更好的定位精度。 通过为我们的求解器提供闭环相对约束，对于大多数序列，RMSE显着降低。

6.3 解决器精度

  我们的求解器的效率是这项工作的关键贡献。 我们测量不同SLAM系统的优化时间，如表3所示。 我们还使用八核ARM CPU（A9 X 4+A15 X 4）测量求解器的速度。 我们将求解器配置为在单线程模式下在A15上运行。 局部BA的优化时间分别为12.18ms，78.14ms和193.72ms，全局BA分别为无环和有环。 我们的求解器显示出应用于移动和功率约束应用的巨大潜力。

表3：使用Intel i7 CPU的局部/全局BA（LBA / GBA）与EuRoC数据集的运行时间（ms）的比较。 多线程已禁用。 运行时不包括前端进程（特征检测和匹配）。 OKVIS [22]在滑动窗口中使用5个关键帧和3个IMU帧，而我们的系统使用50帧，仍然可以实现10倍的加速。 注意，SVO [11]的优化时间不能直接测量。 我们将前端结果提供给iSAM2 [18]来模拟SVO的优化时间。 iSAM2是SVO使用的求解器，也是最先进的增量求解器。 我们还测量了ORB-SLAM [25]的优化时间，它使用g2o [21]作为求解器。 LBA / GBA的运行时间比我们的运行时慢大约40/140倍。 请注意，ORB-SLAM需要更多功能才能实现强大的跟踪，这也是低效率的原因。 如果我们将提取的要素数从默认值1200减少到490，则EuRoC数据集上的3/11序列的跟踪失败。

6.4 与Google Tango进行定性比较

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

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

7、结论

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