VINS-Mono论文全文翻译VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator

---

翻译参考：闭关修炼到走火入魔 以及 Manii 并在此基础上进行了补充和拓展。

VINS-mono代码详细注释：VINS-mono 代码详细注释

论文原文：论文原文链接

演示视频：演示视频链接

摘要

一个单目视觉惯性系统(VINS)是由一个单目相机和一个低成本惯性测量单元(IMU)构成的，这是度量六自由度(DOF)状态估计的最小传感器套件(在尺寸、重量和功率方面)。本文，我们提出了VINS-Mono：一个鲁棒和通用的单目视觉惯性状态估计器。我们的方法从一个健壮的估计器初始化过程开始。采用一种基于紧耦合、非线性优化的方法，通过融合IMU预积分后的IMU测量值和特征观测值，获得高精度的视觉惯性里程计。结合紧耦合方法，回环检测模块能够以最小的计算代价实现重定位。此外，我们还执行4自由度的位姿图优化，以加强全局一致性。更进一步，该系统可以通过一种有效的方法保存和加载地图来实现地图重用。通过全局位姿图优化，可以将当前和以前的地图融合在一起。我们在公共数据集和实际实验上验证了系统的性能，并与其他最先进的算法进行了比较。我们还在微型飞行器(MVP)平台上进行了机载闭环自主飞行，并将算法移植到基于ios的演示中。特别强调的是，本文提出的工作是一个可靠、完整和通用的系统，适用于需要高精度定位的不同应用。我们为PC和iOS移动设备开源了我们的实现方法。

一、引言

状态估计无疑是机器人导航、自主驾驶、虚拟现实和增强现实(AR)等广泛应用中最基本的模块。由于单目相机体积小、成本低、硬件设置简单，仅使用单目摄像机的方法获得了社会的极大兴趣。然而，单目视觉系统无法恢复度量尺度，因此限制了它们在机器人中的实际应用。近年来，利用低成本惯性测量单元(IMU)辅助单目视觉系统的发展趋势日益明显。这种单目视觉惯性系统(VINS)的主要优点是具有可观测的度量尺度，以及翻滚角和俯仰角。这让需要有尺度的状态估计的导航任务成为可能。此外，对IMU测量值的积分可以通过弥补由于光照变化、无纹理区域或运动模糊造成的视觉轨迹损失，显著提高运动跟踪性能。单目VINS不仅广泛应用于地面机器人和无人机上，也可应用于移动设备上。对自我和环境感知而言，它在尺寸、重量、功耗等方面都有很大的优势。

然而（不足之处），有几个问题影响了单目VINS的使用。第一个是严格的初始化。由于缺乏直接的距离测量，很难直接将单目视觉结构与惯性测量融合。同时，我们也认识到VINS是高度非线性的，因此在估计器初始化方面我们遇到了重大的挑战。在大多数情况下，系统应该从一个已知的静止位置启动，并在开始时缓慢而小心地移动，这限制了它在实践中的使用。另一个问题是视觉惯性里程计 (VIO)的长期漂移是不可避免的。为了消除漂移，必须进行回环检测、重定位和全局优化。除了这些关键问题之外，对地图保存和重用的需求还在增长。

为了解决这些问题，我们提出了一种鲁棒的、通用的单目视惯性状态估计器VINS-Mono，VINS-Mono包含以下特性：
　　1)鲁棒的初始化过程，能够从未知的初始状态引导系统；
　　2)基于优化的紧耦合单目VIO，包括相机与IMU的外参标定以及IMU偏差矫正；
　　3)在线重定位和4自由度(DOF)全局位姿图优化；
　　4)位姿图重用，可以保存、加载和融合多个局部位姿图。
在这些特性中，鲁棒的初始化、重定位和位姿图重用是我们的技术贡献，它们来自于我们之前的工作[6]-[8]。工程方面的贡献包括开源系统集成、无人机导航的实时演示和移动应用程序。整个系统已经被成功应用于小规模AR场景，中型无人机导航和大规模状态估计任务，如图1所示

（a）

（b）
图1所提出的是单目视觉-惯性状态估计器的室外实验结果。数据由正常行走条件下的手持单眼相机收集。它包括球场内的两个完整的圆圈和附近车道上的两个半圆。总轨迹长度为2.5公里。这个实验的视频可以在多媒体附件中找到。(a)轨迹（蓝色）和特征位置（红色）。(b)用谷歌地图覆盖的轨迹，以进行视觉比较。

