论文阅读《Direct Sparse Visual-Inertial Odometry Using Dynamic Marginalization》

摘要

我们提出了一种视觉惯性里程计的新方法VI-DSO，该方法通过在组合能量泛函中最小化光度和IMU测量误差来联合估计相机位姿和稀疏场景几何。系统的视觉部分执行了一个光束调整，就像优化稀疏的点集，但不同于基于关键点的系统，它直接最小化了光度误差。这使得系统不仅可以跟踪角点，还可以跟踪任何具有足够大灰度梯度的像素。IMU信息通过测量预积分在多帧之间进行累积，并作为关键帧之间的附加约束插入到优化算法中。我们明确地将尺度和重力方向包含到我们的模型中，并与其它变量(如位姿)一起进行优化。由于使用IMU数据通常不能立即观测到尺度，这使得我们可以用任意的尺度来初始化我们的视觉惯性系统，而不是延迟初始化直到所有的东西都被观测到。我们对旧变量进行部分边缘化，以便在合理的时间内计算更新。为了保持系统的一致性，我们提出了一种新的策略，我们称之为“动态边缘化”。这种技术允许我们使用部分边缘化，即使在初始尺度估计远远不是最优的情况下。我们在具有挑战性的EuRoC数据集上评估了我们的方法，表明VI-DSO优于目前的技术水平。

1 介绍

运动估计和三维重建是机器人的关键任务。一般来说，许多不同的传感器可以用于这些任务：激光测距仪，RGB-D相机[14]，GPS和其它。由于相机是廉价、轻便和小型的被动传感器，它引起了社区的广泛关注。一些实际应用的例子包括机器人导航[25]和(半)自主驾驶[11]。然而，当前的视觉里程计方法在面对低纹理区域或快速机动时缺乏鲁棒性。为了消除这些影响，可以结合使用另一种被动传感器——惯性测量单元(IMU)。它能提供精确的短期运动约束，而且不像视觉那样容易出现异常值。

在本文中，我们提出了一种紧耦合的直接惯性里程计方法。它是基于直接稀疏里程计(DSO)[6]和使用光束调整像光度误差函数，同时优化在综合能量函数中的三维几何和相机位姿。我们用IMU测量值来补充误差函数。这对直接方法特别有益，因为误差函数是非凸的，好的初始化很重要。单目视觉里程计的一个关键缺点是它不能获得环境的度量尺度。增加一个IMU使我们能够观察尺度。然而，根据所执行的运动，这可能会花费无限长的时间，使初始化成为一个具有挑战性的任务。我们没有依赖于单独的IMU初始化，而是将尺度作为一个变量纳入我们的系统模型，并与其它参数一起进行优化。

对EuRoC数据集[2]的定量评估表明，尽管光照条件具有挑战性，但我们可以从快速移动的微型飞行器(MAV)上的视觉惯性系统可靠地确定相机运动和稀疏的3D结构(以公制单位)(图1)。

图1 下图：来自EuRoC数据集的示例图像：低光照、强运动模糊和小纹理对里程计估计构成了重大挑战。我们的方法仍然能够处理所有序列，并且rmse小于0.23米。上图：重建，估计位姿(红色相机)和地面真实位姿(绿色相机)在V1_03_difficult结束部分。

 

总而言之，我们的贡献是：

• 一种直接稀疏视觉惯性里程计系统。
• 提出了一种新的初始化策略，将尺度和重力方向纳入模型，初始化后联合优化。
• 我们引入“动态边缘化”作为一种技术，以便自适应地运用边缘化策略，即使在某些变量发生剧烈变化的情况下。
• 对EuRoC具有挑战性的数据集进行的广泛评估表明，整个系统和初始化策略都优于目前的技术水平。

2 相关工作

利用摄像机和IMU进行运动估计一直是一个热门的研究课题。在这一节中，我们将概述视觉和视觉惯性里程计方法。我们还将讨论在初始方向、速度和尺度是未知的前提下初始化单目视觉惯性里程计的方法。

