Visual-Inertial Monocular SLAM with Map Reuse

摘要-近年来，在视觉里程计技术中有许多优秀的作品，其目的是比较高精度，鲁棒地计算传感器增量运动。然而这些方法都缺乏回环的能力，即使传感器连续重新回到同一地方，轨迹的估计会累积漂移。本文提出了一种新颖的轻耦合的视觉惯导SLAM系统，能够回环检测以及重用地图来获得无漂移的定位模式。本方法可以应用于任何相机配置，本文解决了单目相机大部分的问题，众所周知的尺度不确定性。我们也提出了一种新颖的IMU初始化方法，其计算了尺度，重力方向，速度和陀螺仪加速度计偏置，在几秒钟的时间内且精度高。在最近的微无人机公开数据集上测试获得了典型的尺度误差在1%且厘米的精度。相比于当前的VIO在重新访问场景，本方法提供了更好的精度由于地图的重用和无漂移的累计。

1、介绍

从板载传感器的运动估计在移动机器人和计算机视觉社区是一个热门的话题，它使得新型的技术出现比如，无人驾驶，增强和虚拟现实，服务机器人以及无人机导航。在不同的传感器配置中，视觉惯导配置提供了一个便宜但很大潜能的解决方案。一方面，其可以构建3D模型，对相机进行定位以及识别已经访问过的地方。另一方面，IMU传感器提供了自运动信息，允许恢复单目视觉的尺度信息，用于估计重力的方向，使绝对俯仰和横滚变得可观测。

视觉惯导融合在近年来已成为广泛的研究课题。最近的研究主要集中在紧耦合（联合优化所有的传感器状态）视觉里程计，使用滤波或者基于关键帧的非线性优化。然而，这些方法仅能够计算递增的运动且不能够闭环以及重新利用已经构建的地图。这表明估计的轨迹会累积误差，即使传感器回到了相同的地点。这由于对过去状态的边缘化来维持一个常数的计算或者完全的平滑的使用，在探索中用一个常量的复杂度，但是可能代价会很大正如在回环检测中的块方法。滤波方法能够拓扑上闭环以及重新利用地图，但是全局单位一致性不能实时获得。今年的系统能够重新利用地图，离线构建，执行视觉里程计的跟踪。

基于预积分[11]，它应用SO3的流型，以及由因子图来表示，本文我们提出了视觉惯导ORB-SLAM，据我们所知，基于第一帧的视觉惯导SLAM能够实时的闭环，且重用在线构建的地图。基于ORB-SLAM方法，我们的跟踪优化当前帧，假定地图固定，我们的后端执行局部的BA，优化一个关键帧的局部窗口，包括一个固定帧的外部窗口。相比于完全的平滑，本方法允许常量时间的局部BA，且不会对过去状态进行边缘化，能够重用他们，使用位置识别方法来检测大的回环，且用轻量级的位姿图优化来纠正他们，用完整的BA在独立的线程中，不会影响到实时性。图1表明用我们的系统在连续回到相同地方的序列中可以重构环境。

我们的跟踪和局部BA修正了他们在优化过程中的状态，其可能有偏置的结果。因为这个原因，我们需要一个可靠的视觉惯导初始化，其提供了一个精确的状态估计在开始修正状态之前。最后，我们提供了执行视觉惯导完全的BA，提供了结构，相机位姿，尺度，速度，重力陀螺仪和加速度计偏置的最优方案。完全的BA是一个非线性优化，要求一个好的初始值来收敛。在IV部分，我们提供了一个分治的方法来计算初始值。首先我们用纯单目ORB-SLAM来处理一些帧，估计一个初始结果对于结构和关键帧的位姿，直到一个未知的尺度因子。然后我们计算陀螺仪的偏置，其可以很容易从一直的关键帧方向中获得，以便我们可以正确的旋转加速度计的测量。然后我们解决尺度和重力在没有考虑加速度计偏置情况下，用[11]的方法。为了方便区分重力和加速度偏置，我们使用重力大小的知识，并求解加速度计偏差、细化尺度和重力方向。这样就可以从所有的关键帧中直接获得速度。我们的实验验证了这是有效的，可靠的，精确的初始化方法，更一般的是，可以被应用于基于关键帧的单目SLAM，不用假设初始条件和要求传感器的移动使得所有变量可观测。然而之前的方法联合解决了视觉和IMU，要么忽略了陀螺仪或者加速度计的偏置，我们通过一个简单的步骤来分开这个问题有效计算所有的变量。

II 视觉惯导前置知识

