VINS_mono

2022-03-01

SLAM

字数统计: 8.9k字 | 阅读时长≈ 33分钟

本章想要抽空把之前的VINS_mono系统地学习一下。

简介

论文

摘要

本文提出了一种用于在各种环境中进行状态估计的单目视觉惯性系统 (VINS)。单目相机和低成本的惯性测量单元 (IMU) 构成了用于六自由度状态估计的最小传感器套件。本文算法迭代地优化有界滑动窗口中的视觉和惯性测量，以执行准确的状态估计。视觉结构由滑动窗口中的关键帧保持，而惯性度量测量则通过关键帧之间的预积分来保持。我们的系统对未知状态的初始化、在线相机-IMU 外部参数校准、在球体上定义的统一重投影误差、循环检测和四自由度位姿图优化具有鲁棒性。这些特性使我们的系统实用且易于使用。我们通过与其他最先进的算法进行比较来验证我们系统在公共数据集和真实世界实验中的性能。我们还在 MAV 平台上执行机载闭环自主飞行，并将算法移植到 iOS 应用程序。我们强调所提出的工作是一个可靠且完整的系统，可以在其他智能平台上轻松操作。我们开源了我们的实现代码和 iOS 移动设备上的增强现实 (AR) 应用程序。

I. 总体简介

本文所提出的视觉惯性系统的结构如下图所示。

第一部分是数据测量处理前端，它从每个新图像帧提取、跟踪特征并在两帧之间预积分所有 IMU 数据。
第二部分是初始化过程，它提供必要的初始值（姿态、速度、重力矢量、陀螺仪偏差和 3D 特征位置）来引导非线性系统。
第三部分实现非线性图优化，通过优化所有视觉、惯性信息来解决我们滑动窗口中的状态。
最后，位姿图优化模块（参见第 VIII 节）接收几何验证的重定位结果，并执行全局优化以消除漂移。它还实现了位姿图重用。（VIO 和位姿图优化模块分别在单独的线程中同时运行）

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

符号定义：我们将 $(·)^w$ 视为世界坐标系，其中重力矢量的方向与其 z 轴对齐。 $(·)^b$ 是与 IMU 框架对齐的主体框架。$ (·)^c$ 是相机框架。我们同时使用 R 和四元数 q 来表示旋转矩阵。四元数对应于汉密尔顿符号。我们主要在状态向量中使用四元数，但旋转矩阵也用于 3-D 向量的方便旋转。 $q^w_b$ , $p^w_b$ 是从body坐标系到世界坐标系的旋转和平移。 $b_k$ 是拍摄第 k 张图像时的身体帧。 $c_k$ 是拍摄第 k 张图像时的相机帧。 ⊗ 是两个四元数的乘法运算。 $g^w = [0, 0, g]^T$ 是世界坐标系中的重力矢量。

II 测量预处理

我们在本节中预处理视觉和惯性测量。

对于视觉测量，我们跟踪连续帧之间的特征并检测最新帧中的新特征。
对于 IMU 测量，我们将它们在两个连续帧之间进行预积分。

请注意，IMU 测量受偏差和噪声的影响。因此，我们在 IMU 预集成和优化部分特别考虑了偏差，这对于低成本 IMU 芯片至关重要。

A. 视觉测量前段

对于每张新图像，现有特征由 KLT 稀疏光流算法 [26] 跟踪。同时，检测新的角点特征 [27] 以保持每个图像中的最小数量（100-300）特征。检测器通过设置两个相邻特征之间的最小像素间隔来强制实现均匀的特征分布。在通过异常值拒绝后，特征被投影到一个单位球体。异常值剔除由基本矩阵测试中的 RANSAC 步骤执行。

在此步骤中还选择了关键帧。我们有两个关键帧选择标准。

其中之一是平均视差。如果跟踪到的特征的平均视差超过某个阈值，我们将这张图像视为关键帧。请注意，不仅平移而且旋转都会引起视差；但是，不能在仅旋转运动中对特征进行三角剖分。为了避免这种情况，我们在计算视差时使用 IMU 传播结果来补偿旋转。
另一个标准是跟踪质量。如果跟踪特征的数量低于某个阈值，我们也将此帧视为关键帧。

B. IMU预积分