Nister等人[24]在其工作中引入了视觉里程计(visual odometry)一词，他提出使用稀疏点集的帧到帧匹配来估计相机的运动。大多数早期的方法都是基于图像中检测到的匹配特征，特别是MonoSLAM[5]，这是一种实时的基于EKF的方法。另一个突出的例子是PTAM[15]，它结合了一个用于建图的光束调整后端，以及相对于构造的地图的相机实时跟踪能力。最近，Mural-Artal等人提出了一种基于特征的大规模实时SLAM系统。

与基于特征的方法不同，直接方法使用图像中未处理的灰度来估计相机的运动。在[4]中首次提出了一种可用于立体相机的实时直接方法。Kerl等人开发了几种RGB-D相机运动估计方法。最近，直接的方法也应用于单目相机，以稠密[23]，半稠密[7]和稀疏方式[10]和[6]。

由于IMU传感器的互补性，有许多尝试将其与视觉结合。它们提供了良好的短期运动预测，并使横滚角和俯仰角可以被观测到。一开始，视觉系统只是作为6D位姿测量的提供者，然后插入到组合优化中。这种所谓的松耦合方法在[20]和[8]中提出。它通常更容易实现，因为视觉算法不需要修改。另一方面，紧耦合方法在组合能量函数中联合优化运动参数。它们能够在多传感器数据流中捕捉到更多的相关性，从而获得更精确和更鲁棒的性能。几个突出的例子是基于滤波的方法和基于能量最小化的方法。

与实际使用单目视惯里程计有关的另一个问题是初始化。在开始之后，系统没有图像中观测点的初始位姿、速度和深度值的先验信息。由于被最小化的能量泛函是高度非凸的，一个糟糕的初始化可能导致系统发散。这个问题甚至更加复杂，因为某些类型的运动不允许唯一地确定所有这些值。在[19]中提出了初始化的封闭解，并分析了例外情况，并扩展到[12]中考虑IMU偏差。

3 直接稀疏视惯里程计

下面的方法是基于非线性优化框架中光度和惯性误差的迭代最小化。为了使问题在计算上可行，优化是在最近帧的窗口上进行的，而所有旧的帧都被边缘化。我们的方法以[6]为基础，可以看作是[16]的直接形式。与[26]不同的是，我们通过一个单一的优化函数来联合确定位姿和3D几何形状。这导致更好的精度，特别是在困难序列中。与[9]相比，我们执行了一个完整的光束调整优化，而不是包括无结构的视觉误差项。

该方法通过最小化能量函数来估计位姿和深度，
$$E_{total}=\lambda \cdot E_{photo}+E_{inertial}\text{\hspace{1em}(1)}$$
它由光度误差项$E_{photo}$(第3.2节)和惯性误差项$E_{inertial}$(第3.3节)组成。

该系统主要由两部分并行运行：

• 对每一帧执行粗跟踪，并使用直接图像对齐结合惯性误差项来估计最近一帧的位姿。
• 当创建一个新的关键帧时，我们执行一个视觉惯性光束调整，比如估计所有活动关键帧的几何形状和位姿的优化。

与[22]相比，我们无需等待固定的时间来初始化视觉惯性系统，而是联合优化包括尺度在内的所有参数。这产生了更高的鲁棒性，因为惯性测量从一开始就使用。

3.1 记号

在本文中，我们遵循如下记号。粗体大写字母$HH$表示矩阵，粗体小写字母$xx$表示向量，轻体小写字母$\lambda$表示标量。坐标帧之间的变换矩阵记作${TT}_{i,j}\in SESE(3)$，它将第$j$帧中一点变换到第$i$帧，${pp}_i={TT}_{i,j}{pp}_j$。我们将李代数分量记为$\hat{\xi }\in \mathfrak{se}(3)$，其中$\xi \in {\mathbb{R}}^6$。我们利用它来处理小增量6D位姿，${\xi }_{i,j}^{\prime }={\xi }_{i,j}⊞\xi :=log(e^{{\hat{\xi }}_{i,j}}\cdot e^{\hat{\xi }}{)}^{\vee }$。

