作者丨Clark@知乎

来源丨https://zhuanlan.zhihu.com/p/386659861

编辑丨3D视觉工坊

0. 摘要，Abstract

单目视觉-惯性系统（VINS），由一个相机和低成本惯性测量单元IMU组成，形成了六自由度（DOF）状态估计的最小传感器套件。然而，由于缺乏直接的距离测量对于IMU处理、估计器初始化，外参标定以及非参数优化造成了重大的挑战。本文工作中，我们提出了VINS-Mono：一种鲁棒通用的单目视觉-惯性状态估计器。我们提出的方法从估计器初始化以及故障恢复（failure recovery）的鲁棒过程中开始。采用基于紧耦合的非线性优化方法，融合预积分的IMU测量和特征观测，来获得高精度的视觉-惯性里程计。回环检测模块，结合我们的紧耦合方案，能够以最小计算开销进行重定位。此外，我们还执行了4个自由度的姿态图优化，用来加强全局一致性。我们在公开数据集以及真实世界数据集上验证了我们提出系统的性能，并且与其他最好算法进行比较。我们也在无人机平台上执行了机载闭环自主飞行，并将算法移植到基于iOS系统的演示中。我们强调提出的工作是一个可靠、完整和通用的系统，适用于需要高精度定位的不同应用。我们将算法的PC端和IOS移动设备的实现进行了开源。

关键字：单目视觉-惯性系统，状态估计，传感器融合，同步定位与建图

1. 引言，Introduction

状态估计无疑是许多广泛应用中最基础的模块，例如机器人导航、自动驾驶、虚拟现实和增强现实。那些仅采用单目相机的方法由于小尺寸、低成以及简单的硬件设置，引起了社区的极大的兴趣。然而，单目视觉系统无法恢复尺度信息，因此限制了它们在真实世界机器人应用中的使用。最近，我们看到用低成本惯性测量单元来辅助单目视觉系统日益增长的趋势。单目视觉-惯性系统VINS的主要优势是具有可观测的度量尺度，横滚roll以及俯仰角pitch。这支持需要度量状态估计的导航任务。此外，结合了IMU的测量，通过弥合由于光照的变化、弱纹理区域或者运动模糊而造成的视觉跟踪损失之间的差距，可以显著提升运动跟踪的性能。事实上，单目VINS不仅在移动机器人、无人机和移动设备上广泛使用，它也是充分的自我和环境感知的最小传感器设置。

然而，这些优势都是有代价的。对于单目VINS来说，众所周知，加速激发是需要使得尺度度量是可观测的。这意味着单目VINS估计器不能从一个静态条件开始，而是从一个位置的运动状态启动。同时意识到单目惯性系统的高度非线性，我们看到了在估计器初始化方面的重大挑战。两个传感器的存在也使Camera-IMU的外参标定至关重要。最后，为了消除在可接受的处理窗口内的长期漂移，必须开发一个包含视觉-惯性里程计、回环检测、重定位以及全局优化的完整系统。

为了解决上述所有的问题，我们提出了VINS-Mono，一个鲁棒通用的单目视觉-惯性状态估计器。我们的解决方案是从状态估计器的初始化开始。同样的初始化模块也用于故障恢复（failure recovery）。我们解决方案的核心是一个基于紧耦合滑动窗口非线性优化的鲁棒单目视觉-惯性里程计VIO。单目VIO模块不仅提供精确的局部位姿、速度以及方向估计，还在线执行Camera-IMU的外参标定以及IMU误差矫正。回环是采用DBoW2检测。通过与单目VIO的特征级融合来完成紧耦合设定的重定位。这就可以以最小计算开销实现鲁棒和精确的重定位。最后，几何验证后的回环添加到姿态图中，并由于单目VIO可观测到的横滚角和俯仰角，执行四自由度的姿态图来确保全局一致性。

VINS-Mono结合并提升了我们之前在单目视觉-惯性融合上的工作。其构建在我们的紧耦合、基于优化的单目VIO形式之上，并结合了文献【9】中介绍的初始化过程。文献【10】中第一次尝试移植到移动设备上。与我们之前的工作相比，未来VINS-Mono的改进包括带有误差矫正的改进后IMU预积分、紧耦合重定位、全局姿态图优化、广泛实验评估以及一个鲁棒通用的开源实现。