我们的视觉惯导ORB-SLAM的输入是一个IMU观测和单目相机的流。我们用一个映射函数牌来表示一个常规的针孔模型,R 3 → Ω，其将3D点Xc在相机坐标系C中转换到了图像平面xc中的2D点。

[fu fv]T是焦距，[cu cv]T是主点。映射函数并没有考虑由于相机镜头带来的畸变。当提取图像中的关键点时，我们对他们的坐标系进行去畸变以便他们可以使用[1]与映射的点进行匹配。

IMU，其参考系表示为B，测量传感器的加速度aB和角速度ωB，在时间间隔δt内，通常是数百HZ的频率。两个测量都会被影响，加上传感器噪声，由缓慢变化的加速度计偏置ba和陀螺仪偏置bg。并且加速度计受重力gW的影响，需要减去这个影响来计算运动。IMU方向Rwb（SO3）、位置wPb，速度wVb离散的推进，在世界坐标系W中，可以用以下计算：

在连续两帧之间的运动可以用预积分δR，δv和δP从这两帧之间的所有测量来定义。我们使用最近的IMU预积分描述

其中Jacobian J a（·）和J g（·）解释了改变偏差的影响的一阶近似，而无需显式地重新计算预积分。预积分和雅克比都可以随着IMU测量的到来有效的迭代计算。

相机和IMU被认为是刚体连接在一起的，之间的变换Tcb = [Rcb|cPb]来自于标定的他们参考系统。

III、视觉惯导 ORB-SLAM

本文的视觉惯导系统是基于ORB-SLAM的。本系统有三个并行的线程用于跟踪，局部建图和回环检测。本系统被设计于工作在大规模环境中。通过构建一个共视图，其允许用于跟踪和建图来恢复局部地图通过执行轻量级的位姿图优化在回环的时候。并且ORB-SLAM允许构建环境的地图切换到一个使用更少CPU的纯定位模式（建图和回环检测被屏蔽），由于系统的重定位能力。ORB-SLAM是开源的，在多个公开数据集上获得了很好的效果。本部分我们详细讨论在跟踪，局部建图，回环检测相对于原始系统改变的部分，视觉惯导初始化在IV部分描述。

A、跟踪

我们的视觉惯导跟踪负责跟踪相机位姿，速度和IMU的偏置，以帧的频率来执行。本方法允许我们可靠的预测相机的位姿，而不是使用点到点的运动模型在原始的单目系统那样。一旦相机位姿被预测了，在局部地图中的地图点被映射且与帧中的关键点进行匹配。然后通过最小化所有匹配点特征的重投影误差和IMU的误差项来优化当前帧j。这中优化被更新的地图或者不是局部建图或者二回环检测线程，是不同的，如图2

如图2-a，当跟踪在地图更新之后被执行，IMU误差项将当前帧j与上一帧i相连接：

ρ是损失函数。我们使用g2o里面的高斯牛顿解决了优化问题。在优化之后，图2-b，估计结果和Hessian矩阵作为下一次优化的先验。

假设没有地图更新图2-c，下一帧j+1会用连接到帧j和用上一次优化的结果作为先验进行优化：

B、局部建图

局部建图线程执行局部的BA在一个新的关键帧插入之后。它优化最近的N帧（在局部窗口内），以及这N帧中所有观测到的点。所有其他的关键帧分享局部点的观测（例如，在共视图中连接到局部的关键帧的点），但是不在局部窗口中，贡献于全部的代价但是在优化中是固定的（固定窗口）。关键帧N+1总是在固定的窗口内，因为它包含了IMU的状态。图3描述了在原始ORB-SLAM中局部BA与视觉惯导ORB-SLAM的不同：

结合IMU误差项的代价函数和重投影误差性。注意视觉惯导版本只与视觉版本进行比较，更复杂，因为有9个额外的状态(速度和偏置)对每一帧要优化。一个合适的局部窗口的大小被选择用于实时性能。

局部建图主要负责关键帧的管理。原始的ORB-SLAM策略抛弃冗余的关键帧，以便地图大小不会增长如果在一个较好的构建地图区域内中定位的话。这个策略是系统性的当使用IMU信息时。这对连续的关键帧的运动有了约束。短时在连续关键帧之间的不同性越长，IMU提供的信息就越若。因此，我们允许构图时丢弃冗余的关键帧， 如果这不会使局部BA的局部窗口中的两个连续关键帧的差异超过0.5秒。 为了能够执行完全的BA，在一个回环检测之后或者任何时候精细化地图，我们不允许任何两个关键帧之后的不同超过3s。如果我们关掉全局BA和IMU的约束，我们只需要去限制在局部窗口中的两帧之后的临时偏移。

