VINS-Mono：一种稳健的单目视觉惯性状态估计器

编者序：本文提出的紧耦合滑窗VINS状态估计算法，可在未知状态情况下稳健初始化，具有线外参标定、统一定义在球面上的重投影误差、闭环检测和4自由度位姿图优化的特点。本文是香港科技大学沈邵劼课题组的开源杰作，媲美state-of-the-art。将分为上、下两部分介绍，上部分侧重整体及实验，下部分侧重理论细节及应用。

推荐指数☆☆☆☆☆

一、引言

本文提出的视觉惯性系统结构框图如下所示。第一部分是测量处理前段，负责提取并跟踪每个新图像帧的特征，及帧间IMU数据预积分。第二部分是初始化过程，提供了激活非线性系统的必要初始值，如姿态、速度、重力向量、陀螺仪偏差及3D特征位置。第三部分为非线性图优化，通过联合优化所有的视觉、惯性信息求解滑窗内的状态。另一部分运行另一个线程，负责闭环检测及姿态图优化。

主要分为五部分

1. 传感器数据处理：

• 单目相机Monocular Camera: Feature detection and Tracking
• IMU: Pre-integration

2. 初始化：

• 仅使用视觉构建SfM
• 将SfM结果和IMU预积分结果对齐

3. 基于滑动窗口的非线性优化：

4. 闭环检测：

5. 4自由度全局位姿图优化：

二、各部分概述

测量预处理：对于视觉测量，跟踪连续帧间的特征并检测最近一帧中的新特征；对于IMU测量，连续帧内进行预积分。注意IMU测量受偏差和噪声的影响，因此我们在预积分和优化部分考虑了偏差，这对低成本IMU非常必要。

初始化：单目紧耦合视觉惯性系统是高度非线性系统。由于单目相机不能直接观测到尺度，因此没有好的初始值情况下很难直接融合这两种传感器。一般的，在静止假设的前提下，认为开始几秒的IMU测量值平均是重力向量，IMU传播（propagation）认为是初始姿态。然而，当IMU测量值受非平凡（non-trivial）偏差影响或开始时存在加速运动情况时，这么认为是不合适的。为提高单目视觉惯性系统的成功率，需要稳健的初始化。我们采用松耦合传感器融合方法获得初始值。我们发现仅视觉SLAM或SfM初始化性能良好，大多数情况下可通过Eight-point、Five-point、homogeneous and fundamental等相对运动方法完成初始化。通过将IMU预积分结果与仅视觉对其，可以大体上获得尺度、重力、速度，甚至偏差，这对激活非线性系统是很有好处的，如下图所示。

紧耦合非线性优化：状态初始化完成后，运行基于滑窗的非线性估计器进行高精度状态估计，使用Ceres Solver 求解该非线性优化问题。滑窗说明如下图所示。

闭环检测：我们在一个单独线程里进行闭环检测和维持一个仅姿态图。由于滑窗缺少绝对位置和偏航观测，当检测到闭环时，滑窗将通过重定位贴在姿态图上。关键帧从滑窗中边缘化后将被加入到姿态图，当新加入的关键帧包含闭环信息时执行姿态图优化。IMU测量提供了完整的俯仰角和滚动角，所以仅xyz和偏航角4个自由度上有累积漂移，为避免引入虚假信息，我们直接在这4个自由度上优化姿态图。

三、实验

3.1 数据集对比

算法评估使用的是EuRoc MAV数据集，该数据集利用无人机搭载的双目相机和IMU采集的，并通过VICON和Leica MS50采集ground truth。我们仅仅利用双目左边的相机数据。该数据集包括非平凡IMU偏出和光照变化，非常有代表性有挑战性。本实验选用MH_03_median和MH_05_difficult数据。

对比算法选的是目前state-of-the-art算法OKVIS，该算法是另一种基于优化的滑窗算法。与OKVIS算法相比，我们的算法在稳健初始化和闭环检测上更完整，在每个细节上都不太一样。为描述简单起见，记VINS为仅里程计，VINS_loop为含闭环检测姿态图优化的本文算法，OKVIS_mono和OKVIS_stereo分别为单目和双目OKVIS算法。为公平起见，我们丢掉开始的100个输出，取接下来的150个输出，计算该输出到groud truth的变换并对其，由于单目系统需要更长的时间收敛。