整个系统时完整并易用的。它已经成功地应用于小规模的AR增强现实场景、中等尺度的无人机导航、大尺度的状态估计任务。与其他最好方法相比，它具有优越的性能。为此，我们总结我们的贡献如下：

• ·提出一个能从未知初始状态中引导系统的鲁棒初始化过程；

• ·提出一个带有Camera-IMU外参标定和误差矫正的紧耦合、基于优化的单目视觉惯性里程计；

• ·提出一种在线回环检测与紧耦合重定位方法；

• ·四自由度的全局姿态图优化；

• ·无人机导航、大尺度定位和移动AR应用上的实时性能展示；

• ·完全集成ROS的PC端版本，以及运行在iPhon6s或以上iOS版本的两种开源实现；

本文的其余部分的结构如下。在第2节中，我们讨论相关文献。在第3节中，我们给出了整个系统管道的概述。在第4节中提出了视觉和预积分IMU测量的预处理步骤。在第5节中，我们讨论了估计器的初始化过程。第6节提出了一种紧耦合、自标定、基于非线性优化的单目VIO系统。第7和8节分别提出了紧耦合的重新定位和全局位姿图优化。第9节是实现细节和实验结果。最后，第10节中用一个讨论和可能的未来研究方向总结了全文。

2. 相关工作，Related Work

关于基于单目视觉的状态估计/里程计/SLAM的学术工作是广泛的。值得注意的方法包括PTAM、SVO、LSD-SLAM、DSO和ORB-SLAM。显然，任何企图进行全面相关的综述回顾都是不完整的。然而，在这一节，我们跳过了纯视觉方法的讨论，只关注单目视觉-惯性状态估计中最相关的结果。

处理视觉和惯性测量最简单方法是松耦合传感器融合，其中IMU被视作一个独立的模块，以帮助从运动的视觉结构中获得视觉姿态估计，融合通常由一个扩展卡尔曼滤波（EKF）完成，其中IMU用于状态传播，只有视觉部分的姿态是用于更新。此外，紧耦合的视觉-惯性算法也有基于EKF或图优化的，其中相机和IMU测量是从原始测量级别联合优化的。一种流行的基于EKF的VIO方法是MSCKF。MSCKF在状态向量中保持以前集中相机姿态，并使用跨多个相机视图的相同特征的视觉测量形成多约束的更新。SR-ISWF是MSCKF方法的一个扩展，它使用平方根形式去实现单精度表示并避免了交叉的数值特性。该方法采用逆滤波器进行迭代重线性化，时期等同于基于优化的算法。批处理图优化或者BA捆机调整技术维持并优化了所有的测量值来获得最佳的状态估计。为了实现恒定处理时间，流行的基于图的VIO方法通常通过边缘化过去状态以及测量值，对最近状态的边界大小的滑动窗口进行优化。由于非线性系统迭代求解的计算需求高，基于图的VIO方法很少能在资源有限的平台上实现实时性能。

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

实践中，IMU通常以比相机更高的频率获取数据。不同的方法提出用来处理高帧率的IMU测量数据。最直接的方法是基于EKF方法使用IMU进行状态传播。在一个图优化框架中，一个称作IMU预积分的有效技术被提出，为了避免重复进行IMU的积分计算。这个技术在文献【22】中首次引入，其采用欧拉角参数化旋转误差。我们先前工作【7】中提出了IMU预积分流形（manifold）上的旋转方案。这个工作采用连续时间IMU误差状态动力学推导出协方差的传播。然而，IMU的误差被忽略了。通过增加后验IMU误差矫正，文献【23】进一步改进了预积分理论。

精确的初始值对于引导任何单目VINS系统时至关重要的。在文献【8,24】中提出了一种利用短期IMU预积分产生的相对旋转的线性估计器初始化方法。然而，这个方法不能建模陀螺仪的偏差，并且不能在原始投影公式中建模传感器噪声。在真实世界的应用中，当视觉特征远离传感器套件时，这会导致不可靠的初始化。在文献【25】中介绍了一种用于解决单目视觉惯性初始化问题的封闭解觉方案。之后，在文献【26】中通过增加陀螺仪误差校准提出了这种封闭解决方法的扩展。因为这些方法依赖于IMU测量在更长时间内的双重积分，因此不能建模惯性积分的不确定性。文献【27】中提出一种基于SVO的重新初始化和故障恢复（failure recovery）算法。这是基于松耦合融合框架的实践方法。然而，需要额外的向下的距离传感器来恢复尺度。文献【17】中提出一种基于流行的ORB-SLAM的初始化方法，给定ORBSLAM的一系列关键帧集合，计算视觉-惯性完整的BA捆机调整中尺度、重力方向、速度以及IMU误差的初始估计值。然而，据报道，尺度收敛需要的时间超过了10秒。这可能会给在一开始就需要尺度估计的机器人导航任务带来问题。

