Visual-Inertial Monocular SLAM W

作者: 王东晓_4db3 | 来源:发表于2020-05-11 20:08 被阅读0次

Visual-Inertial Monocular SLAM With Map Reuse总结

王东晓 1900880

摘要

本文针对现在很多视惯融合的系统没有闭环检测导致位姿估计具有很大的漂移的问题，提出了基于视觉和惯导紧耦合的同时定位和建图系统，这个系统不仅有闭环检测，而且可以在已经建图的区域实现无偏的位姿估计。针对于单目具有尺度不确定性的问题，本文提出了新颖的IMU初始化的方法来获得场景的尺度和重力的方向。该系统在micro-aerial数据集上的十一个序列进行了测试，得到了小于尺度1%的尺度漂移和厘米级的定位精度。通过这篇论文的学习和分析，知道了如何搭建紧耦合的视惯SLAM系统；学习了一种IMU初始化的方法；为进一步提升ORB-SLAM2的定位和建图精度提供了思路和方法。

1. 系统简介

本文提出了一种紧耦合VI-SLAM，它具有回环检测，地图重用等特性。由于和ORB-SLAM^[1]^[2]具有血缘关系，因此以下称之为VI-ORB-SLAM简称为VI-ORB。同时VI-ORB系统具有新颖的初始化方法，来计算尺度、重力方向、速度、加速度计和陀螺仪的偏置（下称biases）。

<center> 图1. 在EuRoC[8]数据集的V1_02_medium上VI-ORB-SLAM估计出的地图。顶视是使用估计出来的重力方向，绿色线连接着共视100个点以上的关键帧，这证明系统重用地图的能力，使得当重新观测相同地方时具有零漂移定位能力。</center>

2. 算法解析

2.1 视觉和惯导融合基础

2.1.1 视觉信息

对于视觉信息流，即是图像帧，使用小孔相机模型，相机投影函数 $\pi : \mathbb R ^3 \to \Omega$ ，它将相机坐标系 $\mathrm C$ 下的3D点 $\mathbf {X _C} \in \mathbb R^3$ 变换到图像平面的2D点 $\mathbf {x_c} \in \Omega \subset \mathbb R^2$ ，如公式（1）所示：
$\pi\left(\mathbf{X}_{\mathrm{C}}\right)=\left[\begin{array}{c} {f_{u} \frac{X_{\mathrm{C}}}{Z_{\mathrm{C}}}+c_{u}} \\ {f_{v} \frac{Y_{\mathrm{C}}}{Z_{\mathrm{C}}}+c_{v}} \end{array}\right], \quad \mathbf{X}_{\mathrm{C}}=\left[X_{\mathrm{C}} Y_{\mathrm{C}} Z_{\mathrm{C}}\right]^{T} \tag{1}$
其中 $[f_u \ f_v]^T$ 是焦距长度， $[c_u \ c_v]^T$ 是光心点。这里没有考虑相机畸变，我们先提取特征点再求其去畸变后的坐标。

2.1.2 惯导信息

