VINS-Mono 论文翻译

VINS-Mono: A Robust and Versatile MonocularVisual-Inertial State Estimator

摘要

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

I 引言

状态估计无疑是机器人导航、自主驾驶、虚拟现实和增强现实（AR）等广泛应用中最基本的模块。由于其体积小、成本低且易于硬件设置，仅使用单目相机的方法在该领域获得了极大的关注[1]-[5]。然而，单目纯视觉系统无法恢复真实，因此限制了其在现实机器人应用中的使用。最近，我们看到了一个发展趋势，即使用低成本的惯性测量单元(IMU)辅助单目视觉系统。这种单目视觉-惯性系统(VINS)的主要优点是可以观测真实尺度以及横滚和俯仰角。这将使其可以用于需要真实尺度状态估计的导航任务。此外，IMU测量值的集成可以通过弥补由于照明变化、无纹理区域或运动模糊造成的部分区域的视觉轨迹丢失，显著提高运动跟踪性能。单目VINS不仅广泛应用于地面机器人和无人机，而且在移动设备上也很实用。它在尺寸、重量和功耗方面对自我和环境感知都有很大的优势。

然而，有几个问题影响单目VINS的使用。第一个是严格的初始化。由于缺乏直接的距离测量，很难将单目视觉结构与惯性测量直接融合。同时认识到VINS是高度非线性的这一事实，我们发现在估计器初始化方面存在重大挑战。在大多数情况下，系统应从已知的静止位置启动，并在开始时缓慢小心地移动，这限制了其在实践中的使用。另一个问题是视觉惯性里程计(VIO)的长期漂移是不可避免的。为了消除漂移，必须进行闭环检测、重定位和全局优化。除了这些关键问题外，地图保存和重用的需求正在增长。

为了解决所有这些问题，我们提出了VINS-Mono，这是一种鲁棒且通用的单目视觉-惯性状态估计器，它是我们之前三项工作的组合和扩展[6]–[8]。VINS-Mono包含以下功能：

1. 鲁棒的初始化过程，能够从未知初始状态引导系统；
2. 紧耦合、基于优化的单目VIO，具有相机–IMU外参标定和IMU零偏矫正；
3. 在线重定位和四自由度全局位姿图优化；
4. 位姿图重用，可以保存、加载和合并多个局部位姿图。

在这些特性中，鲁棒的初始化、重定位和位姿图重用是我们的技术贡献，这些贡献来自我们以前的工作[6]–[8]。工程贡献包括开源系统集成、无人机导航实时演示和移动应用。整个系统已成功应用于小型AR场景、中型无人机导航和大型状态估计任务，如图1所示。

I 引言

状态估计无疑是机器人导航、自主驾驶、虚拟现实和增强现实（AR）等广泛应用中最基本的模块。由于其体积小、成本低且易于硬件设置，仅使用单目相机的方法在该领域获得了极大的关注[1]-[5]。然而，单目纯视觉系统无法恢复真实，因此限制了其在现实机器人应用中的使用。最近，我们看到了一个发展趋势，即使用低成本的惯性测量单元(IMU)辅助单目视觉系统。这种单目视觉-惯性系统(VINS)的主要优点是可以观测真实尺度以及横滚和俯仰角。这将使其可以用于需要真实尺度状态估计的导航任务。此外，IMU测量值的集成可以通过弥补由于照明变化、无纹理区域或运动模糊造成的部分区域的视觉轨迹丢失，显著提高运动跟踪性能。单目VINS不仅广泛应用于地面机器人和无人机，而且在移动设备上也很实用。它在尺寸、重量和功耗方面对自我和环境感知都有很大的优势。

然而，有几个问题影响单目VINS的使用。第一个是严格的初始化。由于缺乏直接的距离测量，很难将单目视觉结构与惯性测量直接融合。同时认识到VINS是高度非线性的这一事实，我们发现在估计器初始化方面存在重大挑战。在大多数情况下，系统应从已知的静止位置启动，并在开始时缓慢小心地移动，这限制了其在实践中的使用。另一个问题是视觉惯性里程计(VIO)的长期漂移是不可避免的。为了消除漂移，必须进行闭环检测、重定位和全局优化。除了这些关键问题外，地图保存和重用的需求正在增长。

为了解决所有这些问题，我们提出了VINS-Mono，这是一种鲁棒且通用的单目视觉-惯性状态估计器，它是我们之前三项工作的组合和扩展[6]–[8]。VINS-Mono包含以下功能：

1. 鲁棒的初始化过程，能够从未知初始状态引导系统；
2. 紧耦合、基于优化的单目VIO，具有相机–IMU外参标定和IMU零偏矫正；
3. 在线重定位和四自由度全局位姿图优化；
4. 位姿图重用，可以保存、加载和合并多个局部位姿图。

在这些特性中，鲁棒的初始化、重定位和位姿图重用是我们的技术贡献，这些贡献来自我们以前的工作[6]–[8]。工程贡献包括开源系统集成、无人机导航实时演示和移动应用。整个系统已成功应用于小型AR场景、中型无人机导航和大型状态估计任务，如图1所示。

II 相关工作

关于基于单目视觉的状态估计/里程计/SLAM的学术工作非常广泛。值得注意的方法包括PTAM[1]、SVO[2]、LSD-SLAM[3]、DSO[5]和ORBSLAM[4]。显然，任何进行全面相关概述的尝试都是不完整的。然而，在本节中，我们跳过了对纯视觉方法的讨论，只关注与单目视觉-惯性状态估计最相关的结果。

处理视觉和惯性测量的最简单方法是松耦合的传感器融合[9]，[10]，其中IMU被视为一个独立的模块来辅助视觉结构。融合通常由扩展卡尔曼滤波器(EKF)完成，其中IMU用于状态传播，纯视觉位姿用于更新。此外，紧耦合的视觉-惯性算法基于EKF[11][13]或图优化[14]-[19]，其中相机和IMU测量从原始测量的级别联合优化。一种流行的基于EKF的VIO方法是MSCKF[11]，[12]。MSCKF在状态向量中保持了多个以前的相机位姿，并在多个相机视角中使用相同特征的视觉测量来形成多约束更新。SR-ISWF[20]，[21]是MSCKF的一个扩展。它使用平方根形式[14]来实现单精度表示，并避免较差的数值特性。该方法使用逆滤波器进行迭代重新线性化，使其与基于优化的算法相同。批处理图优化或BA技术维护和优化所有测量值，以获得最佳的状态估计。为了实现常量的处理时间，基于图的VIO方法[15]、[17]、[18]通常通过边缘化过去的状态和测量值，来在最近状态的有限大小滑动窗口上进行优化。由于非线性系统迭代求解的计算量很大，很少有基于图优化的方法能够在资源受限的平台(如手机)上实现实时性能。