本文其余部分的结构如下。在第二部分中，我们讨论了相关的文献。在第三部分中，我们概述了整个系统的框架。在第四部分中，给出了视觉的预处理和IMU测量值的预积分步骤。在第五部分中，我们讨论了估计器的初始化过程。在第六部分中，提出了一种紧耦合、自标定、非线性优化的单目VIO。在第七部分中，提出了紧耦合重定位。在第八部分中，讨论了全局位姿图优化和重用。在第九部分中，展示了实验结果。最后，在第十部分中，本文对未来可能的研究方向进行了探讨和展望。

二、相关工作

基于单目视觉的状态估计/里程计/SLAM方面的学术著作非常广泛。值得注意的方法包括PTAM[1]、SVO、LSD-SLAM、DSO和ORB-SLAM。显然，任何给出完整的相关回顾的尝试都是不完整的。然而，在本节中，我们跳过了关于纯视觉方法的讨论，只关注与单目视觉惯性状态估计最相关的结果。

处理视觉和惯性测量最简单的方法是松耦合的传感器融合，其中IMU作为一个独立的模块来辅助视觉结构。融合通常由扩展卡尔曼滤波器(EKF)完成，其中IMU用于状态传播，而视觉位姿用于更新。进一步说，紧耦合视觉惯性算法要么基于EKF，要么基于图优化，其中相机和IMU测量值是从原始测量水平联合优化的。一个流行的基于EKF的VIO方法是MSCKF。MSCKF在状态向量中维护以前的多个摄像机姿态，并使用多个摄像机视图中相同特征的视觉测量来形成多约束更新。SR-ISWF是MSCKF的扩展。它采用平方根形式实现单精度表示，避免了较差的数值性质。该方法采用逆滤波器进行迭代再线性化，与基于优化的算法等价。批量图优化或集束调整技术（BA）维护和优化所有测量值以获得最优状态估计。为了实现恒定的处理时间，基于图的VIO方法通常通过边缘化过去的状态和测量值来优化最近状态的有界滑动窗口。由于非线性系统迭代求解的计算要求较高，很少有基于图的方法能够在移动电话等资源受限的平台上实现实时性。

对于视觉测量处理，根据残差模型的定义，可以将算法分为直接法和间接法。直接法最小化光度误差，而间接法最小化几何位移。直接方法由于其吸引区域小，需要良好的初始估计，而间接方法在提取和匹配特征方面消耗额外的计算资源。由于其成熟度和鲁棒性，间接方法在实际工程应用中更常见。然而，直接法更容易扩展到稠密建图，因为它们是直接在像素级别上操作的。

IMU通常以比摄像机更高的速率获取数据。不同的方法被提出来处理高速率的IMU测量值。最直接的方法是在基于EKF的方法中使用IMU进行状态传播。在图优化公式中，为了避免重复的IMU积分，提出了一种有效的方法，即IMU预积分(IMU pre-integration)。该技术在文献中首次被提到，它使用欧拉角参数化旋转误差。Shen等人利用连续时间误差状态动力学推导了协方差传递方程。通过加入后验IMU偏置校正，进一步完善了积分理论。

精确的初始值对于引导任何单目VINS都是至关重要的。提出了一种利用短期IMU预积分相对旋转的线性估计初始化方法。该方法不能对原始投影方程中的陀螺偏差和图像噪声进行建模。在[26]中介绍了单目视觉惯性初始化问题的一种封闭形式解决方案。后来，在[27]中提出了通过增加陀螺仪偏差校准来扩展这种封闭形式解决方案。这些方法无法对惯性积分的不确定性进行建模，因为它们依赖于长时间内IMU测量值的双重积分。在[28]中，提出了一种基于SVO的重新初始化和故障恢复算法。需要一个额外的向下的距离传感器来恢复尺度。在[18]中引入了一种建立在流行的ORB-SLAM之上的初始化算法。据研究，其尺度收敛所需的时间超过10秒。这可能会给需要在一开始就进行规模评估的机器人导航任务带来问题。

VIO方法，不管它们所依赖的基本数学公式是什么，在全局的平移和旋转中长期受到漂移的影响。为此，回环检测在长期操作中起着重要的作用。ORB-SLAM能够闭合回环并重用地图，它利用了词袋模型。一个7自由度的(位置，方向和尺度)位姿图优化遵循回环检测。

三、概述