C、回环检测

回环检测线程旨在减少在探索过程中的累积误差，当返回到已知区域时。位置识别模块将最近的一帧与历史的帧进行匹配。这个匹配被验证通过计算一个刚体变换，这个变换对齐了两帧之间的匹配点。最后一个优化被执行来减少轨迹中的累积误差。这个优化在大地图中非常耗时，因此执行一个位姿图优化需要策略。当减少复杂性，结构被忽略，表现良好的收敛。与最初的ORB-SLAM相比，我们执行位姿图优化在6自由度，而不是7自由度，因为尺度是可观测的。位姿图忽略了IMU信息，不会优化速度或者IMU偏置。因此通过旋转他们根据纠正的相关关键帧的方向来纠正速度。然而这不是最优的，偏置和速度应当局部精确于继续使用IMU信息在位姿图优化之后。随后我们执行一个全局的BA在并行的线程中优化所有的状态包括速度和偏置。

IV、IMU初始化

本部分我们提出一种方法来计算初始估计用于视觉惯导的全局BA，包括尺度，重力方向，速度和IMU偏置，由一个单目SLAM算法处理的关键帧集合。本想法是运行单目SLAM几秒钟，假定传感器执行一个运动，该运动使得所有变量可观测。尽管我们基于ORB-SLAM，但是任何其他SLAM可以被使用。唯一的要求是任何两个连续的关键帧在时间上是靠近的(如III-B部分)，为了减少IMU的噪声积分。

初始化被分成两个更简单的子问题：1）陀螺仪偏置被估计；2）尺度和重力近似，认为没有加速度偏置；3）加速度计偏置估计，尺度和重力方向修正；4）速度估计。

A、陀螺仪偏置估计

陀螺仪偏置可以被估计从两个连续的帧的已知方向中。假设一个可忽略的偏置变化，我们优化一个常偏置bg，这最小化来自于相对方向的计算和陀螺仪积分之间的不同，对于所有的连续帧的匹配：

N是关键帧的数量。

B、尺度和重力近似（没有加速度计偏置）

一旦我们估计了陀螺仪偏置，我们可以预积分速度和位置，正确旋转加速度测量来弥补陀螺仪偏置。

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

将（10）代入与两个连续关键帧（3）的位置相关的方程式，并忽略此时的加速度计偏差，如下所示

目标是通过解决关于这些变量的线性系统方程来估计s和gw。为了避免解决N 速度，减少复杂性，我们考虑三个连续关键帧之间的两个关系（11），并在（3）中使用速度关系，这导致以下表达式：

C、加速度计偏置估计，尺度，重力方向精炼

到目前为止，我们还没有考虑加速度计偏置，当计算尺度和重力的时候。知识融合加速度计偏置在(12)中，这极大的增加了系统病态的机会，因为重力和加速度计偏置很难区分。为了增加可观测性，我们引进新的信息，到目前为止我们嗐没有考虑，这就是重力大小G。考虑一个惯性系I，重力方向为ĝ I = {0, 0, −1},

将重力向量表达如下：

D、速度估计

在方程(12)和(19)中，我们考虑三个连续关键帧之间的关系，从而得到的线性系统不具有对应于速度的3N个附加未知数。现在可以使用方程（18）计算所有关键帧的速度，因为已知比例、重力和偏移。为了计算最近关键帧的速度，我们使用（3）中的速度关系。

E、偏置重新初始化在重定位之后

在很长时间之后，当系统重定位时，使用位置识别，我们重新初始化陀螺仪偏置由公式(9)来解决。加速度计偏置由简化的(19)来估计。在这儿唯一不知道的是偏置，因为尺度，重力已经知道了。我们使用20个连续的仅视觉定位的帧来估计两种偏差。

V、实验

、、、、、、

VI、结论

本文提供了一个新颖的视觉惯导SLAM系统，也就是能够实时回环检测，并在已知地图里面重新定位传感器。这允许获得灵偏移的定位，相比于视觉里程计方法。实验证明本单目SLAM系统可以精确恢复尺度，并获得更好的精度比当前的双目视觉惯导里程计。当在相同环境连续定位时。我们认为这种零漂移定位对于虚拟/增强现实系统特别感兴趣，其中当用户在同一工作空间中操作时，预测的用户视点不能漂移。并且我们期望获得精度更高鲁棒性更好通过使用双目和RGB-D相机，这会简化IMU初始化，因为尺度是已知的。我们提出的IMU初始化主要的不足是依赖于单目SLAM系统的初始化。我们能够研究陀螺仪的使用来时单目初始化更快，更鲁棒。