对于视觉测量处理，根据残差模型的定义，算法可以分为直接法和间接法。直接法[2]、[3]、[22]使光度误差最小化，而间接方法[12]、[15]、[17]使几何位移最小化。直接方法由于其较小的吸引域而需要良好的初始猜测，而间接方法在提取和匹配特征时消耗额外的计算资源。间接法由于其成熟性和鲁棒性，在实际工程部署中更为常见。然而，直接方法更容易扩展用于稠密建图，因为它们直接在像素级别上操作。

IMU通常以比摄像机高得多的速率获取数据。人们提出了不同的方法来处理高速率IMU测量值。最直接的方法是在基于EKF的方法中使用IMU进行状态传播[9]，[11]。在图优化形式中，为了避免重复的IMU重积分，开发了一种称为IMU预积分的有效的技术。这项技术最早在[23]中介绍，它使用欧拉角参数化旋转误差。Shen等人[16]利用连续时间误差状态动力学推导了协方差传播。在[19]和[24]中，通过添加后验IMU零偏校正，进一步改进了预积分理论。

准确的初始值对于引导任何单目VINS至关重要。[17]和[25]中提出了一种利用短期IMU预积分的相对旋转的线性估计器初始化方法。该方法无法对原始投影方程中的陀螺仪零偏和图像噪声进行建模。[26]中介绍了单目视觉-惯性初始化问题的闭式解。稍后，[27]中提出了通过添加陀螺仪零偏校准来对此闭式解进行扩展的解决方案。这些方法无法对惯性积分中的不确定性进行建模，因为它们依赖于IMU测量值在较长时间内的双重积分。在[28]中，提出了一种基于SVO[2]的重新初始化和故障恢复算法。需要一个额外的向下距离传感器来恢复真实尺度。[18]中介绍了一种建立在流行的ORB-SLAM[4]之上的初始化算法。据报道，尺度收敛所需的时间可能超过10秒。这可能会给需要在开始时就获得尺度估计的机器人导航任务带来问题。

里程计方法，无论其所依赖的基本数学公式如何，都会受到全局平移和旋转长期漂移的影响。为此，闭环矫正在长期操作中起着重要作用。ORB-SLAM[4]能够矫正闭环并重用地图，它利用了视觉词袋的优势[29]。闭环检测之后是7-DOF[30] (位置、旋转和尺度)位姿图优化。

II 系统概述

本文所提出的单目视觉-惯性状态估计器的结构如图2所示。

系统从测量值预处理(见第IV节)开始，在其中提取和跟踪特征，并预积分两个连续帧之间的IMU测量值。初始化程序(见第V节)提供了所有必要的值，包括位姿、速度、重力矢量、陀螺仪零偏和三维(3-D)特征位置，用于引导后续的基于非线性优化的VIO。VIO(见第VI节)与重定位(见第VII节)模块紧密融合了IMU测量值的预积分和特征观测。最后，位姿图优化模块(见第VIII节)接收几何验证后的重定位结果，并执行全局优化以消除漂移。它页实现了位姿图的重用。VIO和位姿图优化模块在单独的线程中并行运行。

与OKVIS[15]相比(OKVIS是一种最先进的VIO算法，适用于双目相机)我们的算法是专门为单目相机设计的。因此，我们特别提出了一个初始化过程、关键帧选择标准，以及使用和处理大视场(FOV)相机来获得更好的跟踪性能的方法。此外，我们的算法还提供了一个完整的系统，带有闭环矫正和位姿图重用模块。

现在我们定义了本文中使用的符号和坐标系定义。我们认为$(\cdot {)}^w$是世界坐标系。重力方向与世界坐标系的z轴对齐。$(\cdot {)}^b$是机体坐标系，我们将其定义为与IMU坐标系相同。$(\cdot {)}^c$是相机的坐标系。我们使用旋转矩阵$R$和哈密顿四元数$q$来表示旋转。我们主要在状态向量中使用四元数，但旋转矩阵也用于方便的三维向量旋转。$q_b^w$和$p_b^w$是从机体坐标系到世界坐标系的旋转和平移。$b_k$是拍摄第$k$张图像时的机体坐标系。$c_k$是拍摄第$k$张图像时的相机坐标系。$\otimes$表示两个四元数之间的乘法运算。$g^w=[0,0,g{]}^T$是世界坐标系中的重力矢量。最后，我们将$(\hat{\cdot })$表示为特定量的噪声测量或估计。

测量值预处理

视觉前端

对于每一帧新图像，KLT稀疏光流算法跟踪现有特征。同时，检测新的角点特征，以保持每个图像中特征的最小数目（100-300）。检测器通过在两个相邻特征点之间设置像素位置的最小间隔来实施特征分布均匀化。二维特征首先被去畸变，然后经过离群点剔除后投影到单位球面上。使用RANSAC和基础矩阵模型进行离群点剔除。

这一步还选择了关键帧。在VINS视觉前端中有两个关键帧选择标准。第一个是前一个关键帧的平均视差。如果被跟踪的特征在当前帧和上一帧之间的平均视差并且超过某个阈值，则将该帧视为新的关键帧。请注意，不仅平移而且旋转都会导致视差。但是，不能在仅旋转运动中对特征进行三角化。为了避免这种情况，我们在计算视差时使用陀螺测量值的短期积分来补偿旋转。请注意，此旋转补偿仅用于关键帧选择，不涉及VINS公式中的旋转计算。因此，即使陀螺仪含有较大的噪声或者零偏，也只会产生次优的关键帧选择结果，而不会直接影响估计质量。另一个标准是跟踪质量。如果被跟踪特征的数量低于某个阈值，将此帧视为新的关键帧。这个标准是为了避免特征跟丢。

IMU预积分

我们遵循我们之前基于连续时间四元数的惯性测量单元预积分的推导[16]，并包括惯性测量单元零偏的处理[19]和[24]。我们注意到，我们当前的预积分过程与[19]和[24]共享几乎相同的数值结果，但使用了不同的推导。所以，我们在这里只简单介绍一下。关于基于四元数的推导的细节可以在附录A中找到。

1）IMU噪声和零偏

