超详细：VINS-Mono论文中文记录

题目

VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator
VINS-Mono是一个单目视觉惯性状态估计器，其中的鲁棒性，如果从论文来看而不涉及工程细节的话，是指其独特的初始化策略（实际上可以对相机与imu间的外参、imu与相机数据间时间戳进行在线校准，借助imu和视觉的融合而可以应对更复杂苛刻的环境等也增加了其鲁棒性）而多用途则一方面因为其本身高性能（前端使用光流追踪特征点，后端使用滑动窗口，对于边缘端还可以使用只优化位姿的方法减少计算量），从而可以适配不同设备。论文中将VINS-Mono适配到了手持设备、小车和无人机上，都取得了很好的效果。另一方面状态估计本身就是很多应用的基石，自动驾驶，AR/VR都需要实时估计设备状态。

接下来会对VINS-Mono原论文进行一对一的记录（中文笔记），有些地方会结合经验添加说明，如果能耐心读完，基本就知道VINS-Mono论文中讲了什么了。

0. 摘要

为什么做VINS-Mono：六自由度状态估计的最小花费就是一个单目摄像头加一个低成本imu。状态估计器的初始化过程是鲁棒性的。对imu预积分值和相机观测进行紧耦合的非线性优化，以取得更准确的结果。
回环检测模块是紧耦合优化相结合，使重定位的计算代价很小。
利用四自由度的姿态图优化以取得全局一致的地图。保存地图和重用地图的方式很高效。通过姿态图优化，可以将当前地图和之前保存的地图进行合并。
算法效果测试是在公开数据集和真实场景都进行过测试并与其他先进算法进行比较（这里指okvis），并在无人机、手机上进行了应用测试。
最后强调VINS-Mono是一个可靠，完整和通用的系统，适合高精度定位的程序，然后说开源了源代码（pc端和手机端ios）。

• 笔记：从我个人的使用经验来看，VINS-Mono初始化需要设备先运动一会，充分激励imu才行，对于需要系统一启动就要执行任务的设备，这一点其实使用受到了限制。比如你用指南针，需要先把手机绕下8字，手机指南针才能正常工作，这多少有点不方便。另外VINS-Mono对设备标定其实要求很高，标定的不好轨迹很容易发生飞飘的情况。

检索关键词：单目视觉惯性系统(VINSs)，状态估计，传感器融合，SLAM

1. 背景简介

状态估计是很多应用的基础模块，比如机器人导航，自动驾驶，AR和VR等。单目相机由于其价格低，体积小，结构简单，一直是研究人员研究实现状态估计的热门选择，但是单目相机存在尺度不确定的问题，这限制了其实际应用。如果加上imu的辅助，单目相机状态估计不仅可以解决尺度不确定的问题，还可以实现翻滚角和俯仰角的客观（因为imu可以测量重力加速度方向）。
imu在视觉追踪不好的情况下仍然可以发挥作用，imu和相机是两种互补的传感器。所以imu+单目相机（VINS-Mono）不管在地面小车、无人机还是手机上，都在便携性和功耗方面有着巨大优势。
但这样的组合还是有问题的，首先就是初始化的问题，即如何把imu测量和单目视觉测量关联到一起，对于高度非线性的VINS系统，这是极大的挑战，也是限制VINS使用的原因之一。
由于累积误差的原因，状态估计器长期运行不可靠，需要加入回环检测以消除累积误差。回环检测，重定位，全局优化需要加入系统以消除状态估计器的漂移。
地图重用需求也在不断上升。
所以，VINS-Mono为解决这些问题而开发出来，另外，VINS-Mono也是之前三个工作的结合和扩展。

• 笔记：可以看到初始化策略是秦2017年发布的，重定位，全局图优化和地图重用是秦2018年的发布的，手机AR应用是2017年发布。而本论文是2018年5月发表的。

VINS-Mono的特点：

• 1.未知初始状态的鲁棒性初始化过程
• 2.带imu-camera外参校准和imu校准的紧耦合，基于非线性优化的单目VIO系统
• 3.在线重定位和四个自由度的全局姿态图优化。
• 4.姿态图可以保存，加载，并和局部姿态图进行合并。

VINS-Mono的贡献：

• 技术贡献：鲁棒性的初始化、重定位、姿态图重用
• 工程贡献：集成系统的开源项目、无人机部署和手机应用

成功的应用项目：

• 手机上小尺度的AR
• 无人机上中尺度的状态反馈
• 大尺度的状态估计