所提出的单目视惯性状态估计器的结构如图2所示。该系统从测量预处理(参见第四部分)开始，在预处理过程中提取和跟踪特征，对两个连续帧间的IMU测量值进行预积分。初始化过程(参见第五部分)提供了所有必要的值，包括姿态、速度、重力向量、陀螺仪偏差和三维(3-D)特征位置，用于引导后续基于非线性优化的VIO。VIO(参见第六部分)与重定位(参见第七部分)模块紧密地融合了预积分的IMU测量和特征观测。最后，位姿图优化模块((参见第八部分)接受几何验证的重定位结果，并进行全局优化以消除漂移。实现了位姿图的重用。VIO和位姿图优化模块在单独的线程中并行运行。


图2 Block diagram illustrating the full pipeline of the proposed monocular VINS.

下图为VINS-MONO的整体结构总结

四、数据预处理

本节介绍惯性和单目视觉测量的预处理步骤。对于视觉测量，我们跟踪连续帧之间的特征，并在最新帧中检测新的特征。对于IMU测量，我们在两个连续帧之间做预积分。

A.视觉处理前端

利用KLT稀疏光流法对每一个新图像存在的特征点进行跟踪。与此同时，检测新的角点特征以保证每幅图像特征的最小数目(100-300)。该检测器通过在两个相邻特征之间设置最小像素间隔来执行均匀的特征分布（尽量使特征分布更加均匀，减少特征重叠，这样可以减少不必要的计算量，并使建图效果更好）。二维特征首先是不失真的，然后在通过外点剔除后投影到一个单位球面上。利用基本矩阵模型的RANSAC算法进行外点剔除。（这一段讲述了视觉图像特征点的提取与剔除）

在此步骤中还选择了关键帧。我们有两个关键帧选择标准。第一是与上一个关键帧的平均视差。如果在当前帧和最新关键帧之间跟踪的特征点的平均视差超出某个特定阈值，则将该帧视为新的关键帧。请注意，不仅平移，旋转也会产生视差。然而，特征点无法在纯旋转运动中三角化。为了避免这种情况，在计算视差时我们使用陀螺仪测量值的短时积分来补偿旋转。注意这种旋转补偿仅仅用于关键帧的选择上，不涉及VINS公式中的旋转计算。为此，即使陀螺仪含有较大的噪声或存在偏置，也只会导致次优的关键帧选择结果，不会直接影响估计质量。另一个标准是跟踪质量，如果跟踪的特征数量低于某一阈值，我们将此帧视为新的关键帧。这个标准是为了避免跟踪特征完全丢失。

B.IMU预积分

我们遵循我们之前基于连续时间四元数的IMU预积分的推导，并包括了对IMU偏置的处理，如[19]和[24]。我们注意到我们当前的IMU预积分过程与[19]和[24]的数值结果几乎相同，但使用了不同的推导方法。所以，我们在这里只做一个简单的介绍。关于基于四元数的推导的详细信息可以在附录A中找到。

1. IMU噪声与偏置: IMU测量值是在机体坐标系内测量的，它是平衡重力和平台动力的合力，受加速度偏置和陀螺仪偏置以及额外噪声的影响。原始的陀螺仪和加速度计的测量值，被给出：
   
   我们假定额外的噪声在加速度计和陀螺仪的测量中是高斯白噪声。加速度计偏置和陀螺仪偏置被建模为随机游走，其导数为高斯白噪声，所以有：
   
   2)预积分: 对于两个连续的帧，在这两帧的时间间隔内存在多个惯性测量。给出偏执估计值之后，我们从局部坐标系下开始积分
   
   这里，
   
   
   否则，当偏差估计发生显著变化时，我们在新的偏差估计下进行重新传播。这种策略为基于优化的算法节省了大量的计算资源，因为我们不需要重复传播IMU测量值。

五、估计器初始化（松耦合初始化提供优化初始值和状态）

单目紧耦合VIO是一个高度非线性的系统，需要在一开始就进行准确的初始化估计。通过将IMU预积分与纯视觉结构进行松耦合对齐，我们得到了必要的初始值。

A.滑动窗口(Sliding Window)纯视觉SfM

初始化过程从纯视觉SfM估计相机尺度位姿(up-to-scale)和特征位置图开始。

