VINS-Mono论文总结

Abstract

由相机和低成本惯性测量单元（IMU）组成的单目视觉惯性系统：VINS，是度量六自由度（DOF）状态估计的最小传感器组件。然而，不能直接测量距离，在IMU处理，估计器初始化，外部校准和非线性优化方面带来了重大挑战。
在这项工作中，我们介绍了VINSMono：一种功能强大且用途广泛的单目视惯状态估计器。
我们的方法从用于估计器初始化和失败恢复的robust的过程开始。是一种紧耦合的，基于非线性优化的方法，通过融合IMU预积分值和特征观测值来获得高精度的视惯里程计。环路检测模块与紧耦合的公式相结合，可以以最小的计算开销实现重定位。还执行了四个自由度的pose-graph优化，以增强全局一致性。
我们在公开数据集和实际实验中验证了我们系统的性能，并与其他最新算法进行了比较。
我们还在MAV平台上执行机载闭环自主飞行，并将算法移植到基于iOS的演示中。我们强调提出的工作是一个可靠，完整和通用的系统，适用于需要高精度定位的不同应用程序。
PC1和iOS移动设备2实现开源

Introduction

单目相机：无尺度信息
单目+IMU：具有尺度信息，以及侧倾角和俯仰角，均可观察到。 这样可以启用需要度量状态估计的导航任务。 此外，IMU的集成可通过弥补由于照明变化，无纹理区域或运动模糊而导致的视觉轨迹损失带来的误差，来显着改善运动跟踪性能。

但是，所有这些优点都是有代价的。 对于单目VINS，需要加速度才能观察到scale信息。 这意味着单目VINS估计器不能从静止状态开始，而只能从未知的运动状态开始。 同样，认识到视觉惯性系统是高度非线性的这一事实，我们看到了在估计器初始化方面的重大挑战。另外，cameraIMU的外参校准也至关重要。 最后，为了消除窗口中的长期漂移，必须开发一个包括视觉惯性里程计，回路检测，重定位和全局优化的完整系统。

我们的解决方案从动态估算器初始化开始，相同的初始化模块也用于失败恢复。 核心是基于紧耦合的滑动窗口非线性优化的单目VIO。单目VIO模块不仅可以提供准确的local pose，速度和方向估计，还可以执行camera-IMU外参校准和IMU biases校正（在线）。 使用DBoW2 检测循环。 通过与单目VIO进行特征级融合，可以以最少的计算开销实现可靠而准确重新定位。 最后，将经过几何验证的循环添加到pose graph中，因为单目VIO可以观察到侧倾角和俯仰角，所以执行了4-DOF 的pose graph以确保全局一致性。

VINS-Mono结合并改进了我们先前在单目视觉-惯性融合方面的工作。与之前的工作相比，VINS-Mono的进一步改进包括改进的有bias校正的IMU预积分，紧耦合的重定位，全局pose graph优化，广泛的实验评估以及可靠且通用的开源实现。

整个系统是完整且易于使用的。 它已成功应用于小型AR场景，中型无人机导航和大型状态估计任务。 与其他最先进的方法相比，它具有优越的性能。
贡献总结如下：

1. 强大的初始化过程，能够从未知的初始状态引导系统。
2. 紧耦合，基于优化的单目视惯里程计，具有camera-IMU外参校准和IMU biases估计。
3. 在线回环检测和紧密耦合的重定位。
4. 4-DOF 的pose graph优化
5. 无人机导航，大规模定位和移动AR应用程序的实时性能演示。

•在线循环检测和紧密耦合的重新定位。
•四个自由度全局姿势图优化。
•无人机导航，大规模本地化和移动AR应用程序的实时性能演示。
•开源集成在ROS上的PC版本，以及开源iPhone6s或更高版本上运行的iOS版本

Related Work

松耦合传感器融合：

处理视觉和惯性测量的最简单方法，将IMU视为独立模块，协助从SfM得到的仅视觉的pose估计。融合通常由扩展卡尔曼滤波器（EKF）完成，其中IMU用于状态传播，而仅通过视觉来更新pose

紧耦合传感器融合：