这部分参考系使用 $\mathrm B$ 表示，惯导信息流是时间间隔为 $\Delta t$ 的加速度测量值 $\mathbf {a_B}$ 和角速度测量值 $\boldsymbol{\omega_\mathrm B}$ 。这些测量值受噪声和biases影响，包括加速度计偏置 $\mathbf b_a$ 和角速度偏置 $\mathbf b_g$ 。世界坐标系 $\mathrm W$ 下IMU的角度 $\mathbf{R}_{\mathrm{WB}} \in \mathrm{SO}(3)$ ，位置 $\mathrm{_Wp_B}$ 和速度 $\mathrm{_W}\mathbf v_\mathrm B$ ，需要注意的是加速度计需要去除重力 $\mathbf g_\mathrm w$ 的影响，计算公式如公式（2）所示^[3]：
$\begin{aligned} \mathbf{R}_{\mathrm{WB}}^{k+1} &=\mathbf{R}_{\mathrm{WB}}^{k} \operatorname{Exp}\left(\left(\boldsymbol{\omega}_{\mathrm{B}}^{k}-\boldsymbol{b}_{g}^{k}\right) \Delta t\right) \\ _\mathrm{W} \mathbf{v}_{\mathrm{B}}^{k+1} &=_{\mathrm{W}} \mathbf{v}_{\mathrm{B}}^{k}+\mathbf{g}_{\mathrm{W}} \Delta t+\mathbf{R}_{\mathrm{WB}}^{k}\left(\boldsymbol{a}_{\mathrm{B}}^{k}-\boldsymbol{b}_{a}^{k}\right) \Delta t \\ _\mathrm{W} \mathbf{p}_{\mathrm{B}}^{k+1} &=_\mathrm{W} \mathbf{p}_{\mathrm{B}}^{k}+_{\mathrm{W}} \mathbf{v}_{\mathrm{B}}^{k} \Delta t+\frac{1}{2} \mathbf{g}_{\mathrm{W}} \Delta t^{2}+\frac{1}{2} \mathbf{R}_{\mathrm{WB}}^{k}\left(\boldsymbol{a}_{\mathrm{B}}^{k}-\boldsymbol{b}_{a}^{k}\right) \Delta t^{2} \end{aligned}\tag{2}$
连续关键帧之间的预积分项使用文章^[4]中算法来计算 $\Delta \mathbf R$ ， $\Delta \mathbf v$ 和 $\Delta \mathbf p$ ，公式表示为：
$\begin{aligned} \mathbf{R}_{\mathrm{WB}}^{i+1}=& \mathbf{R}_{\mathrm{WB}}^{i} \Delta \mathbf{R}_{i, i+1} \operatorname{Exp}\left(\left(\mathbf{J}_{\Delta R}^{g} \mathbf{b}_{g}^{i}\right)\right) \\ _\mathrm{W} \mathbf{v}_{\mathrm{B}}^{i+1}=&_{\mathrm{W}} \mathbf{v}_{\mathrm{B}}^{i}+\mathbf{g}_{\mathrm{w}} \Delta t_{i, i+1} \\ &+\mathbf{R}_{\mathrm{WB}}^{i}\left(\Delta \mathbf{v}_{i, i+1}+\mathbf{J}_{\Delta_{v}}^{g} \mathbf{b}_{g}^{i}+\mathbf{J}_{\Delta v}^{a} \mathbf{b}_{a}^{i}\right) \\ _\mathrm{W} \mathbf{p}_{\mathrm{B}}^{i+1}=&_{\mathrm{W}} \mathbf{p}_{\mathrm{B}}^{i}+_{\mathrm{W}} \mathbf{v}_{\mathrm{B}}^{i} \Delta t_{i, i+1}+\frac{1}{2} \mathbf{g}_{\mathrm{w}} \Delta t_{i, i+1}^{2} \\ &+\mathbf{R}_{\mathrm{WB}}^{i}\left(\Delta \mathbf{p}_{i, i+1}+\mathbf{J}_{\Delta p}^{g} \mathbf{b}_{g}^{i}+\mathbf{J}_{\Delta p}^{a} \mathbf{b}_{a}^{i}\right) \end{aligned} \tag{3}$
这里雅可比 $\mathbf J^a_{(\cdot)}$ 和 $\mathbf J^g_{(\cdot)}$ 是当biases改变时对应项的一阶近似，这样不需要重新计算预积分。

相机和IMU参考系之间的变换关系为 $\mathbf{T}_{\mathrm{CB}}=\left[\mathbf{R}_{\mathrm{CB}} | _\mathrm{C}\mathbf P _\mathrm{B} \right]$ ，假设这个值通过标定可以事先知道。

2.2 视觉和惯导融合的ORB-SLAM

这部分详细介绍了跟踪、局部地图和回环检测三大线程的改变。

2.2.1 跟踪

跟踪部分负责以图像帧率跟踪传感器位姿、速度和IMU的biases，这里替代原系统使用固定运动模型的方式，可以预测更加可靠的位姿。然后通过投影匹配地图点，进行最小重投影误差和IMU误差的优化。

<center>图2. 跟踪线程的优化示意图。a）跟踪帧 $j$ （地图改变），b）先验（优化结果），c）跟踪帧 $j+1$ （地图未改变），d）先验（边缘化），e）跟踪 $j+2$ （地图未改变），f）先验（边缘化）</center>

优化过程根据地图是否被局部建图线程或闭环线程所更新分为两种情况。

第一种如图2中a）所示，当地图改变或更新之后开始优化帧 $j$ ，它和最新关键帧 $i$ 之间通过IMU约束关联。优化的状态和公式如公式（4）所示。
$\begin{aligned} \theta&=\left\{ {\mathbf R}_{\mathrm {WB}}^j,_{\mathrm W}{\mathbf p}_\mathrm B^{j},_{\mathrm W}{\mathbf v}_\mathrm B^{j},{\mathbf b}_{g}^j,{\mathbf b}_a^j \right\} \\ \theta^*&=\arg\min\limits_{\theta}\left(\sum_k{\mathbf E}_{\mathrm {proj}}(k,j)+\mathbf E_{\mathrm {IMU}}(i,j )\right) \end{aligned} \tag 4$
这里特征点匹配对 $k$ 的重投影误差 $\mathbf E_\mathrm {proj}$ 定义为：
$\begin{aligned} \mathbf{E}_{\mathrm {proj }}(k, j) &=\rho\left(\left(\mathbf{x}^{k}-\pi\left(\mathbf{X}_{\mathrm{C}}^{k}\right)\right)^{T} \mathbf{\Sigma}_{k}\left(\mathbf{x}^{k}-\pi\left(\mathbf{X}_{\mathrm{C}}^{k}\right)\right)\right) \\ \mathbf{X}_{\mathrm{C}}^{k} &=\mathbf{R}_{\mathrm{CB}} \mathbf{R}_{\mathrm{BW}}^{j}\left(\mathbf{X}_{\mathrm{W}}^{k}-_\mathbf{W} \mathbf{p}_{\mathrm{B}}^{j}\right)+_\mathrm{C}\mathbf{p}_{\mathrm{B}} \end{aligned} \tag {5}$
其中 $\mathrm x^k$ 为图像上的特征点位置， $\mathbf X_\mathrm W^k$ 表示世界坐标系下的地图点， $\boldsymbol {\Sigma_k}$ 表示和特征点尺度相关的信息矩阵。