我们将世界定义为一个固定的惯性坐标系，重力作用于负$Z$轴。我们还假设$T_{imu\_cam}$从相机到IMU帧的转换是固定的，并预先标定。因子图用集合$G$表示，而$G_1\cup G_2$表示两个集合的并集。

3.2 光度误差

参考帧$i$中一点$p\in {\Omega }_i$在帧$j$中被观测，其光度误差表示为，
$$E_{p_j}=\sum _{p\in {\mathcal{N}}_p}{w}_p\Vert (I_j[{pp}^{\prime }]-b_j)-\frac{t_j{e}^{a_j}}{t_i{e}^{a_i}}({II}_i[p]-b_i){\Vert }_{\gamma }\text{\hspace{1em}(2)}$$
其中${\mathcal{N}}_p$表示点$pp$附近的小邻域，$I_i$和$I_j$是各自帧中的图像，$t_i$和$t_j$是曝光时间，$a_i$、$b_i$、$a_j$和$b_j$是修正仿射亮度变化的因子，$\gamma$是Huber范数，${\omega }_p$是梯度相关的权重，${pp}^{\prime }$是被投影到$I_j$中的点。

总光度误差可以表示为，
$$E_{photo}=\sum _{i\in \mathcal{F}}\sum _{pp\in {\mathcal{P}}_i}\sum _{j\in obs(pp)}{E}_{{pp}_j}\text{\hspace{1em}(3)}$$
其中$\mathcal{F}$是用于优化的关键帧集合，${\mathcal{P}}_i$是关键帧$i$中的稀疏点集，$obs(pp)$是关键帧集合，它们都观测到了点$pp$。

3.3 惯性误差

为了构造依赖于陀螺仪测量的转速和加速度计测量的线性加速度的误差项，我们使用了[26]中公式(6)、公式(7)和公式(8)中定义的非线性动力学模型。

由于IMU数据的获取频率远高于图像，我们遵循[18]中提出的、[3]和[9]中改进的预积分方法。这允许我们添加一个IMU因子来描述两个相机帧之间的位姿。对于状态${ss}_i$和${ss}_j$(基于公式(9)定义的)，这两帧之间有IMU测量${aa}_{i,j}$和${\omega }_{i,j}$，我们获得了预测$\widehat{s_j}$和其协方差${\hat{\Sigma }}_{s,j}$。相应的误差函数为，
$$E_{inertial}({ss}_i,{ss}_j):=(s_j\boxminus {\hat{s}}_j{)}^T{\hat{\Sigma }}_{s,j}^{-1}(s_j\boxminus {\hat{s}}_j)\text{\hspace{1em}(4)}$$
其中操作符$\boxminus$应用于${\xi }_j⊞(\widehat{{\xi }_j}{)}^{-1}$，对位姿和其它组件进行正常的减法。

3.4 IMU初始化与可观性问题

与单纯的单目系统相比，惯性数据的使用使我们能够观测公制尺度和重力方向。这也意味着必须正确地初始化这些值，否则优化可能会产生分歧。单目视惯系统的初始化是一个研究得很好的问题，[19]中有一个很好的总结。[19]中的表1和表2说明，对于某些运动不可能立即初始化，例如当以零加速度和恒定的非零速度运动时。为了证明这是一个现实世界的问题，而不仅仅是一个理论案例，我们注意到最先进的视觉-惯性SLAM系统[22]使用相机运动的前15秒在EuRoC数据集上进行初始化，以确保所有值都是可观测的。

因此，我们提出了一种新的策略来处理这个问题。我们明确地将比例(和重力方向)作为视觉惯性系统的参数，并将它们与其它值(如位姿和几何)一起进行优化。这意味着我们可以用任意尺度初始化，而不是等到它是可观察的。如下所示初始化各种参数。

• 我们使用与[6]相同的视觉初始化器来计算两帧之间的粗略位姿估计以及几个点的近似深度。它们被归一化，平均深度为1。
• 初始重力方向是通过平均40个加速度计测量值来计算的，即使在高加速度的情况下也能得到足够好的估计。
• 我们用0初始化速度和IMU偏差，用1.0初始化尺度。

