17RAL_Visual-Inertial Monocular SLAM with Map Reuse

---

累啊

On-Manifold Preintegration for Real-Time Visual-Inertial Odometry提出了IMU预积分技术并且给出了详细的数学推导过程。ORB-SLAM的作者在IMU预积分的基础之上，在ORB-SLAM中实现了VIO。因此这篇论文在一定程度上可以看作是IMU预积分技术的工程实践。

Abstract

近年来，视觉-惯性里程计技术取得了优异的成果，这些技术旨在高精度和鲁棒性地计算传感器的增量运动。然而，这些方法缺乏闭环检测能力，即使传感器不断回到同一位置，轨迹估计仍会累积漂移。在本文中，我们提出了一种新颖的紧耦合视觉-惯性同时定位与建图（SLAM）系统，该系统能够闭环检测并重用地图，从而在已建图区域内实现零漂移定位。尽管我们的方法适用于任何相机配置，这里我们聚焦于单目相机这一具有尺度模糊的通用问题。我们还提出了一种新的IMU初始化方法，该方法能够在几秒钟内高精度地计算出尺度、重力方向、速度以及陀螺仪和加速度计的偏置。我们在最近发布的一个微型飞行器公开数据集的11个序列上测试了该系统，获得了典型的1%尺度因子误差和厘米级精度。通过在包含回访场景的序列中与现有视觉-惯性里程计方法进行对比，验证了我们的方法由于地图重用和无漂移累积而具有更高的精度。

关键词—传感器融合、SLAM、基于视觉的导航。

1.Introduction

目前，基于机载传感器的运动估计已成为机器人学和计算机视觉领域的热门话题，因为它能够推动自动驾驶汽车、增强与虚拟现实、服务机器人以及无人机导航等新兴技术的发展。在各种传感器模式中，视觉-惯性组合提供了一种具有巨大潜力的低成本解决方案。一方面，相机能够提供丰富的环境信息，使得可以构建3D模型、定位相机并识别已访问的地点；另一方面，IMU传感器提供了自运动信息，能够为单目视觉恢复尺度信息，并估算重力方向，从而使绝对的俯仰角和横滚角可观测。

近年来，视觉-惯性融合已成为一个非常活跃的研究课题。最新的研究聚焦于紧耦合（即对所有传感器状态进行联合优化）的视觉-惯性里程计，采用滤波[1]–[3]或基于关键帧的非线性优化[4]–[8]方法。然而，这些方法只能计算增量运动，无法实现闭环检测或重用已映射环境的地图。这意味着，即使传感器始终在相同环境中移动，估计的轨迹也会累积无界漂移。这种情况是由于为了保持恒定的计算成本而边缘化过去状态[1]–[3]，[5]，[6]，或使用全局平滑[4]，[7]，在探索阶段复杂度几乎恒定，但在闭环检测时复杂度可能和批处理方法一样高。虽然某些滤波方法[9]能够在拓扑上实现闭环并重用地图，但无法实时地保持全局度量一致性。最新的系统[10]能够重用离线构建的地图，并进行视觉-惯性跟踪。

Note: 图1展示了我们基于视觉-惯性ORB-SLAM系统在EuRoC数据集的V1_02_medium序列中的估计地图和关键帧。顶部视图使用估计的重力方向渲染。绿色连线连接了共享超过100个点观测的关键帧，证明了该系统能够重用地图。与视觉-惯性里程计不同，这种特性允许系统在持续回访相同位置时实现零漂移定位。

基于Lupton和Sukkarieh的预积分方法[11]，Forster等人对SO(3)流形的应用[7]，以及Indelman等人提出的因子图表示[4]，本文提出了一种名为Visual-Inertial ORB-SLAM的系统。根据我们的了解，这是第一个能够实时进行度量闭环并在线重用地图的基于关键帧的视觉-惯性SLAM系统。我们的方法遵循ORB-SLAM的思路[12]，受Klein和Murray工作[13]的启发，追踪过程中假设地图固定，对当前帧进行优化，而后端执行局部捆绑调整（BA），优化包含固定关键帧的局部关键帧窗口。相比于全局平滑，这种方法允许以恒定时间进行局部BA，并且通过不边缘化过去状态，我们能够重用这些状态。我们使用地点识别检测大范围回环，并通过轻量级姿态图优化进行修正，接着在独立线程中执行完全BA，以避免干扰实时操作。图1展示了我们系统在一个连续回访序列中的重建效果。

我们的跟踪和局部BA在优化过程中固定了一些状态，这可能会导致解的偏差。因此，我们需要一个可靠的视觉-惯性初始化，以在开始固定状态之前提供准确的状态估计。为此，我们提出了执行视觉-惯性完全BA的方法，以获得结构、相机姿态、尺度、速度、重力、陀螺仪和加速度计偏置的最优解。此完全BA是非线性优化，需要良好的初始值来收敛。在第四节中，我们提出了一种分治策略来计算此初始解。我们首先使用纯单目ORB-SLAM[12]处理几秒钟的视频，以估计结构和若干关键帧姿态的初始解，但仅限于未知的尺度因子。然后，我们计算陀螺仪的偏置，这可以从关键帧的已知方向轻松估算，从而能够正确旋转加速度计测量值。接着，我们不考虑加速度计偏置，使用[11]中的方法解决尺度和重力。为区分重力和加速度计偏置，我们利用重力大小的已知信息来求解加速度计偏置，并细化尺度和重力方向。此时，可以轻松检索所有关键帧的速度。实验结果表明，该初始化方法高效、可靠且精确。此外，该方法通用，可应用于任何基于关键帧的单目SLAM，不假设任何初始条件，只需传感器运动以使所有变量可观测[15]。与先前方法[15]–[17]联合求解视觉和IMU但忽略陀螺仪或加速度计偏置不同，我们通过将问题分解为更简单的步骤，有效地计算所有变量。