IMU误差项 $\mathbf E_{\mathrm {IMU}}$ 定义为：
$\begin{aligned} \mathbf{E}_{\mathrm{IMU}}(i, j)=& \rho\left(\left[\mathbf{e}_{R}^{T} \mathbf{e}_{v}^{T} \mathbf{e}_{p}^{T}\right] \boldsymbol{\Sigma}_{I}\left[\mathbf{e}_{R}^{T} \mathbf{e}_{v}^{T} \mathbf{e}_{p}^{T}\right]^{T}\right)+\rho\left(\mathbf{e}_{b}^{T} \mathbf{\Sigma}_{R} \mathbf{e}_{b}\right) \\ \mathbf{e}_{R}=& \log \left(\left(\Delta \mathbf{R}_{i j} \operatorname{Exp}\left(\mathbf{J}_{\Delta R}^{g} \mathbf{b}_{g}^{j}\right)\right)^{T} \mathbf{R}_{\mathrm{BW}}^{i} \mathbf{R}_{\mathrm{WB}}^{j}\right) \\ \mathbf{e}_{v}=& \mathbf{R}_{\mathrm{BW}}^{i}\left(_\mathrm W \mathbf{v}_{\mathrm{B}}^{j}-_{\mathrm{W}} \mathbf{v}_{\mathrm{B}}^{i}-\mathbf{g}_{\mathrm{W}} \Delta t_{i j}\right) \\ &-\left(\Delta {\mathbf v}_{ij} + {\mathbf J}_{\Delta v}^ g{\mathbf b}_ g^j+{\mathbf J}_{\Delta v}^a{\mathbf b}_ a^j\right) \\ \mathbf{e}_{p}=& \mathbf{R}_{\mathrm{BW}}^{i}\left(_\mathrm W \mathbf{p}_{\mathrm{B}}^{j}-_\mathbf{W} \mathbf{p}_{\mathrm{B}}^{i}-_\mathrm W \mathbf{v}_{\mathrm{B}}^{i} {\Delta t}_{i j} -\frac{1}{2} \mathbf g _\mathrm W \Delta t_{ij}^2\right) \\ & -\left(\Delta {\mathbf p}_{ij}+{\mathbf J}_{\Delta p}^g{\mathbf b} _g^j+{\mathbf J}_{\Delta p}^a {\mathbf b} _a^j\right) \\ \mathbf{e}_{b}=& \mathbf{b}^{j}-\mathbf{b}^{i} \end{aligned} \tag{6}$
这里 $\boldsymbol\Sigma_I$ 是预积分得到的信息矩阵， $\boldsymbol\Sigma_R$ 是bias的随机游走^[3]， $\rho$ 是Huber鲁棒核函数。

在使用高斯牛顿法算法进行优化后，会对帧 $j$ 产生约束如图2中b）所示，将得到的估计结果和海塞矩阵作为下一次优化的先验。