论文结构：
第二节：相关工作介绍
第三节：VINS-Mono系统总览
第四节：视觉和IMU测量的预处理步骤
第五节：VINS-Mono的鲁棒的初始化过程
第六节：紧耦合、自校准、基于非线性优化的单目VIO
第七节：紧耦合的重定位
第八节：全局姿态图的优化和重用
第九节：VINS-Mono的测试和对比结果
第十节：对VINS-Mono未来可能的研究方向

2.相关工作介绍

2.1 融合方式

摄像头和imu数据融合方法：

• 1.最简单的是松耦合法，此时imu是独立于摄像头的模块，常使用EKF算法，imu数据此时用于状态传播，使用纯视觉位姿进行更新。
• 2.紧耦合，基于EKF或者优化的方式，此时imu数据和摄像头数据在原始数据层面一起进行优化。

MSCKF是基于EKF的紧耦合流行算法。
SR-ISWF（Square Root Inverse Sliding Window Filter）是MSCKF的扩展，使用逆滤波器进行迭代计算，和基于优化的算法等价。
基于图优化或者BA的算法优化所有测量值，以期取得最佳结果。这类算法为了控制求解问题规模，常边缘化过去的状态和测量值（就是滑动窗口法），但求解高度非线性系统本身的耗时性，这类算法难以在计算资源受限的平台（如手机）上进行运行。

2.2 摄像头数据处理

• 直接法：残差为光度误差，需要良好的初始值，在建立稠密地图上具有天然的优势。
• 间接法：残差为几何距离误差（即重投影误差），在计算和匹配特征点上需要耗费额外的时间，但工程上来说其技术成熟，鲁棒性好。（建稠密地图没有优势）

2.3 imu数据处理

imu数据频率远高于摄像头数据频率。
最直接利用imu数据的方式是在EKF中，用imu数据进行状态传播。
图优化中，高效利用imu数据的技术为imu预积分。
最开始imu预积分的旋转参数化形式是欧拉角，然后沈等人推导了imu数据在连续时间内协方差的传递（没错，就是秦的导师沈老师的工作：Tightly-coupled monocular visual inertial fusion for autonomous flight of rotorcraft MAVs），然后又有人通过后验IMU偏置校正，进一步提升了imu预积分的效果。

2.4 初始化

利用短时间imu预积分值取得相对旋转的线性状态估计器初始化策略，不能对陀螺仪的偏置和图像数据噪声进行建模。
单目惯性系统的封闭解提出，然后基于此种方法，并加入了陀螺仪偏置校准的初始化方法被提出，但这类方法依赖长时间的imu双重积分，不能对imu积分的不确定度进行估计。
另外有一种基于SVO的重新初始化和故障恢复算法，但需要一个向下的距离传感器来恢复尺度。
最后是ORB-SLAM初始化方式，但其耗时较长（10s以上），对需要立即开始导航任务的机器人不太好使。
笔记：初始化是单目算法很重要的模块，其初始化结果好不好，直接影响算法表现，而单目算法的初始化都需要传感器设备进行移动来完成。

2.5里程计

不管使用何种数学模型，里程计在长期运行始终会有漂移的问题，回环检测是解决这一问题的办法。
ORB-SLAM有回环检测和地图重用模块，使用词袋模型，并在回环检测中加入了7自由度的姿态图优化。

3. VINS-Mono系统总览

VINS-Mono主要包括四个部分，如下图红框所示

• 1.测量处理线程(measurement preprocessing (see Section IV))：图像特征提取和追踪，在两个图像时间戳间的imu数据被进行预积分。
• 2.初始化线程（The initialization procedure (see Section V)）：提供姿态（相机和imu）,速度（imu坐标系的速度？），重力向量，陀螺仪偏置（为啥没有加速度计的偏置），特征点的三维坐标。这些值用于引导后续VIO的非线性优化。
• 3.视觉惯性里程计（滑动窗口内非线性优化）The VIO (see Section VI) with relocalization (see Section VII)：对特征观测和imu预积分值进行紧耦合优化，该模块带重定位功能。
• 4.姿态图优化模块（the pose graph optimization module (see
  Section VIII)）：利用上面的重定位结果，进行姿态图优化，以消除漂移。

VIO线程和姿态图优化跑在分离的线程上，二者是独立的线程。

后面是一堆符号介绍，这里就略过了。

4 视觉和IMU测量的预处理步骤

该部分对应于系统总览第一部分：measurement preprocessing。

4.1 视觉部分