---

2.视觉-惯性基本原理 (VISUAL-INERTIAL PRELIMINARIES)

我们视觉-惯性ORB-SLAM系统的输入是IMU测量数据和单目相机帧。我们采用一个传统的小孔相机模型 [18]，其投影函数为

$$\pi :{\mathbb{R}}^3\to \Omega$$

将相机参考系 $C$ 中的三维点 $X_C\in {\mathbb{R}}^3$ 转换为图像平面上的二维点 $x_C\in \Omega \subset {\mathbb{R}}^2$：

$$\pi (X_C)=\left[\begin{array}{c}{f}_u\frac{X_C}{Z_C}+c_u\\ f_v\frac{Y_C}{Z_C}+c_v\end{array}\right],X_C={\left[\begin{array}{ccc}{X}_C& Y_C& Z_C\end{array}\right]}^T\text{\hspace{1em}(1)}$$

其中 $[f_u,f_v{]}^T$ 为焦距，$[c_u,c_v{]}^T$ 为主点。这一投影函数不考虑由相机镜头造成的畸变。当我们从图像中提取关键点时，会对其坐标进行去畸变处理，以便它们可以通过公式 (1) 与投影点匹配。

IMU的参考系记为 $B$，用于测量传感器的加速度 ${\mathbf{a}}_B$ 和角速度 ${\mathbf{\omega }}_B$，测量间隔为 $\Delta t$，通常频率为几百赫兹。除了传感器噪声，这些测量值还受到加速度计和陀螺仪的缓慢变化偏置 ${\mathbf{b}}_a$ 和 ${\mathbf{b}}_g$ 的影响。此外，加速度计还受到重力 ${\mathbf{g}}_W$ 的影响，因此在计算运动时需要减去重力的影响。IMU在世界坐标系 $W$ 中的离散化过程，包括其方位 ${\mathbf{R}}_{WB}\in SO(3)$、位置 $\text{w}{\mathbf{P}}_B$ 和速度 $\text{w}{\mathbf{V}}_B$，可以通过以下公式计算 [7]：

$${\mathbf{R}}_{WB}^{k+1}={\mathbf{R}}_{WB}^k\mathrm{Exp}\left(\left({\mathbf{\omega }}_B^k-{\mathbf{b}}_g^k\right)\Delta t\right)$$

$$\text{w}{\mathbf{V}}_B^{k+1}=\text{w}{\mathbf{V}}_B^k+{\mathbf{g}}_W\Delta t+{\mathbf{R}}_{WB}^k\left({\mathbf{a}}_B^k-{\mathbf{b}}_a^k\right)\Delta t$$

$$\text{w}{\mathbf{P}}_B^{k+1}=\text{w}{\mathbf{P}}_B^k+\text{w}{\mathbf{V}}_B^k\Delta t+\frac{1}{2}{\mathbf{g}}_W\Delta t^2+\frac{1}{2}{\mathbf{R}}_{WB}^k\left({\mathbf{a}}_B^k-{\mathbf{b}}_a^k\right)\Delta t^2\text{\hspace{1em}(2)}$$

两个连续关键帧之间的运动可以通过所有中间测量的预积分项 $\Delta \mathbf{R},\Delta \mathbf{v}$ 和 $\Delta \mathbf{p}$ 来定义[11]。我们使用了文献[7]中描述的最新IMU预积分方法：

$${\mathbf{R}}_{WB}^{i+1}={\mathbf{R}}_{WB}^i\Delta {\mathbf{R}}_{i,i+1}\mathrm{Exp}\left(\left({\mathbf{J}}_{\Delta R}^g{\mathbf{b}}_g^i\right)\right)$$

$$\mathbf{w}{\mathbf{V}}_B^{i+1}=\mathbf{w}{\mathbf{V}}_B^i+{\mathbf{g}}_W\Delta t_{i,i+1}+{\mathbf{R}}_{WB}^i\left(\Delta {\mathbf{v}}_{i,i+1}+{\mathbf{J}}_{\Delta v}^g{\mathbf{b}}_g^i+{\mathbf{J}}_{\Delta v}^a{\mathbf{b}}_a^i\right)$$

$$\mathbf{w}{\mathbf{P}}_B^{i+1}=\mathbf{w}{\mathbf{P}}_B^i+\mathbf{w}{\mathbf{V}}_B^i\Delta t_{i,i+1}+\frac{1}{2}{\mathbf{g}}_W\Delta t_{i,i+1}^2+{\mathbf{R}}_{WB}^i\left(\Delta {\mathbf{p}}_{i,i+1}+{\mathbf{J}}_{\Delta p}^g{\mathbf{b}}_g^i+{\mathbf{J}}_{\Delta p}^a{\mathbf{b}}_a^i\right)\text{\hspace{1em}(3)}$$

---

其中，${\mathbf{J}}_g$ 和 ${\mathbf{J}}_a$ 分别为一阶偏置的雅可比矩阵，用于在不显式重新计算预积分的情况下，近似考虑偏置的变化。

其中，雅可比矩阵 ${\mathbf{J}}_{(\cdot )}^a$ 和 ${\mathbf{J}}_{(\cdot )}^g$ 提供了在不显式重新计算预积分的情况下，偏置变化影响的一阶近似。随着IMU测量数据的到达，预积分和雅可比矩阵可以高效地迭代计算 [7]。