实验结果如下图所示。对平移误差来讲，VINS_loop误差最小。对旋转误差来讲，OKVIS更稳定些，在俯仰角和滚动角估计上更准确，可能的原因是本文算法为节约计算资源使用了IMU一阶近似去做预积分，或者OKVIS以更高的频率边缘化IMU约束。

另外，对于仅里程计，本文算法和OKVIS都很好，难分高下，且基于优化的紧耦合算法将来也并不能大幅度提高精度。

3.2 室内环境

在实验室手持设备来回走，经历了行人、黑暗、低特征区域、镜面反射等，代表了典型的日常生活。所用设备及实验结果如下图所示。

3.3 大规模环境

从室内环境转入室外环境：从实验室座位上出发走过实验室、下楼梯去室外到处走走，返回实验室并爬楼梯，回到座位上，整个轨迹10分钟700多米。不用闭环检测时，起点和终点误差距离是[-5.47,2.76,-0.29]，约0.88%；带闭环检测是误差距离为[-0.032，0.09，-0.07]，可忽略。轨迹及与Google earth对齐结果如下图所示。

大规模实验在HKUST校园进行，约710米长，240米宽，60米高度变化。整个轨迹持续1小时34分钟，共5.62km，可实验系统稳定性和持久性。设置关键帧数据库大小为2000，兼具是实现和足够的闭环信息。每当检测到闭环时，优化所有的姿态图。在主频为3.6Hz的Intel i7-4790 cpu上耗时统计及与Google地图比较结果如下图所示。

英文名称：VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator

链接及源码：

https://github.com/HKUST-Aerial-Robotics/VINS-Mono/blob/master/support_files/paper/tro_technical_report.pdf

下

一、总体框架回顾

本文提出的视觉惯性系统结构框图如下所示。第一部分是测量处理前端，负责提取并跟踪每个新图像帧的特征，及帧间IMU数据预积分。第二部分是初始化过程，提供了激活非线性系统的必要初始值，如姿态、速度、重力向量、陀螺仪偏差及3D特征位置。第三部分为非线性图优化，通过联合优化所有的视觉、惯性信息求解滑窗内的状态。另一部分运行另一个线程，负责闭环检测及姿态图优化。

二、测量预处理

2.1视觉预处理前端：

对每张新图像，使用KLT稀疏光流算法跟踪已有特征，同时检测新的角点特征以维持每张图像100-300特征。检测器通过设置相邻特征最小间隔强制特征均匀分布，outlier去除后，特征投影在单位球面上（unit sphere）。Outlier通过RANSAC基础矩阵测试去除。

关键帧的选取原则有两个：平均视差和跟踪的特征数。当跟踪的特征数小于某个门限或者跟踪特征的平均视差超过某个门限时，插入关键帧。请记住，除了平移，旋转也会导致视差，但是纯旋转时特征无法三角定位，我们在计算视差时用IMU propagation结果补偿旋转。

2.2 IMU预积分

对于基于优化的算法来说，IMU预积分可节约大量的计算资源，因为不需要一遍遍计算IMU测量。IMU预积分部分，仅仅与IMU偏差有关，当偏差变化小时，可通过一阶近似调整，否则才需要重新计算。离散时间预积分可通过Euler、Mid-point、RK4等数值计算方法计算。我们程序中采用内点法计算。

经推导整理，IMU测量模型可写成如下形式

三、初始化

3.1 滑动窗口视觉SfM

初始化时，需要先执行视觉SfM，估计up-to-scale相机位姿和特征位置。首先检查最后一帧和前面帧的特征匹配，如果找到多余30个跟踪特征并且平均视差对于20个像素的前面帧，则通过5点法恢复这两帧间的相对旋转和up-to-scale平移，否则将最后一帧留在滑动窗口并等待下一帧。如果5点法成功了，则固定平移的scale并三角化这两帧的所有特征。基于这些三角化的特征，使用PnP（Perspective-n-Point）算法n-Point估计窗口内其余帧的位姿。最后执行全局BA最小化所有特征的重投影误差。由于绝对尺度未知，可用第一帧作为参考帧，其余帧的位姿和特征位置用第一帧表示。