我们在一个滑动窗口中维护多个帧，以满足有限的计算复杂度。首先，我们检查最新帧和所有之前帧之间的特征对应。如果我们能在滑动窗口中的最新帧和任何其他帧之间，找到稳定的特征跟踪(超过30个跟踪特征)和足够的视差(超过20个像素)，我们使用五点法恢复这两帧之间的相对旋转和尺度平移。然后，我们任意设置尺度，并对这两帧中观察到的所有特征进行三角化。基于这些三角化的特征，采用PnP算法来估计窗口中所有其他帧的位姿。最后，应用全局光束平差法(BA)最小化所有特征观测值的总重投影误差。由于我们还没有任何关于世界坐标系的知识，所以我们将第一个相机坐标系设置为SfM的参考坐标系。所有帧的位姿和特征位置表示都是相对于第一个坐标系也就是SfM的参考坐标系的给定相机与IMU之间的外部参数可以将相机帧坐标系下的位姿转换为本体坐标系 (IMU帧) 下的位姿，如

其中s为未知尺度参数，将在下一节中求解。

B.视觉惯性联合校准

视觉惯性对准的示意图如图3所示，其基本思想是将尺度视觉结构与IMU预积分匹配。

图3 估计器初始化的视觉惯性对准过程演示
1)陀螺仪偏置标定: 考虑窗口内连续两帧，从视觉SfM中得到这两帧相对于参考坐标系的旋转，从IMU预积分得到相对约束，我们将IMU预积分项相对于陀螺仪的偏置进行线性化，并最小化以下代价函数:

其中B代表窗口中所有帧的索引。通过这种方法，我们得到了陀螺仪偏置的初始标定结果。然后，我们用新的陀螺仪偏置重新传递所有的IMU预积分项。
2)速度、重力矢量、尺度初始化: 陀螺仪偏置初始化后，我们继续初始化导航的其他必要状态，即速度、重力矢量、尺度。



我们可以将(6)和(9)合并成以下线性测量模型:

这里，


我们可以得到窗口中每一帧的本体坐标系速度，视觉参照系中的重力向量，以及尺度参数。

3)重力细化: 通过约束大小，可以细化之前线性初始化步骤得到的重力向量。在大多数情况下，重力矢量的大小是已知的。这导致重力矢量只剩下2个自由度。因此，我们在其切线空间上用两个变量扰动重力，使其保持2自由度。

图4 2自由度重力扰动图解
4)完成初始化: 细化重力矢量后，通过将重力旋转到z轴上，我们能得到世界坐标系与摄像机坐标系 之间的旋转。然后我们将所有变量从参考系(也就是第一个相机坐标系) 旋转到世界坐标系 。本体坐标系的速度也将被旋转到世界坐标系。视觉SfM的平移分量将被缩放到公制单位。此时，初始化过程已经完成，所有这些度量值都将被输入到一个紧耦合的单目VIO中。（用于后端非线性优化）

六、紧耦合单目VIO

在估计器初始化之后，我们使用一个基于滑动窗口的紧耦合单目VIO进行高精度和鲁棒的状态估计。滑动窗口的阐述正如图5所示。

图5 带有重新定位的单目VIO滑动窗口的图解

A.公式

滑动窗口中的全状态向量被定义为：
其中，Xk是捕获第k帧图像时的IMU状态。它包含了IMU在世界坐标系下的位置、速度和方向，以及在IMU本体坐标系下的加速度计偏置和陀螺仪偏置。n是关键帧的总数，m是滑动窗口中的特征总数，λl是第一次观测到第l个特征的逆深度。
我们使用视觉惯性BA。我们最小化所有测量残差的先验和Mahalanobis范数之和，得到最大后验估计：

其中Huber范数被定义为：

B.IMU测量残差

考虑滑动窗口中连续两帧内的IMU测量，预积分IMU测量的残差可以定义为：

C.视觉测量残差（视觉重投影误差部分）

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

上面是重投影的估计模型，其实VINS代码中也可以使用普通的针孔相机模型。

图6 在单位球面上的视觉残差图解

D.边缘化

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

图7 边缘化策略图解
如图7所示，（第一种情况）当第二个最新帧（倒数第二帧）是一个关键帧时，它将留在窗口中，而最老的帧与其相应的测量值将被边缘化。（第二种情况）但如果第二个最新帧是一个非关键帧，我们丢掉视觉测量值，保留连接到这个非关键帧的IMU测量值。为了保持系统的稀疏性，我们不会边缘化非关键帧的所有测量值。我们的边缘化方案的目的是在窗口中保持关键帧的空间分隔。这确保了特征三角化有足够的视差，并且最大化了在大激励下获得加速度计测量值的可能性。

边缘化是利用Schur补舒尔补边缘化进行的。我们基于所有与被移除状态相关的边缘化测量值构建了一个新的先验。新的先验项被添加到现有的先验项中。
我们注意到边缘化导致线性化点的早期固定，这可能导致次优估计结果。但是，由于VIO可以接受小的漂移，所以我们认为边缘化带来的负面影响并不严重。