相机和IMU被视为刚性连接，它们的参考系之间的转换矩阵 ${\mathbf{T}}_{CB}=\left[\begin{array}{c}{\mathbf{R}}_{CB}|\text{c}{\mathbf{P}}_B\end{array}\right]$ 可以通过标定获得 [19]。

---

3. 视觉惯性 ORB-SLAM (VISUAL-INERTIAL ORB-SLAM)

我们视觉-惯性系统的基础是 ORB-SLAM [12], [20]。该系统包含三个并行线程：跟踪、局部建图和回环检测。该系统被设计用于大规模环境，通过构建共视图图来恢复用于跟踪和建图的局部地图，并在回环闭合时执行轻量级的姿态图优化。此外，ORB-SLAM 允许在建图完成后切换到低CPU消耗的仅定位模式（即禁用建图和回环检测），得益于系统的重定位能力。ORB-SLAM 是开源的，已在公共数据集上进行了广泛评估，并取得了优异的性能。在本节中，我们详细介绍了在跟踪、局部建图和回环检测线程中相对于原始系统的主要更改。视觉-惯性初始化将在第四节中介绍。

A.Tracking

Note : 图2. 跟踪线程中优化过程的演变。(a) 我们开始优化与上一个关键帧 $i$ 通过IMU约束相连的帧 $j$。(b) 优化的结果（估计值和Hessian矩阵）作为下一次优化的先验。© 当跟踪下一帧 $j+1$ 时，帧 $j$ 和 $j+1$ 被共同优化，它们通过IMU约束连接，且帧 $j$ 使用前一次优化的结果作为先验。(d) 在优化结束时，帧 $j$ 被边缘化，结果作为后续优化的先验。(e-f) 这一过程重复进行，直到来自局部建图或回环检测线程的地图更新发生。在这种情况下，优化将当前帧与上一个关键帧相连，丢弃不再有效的先验信息，因为地图已经更改。

• a) 跟踪帧 $j$（地图已更改）
• b) 先验（优化结果）
• c) 跟踪帧 $j+1$（地图未更改）
• d) 先验（边缘化）
• e) 跟踪帧 $j+2$（地图未更改）
• f) 先验（边缘化）

我们的视觉-惯性跟踪负责在帧率下跟踪传感器的姿态、速度和IMU偏置。这使我们能够非常可靠地预测相机的姿态，而不是像原始单目系统那样使用特定的运动模型。一旦相机姿态被预测出来，本地地图中的地图点会被投影并与帧中的关键点进行匹配。然后，我们通过最小化所有匹配点的特征重投影误差和IMU误差项来优化当前帧 $j$。该优化过程是否进行地图更新会根据局部建图或回环检测线程的状态而有所不同，如图2所示。

当在地图更新后立即执行跟踪时（图2(a)），IMU误差项将当前帧 $j$ 与上一个关键帧 $i$ 连接起来：

$$\theta =\left\{{\mathbf{R}}_{WB}^j,\mathbf{w}{\mathbf{P}}_B^j,\mathbf{w}{\mathbf{V}}_B^j,{\mathbf{b}}_g^j,{\mathbf{b}}_a^j\right\}$$

$${\theta }^{\ast }=\underset{\theta }{\mathrm{argmin}}\left(\sum _k{E}_{\text{proj}}(k,j)+E_{\text{IMU}}(i,j)\right)\text{\hspace{1em}(4)}$$

其中，给定匹配点 $k$ 的特征重投影误差 $E_{\text{proj}}$ 定义如下：

$$E_{\text{proj}}(k,j)=\rho \left({\left(x^k-\pi \left(X_C^k\right)\right)}^T{\Sigma }_k\left(x^k-\pi \left(X_C^k\right)\right)\right)$$

$$X_C^k={\mathbf{R}}_{CB}{\mathbf{R}}_{BW}^j\left(X_W^k-\mathbf{W}{\mathbf{p}}_B^j\right)+\mathbf{c}{\mathbf{P}}_B\text{\hspace{1em}(5)}$$

其中，$x^k$ 是图像中的关键点位置，$X_W^k$ 是世界坐标系中的地图点，${\Sigma }_k$ 是与关键点尺度相关的信息矩阵。

IMU误差项 $E_{\text{IMU}}$ 定义为：

IMU误差项 $E_{\text{IMU}}(i,j)$：

$$E_{\text{IMU}}(i,j)=\rho \left(\left(\left[\begin{array}{ccc}{e}_R^T& e_v^T& e_p^T\end{array}\right]{\Sigma }_I\left[\begin{array}{c}{e}_R\\ e_v\\ e_p\end{array}\right]\right)\right)+\rho \left(e_b^T{\Sigma }_R{e}_b\right)$$

其中

$$e_R=\mathrm{Log}\left({\left(\Delta R_{i,j}\mathrm{Exp}\left({\mathbf{J}}_g^{\Delta R}{\mathbf{b}}_g^j\right)\right)}^T{\mathbf{R}}_{BW}^i{\mathbf{R}}_{WB}^j\right)$$

$$e_v={\mathbf{R}}_{BW}^i\left(\mathbf{W}{\mathbf{v}}_B^j-\mathbf{W}{\mathbf{v}}_B^i-{\mathbf{g}}_W\Delta t_{ij}\right)-\left(\Delta v_{ij}+{\mathbf{J}}_g^{\Delta v}{\mathbf{b}}_g^j+{\mathbf{J}}_a^{\Delta v}{\mathbf{b}}_a^j\right)$$