然后，在类似优化的光束调整过程中，对所有这些参数进行联合优化。

3.5 基于SIM(3)的世界表示

为了能够开始跟踪和绘制初步尺度和重力方向，我们需要将它们包含到我们的模型中。因此，除了度量坐标系之外，我们还将DSO坐标系定义为它的缩放和旋转版本。DSO坐标系到度量坐标系的变换定义为 T m _ d ∈ { T ∈ S I M ( 3 ) ∣ t r a n s l a t i o n ( T ) = 0 } \pmb{T}_{m\_d} \in \{ \pmb{T} \in \pmb{SIM}(3) | translation(\pmb{T})=0 \} TTm_d​∈{TT∈SIMSIM(3)∣translation(TT)=0}，其中${\xi }_{m,d}=log(T_{m,d})\in \mathfrak{sim}(3)$。我们给所有的位姿添加一个上标D或M，表示它们在哪个坐标系中表示。在优化中，光度误差总是在DSO坐标系中计算，使其独立于尺度和重力方向，而惯性误差则必须使用公制坐标系。

3.6 尺度感知的视惯优化

我们优化了固定数量的关键帧的位姿、IMU偏差和速度。图2(a)显示了该问题的因子图。请注意，实际上有许多独立的视觉因子连接着两个关键帧，我们已经将它们组合成一个大的因子来连接这个可视化中的所有关键帧。每个IMU因子使用第3.3节描述的预积分方案连接两个随后的关键帧。由于预积分的误差随着关键帧之间的时间增加而增加，我们确保两个连续关键帧之间的时间不大于0.5秒，这与[22]所做的类似。请注意，与他们的方法相反，我们允许第3.6节所述的边缘化程序违反这一约束，这确保了关键帧之间的长期关系可以得到适当的观察。

图2 在边缘化一个关键帧前后的视惯联合优化的因子图。

 

我们的算法的一个重要性质是，优化的位姿不表示在度量帧，但在DSO帧。这意味着它们不依赖于环境的尺度。

非线性优化：我们采用高斯-牛顿算法进行非线性优化。对于每个活动的关键帧，我们定义一个状态向量，
$$s_i:=[({\xi }_{ca{m}_i\_w}^D{)}^T,v_i^T,b_i^T,a_i,b_i,d_i^1,d_i^2,\cdots ,d_i^m{]}^T\text{\hspace{1em}(5)}$$
其中$v_i\in {\mathbb{R}}^3$是速度向量，$b_i\in {\mathbb{R}}^6$是当前IMU偏差，$a_i$和$b_i$是公式(2)中的仿射亮度参数，$d_i^j$是在关键帧中host的点的逆深度。

全状态向量定义如下，
$$s=[c^T,{\xi }_{m,d}^T,s_1^T,s_2^T,\cdots ,s_n^T{]}^T\text{\hspace{1em}(6)}$$
其中$c$表示相机几何参数，${\xi }_{m\_d}$表示DSO坐标系和度量坐标系之间的无平移变换，定义见3.5节。通过对李代数分量应用连接算子$\xi ⊞{\xi }^{\prime }$和对其它分量使用普通加法，我们定义了作用于状态向量的算子$s⊞s^{\prime }$。

利用叠加残差向量$r$定义，
$$J=\frac{dr(s⊞ϵ)}{dϵ}{|}_{ϵ=0},H=J^{T}WJandb=-J^{T}Wr\text{\hspace{1em}(7)}$$
其中$W$是对角权重矩阵。我们通过使用$\delta =H^{-1}b$进行更新。

注意视觉能量项$E_{photo}$和惯性误差项$E_{imu}$没有共同的残差。因此我们可以将$H$和$b$分别分成两个独立的部分，
$$H=H_{photo}+H_{imu}andb=b_{photo}+b_{imu}\text{\hspace{1em}(8)}$$
由于惯性残差将当前的相对位姿与从惯性数据估计的位姿进行比较，因此需要使用相对于IMU的度量坐标系中的位姿。因此，我们定义了惯性残差的附加状态向量。
$$s_i^{\prime }:=[{\xi }_{w\_im{u}_i}^M,v_i,b_i{]}^{T}and{s}^{\prime }=[s_1^{\prime T},s_2^{\prime T},\cdots ,s_n^{\prime T}]\text{\hspace{1em}(9)}$$