E.摄像机速率状态估计的纯运动视觉惯性BA

对于手机等计算能力较低的设备，由于非线性优化计算量较大，紧耦合的单目VIO无法实现相机速率的输出。为此，在充分优化的基础上，我们采用了一种轻量级的纯运动视觉惯性优化，将状态估计提高到相机速率(30Hz)。

纯运动视觉惯性优化的代价函数与(14)中单目VIO的代价函数相同。然而，我们并没有优化滑动窗口中的所有状态，而是只优化了固定数量的IMU最新状态的姿态和速度。我们将特征深度、外部参数、偏置和不希望优化的旧IMU状态视为常量。我们确实使用了所有的视觉和惯性测量来进行纯运动的优化。这使得状态估计比单帧PnP方法更平滑。图8给出了该策略的图解。与在最先进的嵌入式计算机上可能导致超过50ms的完全紧耦合单目VIO相比，这种纯运动的视觉惯性优化大约只需5ms来计算。这使得低延迟的相机频率进行位姿估计对无人机和AR应用特别有利。

F.IMU前向传递以达到IMU速率状态估计

IMU测量的速率比视觉测量高得多。虽然我们的VIO频率受到图像捕获频率的限制，但是我们仍然可以用最近的IMU测量直接传播最新的VIO估计来实现IMU速率级的性能。高频状态估计可以作为回环检测的状态反馈。利用这种IMU速率状态估计进行的自主飞行实验在第九节的C中给出。

七、重定位

我们的滑动窗和边缘化方案限制了计算复杂度，但也为系统引入了累积漂移。为了消除漂移，提出了一种与单目VIO无缝集成的紧耦合重定位模块。重定位过程从一个回环检测模块开始，该模块识别已经访问过的位置。然后建立回环检测候选帧和当前帧之间的特征级连接。这些特征的对应关系紧密地集成到单目VIO模块中，以最小的计算量获得无漂移状态估计。多特征的多次观测直接用于重定位，具有较高的精度和较好的状态估计平滑性。重定位过程如图9(a)所示。

图9 重定位和位姿图优化过程图示 (a)重新定位过程。它开始于纯VIO的姿态估计(蓝色)。记录过去的状态(绿色)。如果最新的关键帧检测到一个回环(参见第7节的A)，如第2幅图中的红线所示，则发生了重新定位。注意，由于使用了特征级对应关系进行重新定位，我们能够合并多个来自过去关键帧的回环检测约束(参见第7节的C)，如后三个图所示。　　(b)全局位姿图优化。当一个关键帧从滑动窗口被边缘化时，它被添加到位姿图中。如果在这个关键帧和任何其他过去的关键帧之间有一个回环，则表示为4自由度相对刚体变换的闭环约束也将添加到位姿图。使用所有相关的位姿约束在一个单独的线程中对位姿图进行优化(见第7节的C)，重定位模块总是根据最新的位姿图配置运行。

A.回环检测

我们利用DBoW2，一种最先进的词袋位置识别方法来进行回环检测。除了用于单目VIO的角点特征外，另外500个角点被检测并由BRIEF描述子描述。额外的角点特征用于在回环检测中实现更好的召回率。描述子被视为查询视觉数据库的视觉单词。DBoW2在时间和空间一致性检查后返回回环检测候选帧。我们保留所有用于特征检索的BRIEF描述子，丢弃原始图像以减少内存消耗。

B.特征恢复

当检测到一个回环时，通过恢复特征对应关系，建立局部滑动窗口与闭环候选帧之间的连接。通过BRIEF描述子匹配找到对应关系。描述子匹配可能会导致一些误匹配对。为此，我们使用了两步进行几何异常值剔除，如图10所示。

图10 为了特征恢复，在回环闭合时进行描述子匹配和异常值去除。(a) BRIEF描述子匹配结果。(b)第一步操作:2D – 2D异常值剔除结果。© 第二步操作:3D – 2D异常值剔除结果。

1)2D- 2D: RANSAC[33]的基本矩阵检验。我们利用当前图像和回环候选图像中恢复的特征二维观测进行基本矩阵检验。
2)3D- 2D: RANSAC[35]的PnP检验。基于特征在局部滑动窗口中已知的三维位置，以及回环候选图像中的二维观测值，进行PnP检验。
剔除外点后，我们将此候选帧视为正确的回环检测并执行重定位。