我们通过结合 IMU 偏差校正 [28] 扩展了我们之前的工作 [7] 中提出的 IMU 预积分。与[28]积分，我们推导出连续时间动态中的噪声传播。此外，IMU 预积分结果也用于初始化过程以校准初始状态。

1）IMU噪声与偏差
$$
\hat{a}t = a_t + b{a_t} +R^t_w g^w +n_a
$$

$$
\hat{w}t = w_t + b{w_t} +n_w
$$

我们假设加速度和角速度测量中的额外的噪声都是高斯白噪声，$n_a\sim \mathcal{N}(0, \sigma^2_a)$, $n_w\sim\mathcal{N}(0,\sigma_w^2)$

加速度偏差和陀螺仪偏差被建模为随机游走，其导数是高斯白噪声，$n_{b_a}\sim \mathcal{N}(0, \sigma^2_{b_a})$, $n_{b_w}\sim \mathcal{N}(0, \sigma^2_{b_w})$
$$
\dot{b}{a_t} = n{b_a}\
\dot{b}{w_t} = n{b_w}
$$
2)预积分，给定对应于图像帧$b_k$ 和 $b_{k+1}$ 的两个时刻，状态变量受时间间隔 $[k, k + 1]$ 期间的惯性测量约束：
$$
p^w_{b_{l+1}} = p^w_{b_k} +v^w_{b_k}\Delta t_k + \int\int_{t\in[k, k+1]}(R^w_t(\hat{a}t - b{a_t})-g^w)dt^2
$$

$$
v^w_{b_{k+1}} = v^w_{b_k} + \int_{t\in[k,t+1]}(R^w_t(\hat{a}t-b{a_t})-g^w)dt
$$

$$
q^w_{b_{k+1}} = q^w_{b_k} \otimes\int_{t \in [k, k+1]}\frac{1}{2}\Omega(\hat{w}t - b{w_t})q^{b_k}_t dt
$$

其中：
$$
\Omega (w) = \begin{bmatrix} -\lfloor w \rfloor_{\times} & w \ -w^T & 0 \end{bmatrix}
$$

$$
\lfloor w \rfloor _ \times =
\begin{bmatrix} -\lfloor 0 & - w_z & w_y \ w_z & 0 & -w_x \ - w_y & w_x & 0 \end{bmatrix}
$$

$\Delta t_x$ 是时间间隔$[k ,k+1]$的持续时间， $\hat{w}_t$, $\hat{a}_t$是原始的IMU测量。它在相机坐标系下，并且被加速度偏差$b_a$, 加速度偏差$b_\omega$ 以及噪声影响。

可以看出，IMU 状态传播需要帧 $b_k$ 的旋转、位置和速度。当这些起始状态发生变化时，我们需要重新传播 IMU 测量值。特别是在基于优化的算法中，每次调整姿势时，我们都需要在它们之间重新传播 IMU 测量值。这种传播策略是计算消耗的。为了避免重复传播，我们采用预积分算法。

将 IMU 传播的参考坐标系改为局部坐标系 $b_k$ 后，我们只能对与线加速度 $\hat{a}$ 和角速度$\hat{\omega}$ 相关的部分进行预积分，如下所示：
$$
R^{b_k}\omega p^w{b_{k+1}} = R.b
$$

3）误差校准：

如果偏差的估计变化很小，我们通过它们关于偏差的一阶近似值进行调整$\alpha^{b_k}{b{k+1}}$,$\beta^{b_k}{b{k+1}}$, $\gamma^{b_k}{b{k+1}}$，
$$
\alpha^{b_k}{b{k+1}} \simeq \hat{\alpha}^{b_k}{b{k+1}} + J^\alpha_{b_a} \delta b_{a_k} + J^\alpha_{b_w} \delta b_{w_k}
$$

$$
\beta^{b_k}{b{k+1}} \simeq \hat{\beta}^{b_k}{b{k+1}} + J^\beta_{b_a} \delta b_{a_k} + J^\beta_{b_w} \delta b_{w_k}
$$

$$
\gamma^{b_k}{b{k+1}} \simeq \hat{\gamma}^{b_k}{b{k+1}} \otimes
\begin{bmatrix}
1 \ \frac{1}{2}J^\gamma_{b_w} \delta b_{w_k}
\end{bmatrix}
$$