IMU测量值在机体坐标系中测量，结合了反重力力和机体动力学，并受到加速度零偏$b_a$、陀螺仪零偏$b_w$和附加噪声的影响。原始陀螺仪和加速度计的测量值$\hat{\omega }$和$\hat{a}$由下式给出
$$\begin{gathered} {\hat{a}}_t=a_t+b_{a_t}+R_w^t{g}^w+n_a \\ {\hat{\omega }}_t={\omega }_t+b_{{\omega }_t}+n_{\omega }\text{\hspace{1em}(1)} \end{gathered}$$
我们假设加速度和陀螺仪测量中的加性噪声为高斯白噪声，$n_a\sim \mathcal{N}(0,{\sigma }_a^2)$，$n_{\omega }\sim \mathcal{N}(0,{\sigma }_{\omega }^2)$。加速度零偏和陀螺仪零偏被建模为随机游走，其导数是高斯白噪声，$n_{b_a}\sim \mathcal{N}(0,{\sigma }_{b_a}^2)$，$n_{b_{\omega }}\sim \mathcal{N}(0,{\sigma }_{b_{\omega }}^2)$
$${\dot{b}}_{a_t}=n_{b_a}，{\dot{b}}_{{\omega }_t}=n_{b_{\omega }}\text{\hspace{1em}(2)}$$

2）预积分

对于两个连续时间帧$b_k$和$b_{k+1}$，在时间间隔$[t_k，t_{k+1}]$中存在若干个惯性测量单元测量值。给定零偏估计，我们将它们整合到局部坐标系$b_k$中

$$\begin{gathered} {\alpha }_{b_{k+1}}^{b_k}=\int {\int }_{t\in [t_k,t_{k+1}]}{R}_t^{b_k}({\hat{a}}_t-b_{a_t})d{t}^2 \\ {\beta }_{b_{k+1}}^{b_k}={\int }_{t\in [t_k,t_{k+1}]}{R}_t^{b_k}({\hat{a}}_t-b_{a_t})dt \\ {\gamma }_{b_{k+1}}^{b_k}={\int }_{t\in [t_k,t_{k+1}]}\frac{1}{2}\Omega ({\hat{\omega }}_t-b_{{\omega }_t}){\gamma }_t^{b_k}dt\text{\hspace{1em}(3)} \end{gathered}$$
这里
$$\Omega (\omega )=\left[\begin{array}{cc}-{\omega }_{\times }& \omega \\ -{\omega }^T& 0\end{array}\right],{\omega }_{\times }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right].\text{\hspace{1em}(4)}$$
$\alpha$，$\beta$和$\gamma$的协方差矩阵$P_{b_{k+1}}^{b_k}$也是一致传播的。可以看出，预积分项(3)可以通过在给定偏差参考帧的零偏$b_k$来从惯性测量单元的测量值单独获得。

3）零偏矫正

如果零偏的估计值发生了轻微的变化，我们通过关于零偏的一阶近似来调整${\alpha }_{b_{k+1}}^{b_k}$、${\beta }_{b_{k+1}}^{b_k}$和${\gamma }_{b_{k+1}}^{b_k}$，如下所示
$$\begin{gathered} {\alpha }_{b_{k+1}}^{b_k}\approx {\hat{\alpha }}_{b_{k+1}}^{b_k}+J_{b_a}^{\alpha }\delta b_{a_k}+J_{b_{\omega }}^{\omega }\delta b_{{\omega }_k} \\ {\beta }_{b_{k+1}}^{b_k}\approx {\hat{\beta }}_{b_{k+1}}^{b_k}+J_{b_a}^{\beta }\delta b_{a_k}+J_{b_{\omega }}^{\beta }\delta b_{{\omega }_t} \\ {\gamma }_{b_{k+1}}^{b_k}\approx {\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_{\omega }}^{\gamma }\delta b_{{\omega }_k}\end{array}\right].\text{\hspace{1em}(5)} \end{gathered}$$

否则，当零偏的估计显著变化时，我们在新的零偏估计下进行重积分。这种策略为基于优化的算法节省了大量的计算资源，因为我们不需要重复进行惯性测量单元测量值积分。

状态估计器初始化

为什么需要初始化:单目紧耦合的VIO是一个高度非线性的系统，他需要一个准确的初始猜测作为后续优化的基础。在VINS中，作者通过对齐IMU预积分值和纯视觉SFM的结果来获得初始值。

这里需要注意的是，在VINS中，在初始化阶段，不优化两个变量，一个是加速度计的零偏(难以从重力当中单独分离出来，在后续的非线性优化过程中逐渐调整)，相机和IMU之间的平移外参(难以准确估计，采用手动测量的方式进行初始化)。

主要思想为使用纯视觉运动结构恢复(SFM)来进行初始化，并将IMU预积分结果与SFM的结果对齐来粗略地恢复尺度，重力，速度甚至零偏，如下图所示

基于滑动窗口的纯视觉SFM

保持一个滑动窗口用于限制计算复杂度。首先，检查最新帧和之前所有帧之间的特征关联性。如果能在滑动窗口的最新帧和任何其他帧(枢纽帧)之间找到稳定的特征跟踪（超过30个跟踪特征点）和足够的视差（超过20个旋转补偿后的像素）。使用五点法来恢复这两帧之间的相对旋转和平移。否则，在窗口中保留最新帧并等待新帧。如果五点算法成功，任意设置一个尺度，并对这两帧中的所有特征进行三角化。基于这些三角化特征，使用PnP方法来估计窗口中所有其他帧的位姿。最后，应用全局BA，以最小化所有特征观测值的总重投影误差。将滑窗口内第一个相机的位姿设置为SFM的参考坐标系。所有帧的姿态和特征位置都相对于参考坐标系进行表示。假设在相机和IMU之间有一个粗略的外参，便可以将所有帧的位姿从相机坐标系转换到机体(IMU)坐标系，
$$\begin{gathered} q_{b_k}^{c_0}=q_{c_k}^{c_0}\otimes (q_c^b{)}^{-1} \\ s{\bar{p}}_{b_k}^{c_0}=s{\bar{p}}_{c_k}^{c_0}-R_{b_k}^{c_0}{p}_c^b\text{\hspace{1em}(1)} \end{gathered}$$

视觉惯性对齐

陀螺仪零偏矫正

在窗口中考虑两个连续帧$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预积分，并最小化以下代价函数：
$$\begin{gathered} \min \delta b_w\sum _{k\in B}||{q_{b_{k+1}}^{c_0}}^{-1}\otimes q_{b_k}^{c_0}\otimes {\gamma }_{b_{k+1}}^{b_k}|{|}^2 \\ {\gamma }_{b_{k+1}}^{b_k}={\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_w}^{\gamma }{\delta }_w\end{array}\right]\text{\hspace{1em}(2)} \end{gathered}$$
其中$B$代表了窗口中的所有帧。这样，我们就得到了陀螺仪零偏$b_{\omega }$的初始标定。然后，使用新的陀螺仪零偏重新积分所有的预积分量。