使用KLT稀疏光流追踪法对最新的图像进行光流追踪。如果追踪的点数量少于一个阈值，则会额外检测一些特征点。
特征点是以第一次观测到的为观测值，经过去畸变后，通过相机模型，投影到一个单位圆上（方便做视觉残差）。
关键帧的判断：a)以视觉测量判断为主。如果追踪的特征点间视差大于某一个阈值，则认为是新的关键帧。另外视差不仅可以通过平移，还可以通过旋转来得到，为解决纯旋转问题，使用了短期imu预积分数据来补偿旋转。b)为保证追踪的连续性，如果某一帧种追踪的特征点数少于一个阈值，则认为其是关键帧。

4.2 imu部分

imu数据模型
$$\begin{gathered} \widehat{a_t}=a_t+n_a+b_{at}+R_w^t{G}_t \\ \widehat{w_t}=w_t+n_b+b_{wt} \end{gathered}$$
1）噪声和偏置
测量值包括偏置和噪声，加速度则还包含重力加速度。
偏置假设服从随机游走模型（其导数服从高斯分布），噪声假设服从高斯分布。

• 笔记：imu数据的处理很像数理统计中使用的方法，一个连续的带自己分布特征的数据，要如何描述其离散性质，对该离散数据进行非线性变换，变换结果和该数据本身分布有何联系。
  2）imu预积分
  补充知识，在世界坐标系下imu积分为
  
  解释：计算imu在世界坐标系下的位移，需要用世界坐标系下的加速度。而imu测量结果都是自己坐标系（即body坐标系下的加速度），需要通过状态量$q_{w{b}_t}$变换为到世界坐标系下状态量。但每一次优化后状态量是会变化的，这样就需要重新积分，这种低效的做法是我们想避免的。

预积分所做的事，就是将状态量分解为上一时刻与当前时刻两部分。上一时刻状态量看为常数，那当前时刻状态量只和imu积分有关，积分运算进行一次就行。原来的积分运算换为如下形式：

所以积分公式可以变为如下：

回到论文：
在相邻两个关键帧（图像帧）之间，对imu数据进行如下预积分计算：
可以看到，这里的加速度值是减去了偏置的，上面推导时默认使用真值（没有偏置的数据）

对预积分量的协方差也会进行计算。

4.3 偏置纠正

当偏置优化结果变化不大时，利用一阶泰勒展开对预积分量进行更新：

如果偏置优化变化较大，则使用新偏置重新计算imu预积分量，方法同第二部分公式。
这种策略可以极大节省计算量。

5 鲁棒的初始化过程

单目惯性系统是高度非线性系统，系统的正常运行高度依赖准确的初始化过程。
初始化过程由松耦合下的imu预积分辅助纯视觉结构还原过程。

5.1 视觉重构

使用滑动窗口中纯视觉的sfm方法。初始化过程从一个sfm过程开始，还原一个缺乏尺度信息的，由相机姿态和特征点位置构成的图模型。
获取视觉计算所需的大位移帧：
因为图像是源源不断地传进来的，开始是不断对最新帧进行特征追踪，计算最新帧与之前所有帧特征的关系。如果最新帧与之前某一帧存在如下关系：对应追踪的特征点不少于30个，特征点间对应位移大于20像素（这里在代码里是平均位移），那么就算找到合适的大位移帧。
利用五点法，计算这两帧之间的位移和旋转（缺乏尺度信息的）。然后任意设置一个尺度，用三角测量方法计算出这两帧中特征点的三维坐标。
基于已三角化的特征点，使用pnp法计算整个滑动窗口中所有帧（一般为10帧）的姿态。
最后，使用完整BA(既包含姿态，又包含特征点)来最小化特征点的重投影误差。
因为还没有关于世界坐标系的信息，这里把相机的第一帧作为sfm过程的参考帧，也就是参考坐标系。
所有帧的姿态和特征点坐标结果都是相对于相机的第一帧（第一帧也就是相机坐标系）。借助于相机和imu的外参$p_c^b$和$q_c^b$，可以把相机坐标系下位姿转换到imu坐标系下：

• 笔记：这里说的，其实是基于imu和相机的外参，计算每一个相机位姿下对应的imu位姿，其参考帧，还是基于相机坐标系，也就是相机坐标系下对应时刻imu位姿。
  这里的尺度信息还是未知的，接下来会进行求解。

5.2 视觉惯性联合

使用imu预积分值，使纯视觉中的尺度信息得以和imu测量相匹配。
A)陀螺仪偏置校准
从纯视觉sfm中可以获得滑动窗口中所有帧得姿态，而从imu预积分中也可也获得连续两帧的相对旋转。最小化如下代价函数：