否则，当偏差的估计发生显著变化时，我们会在新的偏差估计下进行重新传播。这种策略为基于优化的算法节省了大量的计算资源，因为我们不需要重复传播 IMU 测量。

V 估计初始化

单目紧耦合VIO是一个高度非线性的系统，因此在一开始就需要一个准确的初始估计。本文则通过松耦合的IMU与积分与基于视觉重建进行对齐。

A 滑动窗口中纯视觉的SFM

初始化过程从纯视觉 SfM 开始，以估计最大比例的相机姿势和特征位置图。
我们在一个滑动窗口中维护几个帧，以实现有限的计算复杂度。首先，我们检查最新帧和所有先前帧之间的特征对应关系。如果我们能在滑动窗口中找到最新帧与任何其他帧之间的稳定特征跟踪（超过 30 个跟踪特征）和足够的视差（超过 20 个像素）。我们使用五点算法 [34] 恢复这两个帧之间的相对旋转和按比例平移。然后，我们任意设置比例并对这两帧中观察到的所有特征进行三角测量。基于这些三角特征，执行透视点（PnP）方法[35]来估计窗口中所有其他帧的姿势。最后，应用全局全束调整 [36] 以最小化所有特征观察的总重投影误差。

由于我们还没有关于世界坐标系的任何知识，我们将第一个相机坐标系 $(·)^{c_0}$ 设置为 SfM 的参考坐标系。

所有帧的位置$(\bar{p}^{c_0}{c_k},q^{c_0}{c_k})$和特征位置都通过与$(·)^{c_0}$。给定相机与IMU之间的外参，我们即可把坐标从相机坐标系转换到体坐标系:
$$
a^{c_0}{b_k} = q^{c_0}{c_k} \otimes(q^b_c)^{-1}\
s \bar{p}^{c_0}{b_k} =s \bar{p}^{c_0}{c_k} - R^{C_0}_{b_K}p^b_c
$$
其中s是位置的尺度参数，将会在接下来的章节中解决。

B 视觉-惯性对齐

视觉惯性对其的示意图如下图所示。主要的目的就是对其带尺度的IMU预积分信息与视觉架构信息。

1）陀螺仪误差校准：考虑窗口中的两个连续帧$b_k$和$b_{k+1}$，我们从视觉SfM得到旋转$q^{c_0}{b_k}$和$q^{c_0}{b_{k+1}}$，以及从IMU预积分得到相对约束$y^{b_k}{b{k+1}}$。我们将 IMU 预积分项相对于陀螺仪偏差线性化，并最小化以下损失函数：
$$
\min_{\delta b_w} \sum_{k\in\mathcal{B}}||{q^{c_0}{b{k+1}}}^{-1}\otimes q^{c_0}{b_k} \otimes \gamma ^{b_k}{b_{k+1}}||^2
$$

$$
\gamma^{b_k}{b{k+1}} \approx \hat{\gamma}^{b_k}{b{k+1}} \otimes
\begin{bmatrix}
1 \ \frac{1}{2}J^\gamma_{b_w} \delta b_{w_k}
\end{bmatrix}
$$

其中，$\mathcal{B}$ 表示窗口中所有的帧数，通过这种方法，我们获得了陀螺仪偏差的初始标定值$b_w$, 之后，我们通过新的陀螺仪误差重新传播了所有的预积分项$\hat{\alpha}^{b_k}{b{k+1}}$， $\hat{\beta}^{b_k}{b{k+1}}$, 以及$\gamma^{b_k}{b{k+1}}$。

2）速度，重力矢量，矩阵尺度因子初始化

陀螺仪偏差初始化后，我们继续初始化导航的其他基本状态，即速度、重力矢量和度量尺度
$$
\chi_I = [v^{b_0}{b_0},v^{b_1}{b_1},v^{b_n}{b_n}, g^{c_0}, s]
$$
其中$v^{b_k}{b_{k}}$是拍摄第k张图像时身体帧中的速度，$g^{c_0}$是$c_0$帧中的重力向量，并将单目SfM缩放为公制单位。