$$e_p={\mathbf{R}}_{BW}^i\left(\mathbf{W}{\mathbf{p}}_B^j-\mathbf{W}{\mathbf{p}}_B^i-\mathbf{W}{\mathbf{v}}_B^i\Delta t_{ij}-\frac{1}{2}{\mathbf{g}}_W\Delta t_{ij}^2\right)-\left(\Delta p_{ij}+{\mathbf{J}}_g^{\Delta p}{\mathbf{b}}_g^j+{\mathbf{J}}_a^{\Delta p}{\mathbf{b}}_a^j\right)$$

$$e_b={\mathbf{b}}^j-{\mathbf{b}}^i$$

这些公式定义了IMU误差项中

其中：

• $e_R$、$e_v$、$e_p$ 分别表示旋转、速度和位置误差。
• ${\Sigma }_I$ 是预积分信息矩阵。
• $e_b$ 表示偏置误差。
• ${\Sigma }_R$ 是偏置随机游走的信息矩阵。

其中，${\Sigma }_I$ 是预积分的信息矩阵，${\Sigma }_R$ 是偏置随机游走的信息矩阵 [7]，$\rho$ 是Huber鲁棒代价函数。我们使用在g2o [21] 中实现的高斯-牛顿算法来解决该优化问题。在优化完成后（图2(b)），生成的估计值和Hessian矩阵作为下次优化的先验。

假设没有地图更新（图2©），那么下一帧 $j+1$ 将通过链接到帧 $j$ 并使用在上一次优化结束时计算的先验（图2(b)）进行优化：

$$\theta =\left\{{\mathbf{R}}_{WB}^j,\mathbf{W}{\mathbf{p}}_B^j,\mathbf{W}{\mathbf{v}}_B^j,{\mathbf{b}}_g^j,{\mathbf{b}}_a^j,{\mathbf{R}}_{WB}^{j+1},{\mathbf{p}}_W^{j+1},{\mathbf{v}}_W^{j+1},{\mathbf{b}}_g^{j+1},{\mathbf{b}}_a^{j+1}\right\}$$

$${\theta }^{\ast }=\arg \underset{\theta }{\min }\left(\sum _k{E}_{\text{proj}}(k,j+1)+E_{\text{IMU}}(j,j+1)+E_{\text{prior}}(j)\right)\text{\hspace{1em}(7)}$$

其中 $E_{\text{prior}}$ 是一个先验项：

$$E_{\text{prior}}(j)=\rho \left(\left(\left[\begin{array}{cccc}{e}_R^T& e_v^T& e_p^T& e_b^T\end{array}\right]{\Sigma }_p\left[\begin{array}{c}{e}_R\\ e_v\\ e_p\\ e_b\end{array}\right]\right)\right)$$

$$e_R=\mathrm{Log}\left({\bar{\mathbf{R}}}_{BW}^j{\mathbf{R}}_{WB}^j\right)$$

$$e_v=\mathbf{w}{\bar{\mathbf{V}}}_B^j-\mathbf{w}{\mathbf{V}}_B^j$$

$$e_p=\mathbf{w}{\bar{\mathbf{P}}}_B^j-\mathbf{w}{\mathbf{P}}_B^j$$

$$e_b={\bar{\mathbf{b}}}^j-{\mathbf{b}}^j\text{\hspace{1em}(8)}$$

其中：

• $\rho$ 是Huber鲁棒代价函数。
• ${\Sigma }_p$ 是先验信息矩阵。
• $e_R$、$e_v$、$e_p$、$e_b$ 分别表示旋转、速度、位置和偏置误差。

其中，$(\bar{\cdot })$ 和 ${\Sigma }_p$ 是来自前一次优化（图2(b)）的估计状态和Hessian矩阵。完成此优化后（图2(d)），帧 $j$ 被边缘化 [5]。这种连接两个连续帧并使用先验的优化过程会重复（图2(e) 和 (f)），直到发生地图更改，此时先验将失效，跟踪将重新链接当前帧与上一个关键帧（图2(a)）。需要注意的是，这种优化过程（图2(e) 和 (f)）在仅定位模式下总是执行的，因为地图不会更新。

B. Local Mapping

Note : 图3. 原始 ORB-SLAM（上）与提出的视觉-惯性 ORB-SLAM（下）之间的局部束调整对比。在视觉-惯性 ORB-SLAM 中，局部窗口按关键帧的时间顺序获取，而在 ORB-SLAM 中则是通过共视图获取。

局部建图线程在插入新的关键帧后执行局部束调整（local BA）。它优化最近的 $N$ 个关键帧（即局部窗口）及这些关键帧所看到的所有地图点。所有与局部点共享观测的其他关键帧（即在共视图中与任何局部关键帧相连的关键帧），如果不在局部窗口中，则在优化期间固定（即固定窗口），仅对总成本作出贡献。关键帧 $N+1$ 始终包含在固定窗口中，因为它对IMU状态有约束。图3展示了原始 ORB-SLAM 和视觉-惯性 ORB-SLAM 中局部 BA 的区别。其代价函数是 IMU 误差项（公式 (6)）和重投影误差项（公式 (5)）的组合。注意，与纯视觉版本相比，视觉-惯性版本更加复杂，因为每个关键帧多了 9 个状态（速度和偏置）需要优化。为了实现实时性能，需要选择合适的局部窗口大小。