相对旋转的更新使用第二个函数，可以得到一个初始化的偏置的校正值，使用新的陀螺仪偏置值，对滑动窗口内imu预积分量进行重新积分。
B)速度、重力向量、尺度的初始化
陀螺仪偏置初始化后是对导航必要信息的速度、重力向量、尺度的初始化。

对imu预积分量有如下两个公式

该公式和速度，重力向量、尺度信息相关。

通过最小化上面这个代价函数，可以获得滑动窗口中每一帧的速度。相对于相机坐标系的重力向量和尺度值。
C)重力向量优化

重力向量的更新策略是将$g$向量使用$g(\hat{g}+\delta g)$来代替，其中$\hat{g}$是测量出的平均重力向量单位方向，$\delta g$是在正切球空间里的微小扰动。不断迭代这个值，直到$g$向量收敛。

D)初始化的完成
在对重力向量完成优化后，就可以通过把重力向量旋转到z轴来获得世界坐标系和相机坐标系的旋转关系。然后把所有相机坐标系的量旋转到世界坐标系下，此时imu坐标系的量同样也旋转到世界坐标系下。在sfm中的位移关系会缩放到米单位下。到这里，初始化完成，所有米制数据会提供给接下来的紧耦合单目VIO系统。

6 紧耦合的单目VIO系统

完成初始化后，就进入基于滑动窗口的紧耦合单目VIO系统。

6.1 公式

第一个是所有状态的集合。第二个是imu的状态向量，第三个是相机与imu的外参。这里就不解释每个字母含义了，论文里都介绍的很详细。对视觉特征的参数化使用的是逆深度形式。
这里使用的是视觉惯性BA优化方式，使得先验误差和所有测量值残差的马氏距离最小，从而获得最大后验估计。

第一项为先验误差，第二项为imu测量残差，第三项为摄像机测量残差。对视觉测量使用Huber函数以减小特征误匹配带来的影响。

6.2 imu残差

在滑动窗口中连续两帧imu测量残差为：

基于状态量计算得到的相对位移，速度，旋转和imu预积分量的差值。

6.3 视觉残差

与传统方法中将特征点投影到单位平面上计算残差不一样的是，VINS-Mono把像素误差投影到一个单位圆上。即相机残差定义在如下的单位圆：

其原因是这种残差定义方式几乎适用于所有类型的相机，包括广角、鱼眼或全向相机。
将特征点第一次观测的坐标，通过先转到imu局部坐标，然后imu局部坐标转换到世界坐标，imu世界坐标再转换到局部坐标，然后投影到当前相机坐标系，当前坐标系下投影到单位圆，就形成了路标的单位向量。
观测点单位向量与路标单位向量相减，向量差在单位圆上一切平面分解，就得到了两个残差值，即视觉的几何误差残差。

6.4 边缘化残差

为了限制VIO非线性优化的复杂度，使用了边缘化。对imu状态量和视觉特征选择性的进行边缘化，同时把边缘化的状态量转为先验。

如果第二新的帧是关键帧，则把滑动窗口中最老的关键帧边缘化掉（对应于第一种情况），如果第二新的帧不是关键帧，则把该帧对应的特征点舍弃掉，但保留其imu测量值。
为保证稀疏性，非关键帧的测量值不是所有的都会被边缘化掉。（对第二新的帧不是关键帧的情况）。
边缘化方案只是对窗口中的关键帧。它确保三角化特征点过程中计算特征点具有足够的视差，并且加速度计能得到充分的激励。边缘化使用舒尔补。
基于所有与移除状态的测量值，VINS-Mono建立了一个新的先验方法。新的先验被添加到现有的先验中。（VINS-Mono的代码中将姿态和路标信息都进行了边缘化（成为先验信息），这样客观上说是最大程度保留了所有有效信息，但也会导致海参矩阵的求解比较耗时）
我们注意到边缘化导致线性化点的早期固定，这可能导致次优估计结果。然而，由于VIO的小漂移是可以接受的，我们认为由边缘化造成的负面影响并不重要。

• 笔记：边缘化过程中残差的雅可比可能不是在同一点得到的，这是边缘化一直存在的问题，对于该问题，其实也有很多解决办法。但VINS-Mono中认为漂移本身就存在，后续通过回环也可以来进行消除。

6.5 针对相机实时帧率的纯运动视觉惯性状态估计器