第二种如图2中c）所示，此时地图没有改变，因此下一帧 $j+1$ 将会继续加入优化。但第 $j$ 帧无法保证被优化的足够准确，此时加入IMU约束会对 $j+1$ 产生错误的约束，因此需要将第 $j$ 帧一同加入到优化中，上一次的优化结果作为先验加入优化中。优化的状态和公式如公式（7）所示。
$\begin{aligned} \theta&=\left\{ {\mathbf R}_{\mathrm {WB}}^j,{\mathbf p}_{\mathrm W}^j, {\mathbf v}_{\mathrm W}^ j,{\mathbf b}_ g^j,{\mathbf b}_a^j,{\mathbf R}_{\mathrm {WB}}^{j+1},{\mathbf p}_{\mathrm W}^ {j+1},{\mathbf v}_{\mathrm W}^{j+1},{\mathbf b}_ g^{j+1},{\mathbf b}_a^{j+1}\right\} \\ \theta^*&=\arg\min\limits_{\theta}\left(\sum_k{\mathbf E}_{\mathrm {proj}}( k,j+1)+{\mathbf E}_{\mathrm {IMU}}( j, j+1) + {\mathbf E}_{\mathrm {prior}}(j)\right) \end{aligned} \tag{7}$
这里 ${\mathbf E}_{\mathrm {prior}}$ 为先验项：
$\begin{aligned} {\mathbf E}_ {\mathrm {prior}}(j)&=\rho\left( [{\mathbf e}_R^T{\mathbf e}_v^T{\mathbf e}_p^T{\mathbf e}_b^T]\Sigma_ p[{\mathbf e}_R^T{\mathbf e}_v^T{\mathbf e}_p^T{\mathbf e}_b^T]^T\right) \\ {\mathbf e}_ R&={\mathrm {Log}}( \overline{\mathbf R}_{\mathrm {BW}}^j{\mathbf R}_{\mathrm {WB}}^ j) \quad \mathbf e_ v=_{\mathrm W}\overline {\mathbf v}_{\mathrm B}^j- _{\mathrm W} {\mathbf v}_{\mathrm B}^j \\ {\mathbf e}_ p&=_{\mathrm W}\overline{\mathbf p}_{\mathrm B}^j-_{\mathrm W}{\mathbf p}_{\mathrm B}^ j \quad {\mathbf e}_b=\overline{\mathbf b}^ j-{\mathbf b}^ j \end{aligned} \tag{8}$
这里 $(\overline \cdot)$ 和 $\boldsymbol \Sigma_p$ 是上一次优化得到的状态和海塞矩阵，即图2中b）所示的。这次优化完成后，边缘化^[5]掉第 $j$ 帧得到先验，即图2中d）。这样当下一帧 $j+2$ 到来，地图未发生更新，就重复两帧的优化以及边缘化过程，如图2中e）和f）所示。一旦地图被更新之后，会导致之前的先验信息无法提供正确的约束，因此会重新变成与上一关键帧的约束即图2中a）所示。

2.2.2 局部建图

这一部分在插入新的关键帧后执行局部BA（Bundle Adjustment）优化。优化最新的 $N$ 个关键帧构成的局部窗口以及它们观测到的所有点。和局部窗口中的关键帧具有共视关系（共视图中相连）却不在局部窗口的关键帧构成固定窗口，贡献总的残差，但状态固定不被优化。第 $N+1$ 个最新的关键帧总是包含在固定窗口中来提供IMU约束。图3表明了原始的ORB-SLAM和VI-ORB-SLAM在局部BA中的线程中的区别。误差函数结合了重投影误差和IMU误差项如公式（5）和（6）所示。

<center>图3. 原版ORB-SLAM（上方）和本文VI-ORB-SLAM（下方）的局部BA对比。VI-ORB-SLAM的局部窗口提取的是按照时间顺序的关键帧，而ORB-SLAM中是通过共视图提取的。</center>

需要注意的是IMU的引入，导致每个关键帧增加了9个优化的变量，因此也变得更加复杂，需要选择合适的窗口大小来保证实时性。

局部地图同时也负责关键帧的管理。原版ORB-SLAM会有丢弃不好关键帧的策略，保证地图的大小合适，但在VI-ORB-SLAM中将会带来问题。因为IMU约束的积分时间越长，误差越大。因此我们需要设置以下策略来约束关键帧的管理：

局部BA窗口中的两个连续关键帧之间的时间不能大于0.5s
闭环后执行的full-BA中连续两个关键帧之间时间不能大于3s

2.2.3 环路闭合

闭环线程的目的是当回到建过地图的位置时，减少运行中的累计漂移。方法为利用地点重识别模块检测和之前的关键帧进行匹配，然后计算二者间刚体变换^[6]，最后进行一个位姿图优化。

由于尺度可观，这里的刚体变换不是使用原来的7DoF，而是6DoF的。位姿图优化不对速度和IMU的biases进行优化。这显然不是最优的，而bias和速度是局部准确的，可以在之后继续使用IMU的信息。然后会在并行线程进行full-BA，来优化包括bias和速度的所有状态。

2.3 IMU初始化

这部分主要在已知一系列ORB-SLAM关键帧后，计算视觉和惯导相融合的full-BA所需要估计的尺度、重力方向、速度和IMU biases的初始估计。思想就是让ORB-SLAM运行一段时间使得各个状态都变得可观。这种初始化方法适用于任何其它的SLAM系统，只需要保证两个连续的关键帧时间上接近即可。

初始化使用分而治之思想，分为以下子问题：

(1) IMU陀螺仪的bias估计

(2) 不考虑加速度计的bias，尺度和重力近似估计

(3) 加速度计bias估计，尺度和重力方向求精

(4) 速度估计

2.3.1 陀螺仪bias估计