惯性残差造成，
$$H_{imu}^{\prime }=J_{imu}^{\prime T}{W}_{imu}{J}_{imu}^{\prime }and{b}_{imu}^{\prime }=-J_{imu}^{\prime T}{W}_{imu}{r}_{imu}\text{\hspace{1em}(10)}$$
联合优化则需要根据公式(6)中的状态定义得到$H_{imu}$和$b_{imu}$。由于这两种定义主要不同于它们的位姿表示，我们可以这样计算$J_{rel}$，
$$H_{imu}=J_{rel}^T\cdot H_{imu}^{\prime }\cdot J_{rel}and{b}_{imu}=J_{rel}^T\cdot b_{imu}^{\prime }\text{\hspace{1em}(11)}$$
补充材料中详细介绍了$J_{rel}$的计算方法。注意，我们将所有的转换表示为$\mathfrak{sim}(3)$的元素，并将除了${\xi }_{md}$之外的所有转换的尺度固定为1。

利用舒尔补边缘化：为了在合理的时间范围内计算高斯-牛顿更新，我们对较老的关键帧进行部分边缘化。这意味着与这个关键帧相对应的所有变量(位姿、偏差、速度和仿射照明参数)都通过舒尔补被边缘化。图2b显示了边缘化如何改变因子图的。

在[6]中，通过删除影响系统稀疏性的残差项来处理视觉因子的边缘化，在边缘化关键帧本身之前，首先边缘化关键帧中的所有点。

边缘化使用舒尔补进行。由于边缘化导致的因子要求所有连接变量的线性化点保持固定，我们使用[6]中公式(15)来进一步近似线性化点周围的能量。

为了保持系统的一致性，重要的是，对于与边缘因子相关的变量，雅可比矩阵的值都是相同的，否则零空间就会被消除。因此，我们应用“首次估计雅可比矩阵”。对于视觉因子，我们遵循[6]并在线性化点评估$J_{photo}$和$J_{geo}$。在计算惯性因子时，我们为所有与边缘因子相关的变量确定$J_{rel}$的评估点。注意，它总是包含${\xi }_{m\_d}$。

时滞尺度收敛的动态边缘化：第3.6节所述的边缘化程序有两个目的。通过删除旧的状态，并维护系统以前的状态信息，减少优化的计算复杂度。这个过程固定了连接到旧状态的状态的线性化点，所以它们应该已经有了一个很好的估计。在我们的场景中，除了尺度之外，所有变量都是这样。

“动态边缘化”的主要思想是同时保持若干个边缘化先验，当尺度估计距离当前边缘化先验线性化点太远时，重新设置当前使用的边缘化先验。

在我们的实现中，我们使用了三个边缘化先验：$M_{visual}$、$M_{curr}$和$M_{half}$。$M_{visual}$只包含尺度独立的信息，来自于之前的视觉状态，不能用于推断全局尺度。$M_{curr}$包含自我们为尺度设置线性化点以来的所有信息，而$M_{half}$只包含尺度接近当前估计的最近状态。

当尺度估计与$M_{curr}$的线性化点偏差过大时，将$M_{curr}$的值设为$M_{half}$，将$M_{half}$设为$M_{visual}$，线性化点发生相应的变化。这确保了优化总是具有关于以前状态的一些信息，并具有一致的尺度估计。在本节的其余部分，我们将提供实现的细节。

我们定义$G_{metric}$只包含视觉惯性因子(依赖于${\xi }_{m\_d}$)，定义$G_{visual}$包含除边缘化先验之外的所有其它因子。然后，
$$G_{full}=G_{metric}\cup G_{visual}\text{\hspace{1em}(12)}$$
图3描述了因子图的划分。