对于计算能力有限的平台，比如手机，紧耦合单目VIO由于非线性优化沉重的计算量，在这些平台是难以达到相机实时帧率更新的。为此，除了完全优化之外，VINS-Mono还采用了轻量级运动视觉惯性优化，将状态估计频率提高到相机率(30Hz)。
纯运动视觉惯性优化的代价函数与单眼视觉优化的代价函数相同。但不同的是，纯运动状态估计并没有优化滑动窗口中的所有状态，而只是优化了固定数量的最新IMU状态的姿态和速度。将特征深度、外部参数、偏差和不想优化的旧IMU状态视为常数值。
但这确实使用所有的视觉和惯性测量来进行仅限运动的优化。（只不过有些值只是约束，不是优化量）

这种方法比单帧的pnp方法结果更为平滑。
这种方法把原来在嵌入式设备上需要50毫秒的全优化计算降低为5毫秒，使得在低算力设备上能得到相机帧率的姿态估计，特别适合在需要AR或者VR的手机上进行运行。

6.6 imu向前传播以得到imu频率的状态估计

IMU测量的速度比视觉测量的速度要高得多。虽然我们的VIO频率受图像捕获频率的限制，但我们仍然可以直接传播最近的IMU测量的VIO估计，以实现IMU速率性能。高频状态估计可以作为回环状态反馈。第九-c节介绍了一个利用这种imu速率状态估计的自主飞行实验。

7 紧耦合的重定位

滑动窗口和边缘化可以控制计算复杂度，但同时也给系统带来了累计漂移。
VINS-Mono使用了一种紧耦合的重定位方式来消除累计误差。重定位从一个回环检测模块开始，以分辨什么地方之前来过。
当前帧和回环帧在特征程度上的约束得以建立。这些对应特征的约束被集成到了单目VIO系统中，优化至最小以得到无漂移的状态估计。
多特征的多次观测约束直接在重定位中使用，可以得到更高精度和平滑度的状态估计。

7.1 回环检测

使用词袋方法DBoW2进行循环检测。
为了进行回环检测，从图像中抽取了500个特征角点并计算其BRIEF描述子。
额外的角点抽取是为了提高召回率。
描述子作为视觉数据库的检索词。
DBoW2在时间和几何一致性检查后返回候选闭环。数据库中保留所有简短的描述符以进行特征检索，但丢弃原始图像以减少内存消耗。

7.2 特征检索

当检测到一个回环时，需要通过特征对应关系来求解回环帧间关系。特征对应关系通过描述符进行匹配。但特征匹配可能会导致误匹配的问题，对该问题通过如下方式来解决。

• 2D-2D：使用RANSAC的基本矩阵检验。在当前图像和环闭合候选图像中检索到的特征的二维坐标来进行基本矩阵测试，去除离群点。

• 3D-2D:使用RANSAC进行的PnP测试。基于已知的局部滑动窗口中特征在局部滑动窗口中的三维位置，以及在回环帧图像中的二维特征点坐标，可以进行PnP测试。
  在除去异常值后，将该候选对象视为正确的循环检测，并执行重新定位。

• 笔记：如果去除异常点后，正确匹配的特征点大于某一个阈值，则会基于匹配点计算回环帧间的相对关系，如果几何关系满足某一个限制，则认为发生了回环。

7.3紧耦合的重定位

重定位高效地将滑动窗口的帧与过去的姿态关联到了一起。在重定位时，把回环的帧（滑动窗口中的帧）设为固定帧。
回环帧观察到的特征的视觉测量模型，与VIO中的视觉测量模型相同，其残差的计算方法类似。
唯一的区别是，回环帧的姿态取自姿态图（见第八节，即姿态图合并的情况)，或直接来自过去的VIO里程计的输出(如果这是第一次重新定位），并被视为一个常数。
于是，可以很轻易把回环帧的残差添加进优化中：

其中，回环残差的变量是在回环帧中检索到的所有特征的集合。
请注意，尽管待求解函数与（14）略有不同，但要求解的状态的维数保持相同，因为回环帧的姿态被认为是常数。
当滑动窗口中发生了多个回环时，同时使用所有回环帧的所检索的所有对应特征进行优化。
这为重新定位提供了多视图约束，从而获得了更高的精度和更好的平滑度。重新定位后保持一致性的全局优化将在第八节中进行讨论。

8 全局姿态图的优化和重用

在重定位后，为保证过去的姿态能被正确加载到全局地图中，额外的姿态图优化被开发使用了。

8.1 四自由度的累计误差

得益于对重力的测量，在VINS中对翻滚角和俯仰角是完全可观测的。
如图所示：

随着物体的运动，相对于参考帧的相对的3D位置和旋转就改变了。
但是，我们可以通过重力向量的方向定义水平面，那么相对水平面的翻滚角和俯仰角是可以一直被观测到的。因此这两个量可以被当作世界坐标系下的绝对测量。
而3D位置和偏航角则作为参考帧的相对估计。因此对这四个量来说，会有累计漂移。
为了充分利用有效信息，我们认为翻滚角和俯仰角没有漂移，而对其余四个方向进行四自由度的姿态图优化。

8.2 向姿态图中添加关键帧

在经过VIO处理后，关键帧被加入姿态图。
每一个关键帧在姿态图里是一个顶点，该顶点与其他顶点通过两种类型的边连接。

A)序列边：
一个关键帧与它之前的几个关键帧通过序列边建立联系（图中表示与之前两帧），表示与之前几帧的相对位移关系，这种关系直接通过VIO获得。
考虑到关键帧i和之前的一个关键帧j，序列边只包含相对位置$P$和偏航角$\psi$的约束。