VIO方法，无论其依赖的基本数学方法如何，都会在全局平移和旋转中遇到长期的漂移。为此，闭环对于长时间的运行起到了重要的作用。ORB-SLAM能够闭环并重用地图，其利用了Bag-of-Word词袋模型。在回环检测后是一个7自由度（位置，旋转和尺度）姿态图优化。相比之下，对于单目VINS系统，由于IMU的加入，漂移只发生在4个自由度中，即3D平移以及重力方向上的旋转（偏航角yaw）。因此，本文中，我们选择在最小4自由度的设置中优化带有回环约束的姿态图。

3. 概览，Overview

提出的单目视觉-惯性状态估计器的结构如图2所示。系统从测量预处理（第4节）开始，其中包括特征提取和跟踪，以及两个连续帧之间IMU测量值的预积分计算。初始化过程（第5节）提供了所有需要的数值，包括姿态、速度、重力向量、陀螺仪偏差以及3D特征位置，为了引导后续基于非线性优化的VIO系统。带有重定位（第7节）的VIO（第6节）模块紧密的融合了预积分的IMU测量值、特征观测以及闭环检测种重新检测到的特征。最后，姿态图优化模块（第8节）采用几何验证后的重定位结果，并执行全局优化来消除漂移。VIO、重定位以及姿态图优化模块在一个多线程设置中同时运行。每个模块都有不同的运行速率，并确保实时可靠运行。

图2. 黑色方框图阐述的是提出的单目视觉-惯性状态估计器的完整处理管线

4. 测量预处理，Measurement Preprocessing

本小节介绍了惯性和单目视觉测量的预处理步骤。对于视觉测量，我们跟踪连续帧之间的特征并检测最新帧的新特征。对于IMU的测量，我们计算两个连续帧之间的IMU预积分。请注意，我们使用的低成本IMU测量同时受到误差和噪声的影响。因此我们特别考虑了IMU预积分过程中的误差。

A. 视觉处理前端 Vision Processing Front-end

对于每幅新图像，都是用KLT稀疏光流算法跟踪已有特征。同时，检测新的角点特征去维持每幅图像中最少100-300个的特征数目。检测器通过设置两个相邻特征之间最小像素分离来实现统一的特征分部。首先对二维特征进行去畸变，然后在剔除掉异常离群点后，投影到一个单位球体中。离群值剔除是采用带有基础矩阵模型的RANSAC随机采样一致性算法来进行的。

关键帧也在这一步中选取。我们有两种关键帧选取标准。第一个是从过去关键帧之外的平均视差法。如果跟踪特征的平均视差是在当前帧和最新帧之间超过了某个阈值，我们将该帧作为新的关键帧。注意，不仅平移，而且旋转也会引起视差。然而，特征在纯旋转运动中是不能进行三角化。为了避免这种情况，我们在计算视差时采用陀螺仪测量的短期积分来补偿旋转。注意，旋转补偿只用于关键帧的选取步骤中，不包括VINS中旋转计算。为此，即使陀螺仪包含大量的噪声或误差，它也只会影响关键帧选取的结果，而不会直接影响到估计的质量。另一个标准是跟踪质量。如果跟踪特征的数量少于某个阈值，我们也将该帧图像作为新的关键帧。这个标准是为了避免特征跟踪的完全丢失。

B. IMU预积分 Pre-integration

IMU预积分是在文献【22】中首先提出，其以欧拉角来参数化旋转误差。我们先前的工作【7】中提出一种用于IMU预积分的流形上的旋转方案。这个工作利用连续时间IMU误差状态动力学（dynamics）推导了协方差的传播（covariance propagation）。然而，其中IMU误差被忽略了，文献【23】通过添加后验IMU误差校正进一步改进了预积分理论。本文中，我们通过结合IMU误差校正扩展了之前工作中提出的IMU预积分。

5. 估计器初始化，Estimator Initialization