将上式合并后可得代价函数为
$$\min \delta b_w\sum _{k\in B}||{q_{b_{k+1}}^{c_0}}^{-1}\otimes q_{b_k}^{c_0}\otimes {\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_w}^{\gamma }{\delta }_w\end{array}\right]|{|}^2\text{\hspace{1em}(3)}$$
在理想情况下，最小结果为单位四元数，也就是
$${q_{b_{k+1}}^{c_0}}^{-1}\otimes q_{b_k}^{c_0}\otimes {\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_w}^{\gamma }{\delta }_w\end{array}\right]=\left[\begin{array}{c}1\\ \vec{0}\end{array}\right]\text{\hspace{1em}(4)}$$
移项后可得
$$\left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_w}^{\gamma }{\delta }_w\end{array}\right]={{\hat{\gamma }}_{b_{k+1}}^{b_k}}^{-1}\otimes {q_{b_k}^{c_0}}^{-1}\otimes q_{b_{k+1}}^{c_0}\otimes \left[\begin{array}{c}1\\ \vec{0}\end{array}\right]\text{\hspace{1em}(5)}$$
取后三项，可以构建一个形如$Ax=b$的方程
$$2[{{\hat{\gamma }}_{b_{k+1}}^{b_k}}^{-1}\otimes {q_{b_k}^{c_0}}^{-1}\otimes q_{b_{k+1}}^{c_0}{]}_{xyz}=J_{b_w}^{\gamma }{\delta }_w\text{\hspace{1em}(6)}$$
求解这个方程就可以获得$\delta w$，然后加到原来的bias上，之后重新预积分即可。

速度、重力矢量和尺度初始化

在陀螺仪零偏被初始化之后，我们继续初始化其他状态量以便后续的导航。即速度，重力向量以及尺度因子
$${\chi }_I=[v_{b_0}^{b_0},v_{b_1}^{b_1},...,v_{b_n}^{b_n},g^{c_0},s]\text{\hspace{1em}(7)}$$
式中，$v_{b_k}^{b_k}$是拍摄第k帧图像时在机体系中的速度，$g^{c_0}$是$c_0$帧中的重力矢量，$s$将单目SfM换算为公制单位。

考虑窗口中的两个连续的两帧$b_k$和$b_{k+1}$，我们有下列方程：
$$\begin{gathered} {\alpha }_{b_{k+1}^{b_k}}=R_{c_0}^{b_k}(s({\bar{p}}_{b_{k+1}^{c_0}}-{\bar{p}}_{b_k}^{c_0}）+\frac{1}{2}{g}^{c_0}\Delta t_k^2-R_{b_k}^{c_0}{v}_{b_k}^{b_k}\Delta t) \\ {\beta }_{b_{k+1}}^{b_k}=R_{c_0}^{b_k}(R_{b_{k+1}}^{c_0}{v}_{b_{k+1}}^{b_{k+1}}+g^{c_0}\Delta t-R_{b_k}^{c_0}{v}_{b_k}^{b_k})\text{\hspace{1em}(8)} \end{gathered}$$
把公式(8)代入到公式(1)中可以得到一个线性方程组
$${\hat{z}}_{b_{k+1}}^{b_k}=\left[\begin{array}{c}{\hat{\alpha }}_{b_{k+1}}^{b_k}-p_c^b+R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}{p}_c^b\\ {\hat{\beta }}_{b_{k+1}^{b_k}}\end{array}\right]=H_{b_{k+1}}^{b_k}{\chi }_I+n_{b_{k+1}}^{b_k}\text{\hspace{1em}(9)}$$
这里
$$H_{b_{k+1}^{b_k}}=\left[\begin{array}{cccc}-I\Delta t& 0& \frac{1}{2}{R}_{c_0}^{b_k}\Delta t_k^2& R_{c_0}^{b_k}({\bar{p}}_{c_{k+1}}^{c_0}-{\bar{p}}_{c_k}^{c_0})\\ -I& R_{c_0}^{b_k}{R}_{b_{k+1}^{c_0}}& R_{c_0}^{b_k}\Delta t_k& 0\end{array}\right]\text{\hspace{1em}(10)}$$
可以看出$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_k$是两个连续帧之间的时间间隔。通过解决这个线性最小二乘问题
$$\underset{{\chi }_I}{\min }\sum _{k\in B}||{\hat{z}}_{b_{k+1}}^{b_k}-H_{b_{k+1}}^{b_k}{\chi }_I|{|}^2\text{\hspace{1em}(11)}$$
我们可以得到窗口中每一帧的机体系下的速度，视觉参考帧$(\cdot {)}^{c_0}$中的重力向量，以及尺度因子。

重力增强

通过约束重力的大小，可以对由前一线性初始化步骤得到的重力矢量进行细化。在大多数情况下，重力矢量的大小是已知的。这导致重力矢量只剩下2个自由度。因此，我们在其切线空间上用两个变量来对重力进行扰动，从而保持了2自由度。我们的引力向量通过$g(\bar{\hat{g}}+\delta g)$，$\delta g=w_1{b}_1+w_2{b}_2$，式中$g$是已知的重力大小，$\hat{\bar{g}}$是表示重力方向的单位矢量。$b_1$和$b_2$是两个横跨切面的正交基。$w_1$和$w_2$分别是$b_1$和$b_2$对应的二维扰动。我们可以任意找到旋转切线空间的任意一组$b_1$和$b_2$，然后用$g$代入式(8) ，并求解二维$\delta g$与其他状态变量一起。此过程反复多次，直到$\hat{g}$收敛。在代码实现中，涉及到很多工程技巧。

完成初始化

在细化重力矢量之后，我们可以通过将重力旋转到$Z$轴得到世界坐标系和相机枢纽帧之间的旋转$q_{c_0}^w$。然后我们将所有变量从参考坐标系$(\cdot {)}^{c_0}$旋转到世界坐标系$(\cdot {)}^w$。机体坐标系下的速度也将旋转到世界坐标系。视觉SFM的平移分量将按公制单位缩放。至此，初始化过程完成，所有这些度量值将被喂给一个紧耦合的单目VIO。

紧耦合单目VIO

在估计器初始化之后，我们继续使用基于滑动窗口的紧耦合单目VIO进行高精度和鲁棒的状态估计。滑动窗口公式的图示如图所示。

主要公式