B)闭环边
如果一个帧有回环连接，那么它在姿态图中与闭环帧通过闭环边相连。
与序列边类似，闭环边只包含四自由度的相对位移关系。定义和上面的公式类似。
闭环边的值通过重定位获得。

8.3 四自由度的优化

我们将两帧间的残差最小化定义为：

其中R参数前两个表示翻滚角和俯仰角，从VIO中获得且固定为常值。

整个序列边和闭环边的代价函数为：

前面是序列边集合，后面是闭环边集合。
尽管紧耦合的重定位有助于消除错误的闭环，但我们还是加入了huber函数来进一步降低任何可能的误回环影响。
相反，对序列边则没有加任何鲁棒规范函数。因为这些边都是从VIO中取得，而在VIO中已经有了足够的异常点剔除机制。
姿态图优化和重定位分别跑在两个分离的线程，这使得：
1.重定位可以使用最新的姿态图优化结果来进行。
2.即使姿态图优化尚未完成，仍然可以使用现有的姿态图进行重定位。

8.4 合并姿态图

姿态图不仅可以优化当前地图，还可以将当前地图与以前构建的地图合并。
如果我们加载了以前构建的姿态图，并检测到两个姿态图之间的闭环关系，我们可以将它们合并在一起。
由于两者间所有的边都是相对约束，因此姿态图优化通过闭环边连接自动将两个姿态图合并在一起。如图所示。

当前的地图被闭环边拉到之前的地图中。每个顶点和每条边都是相对变量，因此，我们只需要固定姿态图中的第一个顶点（被加载姿态图的第一个顶点）。

8.5 保存姿态图

姿势图的结构非常简单。我们只需要保存顶点和边，以及每个关键帧的词典查询向量（特征点描述子构成的向量）。丢弃原始图像以减少内存消耗，保存的数据结构如下：

其中i为帧索引，$\widehat{p_i^w}$和$\widehat{q_i^w}$分别为VIO的位置和方向。如果此帧有一个闭环帧，则v是闭环帧的索引。$\widehat{p_{iv}^i}$和$\widehat{{\psi }_{iv}}$是这两帧之间的相对位置和偏航角，通过重新定位得到。D（u、v、des）是关键帧的可视化查询向量，每个特性都包含二维位置及其简短的描述子。

8.6 加载姿态图

姿态图的保存格式就是其加载格式。每个关键帧都是姿态图中的一个顶点。顶点的初始姿态是ˆpwi和ˆqwi。回环边由闭环信息ˆpiiv、ψˆiv直接建立。每个关键帧和相邻关键帧建立几条顺序边。加载姿态图后，我们立即进行一次全局4-DOF姿态图优化。姿态图的保存和加载速度与姿态图的规模呈线性关系。

9 实验结果

公开数据集和现场真实世界数据都进行了测试。
首先在公开数据集上和同类先进算法进行一个跑分。
然后在室内，和同类算法来个同台竞技。
大尺度的实验证明其长期有效性。
此外还整了两个程序应用。

• 1）对于无人机，利用VINS-Mono进行一个位置反馈，以控制其遵循预定的轨迹。
• 2）把算法移植到了手机上进行运行。

9.1 数据集对比

A) VIO对比：使用EuRoC MAV 视觉惯性数据集。
摄像机型号及测量频率：Aptina MT9V034 global shutter, WVGA monochrome, 20 FPS
IMU型号及测量频率：ADIS16448, 200 Hz
真值标定设备：VICON and Leica MS50（维科和徕卡的测量及运动追踪设备）