单目紧耦合视觉-惯性里程计是一个高度非线性系统。因为从单目相机中是无法直接观测到尺度信息的，因此很难在没有好的初始值的情况下直接融合这两个测量值。可以假设一个平稳的初始条件去启动单目VINS估计器。然而这个假设不适合于实际应用中经常遇到的运动条件下的初始化。当IMU测量被较大的偏差破坏干扰时，情况会变得更加复杂。实际上，单目VINS系统的初始化始终是最脆弱的步骤。因此需要一个鲁棒稳健的初始化过程来确保系统的适用性。

我们采用松耦合的传感器融合方法去获得初始值。我们发现纯视觉的SLAM方法，或者运动恢复结构SFM方法，都具有很好的初始化特性。在大多数情况，纯视觉系统能够通过相对运动方法得到的初始值来引导自己，比如八点法或五点法算法，以及齐次矩阵的估计。通过对齐度量的IMU预积分与纯视觉SFM的结果，我们可以大致恢复出尺度、重力、速度甚至是偏差bias。但这是不足以引导一个非线性单目VINS估计器的，如图4所示。

图4. 估计器初始化的视觉-惯性对齐过程演示

与文献【17】中在初始化阶段同时估计陀螺仪和加速度计偏差相比，我们选择在初始化步骤忽略加速度计偏差bias。加速度计偏差bias与重力耦合，并且由于相比较于平台动力学重力向量的较大幅度，以及初始化阶段相对短，这些偏差项很难被观测到。加速度计偏差校准的详细分析在我们之前工作【34】中已经提出。

A. 滑动窗口纯视觉SFM Sliding Window Vision-Only SFM

初始化过程从纯视觉SFM运动恢复结构开始，去估计大规模相机姿态以及特征位置。

为了有限的计算复杂度，我们在帧图像上维护一个滑动窗口。首先，我们检查最新帧图像与之前所有帧图像之间的特征一致性。如果我们能找到最新的帧图像和滑动窗口中的任何其他帧图像之间的稳定特征跟踪（超过30个跟踪的特征）或者足够的视差（超过20个旋转补偿的像素），我们就使用五点法（Five-Point）恢复这两帧图像之间的相对旋转和（up-to-scale累计？大规模？）平移。否则，我们在这个滑动窗口内保留最新的帧图像并等待新的帧图像进来。如果五点法成功，我们任意设置尺度，并三角化这两帧中观测到的所有特征。基于这些三角化的特征，执行PnP（Perspective-n-Point，n点透视法）算法去估计滑动窗口内所有其它帧图像的姿态。最后，应用全局完整BA捆机调整去最小化所有特征观测的总重投影误差。由于我们没有任何关于世界坐标

2）速度，重力向量以及度量尺度初始化 Velocity，Gravity Vector and Metric Scale Initialzation：

在陀螺仪偏差初始化后，我们继续初始化导航的其它基本状态，即速度，重力向量以及度量尺度：

3）重力优化 Gravity Refinement：

算法1：寻找一组正交基b_1和b_2

4）完成初始化 Completing Initialization：

6. 紧耦合单目VIO, Tightly-Coupled Monocular VIO

在初始化估计后，我们继续进行一个机遇滑动窗口的紧耦合单目VIO的高精度以及鲁棒状态估计。滑动窗口方案的说明如图3所示。

图3. 带重定位的滑动窗口单目VIO的图示。这是一个与IMU、视觉和回环测量紧耦合的方案。

A. 方案，Formulation

滑动窗口中的完整状态向量定义如下：

图7. 边缘化策略图示。若第二个最新帧是关键帧，那么会在窗口内保留，并边缘化掉最旧的帧及其对应的视觉和惯性测量。边缘化的测量会转变为先验。如果第二个最新帧是非关键帧，会简单移除掉该帧和它对应的视觉测量。然而，预积分的惯性测量值会给非关键帧保留下来，预积分过程会持续到下一帧。

边缘化是使用舒尔补（Schur complement）进行的。我们基于所有与被移除状态相关的边缘化后的测量值构建一个新的先验。

我们确实注意到，边缘化导致了线性化点早早的固定了，这可能会产生次优的估计结果。然而，由于小的漂移对于VIO是可接受的，我们认为边缘化造成的负面影响并不重要。

E. 用于相机频率状态估计的纯运动视觉-惯性捆机调整，Motion-only Visual-Inertial Bundle Adjustment for Camera-Rate State Estimation