局部建图还负责关键帧管理。原始的 ORB-SLAM 策略会丢弃冗余关键帧，以防止在已经构建良好的区域中定位时地图尺寸不断增长。然而，当使用 IMU 信息时，这一策略会出现问题，因为 IMU 对连续关键帧的运动提供了约束。连续关键帧之间的时间差越长，IMU 提供的信息就越弱。因此，我们允许建图丢弃冗余关键帧，但前提是不会导致局部 BA 的局部窗口中两个连续关键帧的时间差超过 0.5 秒。为了能够在闭环后或任何时间进行全局 BA 以细化地图，我们不允许任何两个连续关键帧的时间差超过 3 秒。如果关闭了 IMU 约束下的全局 BA，我们只需要限制局部窗口中关键帧之间的时间间隔。

C. Loop Closing

闭环检测线程的目标是在返回到已映射区域时减少探索过程中累积的漂移。回环检测模块将最近的关键帧与过去的关键帧进行匹配。该匹配通过计算刚体变换对齐关键帧之间的匹配点来验证 [22]。最后，执行优化以减少轨迹中累积的误差。由于在大型地图上该优化可能非常耗费计算资源，因此采用位姿图优化策略，忽略结构信息以降低复杂度，同时表现出良好的收敛性 [12]。与原始 ORB-SLAM 不同，我们在 6 自由度（6 DoF）上进行位姿图优化，而非 7 自由度 [23]，因为比例是可观察的。此位姿图忽略了 IMU 信息，不优化速度或 IMU 偏置。因此，我们通过根据关联关键帧的校正方向旋转速度来进行校正。尽管这不是最优的，偏置和速度应在局部上准确，以便在位姿图优化后继续使用 IMU 信息。之后，我们在一个并行线程中执行全局 BA，优化所有状态，包括速度和偏置。

4.IMU INITIALIZATION

在本节中，我们提出了一种方法，用于计算视觉-惯性全局束调整（full BA）的初始估计，包括尺度、重力方向、速度和 IMU 偏置，基于单目 SLAM 算法处理的一组关键帧。其基本思路是运行单目 SLAM 数秒，假设传感器执行了一种能够使所有变量可观测的运动。尽管我们基于 ORB-SLAM [12] 构建，但任何其他 SLAM 算法都可以使用。唯一的要求是任意两个连续的关键帧之间的时间间隔较短（参见第 III-B 节），以减少 IMU 噪声积分的影响。

初始化被划分为更简单的子问题：(1) 陀螺仪偏置估计，(2) 在不考虑加速度计偏置的情况下进行尺度和重力的近似，(3) 加速度计偏置估计及尺度和重力方向的精细化，(4) 速度估计。

A. 陀螺仪偏置估计

陀螺仪偏置可以仅通过已知的两个连续关键帧的方向来估计。假设偏置变化可以忽略不计，我们优化一个常量偏置 ${\mathbf{b}}_g$，以最小化所有连续关键帧对中陀螺仪积分和由 ORB-SLAM 计算的相对方向之间的差异。

陀螺仪偏置 ${\mathbf{b}}_g$ 的估计可以通过以下公式求得：

$$\arg \underset{{\mathbf{b}}_g}{\min }\sum _{i=1}^{N-1}{\Vert \mathrm{Log}\left({\left(\Delta {\mathbf{R}}_{i,i+1}\mathrm{Exp}\left({\mathbf{J}}_{\Delta R}{\mathbf{b}}_g\right)\right)}^T{\mathbf{R}}_{WB}^{i+1}{\mathbf{R}}_{WB}^i\right)\Vert }^2\text{\hspace{1em}(9)}$$

其中，$N$ 为关键帧的数量。${\mathbf{R}}_{WB}^{(\cdot )}={\mathbf{R}}_{WC}^{(\cdot )}{\mathbf{R}}_{CB}$ 由 ORB-SLAM 计算的方向 ${\mathbf{R}}_{WC}^{(\cdot )}$ 和标定 ${\mathbf{R}}_{CB}$ 得出。$\Delta {\mathbf{R}}_{i,i+1}$ 是两个连续关键帧之间的陀螺仪积分。我们通过高斯-牛顿算法用零偏置作为初始值来求解 (9)。类似表达式的解析雅可比矩阵可以在文献 [7] 中找到。

B. 尺度和重力的近似（无加速度计偏置）

一旦估计出陀螺仪偏置，我们就可以对速度和位置进行预积分，正确旋转加速度测量值以补偿陀螺仪偏置。

由 ORB-SLAM 计算的相机轨迹的尺度是任意的。因此，在相机坐标系 $C$ 和 IMU 坐标系 $B$ 之间进行转换时，我们需要引入一个尺度因子 $s$：

公式 (10) 表示：

$$\mathbf{w}{\mathbf{P}}_B=s\mathbf{w}{\mathbf{P}}_C+{\mathbf{R}}_{WC}\mathbf{c}{\mathbf{P}}_B$$
将 (10) 代入到两个连续关键帧位置之间的关系式 (3) 中，并忽略此时的加速度计偏置，得到：

$$s\mathbf{w}{\mathbf{P}}_C^{i+1}=s\mathbf{w}{\mathbf{P}}_C^i+\mathbf{w}{\mathbf{V}}_B^i\Delta t_{i,i+1}+\frac{1}{2}{\mathbf{g}}_W\Delta t_{i,i+1}^2+{\mathbf{R}}_{WB}^i\Delta {\mathbf{p}}_{i,i+1}+\left({\mathbf{R}}_{WC}^i-{\mathbf{R}}_{WC}^{i+1}\right)\mathbf{c}{\mathbf{P}}_B\text{\hspace{1em}(11)}$$