基于EKF或图优化，共同优化camera和IMU的测量。
一种流行的基于EKF的VIO方法是MSCKF，它在状态向量中保持几个先前的相机pose，在多个相机视图中看到同一特征，形成多个约束，来进行更新。
SR-ISWF是MSCKF的扩展。它使用平方根形式来实现单精度表示并避免不良的数值属性。这种方法采用逆滤波器进行迭代线性化，使其等同于基于优化的算法。批处理图优化或BA技术可维护和优化所有测量值以获得最佳状态估计。为了获得恒定的处理时间，流行的基于图的VIO方法通常通过边缘化过去的状态和测量值，在最近状态的有限大小的滑动窗口上进行优化。
由于对非线性系统进行迭代求解的高计算要求，因此很少有基于图的算法能够在资源受限的平台（如移动电话）上实现实时性能。

对于视觉测量处理，可以根据视觉残差模型的定义将算法分为直接方法或间接方法。 直接方法使光度误差最小，而间接方法使几何位移最小。
直接方法需要良好的初始化，而间接方法在提取和匹配特征时会消耗额外的计算资源。
由于间接方法的成熟性和鲁棒性，因此在实际工程部署中更为常见。 但是，直接方法更容易扩展到dense mapping，因为它们直接在像素级别上操作。

IMU通常以比摄像机更高的速率获取数据，已经提出了不同的方法来处理高速率IMU测量。 最简单的方法是在基于EKF的方法中使用IMU进行状态传播。 在图优化中，开发了一种有效的技术，称为IMU预积分，以避免重复的IMU重积分，该技术使用Euler角表示旋转误差。

准确的初始值对于引导任何单目VINS都至关重要。有一种线性估计器初始化的方法，利用了来自短期IMU预积分的相对旋转。但是，该方法无法对陀螺仪的偏置进行建模，也无法对原始投影方程中的传感器噪声进行建模。在实际应用中，当视觉特征离传感器较远时，会导致初始化不可靠。有人提出了一种单目视觉惯性初始化问题的闭式解。后来有人提出了通过添加陀螺仪bias校准来扩展这种封闭形式的解决方案的方法。这些方法无法对惯性积分的不确定性进行建模，因为它们依赖于IMU测量值在较长时间内的双重积分。后来有人提出了一种基于SVO 的重新初始化和恢复算法。这是一种基于松耦合融合框架的实用方法。但是，需要一个额外的传感器来恢复scale。后来有人引入了基于流行的ORB-SLAM构建的初始化算法。给定一组来自​​ORBSLAM的关键帧，对视觉惯性完整BA计算出比例，重力方向，速度和IMU bias的初始估计。但是，尺度收敛所需的时间可能会超过10秒。这可能会给需要一开始就进行比例估算的机器人导航任务带来麻烦。

VIO方法，无论依赖的基础数学公式是什么，都在全局平移和方向上存在漂移。 为此，回环对于长期操作起着重要作用。 ORBSLAM能够闭环和重用地图，这利用了词袋的优势。闭环检测之后是7自由度（位置，方向和尺度）pose graph优化。 相比之下，对于单目VINS，由于添加了IMU，因此仅在4自由度（即3D平移）中发生漂移，并且绕重力方向（偏航角）旋转。 因此，我们选择在最小4 DOF设置中使用回环约束优化pose graph

Overview

提出的单目视觉惯性状态估计器的结构如下图所示。
该系统从测量预处理开始，在该过程中提取和跟踪特征，和两个连续帧之间的IMU预积分。
初始化过程提供了所有必要的值，包括姿态，速度，重力矢量，陀螺仪偏置和3D特征位置，以引导随后的基于非线性优化的VIO。
带有重定位模块的VIO紧密融合了IMU预积分、特征检测、回环特征检测。最后，pose graph优化模块拿到经过几何验证的重定位结果，并进行了全局优化以消除漂移。
VIO，重定位和pose graph优化模块在多线程设置中同时运行，每个模块具有不同的运行速率和实时保证，以确保始终可靠运行。

Measurement Preprocessing

本节介绍了惯性和单目视觉测量的预处理步骤。 对于视觉测量，我们跟踪连续帧之间的特征并检测最新帧中的新特征。 对于IMU测量，我们将它们在两个连续帧之间进行预积分。 请注意，我们使用的低成本IMU的测量受偏置和噪声的影响。 因此，我们在IMU预整合过程中尤其要考虑bias。

A. Vision Processing Front-end

对于每个新图像，KLT稀疏光流算法会跟踪现有特征。 同时，检测到新的corner特征，以在每个图像中保持最少数量（100-300）个特征。 detector通过设置两个相邻特征之间的最小像素间距来实现均匀的特征分布。 2D特征首先是不变形的，然后丢掉outlier后投影到单位球体（使用RANSAC检测outlier）

在此步骤中也选择关键帧，有两个关键帧选择标准：