对于计算能力低的设备，比如手机，由于对非线性优化的大量计算需求，紧耦合的单目VIO无法实现相机-频率（camera-rate）的输出。为此，我们部署了一个轻量级的纯运动视觉-惯性捆机调整去加速相机频率（30Hz）的状态估计。

纯运动视觉-惯性捆机调整BA的代价函数与文献【22】中单目VIO系统中相同。然而，我们没有优化滑动窗口内所有的状态，而是只优化了固定数量的最新IMU状态的姿态和速度。我们将特征深度、外参、bias偏差以及那些我们不想去优化的旧的IMU状态当作常量值。我们对纯运动的捆机调整BA确实使用了所有的视觉和惯性测量。这导致比单帧PnP方法更加平稳的状态估计。提出的策略如图8所示。与完全紧耦合单目VIO相比，其可能会造成在最新的嵌入式计算机中超过50毫秒的计算时间，纯运动的视觉-惯性捆机调整BA只消耗了5毫秒去进行计算。这使得低延迟的相机频率的姿态估计变为可能，特别有利于无人机或者AR应用。

F. 用于IMU-频率估计的IMU前向传播，IMU Forward Propagation for IMU-Rate State Estimation

IMU测量的频率要比视觉测量频率高得多。尽管我们的VIO频率被图像拍摄频率所限制，但我们仍然能用最新的IMU测量集直接传播最新的VIO估计以达到IMU频率的性能。高频状态估计能够被用于闭环检测中的状态反馈。利用IMU频率的状态估计的自主飞行实验在第9节的D中提出。

G. 故障检测和恢复，Failure Detection and Recovery

尽管我们 紧耦合单目VIO系统对于多种具挑战的环境和运动是鲁棒的，但是由于剧烈的照明变化或运动，故障仍然是不可避免的。主动的故障检测和恢复策略能够提升所提出系统的实用性。故障检测是一个从估计器中检测异常输出的独立模块。我们现在采用下述标准用语故障检测：

• 在最新帧中被跟踪的特征数少于某个阈值；

• 最新两个估计输出之间的位置和旋转中较大的不连续性；

• bias偏差或者外参估计有较大的变化；

一旦故障被检测，系统切换回到初始阶段。一旦单目VIO系统是成功初始化，会创建姿态图的一个新的、单独的部分。

7. 重定位，Relocalization

我们的滑动窗口和边缘化方案限制了计算复杂度，但是它也给系统。引入了累计的漂移。更具体地说，漂移发生在全局3维位置  和绕着重力（航向角）方向。为了消除漂移，提出了无缝集成到单目VIO系统中的紧耦合重定位模块。重定位过程从回环检测模块开始，识别已经访问过位置的。建立闭环候选帧与当前帧之间特征级别的联系。这些特征对应被紧密集成到单目VIO模块中，产生带最小计算开销的无漂移的状态估计。多种特征的多次观测被直接用于重定位，从而导致更高的精度和更好的状态估计平滑性。重定位过程的图示如图9（a）所示，显示了重定位的过程。仅以VIO姿态估计（蓝色）开始。过去的状态被记录下来（绿色）。如果检测到最新帧图像的回环，如第二个图中红色线条所示，重定位就此发生了。注意由于重定位特征级的一致性，我们能从多个历史关键帧中集成闭环约束，如最后三幅图像显示的那样。如图9（b）中显示的是姿态图优化的过程。当关键帧被添加到姿态图中。如果此关键帧和过去的任何关键帧之间存在回环，表示为4自由度的相对刚体变换的闭环约束也将会被添加到姿态图中。姿态图在一个独立线程中使用所有相对位姿约束进行优化，并且重定位模块总是相对于最新的姿态图配置运行。

图9. 重定位和姿态图优化过程的图示。

A. 回环检测，Loop Detection

我们利用DBoW2，一种最先进的词袋模型位置识别方法，用于回环检测。除了用于单目VIO的角点特征，用BRIEF描述子检测并描述出500多个角。额外的角点用于在回环检测上达到更好的召回率。描述子被当作视觉单词在视觉数据库中进行检索。DBoW2在时间和几何一致性检查后返回闭环候选帧。我们保留所有特征检索的BRIEF描述子，但是丢弃掉原始图像以减少内存的消耗。

我们注意到我们的单目VIO系统能够使横滚角roll和俯仰角pitch变得可观测到。因此，我们不需要依赖于旋转不变的特征，比如在ORBSLAM中使用的ORB特征。

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、orb-slam3等视频课程）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~