陀螺仪bias可以从已知的两个连续关键帧间旋转估计出来。假设陀螺仪的bias变化很小，视为恒定。通过最小化陀螺仪积分和ORB-SLAM求出的相对角度差异求出。对于连续关键帧有：
$\underset{\mathrm{b}_{s}}{\operatorname{argmin}} \sum_{i=1}^{N-1}\left\|\operatorname{Log} \left(\left(\Delta \mathbf{R}_{i, i+1} \operatorname{Exp}\left(\mathbf{J}_{\Delta R}^{g} \mathbf{b}_{g}\right)\right)^{T} \mathbf{R}_{\mathrm{BW}}^{i+1} \mathbf{R}_{\mathrm{WB}}^{i}\right)\right\|^{2} \tag{9}$
这里 $N$ 是关键帧数目， $\mathbf{R}_\mathrm{WB}^{(\cdot)} = \mathbf{R}_\mathrm{WC}^{(\cdot)}\mathbf{R}_\mathrm{CB}$ 是由ORB-SLAM计算出来的 $\mathbf{R}_\mathrm{WC}^{(\cdot)}$ 和标定的 $\mathbf{R}_\mathrm{CB}$ 计算得到， $\Delta \mathbf{R}_{i, i+1}$ 是两个连续关键帧之间的陀螺仪积分得到。

2.3.2 尺度和重力估计（未考虑加速度计bias）

ORB-SLAM计算得到的尺度是任意的，因此从相机系变换到IMU坐标系时需要考虑一个尺度因子 $s$ :
$_{\mathrm W}{\mathbf p}_{\mathrm B}= s_{\mathrm W} {\mathbf p}_{\mathrm C} +{\mathbf R}_{\mathrm {WC}} \ _{\mathrm C}{\mathbf p}_{\mathrm B} \tag{10}$
将公式（10）带入公式（3）中两个连续关键帧之间的相对位置方程，忽略加速度计的bias，得到：
$s\ _{\mathrm W}{\mathbf p}_{\mathrm C}^ {i+1}= s \ _{\mathrm W}{\mathbf p}_{\mathrm C}^i + _{\mathrm W}{\mathbf v}_{\mathrm B}^i \Delta t_{i,i+1}+\frac12 \mathbf g_W\Delta t_{i,i+1}^2+{\mathbf R}_{\mathrm {WB}}^i \Delta {\mathbf p}_{ i,i+1}+({\mathbf R}_{\mathrm {WC}}^i-{\mathbf R}_{\mathrm {WC}}^{i+1}) _{\mathrm C}{\mathbf p}_\mathrm B \tag{11}$
通过这个线性方程求解的目的是得到尺度 $s$ 和 $\mathbf g_\mathrm W$ ，为了减小复杂度避免求解 $N$ 个关键帧的速度，考虑连续三个关键帧如公式（11）两两关系和公式（3）中速度关系得到：
$\begin{bmatrix}\boldsymbol\lambda( i) \ \boldsymbol\beta( i)\end{bmatrix} {\begin{bmatrix} s\\\mathbf g_\mathrm w\end{bmatrix}}=\boldsymbol\gamma(i) \tag {12}$
为了符号清晰，这里我们把 $i,i+1,i+2$ 三个关键帧使用1，2，3代表，有：
$\begin{aligned} \boldsymbol\lambda(i)=&\left(_\mathrm{W} \mathbf p_{\mathrm{C}}^{2}-_\mathrm{W} \mathbf{p}_{\mathrm{C}}^{1}\right) \Delta t_{23}-\left(_\mathrm{W} \mathbf{P}_{\mathrm{C}}^{3}-_\mathrm{W} \mathbf{P}_{\mathrm{C}}^{2}\right) \Delta t_{12} \\ \boldsymbol{\beta}(i)=& \frac{1}{2} \mathbf{I}_{3 \times 3}\left(\Delta t_{12}^{2} \Delta t_{23}+\Delta t_{23}^{2} \Delta t_{12}\right) \\ \boldsymbol\gamma(i)=&\left(\mathbf{R}_{\mathrm{WC}}^{2}-\mathbf{R}_{\mathrm{WC}}^{1}\right) {_{\mathrm{C}}} \mathbf p _ \mathrm B \Delta t_{23}-\left(\mathbf{R}_{\mathrm{WC}}^{3}-\mathbf{R}_{\mathrm{WC}}^{2}\right){_{\mathrm{C}}} \mathbf p _ \mathrm B \Delta t_{12} \\ &+\mathbf{R}_{\mathrm{WB}}^{2} \Delta \mathbf{p}_{23} \Delta t_{12}+\mathbf{R}_{\mathrm{WB}}^{1} \Delta \mathbf{v}_{12} \Delta t_{12} \Delta t_{23} \\ &-\mathbf{R}_{\mathrm{WB}}^{1} \Delta \mathbf{p}_{12} \Delta t_{23} \end{aligned} \tag{13}$
我们将所有的三个连续关键帧得到的关系写成 $\mathbf A_{3(N-2)\times4}\mathbf x_{4\times1} = \mathbf B _{3(N-2)\times1}$ 形式，通过SVD分解求得尺度因子 $s^*$ 和重力向量 $\mathbf g_\mathrm W^*$ 。因为我们有 $3(N-2)$ 个方程和4个未知数，因此需要至少4个关键帧。