1. 跟踪的特征的平均视差在当前帧和最新关键帧之间，并且超出某个阈值，则将帧视为新关键帧。平移、旋转都会引起视差，但是，不能在仅旋转运动中进行triangular。为了避免这种情况，我们在计算视差时使用陀螺仪测量的较短积分来补偿旋转。请注意，此旋转补偿仅用于关键帧选择，而在VINS公式中不参与旋转计算。因此，即使陀螺仪包含较大的噪声或存在偏差，也只会导致关键帧选择结果欠佳，并且不会直接影响估计质量。

2. 跟踪数量，如果跟踪的特征的数量低于某个阈值，则我们将此帧视为新的关键帧。此标准是为了避免完全丢失特征轨迹。

B. IMU Pre-integration

IMU预积分用欧拉角来表示旋转误差
式1：

在base frame中的IMU测量值结合了抵抗重力的力和平台动力，并且受到加速度偏差ba，陀螺仪偏差bw和附加噪声的影响。
假设加速度和陀螺仪测量中的附加噪声为高斯，na〜N（0，σ²a），nw〜N（0，σ²w）。
加速度偏差和陀螺仪偏差建模为random walk（随机游走），其导数为高斯，nba〜N（0，σ²ba），nbw〜N（0，σ²bw）
式2：

给定两个对应于图像帧bk和bk + 1的时刻，位置、速度和方向状态可以通过惯性测量在世界坐标系的时间间隔[tk，tk + 1]内传播：

∆tk是时间间隔[tk，tk + 1]之间的持续时间。 可以看出，IMU状态传播需要帧bk的旋转，位置和速度。 当这些起始状态改变时，我们需要重新传播IMU测量值。 特别是在基于优化的算法中，每次我们调整pose时，我们都需要在它们之间重新传播IMU测量值。 这种传播策略对计算要求很高。 为了避免重新传播，我们采用预积分算法。
将参考坐标系从世界坐标系更改为 body frame bk之后，我们只能预先积分与线性加速度aˆ和角速度ωˆ有关的部分，如下所示：
式5：

式6：

可以看出，通过将bk作为参考坐标系，可以仅通过IMU测量获得预积分项。 α(bk bk+1)，β(bk bk+1)，γ(bk bk+1)仅与IMU bias有关，而不与bk和bk+1中的其他状态有关。
当bias的估计值发生变化时，如果变化很小，则通过相对于bias的一阶近似来调整α(bk bk+1)，β(bk bk+1)，γ(bk bk+1)，否则进行重新传播 。
这种策略为基于优化的算法节省了大量计算资源，因为不需要重复传播IMU测量值。

开始时， α(bk bk+1)，β(bk bk+1)为0，而γ(bk bk+1)为单位四元数。 α，β，γ的平均值如下逐步传播。
请注意，附加噪声项na，nw是未知的，在实现中被视为零。 这将得出积分前项的估计值，并用ˆ（·）标记：

δt是两次IMU测量i和i + 1之间的时间间隔
i是与[tk，tk + 1]内的IMU测量相对应的离散的时刻

然后处理协方差传播，将误差项定义为围绕其均值的扰动：

式8：

其中δθ（bk t）是三维小扰动

我们可以得出误差项为式6 连续时间线性化：

P（bk bk + 1）可以通过一阶离散时间协方差更新（初始协方差P（bk bk + 1）= 0）进行递归计算：