3.2 视觉惯性对齐

首先根据SfM估计出的相邻两帧间的旋转和IMU预积分得到的约束，通过将IMU预积分线性化为陀螺仪偏差的函数，通过最小化窗口内所有帧的旋转误差；计算出陀螺仪偏差。然后构建并求解最小二乘问题得到速度、重力向量和尺度的初始估计；进而通过约束重力向量的幅度，在tangent空间求解出重力向量精确值（尤其是重力方向），如下图所示。最后根据旋转矩阵调整速度和尺度，完成初始化。

四、紧耦合非线性优化

4.1 公式

优化公式中包含窗口内的所有关键帧（参数有平移、旋转、速度、加速度偏差和陀螺仪偏差）和所有特征（特征的inverse depth）。优化的损失函数是边缘化的先验信息和视觉与IMU误差的Mahalanobis范数之和，如下所示。

其中IMU剩余误差和视觉剩余误差依次s如下图所示。

我们在边缘化IMU状态和图像特征的同时，根据边缘化状态将测量加入到先验信息。边缘化策略如下图所示。当倒数第二帧是关键帧时，将最早一帧及其相应测量边缘化；如果倒数第二帧不是关键帧，我们仅仅将倒数第二帧的视觉测量扔掉而保留它的IMU测量。该策略增加了系统的稀疏性。边缘化不仅可以保持窗口内的关键帧数量恒定，而且限定了预积分IMU测量的不确定性。我们使用Schur消元实现边缘化，通过边缘化，去掉状态的重要信息得以保留且计算复杂度恒定（important information of the removed states is kept and computation complexity is bounded）。

由于光照强烈变化或剧烈运动，失败不可避免。主动监测错误并从错误中恢复可以增加系统的实用性。错误检测的原则有：跟踪的特征数或最新一帧特征数少于5、最后两个输出的位置或旋转有突变、偏差或外参估计有很大的值。检测到错误后，以最后可能的位姿，重新初始化系统。同时，保留关键帧数据库，用于闭环检测。

五、闭环检测

5.1 闭环检测

我们使用DBoW2词袋模型和BRIEF描述子检测闭环。当新插入关键帧时，视觉前端提取的特征并不够，需要额外提取500个FAST角点特征，并计算所有特征的BRIEF描述子。检测到闭环时，执行时空一致性检查后，DBoW2返回候选关键帧，将原始图像扔掉而保留BRIEF描述子，以节省内存。

5.2 特征检索

当检测到闭环时，需要检索特征建立连接，本文仅建立含深度信息的前端提取的特征间的连接。直接匹配会有很多outlier，我们限定了匹配区域，每个特征仅在对应位置邻域内搜索。然后使用RANSAC检测几何一致性移除outlier。当匹配的特征数超过一定门限时，认为是正确的闭环匹配，融合二者信息，如下图所示。

5.3 重定位

由于新帧和窗口内的其它关键帧有很多相同的特征，因此很容易将闭环帧与窗口内关键帧进行联合优化，加入闭环帧仅增加了视觉约束而没有增加IMU约束，如下所示。

5.4 位姿图加入关键帧

当关键帧被边缘化时，将它加入位姿图，实际上为减少计算量每三个才加入一个。这些关键帧是位姿图的顶点，位姿图的边分为顺序边和闭环边两种。

5.5 4DOF位姿图优化

由于惯性视觉系统可以观测到绝对尺度、俯仰角和滚动角，累积漂移仅出现在位置xyz和偏航角4个自由度上。因此闭环后，需要进行4自由度而不是6或7自由度位姿图优化。其中闭环边的优化函数选huber稳健损失函数，如下所示。

5.6 数据库管理

随着轨迹的增加，数据库变得越来越大，闭环检测和位姿图优化时间越来越长，长时间实时应用困难。我们限定数据库大小恒定，当关键帧数量超过某个门限时，根据关键帧密度分布去掉一些密集的关键帧，从而避免上述问题。

六 应用

无人机平台及IPhone7手机上实验结果如下图所示。其中手机平台是与tango做对比，无人机平台用的是鱼眼相机。