目标是通过对这些变量求解一个线性方程组来估计尺度 $s$ 和重力 $g_W$。为了避免对 $N$ 个速度进行求解并降低复杂度，我们考虑三个连续关键帧之间的两个关系式 (11) 并使用速度关系式 (3)，得到以下表达式：

$$\left[\begin{array}{cc}\lambda (i)& \beta (i)\end{array}\right]\left[\begin{array}{c}s\\ g_W\end{array}\right]=\gamma (i)\text{\hspace{1em}(12)}$$

其中，为了简化表示，将关键帧 $i,i+1,i+2$ 分别记为 1, 2, 3，我们有：

其中，我们定义关键帧 $i,i+1,i+2$ 分别为 1, 2, 3，表示如下：

$$\lambda (i)=\left(\mathbf{W}{\mathbf{p}}_C^2-\mathbf{W}{\mathbf{p}}_C^1\right)\Delta t_{23}-\left(\mathbf{W}{\mathbf{p}}_C^3-\mathbf{W}{\mathbf{p}}_C^2\right)\Delta t_{12}$$

$$\beta (i)=\frac{1}{2}{\mathbf{I}}_{3\times 3}\left(\Delta t_{12}^2\Delta t_{23}+\Delta t_{23}^2\Delta t_{12}\right)$$

$$\gamma (i)=\left({\mathbf{R}}_{WC}^2-{\mathbf{R}}_{WC}^1\right)\mathbf{c}{\mathbf{p}}_B\Delta t_{23}-\left({\mathbf{R}}_{WC}^3-{\mathbf{R}}_{WC}^2\right)\mathbf{c}{\mathbf{p}}_B\Delta t_{12}+{\mathbf{R}}_{WB}^2\Delta {\mathbf{p}}_{23}\Delta t_{12}+{\mathbf{R}}_{WB}^1\Delta {\mathbf{v}}_{12}\Delta t_{12}\Delta t_{23}-{\mathbf{R}}_{WB}^1\Delta {\mathbf{p}}_{12}\Delta t_{23}\text{\hspace{1em}(13)}$$

我们将所有三个连续关键帧的关系式 (12) 叠加为一个系统 $A_{3(N-2)\times 4}{x}_{4\times 1}=B_{3(N-2)\times 1}$，可以通过奇异值分解 (SVD) 来求解尺度因子 $s^{\ast }$ 和重力向量 $g_W^{\ast }$。注意我们有 $3(N-2)$ 个方程和 4 个未知数，因此至少需要 4 个关键帧。

C. 加速度计偏置估计，尺度和重力方向的细化

C. 加速度计偏置估计，尺度和重力方向的细化

到目前为止，我们在计算尺度和重力时没有考虑加速度计偏置。仅在公式 $(12)$ 中加入加速度计偏置将大大增加系统病态的可能性，因为重力和加速度计偏置很难区分 [15]。为了增加可观测性，我们引入了之前未考虑的新信息，即重力的大小 $G$。设定一个惯性参考系 $I$，其重力方向 g ^ I = { 0 , 0 , − 1 } \hat{g}_I = \{0, 0, -1\} g^​I​={0,0,−1}，以及已计算的重力方向 ${\hat{g}}_W=\frac{g_W^{\ast }}{\Vert g_W^{\ast }\Vert }$。我们可以通过以下方式计算旋转 $R_{WI}$：

$$R_{WI}=\mathrm{Exp}(\hat{v}\theta )$$

$$\hat{v}=\frac{{\hat{g}}_I\times {\hat{g}}_W}{\Vert {\hat{g}}_I\times {\hat{g}}_W\Vert },\theta =atan2(\Vert {\hat{g}}_I\times {\hat{g}}_W\Vert ,{\hat{g}}_I\cdot {\hat{g}}_W)\text{\hspace{1em}(14)}$$

并且重力向量 $g_W$ 可以表示为：

$$g_W=R_{WI}{\hat{g}}_{I}G\text{\hspace{1em}(15)}$$

其中，$R_{WI}$ 可以通过参考系 $I$ 中的 $x$ 轴和 $y$ 轴上的两个角度进行参数化，因为绕 $z$ 轴（与重力对齐）的旋转不会影响 $g_W$。这个旋转可以通过扰动 $\delta \theta$ 进行优化：

公式为：

$$g_W=R_{WI}\mathrm{Exp}(\delta \theta ){\hat{g}}_{I}G$$

$$\delta \theta ={\left[\begin{array}{cc}\delta {\theta }_{xy}^T& 0\end{array}\right]}^T,\delta {\theta }_{xy}={\left[\begin{array}{cc}\delta {\theta }_x& \delta {\theta }_y\end{array}\right]}^T\text{\hspace{1em}(16)}$$

使用一阶近似：

$$g_W\approx R_{WI}{\hat{g}}_{I}G-R_{WI}({\hat{g}}_I)\times G\delta \theta \text{\hspace{1em}(17)}$$

将 (17) 代入公式 (11)，并包括加速度计偏置的影响，我们得到：

$$s\mathbf{w}{\mathbf{P}}_C^{i+1}=s\mathbf{w}{\mathbf{P}}_C^i+\mathbf{w}{\mathbf{V}}_B^i\Delta t_{i,i+1}-\frac{1}{2}{R}_{WI}({\hat{g}}_I)\times G\Delta t_{i,i+1}^2\delta \theta$$