C.紧耦合重定位

重定位过程有效地将当前滑动窗口与过去的姿态对齐。在重定位过程中，我们将所有闭环帧的位姿都视为常量。我们使用所有的IMU测量值，局部视觉测量值和恢复的特征对应关系，共同优化滑动窗口。我们可以轻松地为闭合回环帧观察到的恢复特征编写视觉测量模型，使其与VIO中的视觉测量相同，如公式(17)所示。唯一的区别是，从位姿图(见第8节)或直接从上一个里程计的输出（如果这是第一次重定位）获得的回环帧的姿态被视为常数。为此，我们可以稍微修改(14)中的非线性代价函数，附加回环项为：

其中L为闭环帧中恢复的特征的观测集。(l,v)表示在闭环帧v中观察到的第l个特征。注意，虽然代价函数与(14)稍有不同，但是待解状态的维数是相同的，因为回环帧的位姿被视为常量。当使用当前滑动窗口建立多个闭合回环时，我们同时使用来自所有帧的所有闭环特征对应关系进行优化。这为重定位提供了多视图约束，从而获得更高的精度和更好的平滑性。第8节将讨论重定位后保持一致性的全局优化。

八、全局位姿图优化和地图重用

重定位后，将进行额外的位姿图优化步骤，以确保将过去的位姿集合配准到全局一致的配置中。

A.四个累积漂移方向

图11 四个漂移方向的图解。 随着物体的运动，x、y、z和偏航角相对于参照系发生相对变化。绝对的滚动角和俯仰角可以由重力矢量的水平平面来确定。

由于重力惯性测量的好处，横滚角和俯仰角是完全可以在VINS中观测到的。如图11所示，随着物体的运动，三维位置和旋转相对于参照系发生变化。然而，我们可以通过重力矢量来确定水平面，这意味着我们可以一直观察到绝对的横滚角和俯仰角。因此，横滚和俯仰是世界坐标系中的绝对状态，而(x,y,z)和偏航是相对于参考系的相对估计。累积漂移只发生在(x,y,z)和偏航角中。为了充分利用有效的信息，有效地校正漂移，我们对无漂移横滚和俯仰进行了修正，只进行了四自由度的位姿图优化。

B.在位姿图中添加关键帧

关键帧在VIO处理之后添加到姿态图中。每个关键帧作为位姿图中的一个顶点，通过两种类型的边与其他顶点连接，如图12所示。

图12 位姿图说明。 关键帧作为姿态图中的一个顶点，它通过顺序的边和循环的边连接其他顶点。每条边代表相对平移和相对偏航。

1. 顺序边(Sequential Edge): 关键帧与之前的关键帧建立若干条顺序边。顺序边表示直接从VIO获取的两个关键帧之间的相对转换。考虑到关键帧i及其先前的一个关键帧j，顺序边只包含相对位置和偏航角。
   
2. 回环边(Loop Closure Edge)：如果关键帧有一个回环连接，则它通过位姿图中的回环边连接闭合回环帧。同样，回环边只包含一个与(19)定义相同的4自由度相对位姿变换。回环边的值由重定位结果得出。

C. 4自由度位姿图优化

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

其中，ϕ ^ i 和θ ^ i 是从单目VIO获得的横滚和俯仰角的合理估计。

通过最小化以下代价函数，对顺序边和回环边的整个图进行优化：

其中，S是所有顺序边的集合，L是回环边的集合。虽然紧耦合的重定位已经有助于消除错误的回环, 但我们添加了另一个Huber范数ρ(⋅) ，进一步减少任何可能的错误回环的影响。相反，我们不对顺序边使用任何鲁棒范数，因为这些边是从VIO中提取出来的，VIO已经包含了足够多的外点剔除机制。位姿图的优化和重定位(见第7节的C)在两个单独的线程中异步运行。这使得在最优化的位姿图可用时，可以立即使用它进行重新定位。同样，即使当前的位姿图优化尚未完成，仍然可以使用已有的位姿图配置进行重定位。这一过程如图9(b)所示。

D.位姿图管理

该位姿图不仅可以优化当前地图，而且可以将当前地图与以前构建的地图合并。如果我们加载了一个以前构建的地图并检测到两个地图之间的回环连接，我们就可以将它们合并在一起。由于所有的边都是相对约束条件，因此位姿图优化通过回环连接自动将两个地图合并在一起。如图13所示，通过回环边将当前地图拉到之前的地图中。每个顶点和每条边都是相对变量。因此，我们只需要固定位姿图中的第一个顶点。