其中Q是噪声的对角协方差矩阵(σ²a,σ²w,σ²ba,σ²bw）

同时，还可以在初始雅可比行列式Jbk = I的情况下递归计算出相对于δz（bk bk ）的δz（bk bk + 1）的一阶雅可比矩阵Jbk + 1

使用该递归公式，我们得到协方差矩阵P（bk bk + 1）和雅可比行列式Jbk + 1。 相对于偏差的α（bk bk + 1），β（bk bk + 1），γ（bk bk + 1）的一阶近似可以写为：

现在我们可以写下IMU测量模型及其对应的协方差P（bk bk + 1）：

Estimator Initialization

单目紧耦合的视觉惯性里程计是一个高度非线性的系统。 由于无法直接从单目相机观察到scale，因此很难在没有良好初始值的情况下直接融合这两个量。 可以假设静止的初始条件来启动单目VINS估计器。 但是，这种假设是不合适的，因为在实际应用中经常会遇到运动中的初始化。 当IMU测量值被大bias破坏时，情况变得更加复杂。 实际上，对于单目VINS，初始化通常是最脆弱的步骤。 需要一个可靠的初始化过程来确保系统的适用性。

我们采用一种松耦合传感器融合方法来获取初始值。 我们发现纯视觉SLAM或 SfM 具有良好的初始化属性。 在大多数情况下，仅视觉系统可以通过从相对运动方法（例如八点或五点算法或估计齐次矩阵）中导出初始值来引导自身。 通过将IMU预积分与仅视觉SfM结果相匹配，我们可以大致恢复比例，重力，速度甚至偏差。 如下图所示，这足以引导非线性单目VINS估计器。

估计器初始化的视觉惯性匹配过程：

在初始化阶段同时估算陀螺仪和加速度计的偏置，我们选择在初始步骤中忽略加速度计的偏置项。 加速度计的偏置与重力相关，并且由于重力矢量较大，并且初始化阶段的时间相对较短，因此很难观察到这些偏置项。

A. Sliding Window Vision-Only SfM

初始化过程从仅视觉的SfM开始，以估计相机pose和特征位置的图形

我们为有限的计算复杂度维护了一个滑动窗口框架。首先，我们检查最新帧和所有先前帧之间的特征对应关系。如果我们在滑动窗口中的最新帧和任何其他帧之间找到稳定的特征跟踪（超过30个跟踪的特征）和足够的视差（超过20个旋转补偿像素）。我们使用五点法恢复了这两个帧之间的相对旋转和平移。否则，我们会将最新的帧保留在窗口中，然后等待新的帧。如果五点法成功，我们将随机设置scale并对在这两个帧中观察到的所有特征进行triangular。基于这些三角特征，使用PnP方法来估计窗口中所有其他帧的pose。最后，应用全局BA来使所有特征观测的总重投影误差最小。由于我们尚不了解世界坐标系，因此将相机的初始相机坐标系c0设置为SfM的参考坐标系。所有帧pose特征位置均相对于c0表示。假设我们在摄像机和IMU之间有一个的外参，我们可以将pose从相机坐标系转换为body（IMU）坐标系

其中s的缩放参数，用于将视觉结构与scale匹配， 解决此缩放参数是成功初始化的关键。

B. Visual-Inertial Alignment

1.陀螺仪偏置校准：
考虑窗口中的两个连续帧bk和bk + 1，我们从视觉SfM中获得旋转q（c0 bk）和q（c0 bk + 1），以及IMU 预积分得到相对约束γˆ（bk bk + 1） 。 我们将关于陀螺仪偏置的IMU预积分项线性化，并最小化以下成本函数：

其中B是窗口中的所有帧。 我们使用推导的雅可比矩阵得出相对于陀螺仪偏置的γˆ（bk bk + 1）的一阶近似值。 这样，我们得到了陀螺仪偏置bw的初始校准。 然后，我们使用新的陀螺仪偏置重新传播所有IMU预积分项αˆ（bk bk + 1），βˆ（bk bk + 1）和γˆ（bk bk + 1）。

2.速度、重力向量和米制尺度初始化：
速度，重力矢量和scale初始化：初始化陀螺仪偏置后，我们继续初始化导航的其他基本状态，即速度，重力矢量和scale：

可以看出，Rbkc0 ,Rbk+1c0,Pckc0 ,Pck+1c0是从上尺度单目视觉中得到的，∆tk是两个连续帧之间的时间间隔。通过求解这个线性最小二乘问题：

我们可以获取窗口中每个帧的body frame的速度，视觉参考坐标系（·）c0中的重力矢量以及scale参数

3.重力精化
通过约束量值，可以对原线性初始化步骤得到的重力向量进行细化。在大多数情况下，重力矢量的大小是已知的，因此重力矢量仅剩下2个自由度。 所以我们在切线空间上使用两个变量对重力进行重新参数化。
我们的参数化将重力矢量表示为g·gˆ + w1b1 + w2b2，其中g是重力的已知大小，gˆ是表示重力方向的单位矢量，b1和b2是跨越切平面的两个正交基，如下图所示，w1和w2分别是朝b1和b2的对应位移。 我们可以使用算法1通过叉积运算找到一组b1，b2。然后用g·gˆ + w1b1 + w2b2替换（17）中的g，并与其他状态变量一起求解w1和w2。 重复此过程直到gˆ收敛。

4.完成初始化：
完善重力矢量后，我们可以通过世界坐标系和相机坐标系c0之间的旋转q将重力旋转到z轴。 然后，我们将所有变量从参考坐标系c0旋转到世界坐标系w。 body坐标系的速度也将旋转到世界坐标系。 视觉SfM中的平移部分将按比例缩放。 至此，初始化过程完成，所有这些度量值将馈入紧耦合的单目VIO。

Tightly-coupled Monocular VIO

估算器初始化后，我们进行基于滑动窗口的紧耦合单目VIO，以实现高精度和鲁棒状态估算。 下图所示为滑动窗口的图解。

A. Formulation

其中xk是第k张图像被捕获时的IMU状态。 它包含IMU在世界框架中的位置，速度和方向，以及IMU主体框架中的加速度偏差和陀螺仪偏差。 n是关键帧的总数，m是滑动窗口中的特征的总数。λl是第一次观测到的l特征的逆深度
我们使用视觉惯性BA公式， 将所有测量残差的先验和Mahalanobis范数的总和最小化，以获得最大的后验估计：

rB和rC分别是IMU和视觉测量的残差。 B是所有IMU测量值的集合，C是在当前滑动窗口中至少观察到两次的特征集。 {rp，Hp}是来自边缘化的先验信息。 Ceres Solver 用于解决该非线性问题。

B. IMU Measurement Residual

考虑滑动窗口中两个连续帧bk和bk + 1内的IMU测量，根据（13）中定义的IMU测量模型，预积分IMU测量的残差可定义为：

其中xyz提取四元数q的矢量部分以表示误差状态。 δθ是四元数的三维误差状态表示。 [ αˆ，βˆ，γˆ]T是在两个连续图像帧之间的间隔时间内仅使用噪声加速度计和陀螺仪测量值预积分的IMU测量项。
加速度计和陀螺仪偏置也包含在用于在线校正的剩余项中。

C. Visual Measurement Residual

与在一般图像平面上定义重投影误差的传统针孔相机模型相反，我们在单位球面上定义相机测量残差。 几乎所有类型的摄像机（包括广角，鱼眼或全向摄像机）的光学元件都可以建模为连接单位球表面的单位射线。 第一次在第i幅图像中观察到的第l个特征，第j幅图像中的特征观测的残差定义为：

其中[ulci,vlci]是第一次观测第一次出现在i图像中的第l个特征。[u^lci,vlci]是对jth图像中相同特征的观察。πc−1是利用摄像机内参数将像素位置转换成单位向量的反投影函数。由于视觉残差的自由度是2，所以我们将残差向量投影到切平面上，如下图所示，b1,b2是两个任意选择的正交基。我们可以很容易地找到一组b1,b2，如算法1所示。在(22)中使用的P是正切空间中固定长度的标准协方差。

D. Marginalization

为了限制我们基于优化的VIO的计算复杂性，引入了边缘化。 我们从滑动窗口中选择性地边缘化IMU状态xk和特征λl，同时将与边缘化状态相对应的测量值转换为先验值。

如下图所示，当第二个最近的帧是关键帧时，它将停留在窗口中，最旧的帧及其相应的度量被边缘化。 如果第二个最近的帧是非关键帧，则只需删除该帧及其所有相应的视觉度量，但保留连接到该非关键帧的IMU测量。 为了保持系统的稀疏性，我们不会边缘化所有非关键帧的度量。 我们的边缘化方案旨在将空间上分离的关键帧保留在窗口中。 这样可以确保足够的视差来进行特征三角测量，并最大程度地提高了在大的激励下保持加速度计测量值的可能性。

边缘化是使用Schur complement进行的。 我们基于与删除状态相关的所有边缘化度量来构造新的先验。 新的先验将添加到现有先验中。 我们确实注意到，边缘化会导致线性化点的早期固定，这可能会导致估算结果不理想。 但是，由于VIO可以接受小的漂移，因此我们认为边缘化带来的负面影响并不重要。

E. Motion-only Visual-Inertial Bundle Adjustment for Camera-Rate State Estimation

对于计算能力较低的设备（例如手机），由于非线性优化需要大量计算，因此紧耦合的单目VIO无法实现摄像机速率的输出。 为此，我们采用了轻量级的仅运动视觉惯性束调整，以将状态估计提升至摄像机速率（30 Hz）。
仅运动视觉惯性BA的成本函数与（22）中单目VIO的成本函数相同。 但是，我们没有优化滑动窗口中的所有状态，而是仅优化了一定数量的最新IMU状态的姿态和速度。 我们将特征深度，外参，偏置和我们不想优化的旧IMU状态视为常量值。 我们确实将所有视觉和惯性测量用于纯运动束调整。 这比单帧PnP方法有更平滑的状态估计。 下图显示了所建议策略的示意图。与完全紧耦合的单目VIO形成对比的是，在先进的嵌入式计算机上，VIO可能会导致50ms以上的时间，而仅运动视觉惯性束调整 计算仅需5毫秒。 这样可以实现低延迟相机速率的姿势估计，这对于无人机和AR应用特别有用。

F. IMU Forward Propagation for IMU-Rate State Estimation

与视觉测量相比，IMU测量的速率要高得多。 尽管我们的VIO频率受到图像捕获频率的限制，但我们仍可以使用最新的IMU测量值集直接传播最新的VIO估计值，以实现IMU速率性能。 高频状态估计可以用作闭环的状态反馈。

G. Failure Detection and Recovery

尽管我们紧密耦合的单眼VIO在各种挑战性环境和运动中均具有强大的性能。 由于剧烈的照明变化或剧烈的剧烈运动，仍然无法避免故障。 主动故障检测和恢复策略可以提高所提出系统的实用性。 故障检测是一个独立的模块，可检测估计器的异常输出。 我们目前使用以下标准进行故障检测：
•最近一帧中正在跟踪的功能数量少于某个阈值；
•最后两个估算器输出之间的位置或旋转不连续较严重；
•偏置或外参估计值变化较大；
一旦检测到故障，系统将切换回初始化阶段。 单目VIO成功初始化后，将创建pose graph的新的单独部分。

Relocalization

我们的滑动窗口和边缘化方案限制了计算的复杂性，但同时也引入了系统的累积漂移。 更具体地说，在全局3D位置（x，y，z）和围绕重力方向的旋转（偏航）中会发生漂移。 为了消除漂移，提出了与单目VIO匹配的紧耦合的重定位模块。 重定位过程从一个回环检测模块开始，识别已经访问过的地方。 然后在回环候选帧和当前帧之间建立特征级别的连接。 这些特征对应到单目VIO模块中，从而以最小的计算开销实现了无漂移状态估计。 对多个特征的多次观察直接用于重新定位，从而获得更高的准确性和更好的状态估计平滑度。 重新定位过程的图形说明如下图所示。

重定位过程：它从仅VIO的姿势估计（蓝色）开始，记录过去的状态（绿色），如果检测到最新关键帧的循环，如第二个图中的红线所示，则会发生重定位。请注意，由于使用了特征级别的对应关系进行重定位，因此我们能够合并来自多个过去关键帧的回环约束，如最后三个图所示。

A. Loop Detection

我们利用DBoW2 （一种最先进的词袋位置识别方法）进行回环检测。 除了用于单目VIO的corner特征外，BRIEF描述符还检测和描述了500种corner。 corner特征用于在回路检测中实现更好的召回率。 描述符被视为查询可视数据库的可视单词。 在进行时间和几何一致性检查之后，DBoW2返回回环候选对象。 我们保留所有的Brief描述符用于特征检索，但丢弃原始图像以减少内存消耗。

B. Feature Retrieval

当检测到回路时，通过检索特征对应关系来建立局部滑动窗口与回路闭合候选者之间的连接。对应关系通过BRIEF描述符匹配找到。 直接描述符匹配可能会导致大量异常值。 为此，我们剔除outlier，如下图所示。

•2D-2D：使用RANSAC进行fundamental matrix测试。 我们使用当前图像和回环候选图像中检索到的特征的2D观察来执行fundamental matrix测试。
•3D-2D：使用RANSAC进行PnP测试。 基于局部滑动窗口中的知3D位置的特征和回环候选图像中的特征的2D观察，执行PnP测试。

当inliers的数量超过某个阈值时，我们将此候选对象视为正确的回环检测并执行重定位

C. Tightly-Coupled Relocalization

重定位过程有效地将单目VIO保持的当前滑动窗口与过去的pose graph匹配。 在重定位过程中，我们将所有闭环帧的姿势都视为常量。 我们使用所有IMU测量值，局部视觉测量值以及从闭环中检索到的特征对应关系共同优化滑动窗口。 我们可以很容易地为通过闭环框架v观察到的特征编写视觉测量模型，使其与在VIO中进行视觉测量的模型相同，如（25）所示。唯一的区别是，从pose graph或直接从上一个里程计的输出(如果这是第一次重定位)获得的回环帧的姿态(q,p)被视为常数。为此，我们可以在(22)中稍微修改非线性代价函数，增加循环项：环项来稍微修改（22）中的非线性成本函数：

其中L是在回环中观察到的特征的集合。 （l，v）表示在闭环v中观察到的第l个特征。请注意，尽管成本函数与（22）略有不同，但要解决的状态的维数保持不变（因为将回环帧的pose视为常量）。 当使用当前的滑动窗口建立多个回环时，我们同时使用所有帧的所有回环特征进行优化。 这为重定位提供了多视图约束，从而导致更高的准确性和更好的平滑度。 请注意，对过去的pose和回环的全局优化是在重新定位后进行的。

Global Pose Graph Optimization

重定位后，局部滑动窗口将移动并与过去的pose对齐。 利用重定位的结果，增加了这个额外的pose graph优化步骤，以确保将过去的pose集合注册到全局一致的配置中。
由于我们的视觉惯性设置可以完全观察到侧倾角和俯仰角，因此累积的漂移仅发生在四个自由度（x，y，z和偏航角）中。 为此，我们忽略了估计无漂移侧倾和俯仰状态，仅执行4自由度姿态图优化。

A. Adding Keyframes into the Pose Graph

当关键帧从滑动窗口边缘化时，它将添加到pose graph。 该关键帧用作pose graph中的顶点，并且通过两种类型的边与其他顶点连接：

1.序列边：
关键帧将为其先前的关键帧建立多个序列边。 序列边表示局部滑动窗口中两个关键帧之间的相对转换，该值直接从VIO获取。 一个新边缘化的关键帧i及其先前的关键帧j，序列边仅包含相对位置pˆ和偏航角ψˆ。

2.回环边：
如果新边缘化的关键帧具有回环连接，它将通过pose graph中的回环边与回环框架连接。 同样，闭环边仅包含与（27）相同定义的4自由度相对姿势变换。 使用重定位的结果获得闭环边的值

B. 4-DOF Pose Graph Optimization

我们将帧i和j之间的边缘残差最小定义为：

其中φˆ，ˆθi是估计的侧倾角和俯仰角，它们直接从单目VIO获得

通过最小化以下成本函数来优化序列边和回环边的整个图形：

其中S是所有序列边的集合，L是所有回环边的集合。 尽管紧耦合的重新定位已经可以帮助消除错误的回环，但我们添加了另一个Huber范数ρ，以进一步减少任何可能的错误循环的影响。 相比之下，我们不对序列边使用任何规范，因为这些缘是从已经包含足够的异常排除机制的VIO中提取的。

pose graph优化和重定位在两个单独的线程中异步运行。 只要有可能，就可以立即使用最优化的姿pose graph进行重定位。 同样，即使当前的pose graph优化尚未完成，仍可以使用现有的pose graph配置进行重定位。

pose graph优化如上图所示。 当关键帧从滑动窗口边缘化时，会将关键帧添加到pose graph中。 如果此关键帧与任何其他过去的关键帧之间存在回环，则回环约束（公式化为4-DOF相对刚体变换）也将添加到pose graph中。 使用单独线程中的所有相对pose的约束对pose graph进行优化，并且重定位模块始终针对最新的pose graph配置运行。

C. Pose Graph Management

随着距离增加，pose graph的大小可能会无限增长，从长远来看会限制系统的实时性能。 为此，我们实施了下采样过程，以将pose graph数据库保持在有限的大小。 将保留所有具有循环闭合约束的关键帧，而其他过于接近或与相邻对象的方向非常相似的关键帧则可以删除。 关键帧被删除的可能性与其相邻的空间密度成正比。

Experimental Results

我们进行了三个实验和应用到应用程序来评估建议的VINS-Mono系统。 在第一个实验中，我们将提出的算法与公共数据集上的另一种最新算法进行了比较。 我们进行数值分析以显示系统的准确性。 然后，我们在室内环境中测试我们的系统，以评估重复场景中的性能。 进行了大规模实验，以说明我们系统的长期实用性。 此外，我们将建议的系统应用于两个应用程序。 对于空中机器人应用，我们使用VINS-Mono进行位置反馈，以控制无人机遵循预定的轨迹。 然后，我们将方法移植到iOS移动设备上，并与Google Tango进行比较。

A. Dataset Comparison

我们使用EuRoC MAV视觉惯性数据集评估了我们提出的VINS-Mono。 数据集是在微型飞行器上收集的，其中包含双目图像（Aptina MT9V034全局快门，WVGA单色，20 FPS），同步IMU测量（ADIS16448、200 Hz）和ground truth（VICON和Leica MS50）。 我们仅使用左侧摄像机的图像。 在这些数据集中观察到较大的IMU偏置和照度变化

在这些实验中，我们将VINS-Mono与OKVIS进行了比较，OKVIS 是可与单目相机和双目相机配合使用的最新VIO。 OKVIS是另一种基于优化的滑动窗口算法。 如技术部分所述，我们的算法在很多细节上与OKVIS不同。 我们的系统具有完善的初始化和回环功能。 我们使用MH 03中值和MH 05困难两个序列来证明所提出方法的性能。为了公平比较，我们抛出前100个输出，并使用接下来的150个输出与
求真，并比较剩余的估算器输出。

对于序列MH 03中位数，轨迹如上图所示。我们只比较平移误差，因为在该序列中旋转运动可以忽略。 x，y，z误差与时间的关系以及平移误差与距离的关系如下图所示。在误差图中，具有回环功能的VINS-Mono具有最小的平移误差。
我们在MH 05困难数据集结果如下， 所提出的具有回环功能的方法具有最小的平移误差。 由于运动平稳，并且在此序列中没有很大的偏航角变化，因此仅发生位置漂移。 显然，回环能力有效地限制了累积的漂移。

VINS-Mono在所有EuRoC数据集中均表现出色，即使在最具挑战性的序列中，V1 03难度也很大，其中包括震动运动，无纹理区域和明显的照明变化。 由于专用的初始化程序，所提出的方法可以在困难的V1 03中快速初始化。
对于纯VIO，VINS-Mono和OKVIS都具有相似的精度，很难区分哪个更好。 但是，VINS-Mono在系统级别上胜过OKVIS。 它是一个完整的系统，具有强大的初始化和回环功能，可协助单目VIO。

B. Indoor Experiment

在室内实验中，我们选择实验室环境作为实验区域。我们使用的传感器套件如下图所示。它在DJIA3控制器中包含一个单目照相机(20Hz)和一个IMU(100 Hz)。我们手握传感器套件，在实验室以正常的速度行走。如图16所示，我们遇到行人，光线较弱，纹理较少，玻璃和反射。

我们将我们的结果与OKVIS进行了比较，如图17所示。图17(a)是OKVIS的VIO输出。图17(b)是所提出的无回环方法的VIO结果。图17©是所提出的具有重定位和回环的方法的结果。当我们在室内循环时，会出现明显的漂移。OKVIS和只有VIO版本的vins-Mono在x,y,z和偏航角上积累了大量漂移.我们的重定位和回环模块有效地消除了所有这些漂移。

C. Large-scale Environment

1.走出实验室：我们在室内和室外混合的环境中测试vins-mono。传感器套件与图15所示的相同。我们从实验室的一个座位上开始，在室内空间里走来走去。然后我们下了楼梯，在大楼外的操场上走来走去。接下来，我们回到楼里上楼。最后，我们回到了实验室的同一个座位。整个轨道超过700米，持续约10分钟。

轨迹如下图19所示。图19(a)是OKVIS的轨迹。当我们上楼时，OKVIS显示出不稳定的特征跟踪，导致估计错误。我们看不到红色街区楼梯的形状。VINS-Mono的唯一结果如图19(b)所示.有闭环的轨迹如图19©所示。该方法的楼梯形状清晰。闭环轨迹与谷歌地图对齐，以验证其准确性，如上图18所示。

OKVIS x,y和z轴的最终漂移为[13.80,-5.26,7.23]米。VINS-Mono无环闭路的最终为[-5.47,2.76,-0.29]m，占总长度的0.88%，小于OKVIS的2.36%。经回环修正，最终漂移有界于[-0.032,0.09,-0.07]m，与总长度相比，这是微不足道的。虽然我们没有ground truth，但我们仍然可以直观地检查优化后的轨道是否平滑，并能精确地与卫星地图对齐。

2.游校园：这张环绕整个科大校园的非常大规模的数据集是用一个手持的VI-Sensor 4记录下来的。该数据集覆盖的地面长度约为710米，宽度为240米，高度变化为60米。总路径长度为5.62km。数据包含25赫兹图像和200赫兹IMU，持续1小时34分钟。对VINS-Mono的稳定性和耐久性进行测试是一个非常有意义的实验。

在这个大规模的测试中，我们将关键帧数据库的大小设置为2000，以提供足够的循环信息并实现实时性能。我们运行此数据集时，英特尔i7-4790 CPU运行在3.60GHz。估计的轨迹与图20中的谷歌地图一致。与谷歌地图相比，我们的结果在这个非常长时间的测试中几乎没有漂移。