2.3.3 加速度计bias估计，尺度和重力方向求精

到目前为止还未考虑加速度计的bias影响，加速度计bias很容易淹没在重力加速度中^[7]，因而在公式（12）中增加加速度计bias增加了系统病态的几率。为增加可观性，引入重力模长为 $G$ 的约束。使用重力方向为 $\hat{\mathbf g}_\mathrm I = \left\{0,0,-1 \right\}$ 惯性参考系 $I$ 和计算出来的重力方向 $\hat {\mathbf g }_\mathrm W = {\mathbf g }_\mathrm W^* / \|{\mathbf g }_\mathrm W^*\|$ ，可以计算出来 $\mathbf R_\mathrm {WI}$ 如下：
$\begin{aligned} {\mathbf R}_{\mathrm {WI}}&=\operatorname{Exp}(\hat {\mathbf v}\theta) \\ \hat{\mathbf v}&=\frac{\hat{\mathbf g}_\mathrm I\times\hat{\mathbf g}_\mathrm W}{||\hat{\mathbf g}_\mathrm I\times\hat{\mathbf g}_\mathrm W||} , \quad \theta=\operatorname{atan2}(||\hat{\mathbf g}_\mathrm I\times\hat{\mathbf g}_\mathrm W||,\hat{\mathbf g}_\mathrm I\cdot \hat{\mathbf g}_\mathrm W) \end{aligned}\tag {14}$
进而重力向量为
$\mathbf g_\mathrm W = \mathbf R _ \mathrm{WI} \hat{\mathbf g}_{\mathrm I}G \tag{15}$
这里 $\mathbf R_\mathrm {WI}$ 可以使用两个绕x轴和y轴的角度，因为绕z轴旋转是不影响重力向量的。使用SO3上的扰动：
$\begin{aligned} \mathbf g_\mathrm w&={\mathbf R}_{\mathrm {WI}}\operatorname{Exp}(\delta\theta)\hat {\mathbf g}_\mathrm I G \\ \boldsymbol {\delta\theta}&=\begin{bmatrix} \boldsymbol{\delta \theta_{xy}^T} &0\end{bmatrix}^T, \quad \boldsymbol{ \delta \theta_{xy}} = \begin{bmatrix}\delta \theta_x &\delta \theta _y\end{bmatrix}^T \end{aligned} \tag {16}$
对其进行一阶近似：
$\mathbf g_ \mathrm w \approx {\mathbf R}_{\mathrm {WI}} \hat {\mathbf g}_\mathrm I G -{\mathbf R}_{\mathrm {WI}} (\hat {\mathbf g}_\mathrm I)_\times G \boldsymbol{\bf \delta\theta} \tag{17}$
将公式（17）带入公式（11）并考虑加速度计的bias，得到：
$\begin{aligned} s_{\mathrm{W}} \mathbf{p}_{\mathrm{C}}^{i+1}=& s_{\mathrm{W}} \mathbf{p}_{\mathrm{C}}^{i}+_{\mathrm{W}} \mathbf{v}_{\mathrm{B}}^{i} \Delta t_{i, i+1}-\frac{1}{2} \mathbf{R}_{\mathrm{WI}}\left(\hat{\mathbf{g}}_{\mathrm{I}}\right)_{\times} G \Delta t_{i, i+1}^{2} \boldsymbol{\delta} \boldsymbol{\theta} \\ &+\mathbf{R}_{\mathrm{WB}}^{i}\left(\Delta \mathbf{p}_{i, i+1}+\mathbf{J}_{\Delta p}^{a} \mathbf{b}_{a}\right)+\left(\mathbf{R}_{\mathrm{WC}}^{i}-\mathbf{R}_{\mathrm{WC}}^{i+1}\right) {_\mathrm{C} \mathbf{P}_{\mathrm{B}}} \\ &+\frac{1}{2} \mathbf{R}_{\mathrm{WI}} \hat{\mathbf{g}}_{\mathrm{I}} G \Delta t_{i, i+1}^{2} \end{aligned} \tag{18}$
使用公式（12）的三个连续关键帧关系可以消掉速度得到：
$\begin{bmatrix}\boldsymbol \lambda(i)\ &\boldsymbol \phi(i)\ &\boldsymbol \zeta(i)\end{bmatrix}\begin{bmatrix} s\\ \boldsymbol {\delta\theta_{xy}}\\\mathbf b_a\end{bmatrix}=\boldsymbol \psi(i) \tag{19}$
这里 $\boldsymbol \lambda(i)$ 和公式（13）保持相同， $\boldsymbol \phi(i)$ ， $\boldsymbol \zeta(i)$ 和 $\boldsymbol \psi(i)$ 计算如下：
$\begin{aligned} \boldsymbol\phi(i)=&\left[\frac 12{\mathbf R}_{\mathrm {WI}}(\hat {\mathbf g}_\mathrm I) G(\Delta t_{12}^2\Delta t_{23}+\Delta t_{23}^2\Delta t_{12})\right]_{(:,1:2)} \\ \boldsymbol\zeta(i)=&{\mathbf R}_{\mathrm {WB}}^2{\mathbf J}_{\Delta p23}^a\Delta t_{12}+{\mathbf R}_\mathrm {WB}^1{\mathbf J}_{\Delta v_{23}}^a\Delta t_{12}\Delta t_{23}\\ &-{\mathbf R}_{\mathrm {WB}}^1{\mathbf J}_{\Delta p_{12}}^a\Delta t_{23} \\ \boldsymbol\psi(i)=&({\mathbf R}_{\mathrm {WC}}^2-{\mathbf R}_{\mathrm {WC}}^1) _{\mathrm C}{\mathbf p}_{\mathrm B}\Delta t_{23}-({\mathbf R}_{\mathrm {WC}}^3-{\mathbf R}_{\mathrm {WC}}^2){_{\mathrm C}{\bf p}}_{\mathrm B}\Delta t_{12}\\ &+{\mathbf R}_{\mathrm {WB}}^2\Delta {\mathbf p}_{23}\Delta t_{12}+{\mathbf R}_{\mathrm {WB}}^1\Delta {\mathbf v}_{12}\Delta t_{12}\Delta t_{23}\\ &-{\mathbf R}_{\mathrm {WB}}^1\Delta {\mathbf p}_{12}\Delta t_{23}+\frac 12{\mathbf R}_{\mathrm {WI}}\hat {\mathbf g}_{\mathrm I} G\Delta t_{ij}^2 \end{aligned} \tag{20}$
这里 $[]_{(:,1:2)}$ 表示矩阵前两列。将所有三个连续关键帧之间如公式（19）的关系列成 $\mathbf A_{3(N-2)\times6}\mathbf x_{6\times1} = \mathbf B _{3(N-2)\times1}$ 形式，使用SVD分解可以求解出，尺度因子 $s^*$ ，重力方向纠正角 $\boldsymbol {\delta\theta_{xy}^*}$ 和加速度bias $\mathbf b_a^*$ 。因为我们有 $3(N-2)$ 个方程和6个未知数，因此需要至少4个关键帧。我们可以通过计算条件数（最大最小奇异值之间的比）来判断系统是否状态良好（即运动是否足够使得变量可观）。

