相关工作
一种流行的基于EKF的VIO方法是MSCKF[13][14]。MSCKF在状态向量中维护以前的几个摄像机位姿，并使用多个摄像机视图中相同特征的视觉测量来形成多约束更新。SR-ISWF[18][19]是MSCKF的扩展。它采用squareroot形式[20]实现单精度表示，避免了较差的数值性质。该方法采用逆滤波器进行迭代再线性化，使其与基于优化的算法相当。批量图优化或集束调整技术（BA）维护和优化所有测量值以获得最优状态估计。为了达到恒定的处理时间，流行的基于图的VIO方法[8][16][17]通常采用边缘化过去的状态和测量来优化最近状态的有界滑动窗口。由于对非线性系统迭代求解的计算要求很高，很少有基于图的非线性系统能够在资源受限的平台（如手机上）实现实时性能。
对于视觉测量处理，根据视差模型的定义，算法可分为直接法和间接法。直接法[2][3][21]最小化光度误差，而间接法最小化几何位移。直接法因其吸引区域小，需要很好的初始估计，而间接法在提取和匹配特征时需要额外的计算资源。间接法由于其成熟性和鲁棒性，在实际工程部署中得到了广泛的应用。然而，直接法更容易扩展到稠密建图，因为它们是直接在像素级别上操作的。
在实践中，IMU通常以比摄像机更高的速率获取数据。不同的方法被提出来处理高速率的IMU测量值。最简单的方法是在基于EKF的方法中使用IMU进行状态传播[11][13]。在图优化公式中，为了避免重复的IMU重复积分，提出了一种有效的方法，即IMU预积分(IMU pre-integration)。这种方法在[22]中首次提出的，它用欧拉角来参数化旋转误差。在我们先前的工作中[7]，我们提出了一种流形上的IMU预积分旋转公式，利用连续IMU误差状态动力学推导了协方差传递方程。然而IMU偏置被忽略了。在[23]中通过增加后验IMU偏置校正，进一步改进了预积分理论。
精确的初始值对于引导任何单目VINS是至关重要的。在[8][24]中提出了一种利用短期IMU预积分相对旋转的线性估计器初始化方法。但是，该方法不对陀螺仪偏置进行建模，无法在原始投影方程中对现代传感器噪声进行建模。在实际应用中，当视觉特性远离传感器套件时，这会导致不可靠的初始化。在[25]中给出了单目视觉惯性初始化问题的一种封闭解。随后，在[26]中提出了对这种封闭形式的解决方案的扩展，增加了陀螺仪的偏置校准。这些方法依赖于长时间内IMU测量的双重积分，无法模拟惯性积分的不确定性。在[27]中，提出了一种基于SVO的重初始化和故障恢复算法。这是一种基于松耦合融合框架的实用方法。然而，需要额外的朝下的距离传感器来恢复度量尺度。在[17]中引入了一种建立在ORB-SLAM[4]上的初始化算法。给出了一组ORB-SLAM的关键帧，计算了视觉惯性全局BA的尺度、重力方向、速度和IMU偏置的初步估计。然而，尺度收敛所需的时间可能超过10秒。这可能会给需要在一开始就进行尺度估计的机器人导航任务带来问题。
VIO方法，不管它们所依赖的基本数学公式，在全局的平移和旋转中长期受到漂移的影响。为此，回环检测在长期操作中起着重要的作用。ORB-SLAM[4] 利用了词袋模型能够闭合回环并重用地图。回环检测之后进行7自由度（位置、方向和尺度）的位姿图优化。相对于单目VINS，由于IMU的加入，漂移只发生在4自由度，即三维平移，和围绕重力方向的旋转（偏航角）。因此，本文选择在最小四自由度设定下，优化具有回环约束的位姿图。
概述
measurement preprocessing测量预处理：提取和跟踪特征，对两个连续帧间的IMU测量值进行预积分。
Initialization 初始化：初始化过程提供了所有必要的值，包括姿态、速度、重力向量、陀螺仪偏置和三维特征位置，用于引导随后的基于非线性优化的VIO。
local visual-inertial odometry with relocalization 具有重定位的局部VIO：VIO(VI)与重定位(VII)模块紧密地融合了预先积分的IMU测量、特征观测和回环重新检测到的特征。
gobal pose graph optimization 全局位姿图优化：位姿图优化模块接受几何验证的重定位结果，并进行全局优化以消除漂移。
VIO、重新定位和位姿图优化模块在多线程设置中同时运行。每个模块有不同的运行速度和实时保证，以确保在任何时候可靠运行。
测量预处理
本节介绍VIO的预处理步骤。对于视觉测量，我们跟踪连续帧之间的特征，并在最新帧中检测新特征。对于IMU测量，我们在两个连续帧之间做预积分。请注意，我们使用的低成本IMU的测量值受到偏置和噪声的影响。因此，我们在IMU预积分过程中特别考虑偏置。
A 视觉前端预处理
对于每一幅新图像，KLT稀疏光流算法对现有特征进行跟踪[29]。同时，检测新的角点特征[30]以保证每个图像特征的最小数目(100-300)。该检测器通过设置两个相邻特征之间像素的最小间隔来执行均匀的特征分布。二维特征首先是不失真的，然后在通过外点剔除后投影到一个单位球面上。利用基本矩阵模型的RANSAC算法进行外点剔除。
在此步骤中还选择了关键帧。我们有两个关键帧选择标准。第一是与上一个关键帧的平均视差。如果在当前帧和最新关键帧之间跟踪的特征点的平均视差超出某个特定阈值，则将该帧视为新的关键帧。请注意，不仅平移，旋转也会产生视差。然而，特征点无法在纯旋转运动中三角化。为了避免这种情况，在计算视差时我们使用陀螺仪测量值的短时积分来补偿旋转。请注意，此旋转补偿仅用于关键帧选择，而不涉及VINS公式中的旋转计算。为此，即使陀螺仪含有较大的噪声或存在偏置，也只会导致次优的关键帧选择结果，不会直接影响估计质量。另一个标准是跟踪质量。如果跟踪的特征数量低于某一阈值，我们将此帧视为新的关键帧。这个标准是为了避免跟踪特征完全丢失。
B IMU预积分
IMU预积分是在[22]中首次提出的，它将欧拉角的旋转误差参数化。在我们先前的工作中[7]，我们提出了一个流形上的IMU预积分旋转公式。该文利用连续时间的IMU误差状态动力学推导协方差传递函数，但忽略了IMU偏置。文[23]通过增加后验IMU偏置校正，进一步改进了预积分理论。本文通过引入IMU偏置校正，扩展了我们在前面工作[7]中提出的IMU预积分。
IMU的原始陀螺仪和加速度计测量结果如下：
假设加速度计和陀螺仪测量值中的附加噪声为高斯噪声，加速度计偏置和陀螺仪偏置被建模为随机游走，其导数为高斯性的。
给定对应于体坐标系bk和bk+1的两个时刻，位置、速度和方向状态可以在时间间隔[tk,tk+1]间，在世界坐标系下中通过惯性测量值传递：
可见，IMU状态传递需要坐标系bk的旋转、位置和速度。当这些起始状态改变时，我们需要重新传递IMU测量值。特别是在基于优化的算法中，每次调整位姿时，都需要在它们之间重新传递IMU测量值。这种传递策略在计算上要求很高。为了避免重新传递，我们采用了预积分算法。
将参考坐标系从世界坐标系转变为局部坐标系bk后，我们只能对线性的加速度 a ^ 和角速度 w ^ 相关的部分进行预积分，如下所示：
可以看出预积分项(6)能通过将bk视为参考帧的IMU测量值单独得到。
对于离散时间的实现，可以采用不同的数值积分方法，如欧拉积分、中点积分、RK4积分等。这里选择了欧拉积分来演示易于理解的过程(我们在代码中使用了中点积分)。
然后讨论协方差传递问题。
估计器初始化
可以假设一个静止的初始条件来启动单目VINS估计器。然而，这种假设是不合适的，因为在实际应用中经常会遇到运动下的初始化。当IMU测量结果被大偏置破坏时，情况就变得更加复杂了。事实上，初始化通常是单目VINS最脆弱的步骤。需要一个鲁棒的初始化过程以确保系统的适用性。
我们采用松耦合的传感器融合方法得到初始值。我们发现纯视觉SLAM，或从运动中恢复结构(SfM)，具有良好的初始化性质。在大多数情况下，纯视觉系统可以通过从相对运动方法（如八点法[32]或五点法[33]或估计单应性矩阵）中导出初始值来引导自己。通过对齐IMU预积分与纯视觉SfM结果，我们可以粗略地恢复尺度、重力、速度，甚至偏置。这足以引导非线性单目VINS估计器，如图4所示。
与在初始阶段同时估计陀螺仪和加速度计偏置的[17]相比，我们在初始阶段选择忽略加速度计偏置项。加速度计偏置与重力耦合，且由于重力向量相对于平台动力学的大量级，以及初始阶段相对较短，这些偏置项很难被观测到。我们以前的工作对加速度计偏置标定进行了详细的分析[34]。
A 滑动窗口纯视觉SFM
初始化过程从纯视觉SfM估计相机尺度位姿(up-to-scale)和特征位置图开始。
我们保持了一个帧的滑动窗口来限制计算复杂度。首先，我们检查了最新帧与之前所有帧之间的特征对应。如果我们能在滑动窗口中的最新帧和任何其他帧之间，找到稳定的特征跟踪(超过30个跟踪特征)和足够的视差(超过20个的旋转补偿像素)，我们使用五点法[33]恢复这两个帧之间的相对旋转和尺度平移。否则，我们将最新的帧保存在窗口中，并等待新的帧。如果五点算法成功的话，我们任意设置尺度，并对这两个帧中观察到的所有特征进行三角化。基于这些三角特征，采用PnP[35]来估计窗口中所有其他帧的姿态。最后，应用全局光束平差法(BA)[36]最小化所有特征观测的重投影误差。由于我们还没有任何世界坐标系的知识，我们将第一个相机坐标系(·)c0设置为SfM的参考坐标系。所有帧的位姿 ( p ‾ c k c 0 ， q c k c 0 ) 和特征位置表示相对于(·)c0。假设摄像机和IMU之间有一个粗略测量的外部参数 ( p c b , q c b ) (p_c^b,q_c^b) (pcb,qcb)，我们可以将姿态从相机坐标系转换到物体(IMU)坐标系。