图13 地图合并说明 黄色的数字是以前构建的地图。蓝色的数字是当前的地图。根据回环连接合并两个地图。

E.位姿图保存

我们的姿态图的结构非常简单。我们只需要保存顶点和边，以及每个关键帧(顶点)的描述子。原始图像被丢弃，以减少内存消耗。具体来说，我们为第i个关键帧保存的状态是

其中，i是帧的索引，p ^ i w和q ^ i w分别是来自VIO的位置和旋转。如果这个帧有一个回环帧，v是回环帧的索引。p ^ i v和ψ ^ i v是通过重定位得到的两帧之间的相对位置和偏航角。D(u,v,des)是特征集。每个特征都包含二维位置及其BRIEF描述子。

F.位姿图加载

九、实验结果

我们执行数据集、现实世界的实验以及两个应用程序来评估提出的VINS-Mono系统。在第一个实验中，我们将提出的算法与另一个最先进的算法在公共数据集上进行了比较。我们进行了数值分析，以显示我们的系统在细节上的准确性。然后，我们在室内环境中测试我们的系统，以评估在重复场景中的性能。通过一个大场景实验验证了该方法的长期实用性。此外，我们将所提出的系统应用于两个应用程序。在航空机器人应用中，我们使用VINS-Mono作为位置反馈来控制无人机追踪预定的轨迹。然后我们将我们的方法移植到iOS移动设备上。

A. 数据集比较

我们使用EuRoC MAV视觉-惯性数据集评估我们提出的VINS-Mono。这个数据集是在一架微型飞行器上收集的，它包含立体图像(Aptina MT9V034全局快门、WVGA单色、20 FPS)、同步IMU测量(ADIS 16448、200 Hz)和地面真实状态(Vicon和Leica MS 50)。我们只使用左边相机的图像。在这数据集中会观察到较大的IMU偏置和光照变化。

在这些实验中，我们将VINS-Mono和OKVIS进行了比较，这是一种最先进的单目和立体相机VIO。OKVIS是另一种基于优化的滑动窗口算法。我们的算法与OKVIS在许多细节上是不同的，如技术部分所示。我们的系统具有良好的初始化和回环功能。我们使用MH_03_median和MH_05_difficult两组序列来证明该方法的性能。为了简化表示，我们使用VINS来表示我们只使用单目VIO的方法，而VINS_loop表示含重定位和位姿图优化的完全版本****。我们分别用OKVIS_Mono和OKVIS_stereo表示OKVIS使用单目和立体图像的结果。为了进行公平的比较，我们丢弃前100个输出，并使用接下来的150个输出对齐地面真值，并比较其余的估计器输出。

MH_03_median序列轨迹如图14（a）所示。我们只比较平移误差，因为旋转运动在这个序列中是可以忽略的。图15显示了x、y、z误差与时间的关系，以及平移误差与距离的关系。在误差图中，具有回环的VINS-Mono具有最小的平移误差。我们在MH_05_difficult上观察到类似的结果,如图所示,14(b)和16。该方法具有最小的平移误差。

EuRoC数据集中所有序列的均方根误差(RMSE)如表一所示，这是通过绝对轨迹误差(ATE)来评估的。在大多数情况下，循环闭合的单声道在大多数情况下优于其他的。在某些运行距离较短、漂移较小的情况下，如V1_03_difficult和V2_01_easy，环路闭合模块没有显著影响。
在[44]中可以找到更多的基准比较，这表明与其他最先进的算法相比，该系统具有良好的性能。

2)Map合并结果：在不同的起始位置和不同的时间收集5个MH序列，可以将5个MH序列合并成一个全局姿态图。我们根据每个序列中相似的相机视图进行重新定位和姿势图优化。我们只固定第一个序列中的第一帧，其位置和偏航角设置为零。然后，我们将新的序列逐个地合并到以前的地图中。运动轨迹如图17所示。我们还将整个轨迹与地面真值进行了比较。ATE的RMSE为0.21 m，这是一个令人印象深刻的总共500米的结果。该实验表明，通过逐步合并不同时间捕获的新传感器数据，该图随时间“演变”，并保持了整个姿态图的一致性。

B. 真实实验