$$+{\mathbf{R}}_{WB}^i\left(\Delta {\mathbf{p}}_{i,i+1}+{\mathbf{J}}_{\Delta p}{\mathbf{b}}_a\right)+\left({\mathbf{R}}_{WC}^i-{\mathbf{R}}_{WC}^{i+1}\right)\mathbf{c}{\mathbf{p}}_B+\frac{1}{2}{R}_{WI}{\hat{g}}_{I}G\Delta t_{i,i+1}^2\text{\hspace{1em}(18)}$$

给定三个连续的关键帧（如公式 (12)），我们可以消除速度并得到以下关系式：

$$\left[\begin{array}{ccc}\lambda (i)& \varphi (i)& \zeta (i)\end{array}\right]\left[\begin{array}{c}s\\ \delta {\theta }_{xy}\\ b_a\end{array}\right]=\psi (i)\text{\hspace{1em}(19)}$$

其中，$\lambda (i)$ 与公式 (13) 中相同，$\varphi (i)$、$\zeta (i)$ 和 $\psi (i)$ 的计算如下：

$$\varphi (i)={\left[\frac{1}{2}{R}_{WI}({\hat{g}}_I)\times G\left(\Delta t_{12}\Delta t_{23}+\Delta t_{23}\Delta t_{12}\right)\right]}_{(:,1:2)}$$

$$\zeta (i)=R_{WB}^2{J}_{\Delta p_{23}}\Delta t_{12}+R_{WB}^1{J}_{\Delta v_{23}}\Delta t_{12}\Delta t_{23}-R_{WB}^1{J}_{\Delta p_{12}}\Delta t_{23}$$

$$\psi (i)=(R_{WC}^2-R_{WC}^1)c_{pB}\Delta t_{12}+R_{WB}^2\Delta p_{23}\Delta t_{12}+R_{WB}^1\Delta v_{12}\Delta t_{12}\Delta t_{23}-R_{WB}^1\Delta p_{12}\Delta t_{23}+\frac{1}{2}{R}_{WI}{\hat{g}}_{I}G\Delta t_{ij}^2\text{\hspace{1em}(20)}$$

其中，$[\cdot {]}_{(:,1:2)}$ 表示矩阵的前两列。

将三个连续关键帧之间的所有关系式 (19) 叠加起来，我们形成一个线性方程组：

$$A_3^{(N-2)\times 6}{x}_{6\times 1}=B_3^{(N-2)\times 1}$$

可以通过奇异值分解 (SVD) 来求解，得到尺度因子 $s^{\ast }$、重力方向修正 $\delta {\theta }_{xy}^{\ast }$ 和加速度计偏置 ${\mathbf{b}}_a^{\ast }$。

在这种情况下，我们有 $3(N-2)$ 个方程和 6 个未知数，因此至少需要 4 个关键帧来求解系统。我们可以计算条件数（即最大和最小奇异值的比值）来检查问题是否良好（即传感器是否执行了使所有变量可观测的运动）。我们可以重新线性化 (17) 并迭代求解，但在实际中，我们发现第二次迭代并不会带来明显的改进。

D. 速度估计

在公式 (12) 和 (19) 中，我们考虑了三个连续关键帧之间的关系，从而使得生成的线性系统不包含与速度对应的额外 $3N$ 个未知数。现在，由于尺度、重力和偏置已经确定，可以使用公式 (18) 计算所有关键帧的速度。为了计算最近关键帧的速度，我们使用公式 (3) 中的速度关系。

E. 重定位后的偏置重新初始化

当系统在长时间后通过位置识别实现重定位时，我们通过求解公式 (9) 重新初始化陀螺仪偏置。加速度计偏置通过求解简化后的公式 (19) 来估计，其中唯一的未知数是偏置，因为尺度和重力已经确定。我们使用仅基于视觉定位的 20 个连续帧来估计这两个偏置。

5.实验

我们在 EuRoC 数据集 [14] 中评估了第四节详细描述的 IMU 初始化方法以及我们的视觉-惯性 ORB-SLAM。该数据集包含 11 个序列，由微型飞行器（MAV）在两个不同的房间和一个工业环境中飞行时记录。根据光照、纹理、快速/慢速运动或运动模糊情况，这些序列被分为简单、中等和困难三类。数据集提供了同步的全球快门 WVGA 立体图像（20 Hz）、IMU 测量数据（200 Hz）以及轨迹的真实值。这些特性使其成为测试视觉-惯性 SLAM 系统的一个非常有用的数据集。实验是在 Intel Core i7-4700MQ 计算机（8 GB RAM）上，仅处理左侧图像进行的。

A. IMU 初始化

我们在序列 V2_01_easy 中评估了 IMU 的初始化。每次 ORB-SLAM 插入关键帧时，我们都会从头开始进行初始化。我们以较低的帧率运行该序列，以便重复初始化不会干扰系统的正常行为。其目的是分析随着更多关键帧（即更长的轨迹）被初始化算法处理后，各变量的收敛性。图 4 显示了估计的尺度和 IMU 偏置。可以看到，在 10 到 15 秒之间，所有变量都已收敛到稳定值，并且估计的尺度因子非常接近其最优值。此最优尺度因子是通过相似变换 [22] 将估计轨迹与真实值对齐后计算的。图 4 还显示了公式 (19) 的条件数，表明需要一些时间来获得良好条件的问题。这证实了传感器需要执行某种运动，使得所有变量都是可观测的，特别是加速度计的偏置。图 4 的最后一行显示了初始化算法所花费的时间，表现出线性增长的趋势。这种复杂性是由于公式 (12) 和 (19) 中未包含速度项的结果，如果包含速度项，使用 SVD 求解这些系统时的复杂度将是二次的。与 [15], [17] 相比，将初始化细分为更简单的子问题，导致了非常高效的方法。