滑动窗口中的全状态向量定义为
$$\begin{gathered} \chi =[x_0,x_1,...,x_n,x_c^b,{\lambda }_0,{\lambda }_1,...,{\lambda }_m] \\ {x}_k=[p_{b_k}^w,v_{b_k}^w,q_{b_k}^w,b_a,b_g] \\ {x}_c^b=[p_c^b,q_c^b]\text{\hspace{1em}(12)} \end{gathered}$$
其中$x_k$是捕获第k帧图像时的IMU状态。它包含IMU在世界坐标系中的位置、速度和姿态，以及IMU机体坐标系中的加速度偏差和陀螺仪偏差。$n$是关键帧的总数，$m$是滑动窗口中特征的总数。${\lambda }_l$第$l$个特征点在第一次被观测到关键帧中的逆深度。

我们使用视觉惯性BA公式。我们最小化所的先验和测量值的马氏范数之和来获取最大后验估计，公式如下
$$\underset{\chi }{\min }||r_p-H_p\chi |{|}^2+\sum _{k\in B}||r_B({\hat{z}}_{b_{k+1}}^{b_k},\chi )|{|}_{p_{b_{k+1}^{b_k}}}^2+\sum _{(l,j)\in C}\rho (||r_C({\hat{z}}_l^{c_j},\chi )|{|}_{P_l^{c_j}}^2)\text{\hspace{1em}(13)}$$
其中Huber范数定义为
ρ ( s ) = { s , s ≤ 1 2 s − 1 , s > 1 (14) \rho(s) = {\{ \begin{matrix} s,s \leq 1 \\ 2 \sqrt{s} -1, s>1\end{matrix}} \tag{14} ρ(s)={s,s≤12s ​−1,s>1​(14)
其中$r_B({\hat{z}}_{b_{k+1}}^{b_k})$和$r_C({\hat{z}}_l^{c_j})$分别是IMU和视觉测量的残差。$B$是所有IMU测量的集合，$C$是在当前滑动窗口中至少被观察两次的特征特征的集合。 { r p , H p } \{r_p,H_p\} {rp​,Hp​}是边缘化的先验信息。ceres解算器用于解决这个非线性问题。

IMU测量残差

考虑滑动窗口中连续两帧$b_k$和$b_{k+1}$的IMU测量值，IMU预积分的残差可以被定义为
$$\begin{gathered} r_B({\hat{z}}_{b_{k+1}}^{b_k},\chi )=\left[\begin{array}{c}\delta {\alpha }_{b_{k+1}}^{b_k}\\ \delta {\beta }_{b_{k+1}}^{b_k}\\ \delta {\theta }_{b_{k+1}}^{b_k}\\ \delta b_a\\ \delta b_g\end{array}\right] \\ =\left[\begin{array}{c}{R}_w^{b_k}(p_{b_{k+1}}^w-p_{b_k}^w+\frac{1}{2}{g}^w\Delta t_k^2-v_{b_k}^w\Delta t_k)-{\hat{\alpha }}_{b_{k+1}}^{b_k}\\ R_w^{b_k}(v_{b_{k+1}}^w+g^w\Delta t_k-v_{b_k}^w)-{\hat{\beta }}_{b_{k+1}}^{b_k}\\ 2[{q_{b_k}^w}^{-1}\otimes q_{b_{k+1}}^w\otimes ({\hat{\gamma }}_{b_{k+1}}^{b_k}){]}_{xyz}\\ {b_a}_{b_{k+1}}-{b_a}_{b_k}\\ {b_w}_{b_{k+1}}-{b_w}_{b_k}\end{array}\right]\text{\hspace{1em}(15)} \end{gathered}$$
其中$[\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}]$是两个连续图像帧之间IMU测量值的预积分量。在线校正的残差项中还包括加速度计和陀螺仪零偏。

视觉测量残差

与在广义图像平面上定义重投影误差的传统针孔摄像机模型不同，我们定义了单位球面上的相机测量残差。几乎所有类型的相机，包括广角、鱼眼或全向相机，都可以建模为连接单位球面的单位射线。考虑在第$i$幅图像中首次观察到的第$l$个特征，第$j$幅图像中特征观测的残差定义为
$$\begin{gathered} r_C({\hat{z}}_l^{c_j},\chi )=[b_1,b_2{]}^T\cdot ({\hat{\bar{P}}}_l^{c_j}-\frac{P_l^{c_j}}{||P_l^{c_j}||}) \\ {\hat{\bar{P}}}_l^{c_j}={\pi }_c^{-1}(\left[\begin{array}{c}{\hat{u}}_l^{c_j}\\ {\hat{v}}_l^{c_j}\end{array}\right]) \\ {P}_l^{c_j}=(R_b^c(R_w^{b_j}(R_{b_i}^w(R_c^b\frac{1}{{\lambda }_l}{\pi }_c^{-1}(\left[\begin{array}{c}{\hat{u}}_l^{c_i}\\ {\hat{v}}_l^{c_i}\end{array}\right])+p_c^b)+p_{b_i}^w-p_{b_j}^w)-p_c^b)\text{\hspace{1em}(16)} \end{gathered}$$
其中$[{\hat{u}}_l^{c_i},{\hat{v}}_l^{c_i}]$是对出现在第$i$帧图像中的第$l$个特征的第一次观测。$[{\hat{u}}_l^{c_j},{\hat{v}}_l^{c_j}]$是相同特征点在第$j$帧图像中的观测。${\pi }_c^{-1}$是反向投影函数，它使用相机内参将像素坐标转换为一个单位向量。由于视觉残差的自由度是2，我们将残差向量投影到切平面. $b_1$和$b_2$是两个任意选择的正交基，它们跨越${\hat{\bar{P}}}_l^{c_j}$的切平面，如下图所示。

公式(13)中使用的变量$P_l^{c_j}$也从像素坐标转换到单位球面上

边缘化

为了限制基于优化的VIO算法的计算复杂度，我们引入了边缘化。我们从滑动窗口中选择性地边缘化IMU状态$x_k$和特征${\lambda }_l$，同时将对应于边缘化状态的测量值转换为先验。

如上图所示，当倒数第二帧是关键帧时，它将被保留在窗口中，最早的帧被用其相应的测量值边缘化掉。换句话说，如果倒数第二帧是非关键帧，我们会丢掉对应的视觉测量值，并保留与该非关键帧相关的惯性测量单元测量值。我们不会为了保持系统的稀疏性而边缘化掉非关键帧的所有测量。我们的边缘化方案旨在将被边缘化的关键帧与相应的测量值分离开。这确保了特征的三角化有足够的视差，并最大化了保持加速度计测量具有大的激励的概率。

边缘化是利用舒尔补实现的。我们基于与被边缘化状态相关的所有边缘化测量来构建新的先验。新的先验被添加到现有的先验中。

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

以相机帧率运行的纯运动视觉惯性优化

对于低计算能力的设备，例如手机，由于非线性优化的大量计算需求，紧耦合的单目VIO不能实现相机速率的输出。为此，除了完整的优化之外，我们还采用了一种轻量级的纯运动视觉惯性优化，将状态估计提升到相机速率(30 Hz)。

纯运动视觉惯性优化的成本函数与公式(13)中的单目VIO相同。然而，我们没有优化滑动窗口中的所有状态，而是只优化固定数量的最新的IMU状态的姿态和速度。我们将特征深度、外参、零偏和我们不想优化的旧的IMU状态视为常量。我们使用所有的视觉和惯性测量值来进行纯运动优化。这导致比单帧PnP方法更平滑的状态估计。

上图显示了所提出策略的说明。完全紧耦合的单目VIO可能会在最先进的嵌入式计算机上需要超过50毫秒的时间，与此相反，纯运动视觉惯性优化只需要大约5毫秒来计算。这使得低延迟的相机速率位姿估计成为可能，这对于无人机和增强现实应用尤其有利。

以IMU帧率运行的基于IMU姿态解算的位姿估计

惯性测量单元的测量帧帧比视觉测量要高得多。虽然我们的VIO频率受到图像捕获频率的限制，但我们仍然可以通过使用最新的惯性测量单元的测量在最新的VIO估计值上进行积分，以实现以IMU速率运行的估计性能。高频状态估计可以用作控制系统的闭环的状态反馈。

重定位

我们的滑动窗口和边缘化方案限制了计算的复杂性，但也为系统引入了累积漂移。为了消除漂移，提出了一种与单目VIO无缝集成的紧耦合重定位模块。重定位过程从识别已经访问过的地方的闭环检测模块开始。然后建立闭环候选帧和当前帧之间的特征级连接。这些特征对应关系紧密集成到单目VIO模块中，以最小的计算量实现无漂移的状态估计。多个特征的多个观测直接用于重定位，从而产生更高的精度和更好的状态估计平滑度。下图展示了重定位过程的图形化解释

闭环检测

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

特征检索

当检测到一个闭环时，局部滑动窗口和闭环候选帧之间的连接关系通过检索特征对应关系来建立。通过BRIEF描述子匹配来找到对应关系。描述子匹配可能会导致一些错误的匹配对。为此，我们使用两步几何外点剔除，如下图所示

• 2D–2D：使用RANSAC的基础矩阵测试。我们使用当前帧和闭环候选帧中检索特征的2D观测来执行基础矩阵测试。
• 3D–2D:使用RANSAC的PnP测试。基于已知的特征在局部滑动窗口中的三维位置，以及闭环候选帧中的2D观测，我们执行PnP测试。

紧耦合的重定位

重定位过程有效地将当前滑动窗口与过去的位姿对齐。在重定位期间，我们将所有闭环帧的位姿视为常量。我们使用所有惯性测量单元测量值、局部视觉测量值和检索到的特征对应关系来联合优化滑动窗口。我们可以很容易地为闭环帧$v$观测到的三维特征写出视觉测量模型，与VIO的视觉测量模型相同，如公式(16)所示。唯一的区别是，从位姿图或直接从过去的里程计输出(如果这是第一次重定位)中获得的闭环帧的位姿${\hat{q}}_v^w,{\hat{p}}_v^w$被视为常量。为此，我们可以稍微修改公式(13)中的非线性代价函数来添加闭环项
min ⁡ χ { ∣ ∣ r p − H p χ ∣ ∣ 2 + ∑ k ∈ B ∣ ∣ r B ( z ^ b k + 1 b k , χ ) ∣ ∣ p b k + 1 b k 2 + ∑ ( l , j ) ∈ C ρ ( ∣ ∣ r C ( z ^ l c j , χ ) ∣ ∣ P l c j 2 ) + ∑ ( l , v ) ∈ L ρ ( ∣ ∣ r C ( z ^ l v , χ , q ^ v w , p ^ v w ) ∣ ∣ P l c v 2 ) } (17) \min_{\chi}{ \{||r_p - H_p \chi||^2 + \sum_{k \in B}{||r_B(\hat{z}_{b_{k+1}}^{b_k},\chi)||^2_{p_{b_{k+1}^{b_k}}}} + \sum_{(l,j) \in C }{\rho(||r_C(\hat{z}_l^{c_j},\chi)||_{P_{l}^{c_j}}^2)}} + \sum_{(l,v) \in L}{\rho(||r_C(\hat{z}_l^v,\chi,\hat{q}_v^w,\hat{p}_v^w)||_{P_l^{c_v}}^2)}\} \tag{17} χmin​{∣∣rp​−Hp​χ∣∣2+k∈B∑​∣∣rB​(z^bk+1​bk​​,χ)∣∣pbk+1bk​​​2​+(l,j)∈C∑​ρ(∣∣rC​(z^lcj​​,χ)∣∣Plcj​​2​)+(l,v)∈L∑​ρ(∣∣rC​(z^lv​,χ,q^​vw​,p^​vw​)∣∣Plcv​​2​)}(17)
其中$L$是闭环检测中检索到的特征的观测集合。$(l,v)$表示在闭环帧$v$中观察到的第$l$个特征。请注意，尽管代价函数与公式(13)略有不同，但待求解状态的维数保持不变，因为闭环帧的位姿被视为常量。当用当前滑动窗口建立多个闭环约束时，我们同时使用所有帧的所有闭环特征对应关系来进行优化。这为重定位提供了多视图约束，从而获得更高的精度和更好的平滑度。

全局位姿图优化和地图融合

在重定位之后，开发了额外的位姿图优化步骤，以确保将过去的位姿集合注册到全局一致的配置中

四个累积漂移方向

得益于对重力的惯性测量，在VINS滚转角和俯仰角是完全可观的。

如上图所示，随着物体的移动，3d位置和旋转相对于参考框架相对改变。然而，我们可以通过重力矢量来确定水平面，这意味着我们可以一直观察绝对滚转角和俯仰角。因此，滚转和俯仰在世界坐标系中是绝对状态，而x，y，z和偏航角是相对于参考坐标系的相对状态。累积漂移仅出现在x、y、z和偏航角。为了充分利用有效信息并有效校正漂移，我们固定了无漂移的滚转角和俯仰角，只在4自由度上进行位姿图优化。

添加关键帧到位姿图中

在VIO过程之后，关键帧被添加到位姿图中。每个关键帧都作为位姿图中的一个顶点，它通过两种类型的边与其他顶点相连，如下图所示。

• 序列边：一个关键帧建立了与其前面关键帧的几个序列边。序列边表示两个关键帧之间的相对变换，直接取自VIO。考虑关键帧$i$和它之前的一个关键帧$j$，序列边只包含相对位置${\hat{p}}_{ij}^i$偏航角${\hat{\psi }}_{ij}$。
  $$\begin{gathered} {\hat{p}}_{ij}^i={{\hat{R}}_i^w}^{-1}({\hat{p}}_j^w-{\hat{p}}_i^w) \\ {\hat{\psi }}_{ij}={\hat{\psi }}_j-{\hat{\psi }}_i\text{\hspace{1em}(18)} \end{gathered}$$

• 闭环边：如果关键帧有一个闭环连接，它通过位姿图中的闭环边连接到闭环帧。类似地，闭环边只包含一个4自由度相对位姿变换，定义与公式(18)相同。使用重定位的结果来获得闭环边的值。

4自由度位姿图优化

我们将关键帧$i$与关键帧$j$之间的边的残差定义为
$$r_{i,j}(p_i^w,{\psi }_i,p_j^w,{\psi }_j)=\left[\begin{array}{c}R({\hat{\varphi }}_i,{\hat{\theta }}_i,{\hat{\psi }}_i{)}^{-1}(p_j^w-p_i^w)-{\hat{p}}_{ij}^i\\ {\psi }_j-{\psi }_i-{\hat{\psi }}_{ij}\end{array}\right]\text{\hspace{1em}(19)}$$
其中${\hat{\varphi }}_i$和${\hat{\theta }}_i$是滚转角和俯仰角的固定估计值，它们是从单目VIO获得的。

通过最小化以下代价函数，优化了整个位姿图的序列边和闭环边:
min ⁡ p , ψ { ∑ ( i , j ) ∈ S ∣ ∣ r i , j ∣ ∣ 2 + ∑ ( i , j ) ∈ L ρ ( ∣ ∣ r i , j ∣ ∣ 2 ) } (20) \min_{p,\psi}{\{ \sum_{(i,j) \in S}||r_{i,j}||^2 + \sum_{(i,j) \in L}{\rho(||r_{i,j}||^2)} \}} \tag{20} p,ψmin​{(i,j)∈S∑​∣∣ri,j​∣∣2+(i,j)∈L∑​ρ(∣∣ri,j​∣∣2)}(20)
其中是$S$所有序列边的集合，$L$是所有闭环边的集合。虽然紧耦合的重新定位已经有助于消除错误的闭环，但我们仍然添加了另一个Huber范数$\rho (\cdot )$来进一步减少任何可能的错误闭环的影响。相比之下，我们没有对序列边使用任何鲁棒范数，因为这些边是从VIO提取的，它已经包含足够的离群点剔除机制。

位姿图优化和重定位在两个独立的线程中异步运行。这使得能够立即使用最优的位姿图进行重新定位，只要它变得可用。类似地，即使当前位姿图优化尚未完成，使用现有位姿图配置仍然可以进行重定位。这个过程如下图所示

位姿图融合

位姿图不仅可以优化当前地图，还可以将当前地图与先前构建的地图合并。如果我们已经加载了一个先前构建的地图并检测到两个地图之间的闭环连接，我们可以将它们合并在一起。由于所有边都是相对约束，因此位姿图优化通过闭环连接自动地将两个图合并在一起。如下图所示，当前地图被闭环边拉入到先前构建的地图中。每一个顶点和每一条边都是相对变量，因此，我们只需要固定点图中的第一个顶点。

位姿图保存

我们的位姿图的结构非常简单。我们只需要保存顶点和边，以及每个关键帧(顶点)的描述子。原始图像被丢弃以减少内存消耗。具体来说，我们为关键帧$i$保存的状态是
$$[i,{\hat{p}}_i^w,{\hat{q}}_i^w,v,{\hat{p}}_{iv}^i,{\hat{\psi }}_{iv},D(u,v,,des)]\text{\hspace{1em}(21)}$$
其中$i$是关键帧索引，${\hat{p}}_i^w$和${\hat{q}}_i^w$是来自VIO的位置和方向。如果此关键帧有对应的闭环帧，则$v$是闭环帧的索引，${\hat{p}}_{iv}^i$和${\hat{\psi }}_{iv}^i$是这两个帧之间的相对位置和偏航角，这是通过重定位获得的。$D(u,v,des)$是特征集合。每个特征都包含二维位置及其BRIEF描述子。

位姿图载入

我们使用相同的保存格式来加载关键帧。每个关键帧都是位姿图中的一个顶点。顶点的初始姿势是${\hat{p}}_i^w$和${\hat{q}}_i^w$。闭环边直接由闭环信息${\hat{p}}_{iv}^i$，${\hat{\psi }}_{iv}$建立。每个关键帧都与其相邻的关键帧建立多个序列，如公式(18)所示。加载位姿图后，我们立即执行一次全局的4自由度位姿图优化。位姿图的保存和加载速度与位姿图的大小呈线性相关。

实验

我们执行数据集和真实世界的实验和两个应用来评估所提出的VINS-Mono系统。在第一个实验中，我们将所提出的算法与另一种最先进的算法在公共数据集上进行了比较。我们进行了一个数值分析来详细展示我们系统的准确性。然后，我们在室内环境中测试我们的系统，以评估在重复场景中的性能。为了说明长时间的实用性，进行了大规模的实验。另外，我们将所提出的系统应用于两个应用。对于空中机器人应用，我们使用VINS-Mono进行位置反馈，以控制无人机跟随预定的轨迹。然后，我们将我们的方法移植到iOS移动设备上。

A.数据集比较

1）VIO比较

我们使用EuRoC MAV 视觉惯性数据集来评估我们提出的VINS-Mono系统[41]。数据集在微型飞行器(MAV)上收集，该飞行器包含双目图像(Aptina MT9V034 global shutter,WVGA monochrome, 20 FPS)、同步后的惯性测量单元测量值(ADIS16448，200 Hz)和地面真值(VICONand Leica MS50)。我们只使用左相机的图像。

在这个实验中，我们将VINS-Mono与OKVIS [15]进行了比较，OKVIS是一种可以在单目和双目模式下工作的最先进的VIO。OKVIS是另一种基于优化的滑动窗口算法。我们的算法在许多细节上与OKVIS不同，如技术部分所述。我们的系统完成了鲁棒的初始化和闭环矫正。我们详细展示了MH_03_medium和MH_05_hard两个序列的结果。为了简化符号，我们使用VINS来表示只有单目VIO的方法，而VINS_LOOP表示完整的带有重定位和位姿图优化的版本。我们使用OKVIS来表示OKVIS单目模式的结果。

对于序列MH_03_medium，轨迹如图14(a)所示。

[42]估算的相对位姿误差如图15所示。

在误差图中，带有闭环矫正的VINS-Mono在长期内胜过其他方法。闭环矫正模块有效地减少了平移和偏航漂移。如图14(b)和16所示，在MH_05_difficult上的结果相同。

EuRoC数据集中所有序列通过绝对轨迹误差(ATE)进行评价的均方根误差(RMSE)如表1所示。

在大多数情况下，带有闭环的VINS-Mono的效果比其他的要好。在一些行程短、漂移小的情况下，如V1_03_difficult和V2_01_easy，闭环模块对效果没有明显的影响。

更多的基准比较可以在[44]中找到，与其他最先进的算法相比，它展示了本文所提出的系统的良好性能。

2）地图合并结果

在同一地点的不同起始位置和不同时间采集五个MH序列，因此我们可以将五个MH序列合并成一个全局位姿图。我们在每个序列中基于相似的相机视角进行重定位和位姿图优化。我们只固定第一个序列中的第一帧，它的位置和偏航角设置为零。然后，我们将新序列一个接一个地合并到之前的地图中。轨迹如图17所示。

我们还将整个轨迹与地面真值进行了比较。ATE [43]的RMSE是$0.21m$，这在500米长的运行中是一个令人印象深刻的结果。该实验表明，通过增量合并在不同时间捕获的新传感器数据，地图会随着时间的推移而“进化”，并且整个位姿图的一致性得以保持。

B.真实世界实验

1）室内实验

我们使用的传感器套件如图18所示。

它包含一个单目相机(mvBlueFOX-MLC200w, 20 Hz)和一个惯性测量单元(100 Hz)在DJI A3 控制器内部。我们用手拿着传感器套件，以正常的速度行走。我们将我们的结果与OKVIS进行比较，如图19所示。

图19(a)是OKVIS的VIO输出。图19(b)是所提出的具有重定位和闭环矫正的方法的结果。当我们在室内转圈时，明显的VIO漂移出现了。OKVIS在$x$、$y$、$z$和$yaw$中出现了显著的累积漂移。我们的重定位和闭环矫正模块有效地消除了这些漂移。

2）大规模环境

这个环绕整个HKUST校园的非常大规模的数据集是用手持VI传感器记录的。这个数据集覆盖了大约710米长，240米宽，60米高度变化的地方。总路径长度为5.62公里。数据包含25Hz的图像和200Hz的惯性测量单元测量值，持续1小时34分钟。测试VINS-Mono的稳定性和持久性是一项非常有意义的实验。

在这个大规模测试中，我们将关键帧数据库的大小设置为2000，以便提供足够的闭环信息并实现实时性能。我们使用以3.60GHz的英特尔i7-4790CPU运行该数据集。时间统计见表二。

估计的轨迹与图20中的谷歌地图对齐。

与谷歌地图相比，我们可以看到我们的结果在这个非常长时间的测试中几乎没有漂移。

C.应用

1）空中机器人的反馈控制

我们将VINS-Mono应用于空中机器人的自主反馈控制，如图21所示。

我们使用的是一款分辨率为752×480的前置全局快门相机(MatrixVision mvBlueFOX-MLC200w)，并为其配备了$190°$鱼眼镜头。DJI A3 飞行控制器用于惯性测量单元和姿态稳定控制。机载计算机是一款以在3.00GHz运行的英特尔i7-5500U CPU。传统的针孔相机模型不适合大FOV相机。我们用MEI[45]模型来建模这个相机，用工具箱[46]标定。

在这个实验中，我们测试了使用VINS-Mono的状态估计结果时的自动轨迹跟踪的性能。该实验禁用了闭环矫正。如图22所示，四旋翼飞行器被命令以8字形轨迹飞行，每个圆的半径为$1.0m$。

在轨迹周围放置四个障碍物，以验证VINS-Mono无闭环矫正模式下的准确性。在实验过程中，四旋翼飞行器连续四次跟随这个轨迹。100Hz的机载状态估计能够对四旋翼飞行器进行实时反馈控制。

地面真实值使用OptiTrack获得。总轨迹长度为61.97米。最终漂移为$[0.08，0.09，0.13]m$，导致$0.29\%$的位置漂移。图23显示了平移和旋转的细节以及它们相应的误差。

2）移动设备

我们将VINS-Mono移植到移动设备上，并展示了一个简单的增强现实应用程序来展示其准确性和鲁棒性。我们将我们的移动端实现命名为VINS-Mobile。VINS-Mobile在iPhone设备上运行。我们使用iPhone拍摄的分辨率为$640\times 480$的$30Hz$图像，以及内置的InvenSense MP67B六轴陀螺仪和加速度计获得的100Hz的IMU数据。首先，我们在平面上插入一个虚拟立方体，它是从被估计的视觉特征中提取的，如图24(a)所示。

然后，我们拿着设备，以正常的速度在房间内外走动。当检测到闭环时，我们使用4自由度位姿图优化来消除$x$、$y$、$z$和$yaw$的累积漂移误差。在行驶约264米后，我们返回出发地点。最终结果可以在图24(b)中看到，

VINS回到了起点。由于采用了4自由度位姿图优化，消除了总轨迹的累计漂移。与开始相比，立方体被配准到图像上的相同位置也证明了这一点。

未来工作

在这篇文章中，我们提出了一个鲁棒和通用的单目视觉惯性估计器。我们的方法包含惯性测量单元预积分、估计器初始化、在线外参标定、紧耦合VIO、重定位和高效全局优化方面都有先进和新颖的解决方案。我们通过与其他最先进的开源实现进行比较来展示性能。为了社区的利益，我们开放了PC和IOS实现的源代码。

尽管基于特征的VINS估计器已经达到了现实部署的成熟程度，但我们仍然看到了未来研究的许多方向。单目VINS视运动和环境而定，可能达到弱可观甚至退化的状态。我们感兴趣的是在线方法来评估单目VINS的可观测性，以及在线生成运动规划来恢复可观测性。另一个研究方向涉及单目VINS在各种消费设备上的大规模部署，例如安卓手机。该应用要求在线标定几乎所有传感器的内部和外部参数，以及在线识别标定质量。最后，给定单目VINS的结果，我们有兴趣生成稠密的地图。我们在应用于无人机导航的单目视觉-惯性稠密建图上的第一个结果在[47]中展示。然而，为了进一步提高系统的精度和鲁棒性，仍然需要进行广泛的研究。