• 笔记：EuRoC MAV数据集是由苏黎世联邦理工学院的自主系统实验室发布的（瑞士的两所联邦理工学院之一，位于德语区苏黎世市区，是欧洲大陆国家大学排名最高的大学，位居全球第25名。有人认为，苏黎世联邦理工学院是做SLAM的殿堂级院校）。其背景是欧洲机器人挑战赛（大概是这个名字），为了评估参赛选手的算法。这个数据集也是被很多队伍使用了，大家都认为这个数据集还是不错的，所以是比较公认的测试数据集。该数据集是一个双目+imu的视觉惯性数据集，由一个微型飞行机器人（MAV）在两个场景中采集，一个是苏黎世联邦理工学院的机器大厅（machine hell）,一个是类似于书房的场景。
• EuRoC MAV数据集链接

VINS-Mono是单目算法，图像数据只使用数据集中的左目图像。

与之同台竞技的算法是OKVIS，同样也是一个滑动窗口非线性优化的VIO算法，只不过没有回环检测部分。

使用两个数据集进行对比：MH_03_medium and MH_05_difficult，一个中等难度一个困难难度。
VINS-Mono使用两种模式进行对比，带回环检测和不带回环检测的模式。

MH_03_medium数据集结果

有的地方OKVIS效果好，有的地方VINS-Mono效果好，所以接下来是误差统计（不带回环的OKVIS效果堪比带回环的VINS-Mono，果然苏黎世联邦理工学院还是强）

第一个平移误差，看起来回环确实能控制累计误差（误差不会随着机器人运动距离增加而增加）。而不带回环的版本，后10米阶段vins-mono开始优于OKVIS。
第二个偏航角误差，则OKVIS是优于不带回环的vins,效果和带回环的vins相当。
第三个旋转误差，VINS在20米后误差变化不大，而OKVIS则随着移动距离的增大后误差增加（有误差累积）。

MH_05_difficult 数据集结果

同样，轨迹上看来，两者都有好于对方的部分，精确比较还得进行统计。

各误差比较情况和MH_03_medium数据集结果很类似，平移和旋转是VINS优于OKVIS，但偏航角的估计OKVIS好于VINS。

使用EuRoC全部数据集进行测试，利用绝对轨迹误差作为指标来统计，结果如下：

可以看到，大多数情况下VINS带回环版本表现最好。

还有更多的比较结果可以在如下论文中获得：

总得说来，与其他最先进的算法相比，VINS-Mono具有良好的性能.

B)地图合并结果
EuRoC的五个MH数据集是在不同起点采集（但都是在machine hell采集），利用VINS的全局姿态图合并功能，可以将5个MH序列合并成一个全局姿态图。

利用重定位和姿态图优化，在同一个相机数据下，把第一个序列（第一个数据集下的轨迹估计）中的第一帧的位置和偏航角设置为零（作为基础）。然后把剩下的数据集轨迹重定位并合并到第一个数据集轨迹上，合并结果如图所示：

合并的轨迹和真值的均方根误差（绝对误差）为0.21米，这个对于500米长度的总运行距离来看，还是很令人印象深刻的。
这个实验表明，VINS-Mono具有增量建图的能力。

• 笔记：500米距离，视觉惯性状态估计器的平均误差在20cm，这个也可以作为视觉方案作为状态估计所能达到的一个精度范围了。

9.2 现实数据实验

A)室内实验
实验装置：单目相机(mvBlueFOXMLC200w，20Hz)，DJIA3控制器（内包含一个IMU，100Hz)
手持传感器套件，人拿着以正常的速度行走。
结果还是与OKVIS进行比较。

实验结果：

在室内旋转时，VIO会产生明显的漂移（对两种算法都是这样）。OKVIS在x、y、z和偏航角上有明显的漂移积累。而VINS的重新定位和闭环模块有效地消除了这些漂移。

B)室外大尺度实验
这个非常大的数据集是在整个HKUST校园（香港科技大学内）使用手持VI-Sensor记录的。

VI-Sensor也是由苏黎世联邦理工学院的自主系统实验室联合开发的，如图：

该数据集覆盖了长度约710米、宽240米、高度变化约60米的地方。总路径长度为5.62km。数据包含25hz图像和200hz的IMU，持续1h34min。这是测试VINS稳定性和耐久性的一个非常重要的实验

在这个大规模的测试中，将关键帧数据库的大小设置为2000（姿态图中包含2000帧？），以提供足够的回环信息并达到实时性能。
使用CPU:Intel i7-4790 3.60GHz CPU
时间统计数据见表二。

可以看到主要耗时部分在回环检测和姿态图优化。

估计的轨迹与谷歌地图对齐结果

可以看到我们的结果在这个非常长期的测试中几乎没有漂移。