所提出的初始化方法能够可靠地估计重力、偏置、尺度和速度，从而开始融合 IMU 信息。对于 EuRoC 数据集，我们观察到 MAV 探索 15 秒总能提供准确的初始化。未来的工作中，我们希望研究一种自动判定初始化成功的标准，因为我们发现仅依赖条件数的绝对阈值不够可靠。

B. SLAM 评估与对比现有技术

Note : 图 5. 我们的方法与当前最先进的直接双目视觉-惯性里程计方法 [6] 的相对位姿误差（RPE）[25] 比较。由于地图重用，我们的 SLAM 系统在较长路径上误差不会增长，而视觉-惯性里程计方法的漂移无法被补偿。请注意，[6] 使用的是双目，而我们的结果是单目。

我们在 EuRoC 数据集的 11 个序列上评估了 Visual-Inertial ORB-SLAM 的精度。在 MAV 开始探索时启动序列的处理。局部 BA 的局部窗口大小设为 10 个关键帧，IMU 初始化在单目 ORB-SLAM 初始化 15 秒后进行。系统在 IMU 初始化后立即执行一次全局 BA。表 I 显示了每个序列的关键帧轨迹平移均方根误差（RMSE），计算方式参考文献 [24]。我们使用原始的 Vicon 和 Leica 地面真实数据作为对比，因为后处理的地面真实数据已经使用了 IMU。我们观察到视觉惯性传感器与原始地面真实数据在 Vicon Room 2 序列中有 -0.2 秒的时间偏移，在 Machine Hall 序列中有 0.2 秒的偏移，我们在对齐两条轨迹时进行了校正。这个比例因子可以看作是轨迹和重建的残余尺度误差。我们的系统能够实时处理所有这些序列，除了 V1_03_difficult 序列，其中运动过于剧烈，单目系统无法存活 15 秒。我们的系统能够以公制尺度恢复运动，尺度误差通常低于 1%，在 30 平方米的房间环境中达到了典型的 3 厘米精度，在 300 平方米的工业环境中达到了 8 厘米精度。为了显示由于尺度误差带来的精度损失，我们还显示了如果系统能够恢复真实尺度的 RMSE，参见 GT 尺度列。我们还展示了通过在执行结束时进行视觉-惯性全局 BA，可以进一步提高精度和尺度估计，详见 Full BA 列。序列 V1_02_medium 的重建结果可见图 1，以及附带的视频。

为使我们的结果更具背景性，我们在表 I 中将仅视觉系统的结果作为基准。我们的视觉-惯性系统更为稳健，能够处理 V2_03_difficult 序列，能够恢复公制尺度且不受尺度漂移的影响。视觉-惯性系统的精度类似于仅视觉系统在理想情况下恢复真实尺度时的精度。然而，正如在第 III-B 节中所述，视觉-惯性束调整的计算成本更高，因此局部 BA 的局部窗口必须比仅视觉系统的小。这也解释了 GT 尺度视觉-惯性结果在没有全局 BA 时略差的表现。实际上，视觉-惯性全局 BA 通常在 7 秒内的 15 次迭代中收敛，而仅视觉全局 BA 在不到 1 秒的 5 次迭代中收敛。

最后，我们将视觉-惯性 ORB-SLAM 与当前最先进的用于双目相机的直接视觉-惯性里程计方法 [6] 进行了比较，该方法在 Vicon Room 1 序列中展示了结果，从而可以进行直接比较。图 5 显示了相对位姿误差（RPE）[25]。为了计算我们方法的 RPE，需要恢复帧轨迹，因为我们的后端仅优化关键帧。为此，在跟踪每一帧时，我们存储相对于参考关键帧的相对变换，以便在执行结束时从估计的关键帧位姿中检索出帧的位姿。实验结束时，我们没有执行全局 BA。可以看到，视觉-惯性里程计方法的误差随着行驶距离增加而增大，而我们的视觉-惯性 SLAM 系统由于重用地图而不会累积误差。双目方法 [6] 能够在 V1_03_difficult 序列中运行，而我们的单目方法失败。我们的单目 SLAM 成功恢复了度量尺度，并在短路径上达到相当的精度，在这种情况下，与里程计相比，SLAM 的优势不明显。这是我们基于特征的单目方法的显著成果，相较于 [6] 的直接法和双目系统，表现尤为突出。

6.结论

我们提出了一种新型的紧耦合视觉-惯性 SLAM 系统，能够实时闭环，并在已映射区域内重用地图进行传感器定位，从而实现零漂移定位，相较于漂移不断累积的视觉里程计方法更为出色。实验表明，我们的单目 SLAM 高精度地恢复了公制尺度，并在同一环境中持续定位时优于当前最先进的双目视觉-惯性里程计方法。我们认为，这种零漂移定位对虚拟/增强现实系统尤为重要，因为在用户操作于同一工作空间时，预测的用户视角不应出现漂移。此外，我们预计通过使用双目或 RGB-D 相机可以获得更好的精度和鲁棒性，同时简化 IMU 初始化过程，因为尺度已知。我们提出的 IMU 初始化的主要弱点在于它依赖于单目 SLAM 的初始化。我们希望进一步研究利用陀螺仪来加速和增强单目初始化的速度和鲁棒性。

参考

Visual-Inertial Monocular SLAM with Map Reuse 论文笔记

---