2.3.4 速度估计

目前为止我们还没得到速度的 $3N$ 个未知数。由于其它变量已知，我们可以使用公式（18）求出所有关键帧的速度，对于最新的几个关键帧可以使用公式（3）中的关系求出速度。

2.3.5 重定位后的bias重新初始化

当发生重定位后，使用公式（9）求出陀螺仪bias，此时已知尺度和重力公式（19）得到了简化，直接求出加速度计bias。本系统使用只用视觉估计的20个连续帧计算这两个bias。

3. 实验分析

这部分对本文初始化算法和VI-ORB-SLAM算法在EuRoC数据集^[8]上的左侧单目进行了评估。

A. IMU初始化

在V2_01_easy序列对初始化进行测试，提取每个关键帧插入时系统的信息，并保证一直处于初始化状态，来分析各个状态的收敛情况。

<center> 图4. 在V2_01_easy进行IMU初始化 </center>

从图4中可以发现，在10-15秒之间所有的变量都收敛到稳定值且尺度因子也接近最优值。图4也展示了公式（19）的条件数，表示需要时间来使系统达到稳定的状态。这确保了传感器充分运动使得所有的状态都变得可观，尤其是对于加速度计bias。图4中最后一行显示了所花费的时间，是呈线性增长的，但这里不包括公式（12）和（19）的求解。从实验中可以看出，将初始化拆分为简单的子问题，得到了更好的效率和效果。

本文提出的初始化方法保证了融合IMU时具有一个比较可靠的重力，biases，尺度和速度估计值。对于RuRoC数据集，我们发现15s可以得到一个较精确的初始化结果。未来的工作需要去制定一个自动策略来判断是否初始化完成。我们在实验中发现对于条件数制定一个绝对阈值的策略是不足够可靠的。

B. SLAM评估并与SOTA的对比