考虑窗口中连续的两个帧$b_k$和$b_{k+1}$，我们有以下等式：
$$
α^{b_k}{b{k+1}}=R^{b_k}{c_0}(s(\bar{p}^{c_0}{b_{k+1}}−\bar{p}^{c_0}{b_k})+\frac{1}{2}g^{c_0}Δt^2_k−R^{c_0}{b_k}v^{b_k}{b_k}Δt_k) \
β^{b_k}{b_{k+1}}=R^{b_k}{c_0}(R^{c_0}{b_{k+1}}v^{b_{k+1}}{b{k+1}}+g^{c_0}Δt_k−R^{c_0}{b_k}v^{b_k}{b_k})
$$
我们可以将(6)和(9)组合成以下线性测量模型：

可以看出，$R^{c_0}{b_k}$、$R^{c_0}{b_{k+1}}$、$\bar{p}^{c_0}{c_k}$、$\bar{p}^{c_0}{c_{k+1}}$是从up-to-scale单目视觉SfM得到的。Δtki是两个连续帧之间的时间间隔。

通过求解这个线性最小二乘问题:
$$
\min_{\chi_I}∑{k∈B}||\hat{z}^{b_k}{b_{k+1}}−H^{b_k}{b{k+1}}\chi_I||^2
$$
我们可以得到窗口中每一帧的体帧速度，视觉参考帧中的重力向量( ·)c0，以及比例参数。

3)尺度优化，从前面的线性初始化步骤获得的重力矢量可以通过限制大小来细化。在大多数情况下，重力矢量的大小是已知的。这导致重力矢量只剩下 2个自由度。因此，我们在其切线空间上使用两个变量来扰动重力，从而保留 2-DOF。我们的重力向量受到$\mathcal{g}(\bar{\hat{\mathcal{g}}}+δg)$的扰动，$δg=w_1b_1+w_2b_2$，其中g是已知的重力大小，$\hat{\bar{g}}$是表示重力方向的单位向量。$b_1$和$b_2$是跨越切平面的两个正交基，如图图 4.$w_1$ 和 $w_2$ 分别是对 $b_1$ 和 $b_2$ 的二维扰动。我们可以任意找到$b_1$和$b_2$的任意集合旋转切线空间。然后，我们将(9)代入$\mathcal{g}(\bar{\hat{\mathcal{g}}}+δg)$，并与其他状态变量一起求解2-Dδg。这个过程反复多次，直到 g 收敛。

4）完成初始化：对重力向量进行细化后，我们可以通过将重力旋转到z轴得到世界坐标系和相机坐标系$c_0$之间的旋转$q^w_{c_0}$。然后我们将所有变量从参考坐标系(·)c0 旋转到世界坐标系(·)w。身体坐标系速度也将旋转到世界坐标系。来自视觉 SfM 的平移分量将被缩放为公制单位。至此，初始化过程完成，所有这些度量值将被馈送到紧密耦合的单目 VIO。

VI 稠密紧耦合的单目VIO

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

A. 形式

滑动窗口中的全状态向量定义为

其中 $x_{k}$ 是捕获第 k 个图像时的 IMU 状态。它包含了IMU在世界坐标系中的位置、速度和方向，以及IMU体坐标系中的加速度偏差和陀螺仪偏差。n是关键帧的总数，m是滑动窗口中的特征总数。λ是第一个特征来自它的第一次观察的逆深度。

我们使用视觉惯性束调整公式。将所有测量残差的先验和马氏范数之和最小化，以获得最大后验估计为

其中 Huber 范数 [37] 定义为：

其中，$r_B(\hat{z}^{b_k}{b{k+1}},\chi)$和$r_C(\hat{z}^{c_j}_l,\chi)$分别是IMU和视觉测量的残差。残差项的详细定义将在第 VI-B 和 VI-C 节中介绍。B 是所有 IMU 测量的集合，C 是在当前滑动窗口中至少观察到两次的特征集合。${r_p,H_p}$是边缘化的先验信息。 Ceres 求解器 [38] 用于解决这个非线性问题。

huber损失函数可以降低误匹配的影响；

B. IMU测量残差

考虑滑动窗口中两个连续帧 $b_k$ 和 $b_{k+1}$ 内的 IMU 测量值，预积分 IMU 测量值的残差可以定义为:

其中$[·]{xyz}$提取四元数q的向量部分以表示错误状态。$δθ^{b_k}{b_{k+1}}$是四元数的3-D错误状态表示。$[\hat{α}^{b_k}{b{k+1}},\hat{β}^{b_k}{b{k+1}},\hat{γ}^{b_k}{b{k+1}}$是预积分IMU 两个连续图像帧之间的测量项。加速度计和陀螺仪偏差也包含在在线校正的残差项中。

零偏在质量不好的IMU下，变化比较快

C. 视觉测量残差

与在广义图像平面上定义重投影误差的传统针孔相机模型相比，我们在单位球面上定义相机测量残差。几乎所有类型相机的光学器件，包括广角相机、鱼眼相机或全向相机，都可以建模为连接单位球体表面的单位射线。

考虑在第 i 个图像中首先观察到的第 l 个特征，在第 j 个图像中的特征观察的残差定义为：

其中$[\hat{u}^{c_i}_l,\hat{v}^{c_i}_l]$是第l个特征发生在第i个图像中的第$l$个观测值。$[\hat{u}^{c_j}_l,\hat{v}^{c_j}_l]$是第j个图像中相同特征的观测值。$π^{−1}_c$是反投影函数，它将像素位置转换为使用相机内在参数的单位向量。由于视觉残差的自由度为 2，我们将残差向量投影到切平面上。$b_1$ 和 $b_2$ 是两个任意选择的正交基，它们跨越$\hat{\bar{\mathcal{P}}}^{c_j}_l$ 的切平面，如图 6 所示。方差 $P^{c_j}_l$，如 (14) 中使用的 , 也从像素坐标传播到单位球体上。

D. 边缘化

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

如图7所示，当第二个最新帧是关键帧时，它将留在窗口中，最旧的帧被边缘化掉及其相应的测量值。否则，如果第二个最新帧是非关键帧，我们会抛出视觉测量并保留连接到该非关键帧的 IMU 测量。为了保持系统的稀疏性，我们不会边缘化所有非关键帧的测量。我们的边缘化方案旨在将空间分离的关键帧保留在窗口中。这确保了特征三角测量有足够的视差，并最大限度地提高了在大激励下保持加速度计测量的概率。边缘化是使用 Schur 补码 [39] 进行的。我们基于与移除状态相关的所有边缘化测量构建一个新的先验。新的先验被添加到现有的先验上。我们注意到边缘化导致线性化点的早期修复，这可能导致次优估计结果。但是，由于 VIO 可以接受小的漂移，因此认为边缘化造成的负面影响并不重要。

E. Camera-Rate State 估计的仅运动视觉惯性优化

对于手机等计算能力较低的设备，由于非线性优化的大量计算需求，紧耦合的单目 VIO 无法实现相机级输出。为此，除了完全优化之外，我们还采用了轻量级的仅运动视觉惯性优化来将状态估计提升到相机速率（30 Hz）。仅运动视觉惯性优化的成本函数是与 (14) 中的单眼 VIO 相同。

核心思想：然而，我们并没有优化滑动窗口中的所有状态，而是只优化固定数量的最新 IMU 状态的位姿和速度。我们将特征深度、外部参数、偏差和我们不想优化的旧 IMU 状态视为恒定值。

我们确实使用所有视觉和惯性测量来进行仅运动优化。这导致比单帧 PnP 方法更平滑的状态估计。所提出策略的说明如图 8 所示。与在最先进的嵌入式计算机上可能导致超过 50 ms 的完全紧密耦合的单目 VIO 相比，仅运动视觉惯性优化只需要大约 5 毫秒的计算时间。这实现了低延迟相机速率姿态估计，这对无人机和 AR 应用特别有益。

通过限制优化规模，固定某些优化项以提高速率；

F. 用于 IMU 速率状态估计的 IMU 前向传播

IMU 测量的速率远高于视觉测量。尽管我们的 VIO 的频率受到图像捕获频率的限制，但我们仍然可以使用最近的 IMU 测量直接传播最新的 VIO 估计，使性能达到 IMU速率。高频状态估计可以用作闭环闭合的状态反馈。使用该 IMU 速率状态估计的自主飞行实验在第 IX-C 节中介绍。

VII 重定位

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

A. 回环检测

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

B. 特征检索

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

1) 2-D–2-D：使用 RANSAC [33] 进行的基本矩阵测试。我们使用当前图像和闭环候选图像中检索到的特征的 2-D 观察来执行基本矩阵测试。
2) 3 -D–2-D：使用 RANSAC [35] 进行的 PnP 测试。基于已知的特征在局部滑动窗口中的 3-D 位置和闭环候选图像中的 2-D 观察结果，我们执行 PnP 测试。

