文章目录

摘要
1. 介绍
2. Related Work
3. Framework
- 3.1. Constraint Functions
- 3.2. Local and Global Optimization
4. Efficient Solver for VI-SLAM
- 4.1. General Incremental BA Solver
- 4.2. Improvement for Local BA
- 4.3. Incremental PCG for IBA
5. Relative Marginalization
6. 评估
7、结论

摘要

由于视觉特性和惯性测量的互补性，视觉-惯性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求解算法，其贡献如下:

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

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的目标是利用在时间点ttt之前的视觉和惯性测量来估计运动状态CtC_tCt以及一组3D点{X1,⋅⋅⋅,Xnt}\{X_1, · · · , X_{nt}\}{X1,⋅⋅⋅,Xnt}。

运动状态：

相机姿态：

IMU状态：

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

高斯白噪声：

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

ρjρ_jρj：第j个点的逆深度；sjs_jsj是提取第j个点的源帧,x¯x¯x¯就是x的齐次坐标

视觉约束定义为:

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

更多IMU约束的细节在这儿

3.2. Local and Global Optimization

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

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

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

全局BA以相对较低的频率与本地BA并行运行。Global BA优化了从局部滑动窗口移除但在Global map中被选择为关键帧的帧。在全局BA中，如果它携带超过N个（实验中为20）从所有其他帧中看不到的特征，则该帧在全局BA中被选择作为关键帧。全局BA的代价函数为：
其中LiL_iLi为第i个相对位姿约束所涉及的关键帧集合。Loop closure触发全局BA，该BA应考虑映射一致性。对于一个典型的循环约束，∣Li|L_i∣Li| = 2。
Relative Marginalization
相对边缘化在局部BA的最后一个关键帧和从局部BA中删除的最后一个关键帧之间产生相对位姿约束,(如图2中Ckm−1C_{k_{m-1}}Ckm−1与CkmC_{k_m}Ckm之间的约束关系)，以便从全局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掉点后对应的信息矩阵，用舒尔补表示。

增量更新[Si1,i2∣si][S_{i_1,i_2} |s_i][Si1,i2∣si]：

其中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)中定义的[Si1i2j∣sij][S^j_{i_1i_2} |s^j_i][Si1i2j∣sij]的很大一部分必须重新评估，并且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)。其核心思想是将原始长特征轨迹XjX_jXj分割为几个短的重叠子轨迹Xj1、Xj2X_{j_1}、X_{j_2}Xj1、Xj2、···，如图4所示

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

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

(13)中定义的[Q¯i1i2j∣q¯ij][Q¯^j_{i_1i_2} | q¯^j_i][Q¯i1i2j∣q¯ij]可以进行增量更新以进一步加速:

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

4.3. Incremental PCG for IBA

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

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

对于状态在最后一次迭代中没有改变的摄像机iii:初始化δφci+=δφci−δφ^+_{c_i} = δφ^-_{c_i}δφci+=δφci−，其余的仍然使δφci+=0δφ^+_{c_i} =0δφci+=0

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

5. Relative Marginalization

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

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

注【 Marginalization 中的

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

《论文笔记》Collaborative Visual Inertial SLAM for Multiple Smart Phones
时间:2021 作者: 创新点: 整体结构感觉和CCM_SLAM很像(也是中心化方式),在部分细节上做了创新:引入了VIO做单机SLAM,而且场景更换为移动场景.基于VIO的multi-map f ...
【每日论文阅读】Collaborative Visual Inertial SLAM for Multiple Smart Phones
文章目录论文阅读:Collaborative Visual Inertial SLAM for Multiple Smart Phones 摘要主要贡献算法逻辑算法结果论文阅读:Collab ...
【论文笔记】显著性信息辅助的视觉SLAM系统 SBAS: Salient Bundle Adjustment for Visual SLAM
重庆大学, 汽车工程专业, 重庆大学机械传动重点实验室本文并不是第一个利用显著性预测结果改进SLAM系统的工作,但是之前的工作都没有意识到显著性检测模型存在中心偏差的问题,这导致在没有显著性区域的 ...
【论文笔记】CondConv: Conditionally Parameterized Convolutions for Efficient Inference
论文论文题目:CondConv: Conditionally Parameterized Convolutions for Efficient Inference 论文地址:https://arxi ...
[论文笔记|sonar+VIO]:Sonar Visual Inertial SLAM of Underwater Structures
摘要为了适应水下声呐传感器的数据,本文提出了一种最先进的视觉-惯性状态估计方案(OKVIS)的扩展.水下领域在可用视觉数据的质量方面提出了独特的挑战;因此,利用声呐数据增强外传感器能改善水下结构的重 ...
论文笔记 NAACL 2021|Document-level Event Extraction with Efficient End-to-end Learning of Cross-event De
文章目录 1 简介 1.1 动机 1.2 创新 2 背景知识 3 方法 3.1 基础模型 3.2 跨事件依赖 4 实验 1 简介论文题目:Document-level Event Extractio ...
Bundle Adjustment (BA) in vSLAM or SFM
文章目录 Overview BA as a NonLinear Least Squares Problem Solved with LM JTJ\mathbf{J}^T \mathbf{J}JTJ s ...
ORB-SLAM3 论文笔记
ORB-SLAM3 论文笔记这篇博客 ORB-SLAM3系统相机模型的抽象(Camera Model) 重定位的问题图片矫正的问题视觉惯性SLAM的工作原理相关公式 IMU初始化跟踪和建图 ...
《论文笔记》COVINS: Visual-Inertial SLAM for Centralized Collaboration
时间:2021 作者: Multi-UAV Collaborative Monocular SLAM的后续之作,关于之前那篇可参照:<论文笔记>Multi-UAV Collaborat ...

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