在EuRoC数据集上测试了VI-ORB-SLAM的精度。局部BA的局部窗口大小设置为10个关键帧，IMU初始化在运行15秒后执行。表格一展示了每一个序列关键帧轨迹的位移RMSE（Root Mean Square Error），同时也测得了轨迹和重建的尺度误差。VI-ORB-SLAM除了V1_03_difficult其它的序列都可以成功实时运行，因为改序列运动太极端以致于无法完成15s的初始化。最终系统实现了小于1%的尺度恢复误差，在20平方米的房间环境内达到3cm的典型精度，在300平方米的工厂环境中达到8cm的典型精度。为了展示尺度误差对精度的损失，表一中还给出了尺度对齐后的精度，详见GT scale列。同时表一也说明了进行视觉惯性的full-BA有助于尺度和姿态的估计，详见 Full BA列。对于序列V1_02_medium序列的重建结果如图1所示。

<center> 表格一 EuRoC数据集上关键帧轨迹精度</center>

在表一中，使用ORB-SLAM作为对比基准，VI-ORB-SLAM能够处理V2_03_difficult更加鲁棒，并且能够恢复度量尺度防止尺度漂移。它的精度接近具有准确尺度的ORB-SLAM系统，但由于IMU的加入BA优化的代价变得更加昂贵，所以局部BA的窗口大小会小于ORB-SLAM。这解释了没有进行full-BA的VI-ORB-SLAM效果差于ORB-SLAM的原因。实际上VI-ORB-SLAM的full-BA会在7秒内的15次迭代后收敛，而ORB-SLAM在小于1秒的时间内5次迭代即可收敛。

为了测试VI-ORB-SLAM的重用地图能力，首先在第一个序列运行并执行full-BA得到环境地图。然后再运行剩余的序列进行重定位并运行SLAM。由于存在地图V1_03_difficult也可以成功定位。在V1，V2和MH的环境中RMSE分别为0.037，0.027和0.076，尺度因子误差为1.2%，0.1%和0.2%。结果表明了当重新回到原场景后可以得到无漂移累积的定位，因为RMSE没有变得更大。

<center>图5 本文方法和出色的直接双目VIO[9]的相对位姿误差（RPE）对比。由于地图重用，本文SLAM误差没有随着轨迹增长增加，且[9]中使用双目，本文使用单目。

最后将VI-ORB-SLAM和目前出色的直接法双目VIO（Visual-Inertial Odometry）^[9]]进行对比。由于地图重用机制，本文算法没有误差累积，而VIO算法随着轨迹长度误差增加。与双目直接VIO方法对比，本文基于特征点的方法效果显著。

4. 总结

本文提出了一种新颖的紧耦合视觉和惯导融合的SLAM系统，并且具有实时闭环检测和地图重用能力。这使得实现了零漂移定位，而VIO算法漂移会无限制增长。实验表明了本文单目SLAM算法恢复尺度同时具有高精度，相同环境下取得了比目前杰出系统更加优秀的效果。这种零漂移定位是虚拟/增强现实（VR/AR，Virtual/Augmented Reality），这种在相同区域操作的系统所必须的特性。更多地，如果使用双目或者RGB-D相机能够得到更好的精度和鲁棒性，并且可以大大简化初始化过程。本文初始化算法的不足之处在于依赖单目SLAM的初始化效果，希望接下来研究可以利用陀螺仪使得单目初始化更加迅速且鲁棒。

参考文献

R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “ORB-SLAM: A versatile and accurate monocular SLAM system,” IEEE Trans. Robot., vol. 31, no. 5, pp. 1147–1163, Aug. 2015. ↩
R. Mur-Artal and J. D. Tardos, “ORB-SLAM2: an Open-Source SLAM system for monocular, stereo and RGB-D cameras,” arXiv:1610.06475, Oct. 2016. ↩
C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual-inertial odometry,” IEEE Trans. Robot., to be published. doi: 10.1109/TRO.2016.2597321. ↩ ↩
T. Lupton and S. Sukkarieh, “Visual-inertial-aided navigation for highdynamic motion in built environments without initial conditions,” IEEE Trans. Robot., vol. 28, no. 1, pp. 61–76, Feb. 2012 ↩
S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale, “Keyframe-based visual–inertial odometry using nonlinear optimization,” Int. J. Robot. Res., vol. 34, no. 3, pp. 314–334, Aug. 2015. ↩
B. K. P. Horn, “Closed-form solution of absolute orientation using unit quaternions,” J. Opt. Soc. Amer. A, vol. 4, no. 4, pp. 629–642, 987. ↩
A. Martinelli, “Closed-form solution of visual-inertial structure from motion,” Int. J. Comput. Vis., vol. 106, no. 2, pp. 138–152, Jan. 2014. ↩
M. Burri et al., “The EuRoC micro aerial vehicle datasets,” Int. J. Robot. Res., vol. 35, no. 10, pp. 1157–1163, Jan. 2016. ↩
V. Usenko, J. Engel, J. Stueckler, and D. Cremers, “Direct visual-inertial odometry with stereo cameras,” in Proc. IEEE Int. Conf. Robot. Autom., 2016, pp. 1885–1892. ↩