在异常值拒绝后，我们将此候选作为正确的回环检测并执行重新定位。

C. 紧耦合的重定位

重定位过程有效地将当前滑动窗口与过去的姿势对齐。在重定位期间，我们将所有闭环帧的姿态视为常数。我们使用所有 IMU 测量、局部视觉测量测量和检索到的特征对应来共同优化滑动窗口。我们可以很容易地为闭环框架观察到的检索特征写出与 VIO 中的视觉测量相同的模型，如（17）。唯一的区别是闭环帧的位姿 $(\hat{q}^w_v,\hat{p}^w_v)$，取自位姿图（参见第 VIII 节），或直接取自过去的里程计输出（如果这是第一次重新定位），被视为一个常数。为此，我们可以用额外的循环项稍微修改（14）中的非线性成本函数

其中$\mathcal{L}$是在闭环帧中检索到的特征的观察集合。$(l，v)$表示在闭环帧v中观察到的第一个特征。

请注意，尽管成本函数与（14）略有不同，但要解决的状态的维数保持不变，因为闭环帧的位姿被认为是常数。

当使用当前滑动窗口建立多个闭环时，我们同时使用来自所有帧的所有闭环特征对应关系进行优化。这为重定位提供了多视图约束，从而获得更高的精度和更好的平滑度。重新定位后保持一致性的全局优化将在第 VIII 节中讨论。

VIII全局图优化与地图重用

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

A. 四个累积漂移方向

得益于重力的惯性测量，在 VINS 中可以完全观察到滚动角和俯仰角。如图 11 所示，随着物体的移动，3-D位置和旋转相对于参考系发生了相对变化。然而，我们可以通过重力矢量来确定水平面，这意味着我们一直在观察绝对横滚和俯仰角。因此，滚动和俯仰区域是世界坐标系中的绝对状态，而 x、y、z 和偏航是相对于参考坐标系的相对估计。累积漂移仅发生在 x、y、z 和偏航角。为了充分利用有效信息并有效地纠正漂移，我们修复了无漂移横滚和俯仰，并且仅在 4-DOF 中进行位姿图优化。

涉及到vins的能观性：Kelly[6] ， Li[5]分析了Vins有四个自由度是不可观的：1）绝对的位置position; 2) 重力方向绝对的yaw角. 我们目前估计的位置和yaw都是相对于参考系来说的.

B. 将关键帧添加到姿态图中

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

1）序列边：一个关键帧与其先前的关键帧建立了几个序列边。序列边表示两个关键帧之间的相对变换，直接取自 VIO。考虑到关键帧$i$和它之前的关键帧$j$，顺序边只包含相对位置$\hat{p}^i_{i,j}$和偏航角$\hat{\phi}_{ij}$

2) 闭环边：如果关键帧有回环连接，它通过位姿图中的闭环边连接闭环框架。类似地，闭环边缘仅包含一个 4-DOF 相对位姿变换，其定义与 (19) 相同。使用重定位的结果获得闭环边缘的值。

C. 4-DOF 位姿图优化

定义第i帧和第j帧之间边缘的残差最小为

其中$\hat{\phi}_i$和$\hat{θ}_i$是从单目 VIO 获得的滚动角和俯仰角的固定估计值。

通过最小化以下成本函数来优化序列边和闭环边的整个图：

其中 $\mathcal{S}$ 是所有连续边的集合，$\mathcal{L}$ 是所有闭环边的集合。尽管紧耦合的重定位已经有助于消除错误的闭环，但我们添加了另一个 Huber normρ(·) 以进一步减少任何可能的错误循环的影响。相反，我们没有对序列边使用任何鲁棒的正则项，因为这些边是从 VIO 中提取的，它已经包含足够的异常值拒绝机制。

位姿图优化和重定位（参见第 VII-C 节）在两个单独的线程中异步运行。这使得无论何时可用，都可以立即使用最优化的位姿图进行重新定位。类似地，即使当前的位姿图优化尚未完成，仍然可以使用现有的位姿图配置进行重新定位。该过程如图 9(b) 所示。

D. 位姿图合并

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

E. 姿态图保存

我们的位姿图的结构非常简单。我们只需要保存顶点和边，以及每个关键帧（顶点）的描述符。丢弃原始图像以减少内存消耗。具体来说，我们为关键帧保存的状态是