1)室内实验：我们使用的传感器套件如图18所示。它包含一个单眼摄像机(mvBlueFOXMLC200w，20Hz)和DJIA3控制器内的一个IMU(100Hz)。我们手持传感器套件，以正常的速度行走。我们将我们的结果与OKVIS进行了比较，如图19所示。图19(a)是来自OKVIS的VIO输出。图19(b)是所提出的具有重新定位和循环闭路的方法的结果。当我们在室内旋转时，发生了明显的VIO漂移。OKVIS在x、y、z和偏航角上积累了显著的漂移。我们的重新定位和回路闭合模块有效地消除了这些漂移。

2)大规模环境：这个围绕整个HKUST校园的非常大规模的数据集是用手持式VI-传感器记录的。该数据集覆盖了长约710米，宽约240米，高度变化约60米的地方。总路径长度为5.62公里。数据包含25Hz图像和200HzIMU，持续1h和34min。测试vin-mono的稳定性和耐久性是一个非常重要的实验。

在这个大规模的测试中，我们将关键帧数据库的大小设置为2000，以提供足够的循环信息并实现实时性能。我们使用运行在3.60GHz下的Inteli7-4790CPU运行这个数据集。时间统计数据如表二所示。估计的轨迹与图20中的谷歌地图对齐。与谷歌地图相比，我们可以看到我们的结果在这个非常长时间的测试中几乎没有漂移。

C. 应用

1）在空中机器人上的反馈控制：我们将VINS- Mono应用于空中机器人的自动反馈控制，如图21所示。我们使用了一个前瞻性的全球快门相机(矩阵视觉mvbluefox-MLC200w)，具有752×480分辨率，并配备了一个190º鱼眼镜头。A DJIA3飞行控制器用于IMU测量和姿态稳定控制。机载计算资源是一个英特尔i7-5500UCPU，运行在3.00GHz。传统的针孔相机型号不适合大型FOV相机。我们使用MEI模型，由工具包[46]校准。

在本实验中，我们测试了VINS-Mono状态估计下自主轨迹跟踪的性能。此实验禁用回环检测。四转子被命令跟踪一个图8的模式，每个圆的半径为1.0m，如图22所示。在轨迹周围设置四个障碍，以验证没有环路关闭的VINS-Mono的准确性。四转子在实验过程中连续四次跟踪这个轨迹。100Hz的机载状态估计值（参见第VI-F节）可实现对四转子的实时反馈控制。

使用OptiTrack获得地面真值。总轨迹长度为61.97m。最终的漂移为[0.08、0.09、0.13]m，导致0.29%的位置漂移。平移和旋转的细节及其相应的误差如图23所示。

2)移动设备：我们将VINS-Mono移植到移动设备上，并提出了一个简单的AR应用程序来展示其准确性和鲁棒性。我们将我们的移动实现命名为vins-Mobile。vins-Mobile运行在iPhone设备上。我们使用了由iPhone捕获的640×480分辨率的30Hz图像，以及由内置的InvenSenseMP67B6轴陀螺仪和加速度计获得的100Hz的IMU数据。首先，我们在平面上插入一个虚拟立方体，从估计的视觉特征中提取，如图24(a)所示。然后，我们拿着设备，以正常的速度进出房间。当检测到循环时，我们使用4-DOF姿态图优化(见第VIII-C节)来消除x、y、z和偏航漂移。之后行驶了大约264米后，我们返回了起始位置。最终结果如图24(b)所示。VINS返回到起点。通过4-DOF姿态图的优化，消除了总轨迹的漂移。这个事实也证明了这一点多维数据集注册到图像上的同一位置。

十、结论及未来工作

在本文中，我们提出了一种鲁棒且通用的单目视觉惯性估计器。我们的方法具有最先进的和新的解决方案的IMU预积分、估计器初始化、在线外部校准、紧密耦合的VIO、重新定位和有效的全局优化。通过与其他最先进的开源实现相比，我们显示出了优越的性能。为了社区的利益，我们开源了PC和iOS实现。

虽然基于特征的VINS估计器在现实世界部署上已经相当成熟度，但我们仍然看到了未来研究的许多方向。单目VINS可达到弱观察甚至退化条件。我们感兴趣的是在线方法来评估单目VINS的可观察性特性，并在线生成运动计划，以恢复可观察性。另一个研究方向涉及在各种消费设备上的大规模部署，如安卓手机。该应用程序需要对几乎所有传感器的内部和外部参数进行在线校准，以及在线识别校准质量。最后，我们感兴趣的是产生来自单目VINS的密集地图的结果。我们的第一个结果是将在[47]中提出的单目视觉惯性稠密建图应用于无人机导航。然而，对于进一步提高系统的准确性和鲁棒性，仍需要广泛的研究。

这篇好文章是转载于：学新通技术网