• 笔记：如果这个是绕了整个学校一圈，那香港科技大学校区好像不是很大（长710米，宽240米。）但算法能持续一个半小时左右，为达到实时性，对关键帧数据库的大小进行了限制，但这个的限制是如何做到的并没有说明，但这表明，随着追踪轨迹变长，回环检测和姿态图优化越来越耗时，这一部分对算法实时性会有较高影响。

9.3 应用

A)无人机的反馈控制
将VINS-Mono应用于一个飞行机器人的自动反馈控制中：
摄像头：型号为MatrixVision mvBlueFOX-MLC200w 分辨率： 752 × 480 ，全局快门摄像头，安装位置为飞行器前方。该摄像头为一个190º的鱼眼镜头。
imu:使用DJIA3飞行控制器获取IMU测量数据，同时用于飞行器姿态稳定控制
CPU: Intel i7-5500U (3.00 GHz)
因为传统的针孔相机型号不适合大型FOV相机，使用MEI相机模型，并使用toolkit进行校准。

在本实验中，使用VINS-Mono来进行状态估计，以测试其自主轨迹跟踪性能。本实验关闭了VINS-Mono的重定位功能。四翼飞行器绕8字飞行，8字每个圆半径为1.0m，在轨迹周围设置四个障碍来验证没开回环功能的VINS-Mono的准确性。四翼飞行器在实验过程中连续四次跟踪这个轨迹。100Hz（imu频率）机载状态估计（参见第VI-F，紧耦合单目VIO中，利用imu数据，对最新的VIO估计进行状态传播，可以得到imu频率的状态估计）可实现四翼飞行器的实时反馈控制。

实验追踪结果如下：

轨迹总长度为61.97米（飞了四圈的轨迹长度），最终位置误差为[0.08, 0.09, 0.13]，误差距离与轨迹的比值为0.29%。详细的平移和旋转对比值及其误差如图所示：

可以看到位置的偏差基本在0~0.2米范围内波动，相比于偏航角，俯仰角和翻滚角的偏差更大一些。

B)移动设备

将VINS-Mono移植到移动设备上，并提出了一个简单的AR应用程序来展示它的准确性和鲁棒性。

把移动设备上运行的版本称为VINS-Mobile。
VINS-Mobile在一个iphone上运行。
使用手机获取的照片，30Hz，640x480的分辨率。
imu数据，使用iphone内置的 InvenSense MP67B传感器（可以获得三自由度的加速度和角速度）。
在平面上插入一个虚拟立方体，该平面从估计的视觉特征中提取。

然后人拿着设备，以正常的速度从房间内走进走出。
因为使用imu，所以在回环检测发生时，使用4-DOF姿态图优化来消除x、y、z和yaw的漂移。
走了大概264米后，回到出发点，结果如图：

从方块还在原点（其实有一些微小的偏差，但也足够精确了），可以证明，利用四自由度的姿态图优化能完全消除里程计的累计漂移。

10 结论与展望

在这篇文章中，提出了一种鲁棒且多用途的单目视觉惯性状态估计器VINS-Mono。
VINS-Mono具有对IMU预积分、估计器初始化、在线外参校准、紧耦合的VIO、回环重定位和有效的全局优化的最先进、新颖的解决方案。
通过与其他最先进的开源算法相比，VINS-Mono显示出了优越的性能。
论文介绍的PC和iOS实现现已对社区开源。
尽管基于特性的VINS估计器已经达到了现实世界部署的成熟度（解决方案的成熟度在论文中得到了验证），但这里仍然有许多研究的方向。

• 单目惯性系统在一些运动情况（比如匀速或者匀加速，此时imu会失灵）和环境（一些缺乏纹理的环境，基于特征点的视觉方案可能会失效），VINS可能会处于弱可观察甚至观测退化的状态。我们感兴趣的研究方向是利用实时在线方法来评估单目视觉所处在状态的可观察性，以及实时生成运动方案来恢复可观察性。
• 另一个研究方向是关于VINS在各种消费设备上的大规模部署，如安卓手机。在这些设备上部署的话，需要对这些设备的传感器的内参和外参进行在线校准，并对校准质量进行在线评估。
• 最后，我们感兴趣的是利用单目VINS的状态估计结果来生成稠密点云地图。我们的第一个利用单目VINS生成稠密点云地图以应用于无人机导航的成果发表在如下文章中：

最后总结，为了进一步提高系统的准确性和鲁棒性，仍需要进行大量的研究。

完结，撒花。

论文及代码

1.VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator-pdf
2.VINS-Mono Github
3.VINS-Fusion Github