1. VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator
2. 摘要
单目VI系统，由单目相机和一个低成本IMU组成，形成6自由度的状态估计器。
然而，由于单目相机缺乏直接的距离测量，在IMU处理、估计器初始化、外部校准和非线性优化方面提出了重大的挑战。
在本文，提出了\(VINS-Mono\)，一种鲁棒、通用的单目VI状态估计器。
作者提出的方法从一个鲁棒的处理开始，可用于初始化和故障恢复。通过紧耦合的形式和非线性优化的方法，通过融合的IMU数据预积分和特征观测来获得高精度的VI里程计
在回环检测模块，结合了紧耦合的公式，使得重定位部分的计算量得以减小
最后，增加了四个自由度的位姿图优化，以增强全局一致性。
在公共数据集和真实世界的实验中验证了我们的系统的性能，并与其他最先进的算法进行了比较，我们还在MAV平台上进行了机载闭环自主飞行，并将算法移植到基于ios的演示中，作者强调，所提出的工作是一个可靠、完整和通用的系统，适用于需要高精度定位的不同应用
3. 介绍
仅使用单目摄像机的方法因其体积小、成本低和易于硬件设置而引起了社区的极大兴趣，然而，单目视觉系统无法恢复度量尺度，因此限制了它们在现实机器人应用中的使用，近年来，我们发现利用低成本惯性测量单元(IMU)来辅助单目视觉系统的发展趋势。
这种单目视觉惯性系统(VINS)的主要优点是具有可观测的尺度，以及滚转角(roll)和俯仰角(pitch)，这将可以支持需要尺度的状态估计的导航任务
另外，对IMU测量进行积分，可以通过消除视觉轨迹损失之间的差距来显著提高运动跟踪性能，这些视觉轨迹间隙损失来源于：光度变化、纹理缺乏的场景或者是运动模糊，等等。
事实上，单目VINS不仅广泛应用于移动机器人、无人机和移动设备，它也是实现充分自我感知和环境感知的最低传感器设置。
1） 初始化
然而，这些优点同时也带来一些代价，对于单目VINS，加速度激励是使度量尺度可见的必要条件，这意味着单目VINS估计器不能从静止状态出发，而是从未知的运动状态出发(意思是: 初始化过程需要有一定的运动)。同时，我们也认识到视觉惯性系统是高度非线性的，因此在估计初始化方面我们面临着巨大的挑战。另外，由于使用两个传感器，两个传感器的存在也使得相机-IMU的外部标定变得至关重要。
因此，作者提出VINS-Mono，我们的解决方案从动态的评估器初始化开始，对于故障恢复，也使用同样的初始化模块。
2） 核心处理
该方法的核心是一种基于紧耦合滑动窗口非线性优化的VIO方法，单眼VIO模块不仅提供精确的位置、速度和方位估计，它还执行相机-IMU外部校准和IMU偏差的在线标定矫正。
3） 回环、重定位
回环检测部分，使用DBOw2，通过与单目VIO进行特征级融合，在紧密耦合的环境中实现重定位功能。这使得重定位所需的计算开销大大减小
4） 位姿图
最后，几何验证的回环添加到位姿图，得益于单目VIO的可观察的roll和pitch角，可执行一个四自由度(DOF)位姿图，以确保全局的一致性。
5） 小结
与我们之前的工作相比，VINS-Mono的进一步改进包括改进的IMU预集成和偏差校正、紧耦合重定位、全局位姿图优化、广泛的实验评估和健壮的、通用的开源实现
整个系统功能齐全，使用方便。它已成功地应用于小型AR场景、中型无人机导航和大型状态估计任务，优越的性能已显示出与其他先进的方法。为此，我们总结了我们的贡献如下:
- 一个鲁棒的初始化过程，能够从未知的初始状态引导系统
- 种紧密耦合的、基于优化的单目视觉惯性测程方法，具有摄像机-IMU外部校准和IMU偏置估计功能
- 在线回环检测以及紧耦合的重定位
- 四自由度的位姿图优化
- 用于无人机导航、大规模定位和移动AR应用的实时性能演示
- 与ROS完全集成的PC版本和运行在iPhone6s或以上的iOS系统的开源版本
4. Relative Work
。。。
5. 系统概述
提出的VI状态估计器如图2所示
该系统从测量预处理开始，其中包括特征提取和跟踪，IMU测量之间的两个连续帧预积分
初始化过程(V节提供了所有必要的值，包括姿态、速度、重力向量、陀螺仪偏差和3D特征位置)，用于引导后续基于非线性优化的VIO
VIO与重定位模块紧密地融合了IMU测量的预积分、特征观测和从回环中重新检测的特征
最后，位姿图优化模块经过采用几何验证的重定位结果，进行全局优化以消除漂移。
下面是一些角标的定义
- \((\cdot)^w\)表示世界坐标系(导航坐标系)
- 重力g的方向与世界坐标系的z轴对齐，\(g^w=[0,0,g]^T\)代表了世界坐标系的重力向量
- \((\cdot)^b\)是机体坐标系，也定义为IMU的坐标系
- \((\cdot)^c\)是相机坐标系
- 使用旋转矩阵\(R\)和哈密顿四元数\(q\)来表示旋转(我们主要在状态向量中使用四元数，但旋转矩阵也用于方便地旋转三维向量)
- \(q_b^w,p_b^w\)代表了从body坐标系到world坐标系的旋转和平移变换
- \(b_k\)是在第k帧图片的机体坐标系
- \(c_k\)是在第k帧图片的相机坐标系
- \(\otimes\)代表了两个四元数之间的乘法运算
- \((\hat{\cdot})\)用来表示噪声观测或者是不确定性的估计
5.1. 测量预处理
本节介绍惯性和单目视觉测量的预处理步骤：
- 对于视觉测量，我们跟踪连续帧之间的特征，并在最新帧中检测新特征
- 对于IMU测量，我们预积分两帧之间的数据（注意，我们使用的低成本IMU的测量受到偏差和噪声的影响。因此，我们特别在IMU预积分过程中考虑了bias）
5.1.1. 视觉前端处理
对于每一帧新的图像，使用KLT系数光流法来跟踪已有的特征点，同时，会检测新的角点特征，来保持每帧图片中最小的特征点数目在(100~300)之间，该特征点检测器通过设置两个相邻特征之间的最小像素间隔来实现均匀的特征分布。
首先对二维特征点进行去畸变处理，经过外点去除后将其投影到单位球面上？？ （外点去除采用基础矩阵模型RANSAC处理）
关键帧选取也在这个步骤中进行，主要根据两条准则来选取
一是与前一帧的平均视差 (如果当前帧和最新关键帧 跟踪到的特征点的平均视差超过了阈值范围，就认为当前帧可作为新的关键帧，需要注意的是，这里不仅仅是平移，旋转也可以导致视差的变化)。另外，如果仅有旋转，那么跟踪得到的特征点无法完成三角化过程，为了避免这种情况，在计算视差时，我们使用陀螺测量的短期积分来补偿旋转。
注意：这个旋转补偿仅用于关键帧的选取，不包含在VINS公式的旋转项中，最后，尽管IMU角速度测量包含了较大的噪声或者收到bias的影响，这只会导致次优的关键帧选取结果，并不会直接影响最终估计的质量。
另外一个准则是: 跟踪质量，如果跟踪的特征点数量低于阈值，那么这一帧会作为新的关键帧，这个标准是为了避免完全丧失特征的情况
5.1.2. IMU预积分
IMU预积分首先被[22]提出，使用欧拉角形式对旋转误差进行参数化。
在我们先前的工作中，提出了一种流形空间的IMU预积分，这使得可以利用连续时间IMU误差状态动态方程推导出协方差传播，然而，先前的工作忽略了IMU的bias的影响，在这篇论文中，我们通过加入IMU偏置校正来扩展我们在之前的工作[7]中提出的IMU预积分。
IMU的原始角速度和加速度测量值\(\hat{w},\hat{a}\)可以表示为如下:
IMU测量值，是在body系下的，结合了重力，机体的动态，加速度bias，角速度bias，噪声。我们假设，额外的噪声项是高斯噪声，即\(n_a \sim \mathcal{N}(0,\sigma_a^2), n_w \sim \mathcal{N}(0,\sigma_w^2)\)。加速度bias和角速度bias使用随机游走模型，也可以使用高斯分布来表示，即\(n_{ba}\sim \mathcal{N}(0,\sigma_{ba}^2),n_{bw}\sim\mathcal{N}(0,\sigma_{bw}^2)\)，并且有:
那么，给定两个图像帧所对应的时刻，可以在时间步\([t_k,t_{k+1}]\)内使用IMU测量对世界坐标系下的位置，速度和旋转状态进行传播:
其中，\(\Delta t_k\)是\([t_k,t_{k+1}]\)的时间间隔
可以看到，IMU状态传播需要第k帧的的位置，速度和旋转状态。
当起始状态（即第k帧的位置，速度和旋转）发生变化的时候，我们需要对IMU的测量信息重新传播，即需要重新积分。这样一来，特别是在基于优化的算法中，每次我们调整姿态时，都需要在它们之间重新传播IMU测量值，这样的传播策略需要很强的计算能力.
为了避免这样的重传播情况，我们采用预积分算法：把参考坐标系从world frame变为k时刻的机体坐标系\(b_k\)，我们可以得到预积分:
上面的
- \(R_{w}^{b_{k}}\): 将数据从世界坐标系转换到k时刻的机体坐标系的旋转变换
- \(R_{w}^{b_k} p_{b_{k+1}}^{w}\)指的是将k+1时刻在世界坐标系下的机体位置转换到k时刻的机体坐标系\(b_k\)后得到的东西
- \(p_{b_{k+1}}^{w}\)指的是将k+1时刻在世界坐标系下的机体位置
- \(R_{w}^{b_k} v_{b_{k+1}}^w\)是k+1时刻在世界坐标系下的机体速度转换到k时刻的机体坐标系\(b_k\)后得到的东西
- \(v_{b_{k+1}}^w\)是k+1时刻在世界坐标系下的机体速度
- \(q_{w}^{b_k}\)是k时刻世界坐标系下的机体姿态，也就是从世界坐标系转换到k时刻的机体坐标系的旋转变换
- \(R_{t}^{b_k}\)是指在\([t_k,t_{k+1}]\)内的某个时刻t，机体相对于
k时刻的机体坐标系的姿态，即表示从时刻\(t \in [t_k,t_{k+1}]\)到k时刻的机体坐标系的旋转变换 - \(\alpha_{b_{k+1}}^{b_{k}}\)指相对于k时刻机体坐标系的位置积分，即这个位置积分量是在
k时刻的机体坐标系\(b_{k}\)下的 - \(\beta_{b_{k+1}}^{b_{k}}\)指相对于k时刻机体坐标系的速度积分，即这个速度积分量是在
k时刻的机体坐标系\(b_{k}\)下的
可以看到，通过改变参考坐标系为第k帧的机体坐标系\(b_k\)，可以使得这些预积分项\(\alpha,\beta,\gamma\)基本上只与IMU的测量有关，与第k帧的位置，速度和旋转无关，
即\(\alpha_{b_{k+1}}^{b_k},\beta_{b_{k+1}}^{b_k},\gamma_{b_{k+1}}^{b_k}\)与IMU的偏置\(b_{at},b_{wt}\)有关，然而这个偏置变化一般较小，如果变化了，我们可以使用一阶近似，然后重新计算预积分，否则，预积分只需计算一次就可以了{这里说的操作，对应于式(12)}
这个预积分策略大大减少了运算量，因为这样使得不需要每次更新完状态之后都重新传播IMU的测量
对于离散时间实现
对于离散时间，有不同的数值积分方法，如欧拉法，中点法，RK4法等等，在这里，选择了欧拉法进行推导，方便理解(实际代码中，使用了中点法积分)
在预积分起始时刻，即\([t_k,t_{k+1}]\)中的\(t_k\)时刻，\(\alpha_{b_k}^{b_k},\beta_{b_k}^{b_k}\)都是零，而\(\gamma_{b_k}^{b_k}\)则是单位四元数，因为还没有开始积分
那么，\(\alpha,\beta,\gamma\)的均值根据下面的步骤来进行传播，(需要注意的是，噪声项\(n_a,n_w\)是未知的，置为0)，这产生了预积分项的估计值，使用\((\hat{\cdot})\)来表示:
其中，
- \(i\)表示在\([t_k,t_{k+1}]\)时间段内的离散化时刻，(即两帧图像之间有很多个时刻的IMU测量值)
- \(\delta t\)是IMU测量间隔
然后，我们需要处理协方差的传播，因为4自由度的旋转四元数\(\gamma_{t}^{b_k}\)是超参数化的，我们定义其误差项为 在其均值附近的一个小扰动:
其中，\(\delta \theta_t^{b_k}\)是三维的小扰动
因此，可以推导出连续时间下，关于式(6)的误差项进行线性化之后的动态方程:
这里，有点跟误差状态卡尔曼 类似，具体见里面的式(166)
\(P_{b_{k+1}}^{b_k}\)可以使用离散时间的一阶近似进行递归计算得到，初始化协方差\(P_{b_{k}}^{b_k}=0\):
其中，Q是关于噪声项\((\sigma_{\alpha}^2,\sigma_{w}^2,\sigma_{b_a}^2,\sigma_{b_w}^2)\)对角型的协方差矩阵
同时，关于预积分误差项\(\delta z_{b_{k+1}}^{b_k}\)分别对于预积分误差项\(\delta z_{b_{k}}^{b_k}\)的一阶雅克比\(J_{b_{k+1}}\)，也可以使用初始化的雅克比\(J_{b_k}=I\)进行递归计算得到，即:
使用递归公式，我们可以得到协方差矩阵\(P_{b_{k+1}}^{b_k}\)，以及雅克比\(J_{b_{k+1}}\)
\(\alpha_{b_{k+1}}^{b_k},\beta_{b_{k+1}}^{b_k},\gamma_{b_{k+1}}^{b_k}\)相对于bias的一阶近似可以写成:
其中，
- \(J_{b_a}^\alpha\) 是\(J_{b_{k+1}}\)的子块，其位置与\(\frac{\delta \alpha_{b_{k+1}}^{b_k}}{\delta b_{a_{k}}}\)相对应
- 对于\(J_{b_w}^\beta,J_{b_a}^\beta,J_{b_{w}}^\beta,J_{b_{w}}^\gamma\)也同样
当bias的估计发生轻微变化时，我们使用(式12)来矫正预积分的结果，而不是使用重新传播的方式。
现在，我们可以写成IMU测量(预积分)模型以及对应的协方差矩阵\(P_{b_{k+1}}^{b_k}\)
实际上，这里就是将(式5)关于预积分项移到等式左侧，其他移到右侧，得到的结果
5.2. 初始化
单目紧耦合的VI里程计是高度非线性系统，因为尺度不能直接从单目相机中获取，因此如果没有很好的初始化值，很难直接对两种测量信息进行融合。
一方面可能会假设一个静止的初始条件来启动VINS估计器，然而，这种假设是不恰当的，由于运动下的初始化是经常遇到的现实世界的情况。当IMU的测量收到大的bias影响的时候，这种情况变得更加复杂。
实际上，对于单目VINS，初始化通常式最脆弱的步骤，系统的可行性需要一个鲁棒的初始化处理。
采用了紧耦合的传感器融合方法来获取初始值，我们发现，仅仅使用视觉的SLAM，或者是Sfm，初始化具有一个好的属性。在大多情况下，visual-only的系统可以通过相对运动关系方法如八点法、五点法或者是估计单应矩阵H，来进行初始化。
通过使用visual-only Sfm的结果来对齐IMU预积分，我们可以粗略的恢复出尺度、重力、速度、甚至是bias，这对于单目VINS的初始化而言已经足够了，如图4所示。
与文献[17]相反，在初始化阶段，其同步估计角速度和加速度的bias。而我们选择在初始化阶段，忽略加速度的bias。加速度的bias与重力耦合在一起，相对于较大的重力向量，小量的平台的动态在这个初始化阶段，这些bias项很难被观测到。
5.2.1. 滑动窗口：Vision-Only SFM
初始化处理的开始阶段：先使用vision sfm来估计相机的pose和特征点的position
为了降低运算的复杂度，我们保持一个有界的滑动窗口。首先，我们检查最新的一帧与之前的所有帧的特征点的相关性。如果我们在最新的一帧与滑动窗口中的任意一帧中，可以找到稳定的特征点跟踪(more than 30 tracked features)同时，视差足够大(大于 20 个 rotation-compensated pixels)。我们使用五点法来恢复出这两帧之间的相对旋转和平移量[文献33]。否则，我们我们保存最新一帧在滑动窗口中，等待其他新的一帧到来。
如果五点法计算成功，我们设置一个任意的尺度，然后对两帧之间的特征点关联进行三角化，然后基于这些三角化得到的特征点(三角化变成了3D点)，对滑动窗口内的其他帧使用PnP算法来估计出滑窗内其他帧的位姿估计。
最后，使用global BA来进行优化，以最小化所有特征点的重投影误差。因为我们没有任何关于世界坐标系的先验知识，因此我们设置第一帧的相机坐标系作为sfm的参考坐标系\((\cdot)^{C_0}\)，所有帧的位姿\((\bar{p}_{C_k}^{C_0},q_c^b)\)以及特征点都是相对于坐标系\((\cdot)^{C_0}\)而言的。假设我们有一个粗略的关于机体坐标系和相机坐标系的外参\((p_c^b,q_c^b)\)，我们可以把每一帧的相机坐标系转换为机体坐标系:
其中，\(s\)是以米为单位的尺度因子，求解尺度参数是成功初始化的的关键
5.2.2. VI对齐
5.2.2.1. Gyro Bias标定
考虑滑动窗口中连续的两帧的机体坐标系\(b_k,b_{k+1}\)，我们从视觉的SFM中可以获得这两帧机体坐标系相对于参考坐标系的旋转\(q_{b_k}^{c_0},q_{b_{k+1}}^{c_0}\)，另外，我们可以从IMU预积分数据中获取这两个图像帧之间的预积分约束\(\hat{\gamma}_{b_{k+1}}^{b_k}\)，我们对IMU预积分项相对于gyro bias参数进行线性化，然后最小化下面的损失函数:
其中
- \(\mathcal{B}\)是滑动窗口中所有帧的集合
- 通过使用前面推导的bias雅克比，我们有\(\hat{\gamma}_{b_{k+1}}^{b_k}\)关于gyro bias的一阶近似
- 在这种方式下，我们获得了关于gyro bias的初始标定
- 然后我们使用新的gyro bias值，对IMU的预积分项\(\alpha_{b_{k+1}}^{b_k},\beta_{b_{k+1}}^{b_k},\gamma_{b_{k+1}}^{b_k}\)进行重传播
5.2.2.2. 速度、重力向量、尺度的初始化
在gyro bias初始化完成之后，我们进一步初始化其他状态，定义这些状态为:
其中:
- \(v_{b_k}^{b_k}\)是在机体坐标系\(b_k\)的速度
- \(g^{c_0}\)是在参考坐标系\(c_0\)坐标系的重力向量
- \(s\)是单目sfm的尺度参数，以米为单位
考虑两个连续的坐标系\(b_k,b_{k+1}\)，把(式5)中的世界坐标系换成参考坐标系\((\cdot)^{c_0}，\)那么(式5)可以写成如下形式:
我们可以结合(式14)和(式17)到如下线性测量模型:
可以看到，
- \(R_{b_{k}}^{c_0},R_{b_{k+1}}^{c_0},\bar{p}_{c_k}^{c_0},\bar{p}_{c_{k+1}}^{c_0}\)都是从单目sfm中获取到的值
- \(\Delta t\)是两个连续图像帧之间的时间间隔
通过求解如下线性最小二乘:
我们可以获取每一帧的机体坐标系的速度，以及在参考坐标系\((\cdot)^{c_0}\)的重力向量以及尺度参数
5.2.2.3. 重力对齐
通过量级的约束，可以对原线性初始化步骤(即上面的步骤)得到的重力矢量进行修正。
在大多情况下，重力向量的量级是已知的，这会导致重力向量仅有2自由度，因此，我们对重力向量在其正切空间使用2个变量来重新参数化
我们的参数化使用\(g\cdot \bar{\hat{g}}+w_1 b_1 +w_2b_2\)来表示重力向量
其中
- \(g\)是已知重力的量级
- \(\bar{\hat{g}}\)是代表重力方向的单位向量
- \(b_1,b_2\)是两个正交基章程的正切平面，如图5所示
- \(w_1,w_2\)是对应于\(b_1,b_2\)两个基的位移
使用下面的算法1进行叉乘，我们可以找到\(b_1,b_2\)，然后我们将(式17)中的重力向量\(g\)使用\(|g|\cdot \bar{\hat{g}}+w_1 b_1 +w_2b_2\)来替换，然后求解\(w_1,w_2\)以及其他变量，迭代至收敛
5.2.2.4. 完成初始化
对重力矢量的修正之后，我们可以通过旋转重力向量到z轴来得到世界坐标系和参考坐标系\(c_0\)之间的旋转\(q_{c_0}^{w}\)
然后我们将参考坐标系\((\cdot)^{c_0}\)中的所有变量旋转到世界坐标系\((\cdot)^w\)中，机体坐标系的速度也被转换到世界坐标系中。
视觉SfM的平移分量将缩放为以米为单位，此时，初始化过程已经完成，所有这些度量值将被提供给一个紧耦合的单目VIO
5.3. 紧耦合VIO
在估计器初始化之后，我们使用基于滑动窗口的紧耦合单目VIO进行高精度和稳健的状态估计，如图3所示
5.3.1. 系统状态
滑动窗口内全部状态向量定义如下:
其中，
- \(x_k\)是IMU在第k帧图像对应的时刻的状态，包含了IMU（机体）在世界坐标系下的位置、速度和姿态，以及在机体坐标系下的IMU加速度bias和角速度bias
- n表示所有的关键帧数量
- m表示滑动窗口内所有的特征点的数量
- \(\lambda_l\)表示第l个特征点相对于其第一次观测的逆深度值
我们的目标是最小化关于以下3个部分的和：
- 先验项
- 视觉重投影残差项
- IMU预积分残差项
即目标函数如下所示:
其中，
- \(r_{B}(\hat{z}_{b_{k+1}}^{b_k},X)\)表示IMU的测量残差
- \(r_c(\hat{z}_{l}^{c_j},X)\)表示视觉测量残差
- \({B}\)是IMU测量数据的集合
- \({C}\)是当前滑动窗口中至少被观测到两次或以上的特征点集合
- \({r_p,H_p}\)是来自
边缘化得到的先验信息 - 我们采用了Google的Ceres优化库来求解这个非线性问题
5.3.2. IMU测量误差
考虑滑动窗口中两个连续的图像帧时刻所对应的机体坐标系\(b_k,b_{k+1}\)，根据(式13)定义的IMU测量模型，可以得到IMU的预积分參差如下:
其中,
- \([\cdot]_{xyz}\)提取四元数中的角度部分，作为误差状态的表达，即\(\delta\theta_{b_{k+1}}^{b_k}\)是关于四元数的误差状态
- \([\hat{\alpha}_{b_{k+1}}^{b_k},\hat{\beta}_{b_{k+1}}^{b_k},\hat{\gamma}_{b_{k+1}}^{b_k}]^T\)是关于IMU的在两个图像帧之间的预积分项，这一项仅与包含噪声的加速度和角速度测量有关
- 加速度计和陀螺仪的bias被包含在參差项中，用以实现在线的矫正
5.3.3. 视觉测量误差
与传统的针孔相机模型(在广义成像平面上定义投影误差)相反，我们在一个单位球面上定义了相机的测量误差，原因是几乎所有类型的照相机，包括广角照相机、鱼眼照相机和全向照相机，其光学特性都可以被模拟成以单位球面为单位的射线。
考虑第l个特征的第一次被第i帧图像观测到，那么在第j帧再次观测到这个特征点时，可以建立如下视觉残差：
其中，
- \([u_l^{c_i},v_l^{c_i}]\)表示第l个特征的第一次被第i帧图像观测所在的像素位置
- \([u_l^{c_j},v_l^{c_j}]\)表示第l个特征被第j帧图像观测所在的像素位置
- \(\pi_c^{-1}\)表示使用相机模型的内参，将像素点坐标反投影成一个单位向量（3D点）
- \(P_l^{c_j}\)用于(式22)，是在正切空间中一个固定长度的标准协方差
- \(\hat{\bar{\mathcal{P}}}_l^{c_j}\)是将
第j帧观测到第l个特征点的像素坐标反投影得到的3D向量 - \(\mathcal{P}_l^{c_j}\)是将
第i帧观测到第l个特征点的像素坐标 1）先反投影成3D向量(归一化平面上的) 2）再利用逆深度，得到在第i帧相机坐标系下的3D点 3）然后根据IMU和相机的外參，将该3D点变换到IMU坐标系下 4）根据(第i帧)待估计的机体位姿\(T_{b_i}^{w}\)，将该点变换到世界坐标系下 5）根据(第j帧)待估计的机体位姿\([T_{b_j}^{w}]^{-1}\)，将该点投影到第j帧机体坐标系下 6）然后，再次根据IMU和相机外參，将该点转换到第j帧的相机坐标系下
因为视觉參差的自由度是2,我们将參差向量投影到正切空间。\(b_1,b_2\)是在正切平面\(\hat{\bar{\mathcal{P}}}_l^{c_j}\)上的两个任意选择的正交基，如图6所示。我们可以使用之前提到的算法1来找到其中的一组\(b_1,b_2\)
5.3.4. 边缘化
为了控制基于优化方法的VIO的计算复杂度，采用了边缘化的方法。
我们选择性地对弹出滑动窗口的机体状态\(x_k\)和特征点\(\lambda_l\)进行边缘化，然后将它们的测量信息转化为对应的边缘化信息，作为先验
如图7所示，当有新的一帧到来时，首先判断最新的一帧是否为关键帧：
- 如果是关键帧，那么将其插入到滑动窗口中，同时，弹出滑动窗口中排在最前面（最old的）的那一帧，并且对其视觉和IMU测量信息进行边缘化，转化为先验矩阵\(H_{prior}\)
- 如果不是关键帧，那么对于这个最新的一帧，不插入到滑动窗口中，并且舍弃其视觉信息，保留IMU的测量信息（因为需要其预积分信息来进行向前推算）
关键帧选取+边缘化策略是为了保持滑动窗口中的关键帧保持一定的空间分布，这确保了足够的视差来对特征点进行三角化，并且使得加速度计测量值保持在一个较大的激励下的概率更大。
边缘化的实施采用的是Schur complement，我们基于所有与被移除状态相关的边缘化属性来构建一个新的先验，然后新的先验与旧的先验相加。
Motion-only BA
对于移动终端等计算能力较低的设备，由于非线性优化的计算量较大，紧耦合单目VIO系统无法达到相机帧率的输出。为了达到这个目的，我们使用了一个轻量级的Motion-only的视觉惯性BA来使得状态估计可以达到相机速率(30赫兹)
损失函数与之前定义的单目VIO损失函数一样(式22)，但是，相比与之前对全部状态进行优化，在这里我们只优化了固定数目的IMU（机体）状态的位姿和速度，把特征点深度、IMU和相机外參、bias、以及旧的IMU(机体)状态等这些看作是固定不变的常量。
我们使用所有的视觉和惯性测量来进行这个Motion-only的BA优化，这个结果比直接对最新一帧使用Pnp（前端跟踪）算法更加平滑。
图8给出了该策略的实例
与全状态紧耦合的单目VIO(在最先进的嵌入式计算机上可能需要50ms)相比，Motion-only的视觉-惯性BA优化只需要大约5ms的时间
IMU状态估计的正向传播
虽然我们的VIO的频率受限于图像捕获频率，但我们仍然可以直接使用IMU的测量数据来对最新的VIO状态估计进行传递。
高频状态估计值可用于闭环的状态反馈，后面提出了一种基于这IMU-rate状态估计的自主飞行实验
故障诊断和恢复
尽管提出的VIO对于挑战环境和运动下可以保持一定的鲁棒性。但是由于强烈的光照变化或剧烈的运动等情况下，故障还是不可避免的发生。主动故障检测与恢复策略可以提高系统的实用性，故障检测模块是一个独立于状态估计器的模块，用来检测估计器的异常输出，检测异常有如下准则:
- 在最新帧中被跟踪的特征数小于某个阈值
- 估计器的最后两次输出（位置、姿态）有较大的不连续性
- bias或外參估计产生了较大的变化
一旦检测出异常，系统将切换回初始化阶段，一旦单目VIO重新初始化完成，将会重新创建新的一段pose Graph
重定位
我们的滑动窗口和边缘化方案控制了计算复杂度，但也为系统引入了累积漂移。更具体地说，漂移发生在全局的3D位置(x, y, z)和绕重力方向(yaw)旋转，VIO（4自由度不可观）
为了消除漂移，提出了一种可以与单目VIO状态估计器无缝整合的紧耦合重定位模块，重定位首先启动回环检测模块，用来检测该环境是否曾经探查过，然后建立回环候选关键帧和（最新）当前帧之间的特征级的关联，这些特征关联被紧密地整合到单目VIO模块中，以此来实现零漂移的状态估计。
多个观测值的多重特征直接用于重定位，获得了更高的精度和更好的状态估计平滑度，重定位的处理过程如图9(a)所示。
回环检测
我们使用DBoW2，一种词袋模型，用于回环检测中。除了单目VIO模块中检测到的角点特征外，还有500个以上的额外角点被检测到，并使用BRIEF描述子进行描述。额外的角特征用于在循环检测时实现更好的召回率。
将描述符作为视觉描述单词来查询视觉数据库，经过时间和几何一致性检查后，DBoW2将返回回环候选帧。我们对所有特征点保留所有BRIEF描述符，但丢弃原始图像以减少内存消耗。
我们注意到我们的单眼VIO能够渲染roll和pitch可观，因此，我们不需要依赖于旋转不变的特性，比如ORB_SLAM中使用的ORB特征点
特征恢复
当检测到一个回环时，通过恢复特征关联，建立起当前滑动窗口和闭环候选帧之间的连接，特征关联采用BRIEF描述符进行匹配来完成。直接的描述符匹配可能会产生大量的外点，为了解决这个问题，我们采用两步骤的外点去除，如图10所示
- 2D-2D： 使用RANSAC进行基础矩阵F的测试，对当前帧图像的2D观测点与回环候选帧图像（实际上是特征点）进行基础矩阵F测试
- 3D-2D： 使用RANSAC的PnP测试，基于当前滑动窗口中已知的3D点，以及回环候选帧图像中的2D特征点，进行PnP测试
当两个测试的内点数达到一定阈值的时候，我们认为这个回环候选帧就是正确的回环，然后进行重定位。
紧耦合的重定位
重定位处理可以有效地对齐当前由单目VIO维护的滑动窗口到过去的位姿。重定位过程中，我们把所有闭环关键帧的位姿都作为约束，然后使用所有的IMU测量值、局部视觉测量值和从回环检索到的特征关联来共同优化当前滑动窗口
根据回环检测得到的回环帧\(v\)与当前帧的特征关联，我们可以写出视觉误差项，与前面VIO模块中的(式25)一致，唯一不同的是，回环帧的位姿\((\hat{q}_v^{w},\hat{p}_v^w)\)可以从pose Graph中获取，或者是直接使用过去的里程计输出（如果这是第一次重定位的话），并且回环帧的位姿\((\hat{q}_v^{w},\hat{p}_v^w)\)作为常数对待，不参与优化，只提供约束。
为了实现紧耦合的重定位，我们可以用附加的闭环约束对式(22)中的非线性目标函数稍作修改，得到:
其中，
- \(\mathcal{L}\)是从闭环关键帧与当前滑动窗口恢复出来的特征关联的集合
- \((l,v)\)表示在闭环关键帧中观测到的第l个特征点
需要注意的是，尽管目标函数与(式22)稍有不同，但是待求解的状态量的维度是一样的，这是因为闭环关键帧的位姿作为常数处理
如果当前滑动窗口建立起多个闭环时，我们同时使用全部的闭环帧的特征关联来进行优化。这为重新定位提供了多视图约束，提高了精度和平滑度
注意
这里的优化都是针对于当前滑动窗口的优化，对于过去的位姿、闭环关键帧位姿的全局优化，在下一节进行讨论。
全局Pose Graph优化
重定位后，当前滑动窗口，将与过去的位姿进行对齐，减少漂移。利用重定位的结果，这一步额外的Pose Graph优化步骤是用于确保过去的位姿拥有较好的全局一致性。
因为我们的VI处理中，roll和pitch是可观的（相对于世界坐标系-ENU或其他），那么累积漂移只会发生在其余四个自由度（3D位置、沿重力方向旋转的yaw角）。
为了解决这个问题，我们忽略不产生漂移的roll和pitch状态量，然后只进行一个4自由度的Pose Graph优化
向Pose Graph中添加关键帧
当一个关键帧从滑动窗口中被边缘化掉的时候，它将被插入到Pose Graph里面，这个关键帧在这个Pose Graph里面作为一个顶点，然后它可以使用两种类型的边来连接其他顶点：
序列边：一个关键帧会与之前的几个关键帧建立起一些
sequential edge（序列边）。一条序列边代表了两个关键帧的相对变换关系，这个相对变换可以直接从VIO获取，考虑一个较的关键帧i和其前一帧j，那么这个序列边仅仅包含相对位置\(\hat{p}_{ij}^i\)和相对yaw角\(\hat{\psi}_{ij}\):
回环边：如果这个被边缘化的关键帧与之前的回环帧建立了回环连接，那么这个回环连接同样被加入到Pose Graph中。相似的，回环边也仅仅包含4自由度的相对关系，如上面的(式27)所示，这个回环边的数据来自重定位得到的结果
4自由度的Pose Graph优化
我们定义连接着关键帧i和关键帧j的边所代表的残差如下:
其中，
- \(\hat{\phi}_i,\hat{\theta}_i\)分别是roll和pitch的估计值，可以直接从单目VIO状态估计器中直接获取
那么，对于Pose Graph中的所有边（序列边、回环边），可以构建出如下损失函数:
其中
- \(\mathcal{S}\)代表所有序列边
- \(\mathcal{L}\)代表所有回环边
虽然紧耦合重定位模块有助于消除错误的闭环，但是对于回环边，我们仍添加Huber正则项\(\rho(\cdot)\)进一步减少可能错误的闭环所带来的影响。
相反的是，对于序列边，我们不使用鲁棒性核函数，因为这些边是从紧耦合VIO状态估计模块中来的，已经经过外点去除，满足内点条件了。
位姿图优化和重定位模块在两个单独的线程中异步进行，这使得可以在任何时候使用最优化的位姿图进行重定位。同样，即使当前位姿图优化尚未完成，也可以使用现有的（旧的）位姿图进行重定位，这个过程如图9(b)所示。
Pose Graph管理
当行驶距离增加时，位姿图的大小可能会一直增大，从而限制了系统的长期实时性，为了解决这个问题，我们实现了一个降采样的过程，以保持Pose Graph数据库在限制的大小范围。
所有具有闭环约束的关键帧都将保留，而其他要么太接近要么与它的邻居有相似方向的关键帧则可能被删除，关键帧被移除的概率与它的邻居的空间密度成正比。
实验
作者进行了3个实验以及两个应用来测试所提出的VINS-Mono系统。
在第一个实验中，在公共数据集上进行测试，对比了提出的方法和其他先进方法的性能，然后进行数值分析，展示提出系统的准确性。
然后，在室内环境中测试系统，以评估在重复场景中的性能
最后进行了大规模的实验，验证了系统的长期实用性。
此外，我们将所提出的系统应用于两个应用。对于无人机的应用，我们使用VINS-Mono进行位置反馈，以控制无人机跟随预定的轨迹。然后我们将我们的方法移植到一个iOS移动设备上，并与Google Tango进行比较
公开数据集测试对比
使用EuRoCMAV的视觉-惯性数据集来评估提出的VINS-Mono系统，这个数据集由无人机的板载立体相机(Aptina MT9V034 global shutter, WVGA monochrome, 20FPS)和同步IMU（ADIS16448, 200Hz），以及真值数据组成。在测试的时候，只采用左相机的图像，在这些数据中可以观测到较大的IMU bias以及光照变化。
在这个实验中，我们比较了VINS-Mono和OKVIS（一种支持单目、双目的先进方法）算法，OKVIS也是一种基于滑动窗口优化的算法，本文提出的VINS-Mono和OKVIS在很多细节上不一样，我们的系统完整的，具有鲁棒的初始化和回环检测。
实验使用两个数据序列，分别是:
- MH_03_median
- MH_05_difficult
为了方便描述，采用如下符号简化描述
VINS表示仅仅是VIO（视觉-惯性里程计）(没有回环部分)VINS_loop表示带有完整版的重定位和位姿图优化的VINS
为了公平对比，我们采取丢弃头100个输出，然后使用接下来的150个输出来与真值进行对齐，然后比较接下来的输出。
对于MH03序列，其轨迹如图11所示
我们仅仅比较平移误差，因为在这个序列中，旋转运动是微不足道的。
如图所示，VINS_loop的平移误差最小
对于MH05序列，也可以观察到近似的结果，
由于在这个序列中运动平稳，偏航角变化不大，所以基本上只发生位置漂移，显然，具有回环功能的方法消除了大部分的漂移。
在roll和pitch角的估计上，OKVIS的估计更加准确一些，一个可能的原因是VINS-Mono使用的预积分，这是使用一阶近似来进行IMU的传播，以节省计算资源。
对于单纯的VIO, VINS-Mono和OKVIS都有相似的Accuracy，很难区分哪一个更好然而，VINS-Mono在系统级别上优于OKVIS，因为VINS-Mono是一个完整的系统，具有鲁棒的初始化和闭环模块，可以辅助单目VIO
室内环境对比
在室内实验中，我们选择实验室环境作为实验区域，使用的设备如图15所示
我们用手拿着传感器，在实验室里以正常的速度行走，在实验过程中，我们遇到行人，低光照条件，纹理缺乏的区域，玻璃反射区域，如图16所示，在多媒体附件中可以找到该视频。
我们与OKVIS进行了对比，如图17所示
大范围环境测试
实验室外面
我们对VINS-Mono在室内外混合场景下进行了测试，这些传感器套装与图15所示的设备一样。
我们从实验室的一个座位出发，然后我们下了楼梯，绕着大楼外面的操场走，接下来，我们回到大楼，上了楼，最后，我们回到了实验室的同一个座位上。
整个轨迹超过700米，持续接近10分钟，这个实验的视频可以在附件的多媒体中找到。
轨迹如图19所示，
在提出的方法中，可以看到，楼梯的形状是清晰的。最终，将闭环轨迹与Google地图进行比对，验证其准确性，如图18所示
OKVIS的最终漂移是[13.80，-5.26，7.23]米，分别在x、y和z轴上。无回环的VINS-Mono的最终漂移是[-5.47, 2.76,-0.29]米，其占轨迹长度的0.88%，小于OKVIS的2.36%，带有回环功能的VINS-Mono，最终的漂移是[-0.032, 0.09, -0.07]米，对于整条轨迹长度而言，是微不足道的。
虽然我们不知道真值，但我们仍然可以从视觉上判断出优化后的轨迹是平滑的，并且可以精确地与卫星地图对齐。
绕校园环境测试
这个庞大的数据集是用手持型VI-Sensor来记录的，它覆盖了整个科大校园，数据集覆盖的地面长约710m，宽约240m，高约60m，路径总长度是5.62km。数据包含了25Hz的图像和200Hz的IMU数据，持续了1个小时34分钟，这对于测试VINS-Mono的稳定性和耐久性是一项非常有意义。
在这个大规模的测试中，为了提供足够的循环信息和实现实时性能，我们设置关键帧数据库的大小2000帧，然后使用Intel i7-4790在3.6GHz的频率下工作，测试如表格1所示。
应用1：无人机的反馈控制
我们使用VINS-Mono作为无人机的自动反馈控制，如图21所示，我们使用前视全局快门相机(MatrixVision mvBlueFOX-MLC200w)具有752x480的分辨率，并配备了190度鱼眼镜头。然后使用了DJI A3飞行控制器，用于获取IMU测量以及稳定机体姿态。
板载的计算资源是Intel i7-5500U 的CPU，频率为3.00GHz，对于广角相机而言，传统的pinhole相机模型不适用，这里采用了MEI模型[文献42]，然后使用了[文献43]介绍的工具来标定。
在本实验中，我们使用VINS-Mono的状态估计来测试自动轨迹跟踪的性能，在本实验中，关闭回环功能，命令四旋翼飞行器跟踪一个”8”字图形，每个圆的半径为1.0米，如图21所示
在预定义轨迹周围设置四个障碍物，以验证无闭环情况下VINS-Mono的精度。
真值使用OptiTrack来获得，整条轨迹长度61.97米，最终漂移为[0.08,0.09,0.13]米，位置漂移为0.29%，具体的误差如图23所示