其中i是帧索引，$\hat{p}^w_i$和$\hat{q}^w_i$分别是来自VIO的位置和方向。如果这个框架有一个闭环框架，相对于闭环框架的索引。$\hat{p}^i_{iv}$和$\hat{ψ}_{iv}$是这两个框架之间的相对位置和偏航角，它是通过重定位获得的。$D(u, v,des)$是特征集。每个特征都包含二维位置及其简要描述符。

F. 姿态图加载

我们使用相同的保存格式来加载关键帧。每个关键帧都是位姿图中的一个顶点。顶点的初始位姿是$\hat{p}^w_i$和$\hat{q}^w_i$。循环边缘由回环信息$\hat{p}^i_{iv},\hat{\psi}^i_{iv}$直接建立。每个关键帧与它的相邻关键帧建立几个连续的边缘，如（19）。加载位姿图后，我们立即执行全局 4-DOF 位姿图。位姿图保存和加载的速度与位姿图的大小呈线性相关。

IX 实验结果

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

A 公开数据集

B 现实场景实验

C 应用

X 结论与未来工作

在本文中，我们提出了一种强大且通用的单目视觉惯性估计器。我们的方法在 IMU 预集成、估计初始化、在线外部校准、紧耦合 VIO、重定位和高效全局优化方面具有最先进和新颖的解决方案。我们通过与其他最先进的开源实现进行比较来展示卓越的性能。为了社区的利益，我们开源了 PC 和 iOS 实现。虽然基于特征的 VINS 估计器已经达到了实际部署的成熟度，但我们仍然有未来研究的方向。根据运动和环境的不同，单眼 VINS 可能会达到微弱可观察甚至退化的条件。我们对评估单目VINS 的可观测性特性的在线方法以及在线生成运动计划以恢复可观测性感兴趣。

另一个研究方向涉及单目 VINS 在各种消费设备（如 Android 手机）上的大规模部署。此应用程序需要对几乎所有传感器的内在和外在参数进行在线校准，以及校准质量的在线识别。最后，我们有兴趣在给定单目 VINS 结果的情况下生成密集地图。我们在无人机导航中应用的单目视觉惯性密集映射的第一个结果在 [47] 中提出。但是，仍然需要进行广泛的研究以进一步提高系统的准确性和鲁棒性。

附录基于四元数的IMU预积分

代码框架

1. 前端

cv::goodfeaturesToTrack检测Harris角点

cv::CalcOpticalFlowPyrLK跟踪相邻帧的角点

cv::findFundamentalMat 去除异常点

统一的球面模型

2. VINS主估计器

2. 后端

将所有路标点边缘化掉可以得到一个共视矩阵；

路标点的参数化形式；

数据关联代表的特征点之间的匹配啥的；

工程化的角度的看，VIO中好一点的还是VINS，

IMU预积分的P V Q来自于IMU的预积分，先得到初始值，然后通过联合优化进行进一步的优化；

VINS没有处理过纯旋转问题，可以检测一下纯相信IMU；求单应性矩阵的问题；

VINS相对于OKVIS 效率要高一些；

像素矩阵的2*2矩阵通过标定值得到协方差

边缘化后，fill in 与信息损失怎么平衡？

精度提高：

特征关联，光流

传感器，时间戳同步，少一点模糊

安卓的rolling shutter不准；运动模糊比较重，安卓运算能力有限；

bias与噪声的话 bias影响更大一些，噪声给个先验值得话其实差不多；

双目VIO对初始化得要求高吗？

VINS的共视非常稠密，因此边缘化对滑窗内所有帧都有影响；

IMU误差的雅可比矩阵与IMU传播的雅可比

IMU传播的雅可比是关于bias的一个雅克比矩阵；

3. 初始化

VINS的初始化过程

先做一个纯视觉的SfM，计算初始位置，然后通过三角化恢复出初始位姿

IMU与单目要进行对齐，在滑动窗口计算出初始值（尺度，速度，位移，零偏，外参）

4. 回环检测

关键帧的选择

像素差（归一化像素平面的像素差）

需要保证两帧间的视差，避免纯旋转；

关键帧需要回环检测，四自由度的位姿图优化

5. 后端优化

四自由度的因子图优化；

简介

论文

摘要