图3 将图2a中的因子图分区为G_{metric}和G_{visual}。G_{metric}包含所有的IMU因子，而G_{visual}包含不依赖于\xi_{m\_d}的因子。请注意，它们都不包含任何边缘化因子。

 

我们定义了三个不同的边缘化因子$M_{curr}$、$M_{visual}$和$M_{half}$。为了优化，我们总是使用图来计算更新，
$$G_{ba}=G_{metric}\cup G_{visual}\cup M_{curr}\text{\hspace{1em}(13)}$$
当关键帧$i$被边缘化时，我们用$G_{visual}\cup M_{visual}$中被边缘化的帧$i$产生的因子来更新$M_{visual}$。这意味着$M_{visual}$包含了所有边缘视觉因子，没有边缘惯性因子使其独立于尺度。

对于每一个边缘化帧$i$，我们定义
$$s_i:=scaleestimateatthetime,iwasmarginalized\text{\hspace{1em}(14)}$$
我们定义$i\in M$，当且仅当$M$包含在时刻$i$被边缘化的惯性因子。利用这一点，我们强制下列约束惯性因子。
$$\forall i\in M_{curr}:s_i\in [s_{middle}/d_i,s_{middle}\cdot d_i]\text{\hspace{1em}(15)}$$
$$\forall i\in M_{half}:s_i\in \{\begin{array}{l}[s_{middle},s_{middle}\cdot d_i],if{s}_{curr}>s_{middle}\\ [s_{middle}/d_i,s_{middle}],otherwise\end{array}\text{\hspace{1em}(16)}$$
其中$s_{middle}$是当前允许尺度区间的中间值(初始化为$s_0$)，$d_i$是$i$时刻尺度区间的长度，$s_{curr}$是当前尺度估计。

我们通过边缘化在$G_{ba}$中的第$i$帧来更新$M_{curr}$，通过边缘化$G_{metric}\cup G_{visual}\cup M_{half}$中的第$i$帧来更新$M_{half}$。

为了保持公式(15)和公式(16)中的约束，我们在每次边缘化发生时都应用算法1。通过遵循这些步骤，一方面我们确保满足约束条件使得当前使用的边缘化因子的尺度差异保持小于$d_i^2$。另一方面，该因子总是包含一些惯性因子，使尺度估计在任何时候都能发挥作用。还请注意，$M_{curr}$和$M_{half}$有单独的首次估计雅可比矩阵，当使用各自的边缘化因子时使用。图4显示了该系统的实际工作原理。

 

图4 在EuRoC数据集的V1_03_difficult序列上运行的尺度估计。我们显示当前尺度估计(蓝色粗体)、地面真实尺度(红色粗体)和当前尺度间隔(浅线)。垂直虚线表示边的变化(蓝色)和超过尺度区间边界(红色)。实际上，这意味着M_{curr}包含了自最后一条蓝色或红色虚线之后的惯性因子，这条虚线位于最后一条红色虚线之前。比如在16秒时，它包含了9秒蓝线之后的所有惯性数据。

 

这个策略的一个重要部分是$d_i$的选择。它应该很小，以保持系统的一致性，但也不能太小，以使$M_{curr}$总是包含足够的惯性因子。因此我们选择动态调整参数如下。在任何时候，步骤$i$都要计算，
d i = m i n { d m i n j ∣ j ∈ N ∖ { 0 } , s i s i − 1 < d i } (17) d_i = min\{ d_{min}^j | j \in \mathbb{N} \setminus \{0\} , \frac{s_i}{s_i-1} < d_i \} \tag{17} di​=min{dminj​∣j∈N∖{0},si​−1si​​<di​}(17)
这确保了$M_{half}$不会在$M_{curr}$与$M_{half}$交换的同时被重置为$M_{visual}$。因此，它防止了$M_{curr}$完全不包含惯性因子的情况，使尺度估计更可靠。在我们的实验中，将$d_{min}$设置为$\sqrt{1.1}$。

3.7 粗略的视惯跟踪

粗跟踪负责计算每一帧的快速位姿估计，也用作3.6节中详细描述的联合优化的初始化。我们在当前帧和最新关键帧之间执行传统的直接图像对齐，同时保持几何形状和尺度固定。惯性残差使用前面描述的IMU预积分方案放置在后续帧之间。每次联合优化完成一个新的帧，粗跟踪重新初始化与新的估计尺度、重力方向、偏差和速度以及新的关键帧作为参考的视觉因子。与联合优化相似，我们进行部分边缘化以保持更新时间的约束。在估计了新一帧的变量后，我们边缘化除关键帧位姿和最新一帧的变量之外的所有变量。与联合优化相比，我们不需要使用动态边缘化，因为尺度不包含在优化中。

4 结果

我们在公开可用的EuRoC数据集[2]上评估我们的方法。性能与[6]、[1]、[21]、[26]、[16]和[13]进行比较。我们还提供了更多评估的补充材料和视频，网址是vision.in.tum.de/vi-dso。

4.1 鲁棒的定量评价

为了获得准确的评估结果，我们对数据集的每个序列(使用左侧相机)运行我们的方法10次。我们直接比较仅使用视觉的DSO[6]和ROVIO[1]的结果。由于DSO无法观察到尺度，我们在一些图中使用最优地面真实尺度(带有“gt- scaling”的描述)进行评估，以便进行公平的比较。对于所有其它结果，我们使用最终尺度估计(我们的方法)或1(其它方法)来缩放轨迹。对于DSO，我们使用了与他们的论文一起发表的结果。我们为每个序列使用相同的开始和结束时间来运行我们的方法和ROVIO。注意，当使用这些开始时间时，无人机在某些序列中有很高的初始速度，这对我们的IMU初始化尤其具有挑战性。图5显示了每次运行的均方根误差(rmse)，图6显示了累积误差图。显然，我们的方法比DSO和ROVIO要好得多。没有惯性数据，DSO无法对所有序列进行处理，尤其是V1_03_difficult和V2_03_difficult，也无法正确缩放结果。另一方面，ROVIO非常鲁棒，但作为一种基于滤波的方法，它不能提供足够的准确性。

图5 不同方法的rmse在EuRoC数据集的每个序列(列)上运行10次(行)。

 

图6 EuRoC数据集上的累积误差图(RT表示实时)。实验表明，附加IMU不仅提供了可靠的尺度估计，而且显著提高了精度和鲁棒性。

 

表1显示了与其它几种方法的比较。对于我们的结果，我们显示了图5c中绘制的10个排列中每个序列的中值误差。这使得结果非常有意义。对于其它方法，不幸的是只报告了一个结果，所以我们必须假设它们也是有代表性的。[16]和[13]的结果取自[13]。[21]的结果(在他们的论文中报道)与其它方法略有不同，因为它们显示的是关键帧轨迹的误差，而不是完整轨迹的误差。这是一个轻微的优势，因为关键帧在它们的方法中是光束调整的，这不会发生在其它帧中。

表1 几种方法在EuRoC数据集上估计轨迹的精度。请注意，ORB-SLAM在某些序列上表现出色，令人信服。然而，由于我们的方法直接作用于传感器数据(颜色和IMU测量)，我们观察到类似的精度和更好的鲁棒性——即使没有闭环。此外，本文提出的方法是唯一一个在任何序列上都不会失败的方法。

 

与VI ORB-SLAM相比，我们的方法在几个序列上的rmse优于它。由于ORB-SLAM是一种SLAM系统，而我们的方法是一种纯里程计方法，这是一个显著的成就，特别是考虑到评估的差异。注意，Vicon房间序列(V*)是在一个小房间中执行的，包含许多环形运动，其中由SLAM系统完成的回环显著提高了性能。此外，我们的方法更鲁棒，因为ORB-SLAM无法跟踪一个序列。即使只考虑ORB- SLAM工作的序列，我们的方法也有较低的最大rmse。

与[16]和[13]相比，我们的方法明显优于它们。它在每一个序列上都比单目版本好，甚至在11个序列中的9个序列上击败了立体版本和SLAM版本。

总之，我们的方法是唯一一个能够成功跟踪所有序列除了ROVIO。

我们还比较了EuRoC V1_0*-序列上[21]和[26]的相对位姿误差(图7)。虽然我们的方法在简单序列上不能击败SLAM系统和立体视觉方法，但在中等序列上我们的性能优于[26]和[21]。在困难序列中，我们的表现优于这两个竞争者，即使我们既没有使用立体也没有使用回环。

图7 在EuRoC数据集上评估了视惯ORB-SLAM[21]、视惯立体LSD-SLAM[26]和我们的方法的相对位姿误差。虽然提出的VI-DSO不使用回环(如[21])或立体(如[26])，但在准确性和鲁棒性方面，VI-DSO是相当有竞争力的。注意，带回环的[21]平均来说更准确一些，但是在V1_03_difficult时它完全失败了。

 

4.2 初始化的评估

只有少数几个方法可以与我们的初始化进行比较。像[19]这样的一些方法还没有在真实数据上测试过。虽然[12]提供了真实数据的结果，但使用的数据集具有一个向下看的相机和一个具有许多特征的环境，在难度方面无法与EuRoC数据集相比。此外，他们没有解决后期可观测性的问题，这表明在数据集的开始阶段进行了适当的运动。作为一种基于滤波的方法，ROVIO不需要特定的初始化过程，但它在准确性方面也不具有竞争力，因此与本文的讨论无关。视惯LSD-SLAM使用的是立体视觉，因此不存在尺度估计的主要问题。因此，我们将我们的初始化过程与视觉-惯性ORB-SLAM[21]进行了比较，因为这两种方法都在具有挑战性的EuRoC数据集上工作，并且必须估计尺度、重力方向、偏差和速度。

与[21]相比，我们的估计尺度总体上更好(表1)。在大多数序列上，我们的方法提供了更好的尺度，我们的平均尺度误差(0.7%比1.0%)和最大尺度误差(1.2%比3.4%)都更低。此外，我们的方法更鲁棒，因为[21]的初始化过程在V1_03_difficult上失败。

除了数字之外，我们认为我们的方法在总体结构上更优越。虽然[21]必须等待15秒直到执行初始化，但我们的方法几乎立即提供了一个近似的比例和重力方向，并且随着时间的推移而增强。在[21]中，位姿估计必须在没有任何IMU数据的情况下工作15秒，而在我们的方法中，惯性数据从一开始就用于改进位姿估计。这可能是我们的方法能够处理V1_03_difficult的原因之一。最后，我们的方法更适合机器人应用。例如，无人驾驶飞机不能在没有重力方向和尺度的情况下飞行15秒，希望之后的尺度是可观测的。相比之下，我们的方法从一开始就提供了这两者。连续缩放也不是一个大问题，因为应用程序可以使用未缩放测量来构建一致的地图和提供飞行目标，而缩放测量可以用于控制器。图8为MH_04的尺度估计。

图8 MH_04的尺度估计(就跟踪精度而言，10次运行的中位数结果)。请注意，尽管初始化距离最优值很远，但估计的尺度如何收敛到正确的值。

 

总之，我们认为我们的初始化过程超出了目前的水平，并认为用一个非常粗略的尺度估计来初始化，并在姿态估计中联合估计，这在未来将是一个有用的概念。

5 结论

我们提出了一种新的直接稀疏视觉惯性里程计的公式。我们在我们的模型中明确地包括尺度和重力方向，以便处理尺度不能立即观察到的情况。由于初始尺度可能离最佳尺度很远，我们提出了一种新的技术，称为动态边缘化，我们保持多个边缘化先验，并约束最大尺度差。大量的定量评估表明，所提出的视觉惯性里程计方法在完整的系统和IMU初始化过程中都优于当前的技术水平。特别是，实验证实，惯性信息不仅提供了可靠的尺度估计，而且大大提高了精度和鲁棒性。

致谢

我们感谢Jakob Engel发布了DSO的代码，感谢他对首次估计雅可比矩阵的有益评论，感谢[21]的作者为图7中的比较提供了他们的数字。

参考文献

略