SVO：快速的半直接单目视觉里程计

摘要：我们提出了一种半直接单目视觉里程计算法，它比当前最先进的方法精确、稳健且速度更快。半直接法消除了运动估计中昂贵的特征提取和匹配技术的需要，直接在像素级上操作，做到了在高帧率下的亚像素精度（subpixel precision），以及采用概率建图（probabilistic mapping）方法对异常测量值进行显式建模，从而得到较少的异常值和更可靠的点。精确的高帧率运动估计在纹理较少、重复性和高频的场景中具有更强的鲁棒性。该算法应用于GPS无效环境下的无人机状态估计，在嵌入式计算机上以每秒55帧的速度运行，在消费型笔记本电脑上以每秒300帧以上的速度运行。我们将我们的方法称为SVO（半直接视觉里程计）。

I. 引言

  微型飞行器 (MAV) 很快将在灾害管理、工业检测和环境保护方面发挥重要作用。对于此类操作，仅基于 GPS 信息进行导航是不够的。 精确的完全自主操作需要 MAV 依赖替代定位系统。 因此，为了最小的重量和功耗，^[1]-[5]建议仅使用一个向下看的相机与惯性测量单元结合使用。这种设置允许在室外区域完全自主的航点跟踪^[1]-[3]以及 MAV 和地面机器人之间的协作^{[4] [5]}。

  据我们所知，所有用于 MAV 的单目视觉里程计 (VO) 系统^{[1] [2] [6] [7]}都是基于特征的。然而，在 RGB-D 和基于立体的 SLAM 系统中，基于光度误差最小化的直接方法 ^[8]-[11]正变得越来越流行。

  在这项工作中，我们提出了一种半直接 VO，它将基于特征的方法（跟踪许多特征、并行跟踪和映射、关键帧选择）的成功因素与直接方法的准确性和速度相结合。 MAV 的高帧率 VO 有望提高稳健性和更快的飞行机动。

  这项工作的开源实现和视频可在以下网址获得：http://rpg.ifi.uzh.ch/software。

A. 视觉运动估计方法的分类

  同时从视频中恢复相机姿态和场景结构的方法可以分为两类:

  a) 特征法：标准方法是在每个图像中提取一组稀疏的显着图像特征（例如点、线）；使用不变特征描述符在连续帧中匹配它们；使用对极几何稳健地恢复相机运动和结构；最后，通过重投影误差最小化来细化姿势和结构。大多数 VO 算法^[12]遵循此程序，独立于应用的优化框架。这些方法成功的一个原因是强大的特征检测器和描述符的可用性，即使在较大的帧间移动时，它们也允许图像之间的匹配。基于特征的方法的缺点是依赖检测和匹配阈值，需要稳健的估计技术来处理错误的对应关系，以及大多数特征检测器针对速度而不是精度进行优化的事实，这样运动估计的漂移 必须通过对许多特征测量进行平均来补偿。

  a) 直接法：直接方法^[13]直接从图像中的光强值估计结构和运动。与仅考虑到某些特征位置的距离的基于特征，局部强度梯度幅度和方向用于优化的方法相比。直接方法利用图像中所有信息，即使来自梯度很小的区域，且已被证明在具有很少纹理的场景^[14]或在相机散焦和运动模糊的情况下的鲁棒性方面优于基于特征的方法^[15]。光度误差的计算比重投影误差更密集，因为它涉及扭曲和整合大图像区域。 然而，由于直接方法直接对图像的强度值进行操作，因此可以节省特征检测和不变描述符计算的时间。

B. 相关工作

  与^{[1] [2] [7]}相比，大多数用于 MAV 的单目VO算法都依赖于PTAM^[16]。PTAM是一种基于特征的SLAM算法，通过跟踪和映射许多（数百）特征来实现鲁棒性。同时，它通过并行运动估计和映射任务，并依靠高效的基于关键帧的束调整（BA）实时运行^[17]。然而，PTAM是为小型桌面场景中的增强现实应用而设计的，需要进行多次修改（例如，限制关键帧的数量），以允许在大规模室外环境中操作^[2]。

  早期的直接单目SLAM方法跟踪并绘制了少数有时手动选择的平面面片^[18]–[21]。第一种方法^{[18] [19]}使用滤波算法来估计结构和运动，而后来的方法^[20]–[22]使用非线性最小二乘优化。所有这些方法都是估计补丁的表面法线，这允许在广泛的视点范围内跟踪面片，从而大大减少估计中的漂移。文章^[19]–[21]的作者实现了实时性能，但是，只有少数选定的平面区域和小数据集具有实时性能。文章^[22]中提出了一种用于车载全向摄像机的VO算法。在^[8]中，放松了局部平面性假设，并提出了针对立体相机计算的任意三维结构的直接跟踪。在^[9]-[11]中，同样的方法也适用于RGB-D传感器。在DTAM^[15]中，引入了一种新的直接方法，该方法通过最小化全局空间正则化能量泛函数来计算每个关键帧的密集深度图。通过使用深度贴图直接对齐整个图像，可以找到相机的姿势。这种方法的计算强度非常高，只有通过GPU的大量并行才能实现。为了减少计算需求，文章^[23]中描述的方法，在本工作的审查过程中发表，只使用强梯度的像素。

C. 贡献与概要

  我们提出的半直接视觉里程计（SVO）算法采用特征对应，然而特征响应是直接运动估计的隐式结果，而不是显式特征提取和匹配的结果。因此只有在选择关键帧初始化新的3D点时，才需要进行特征提取(见图1)。其优点是通过减少每帧的特征提取而提高了速度，通过亚像素特征对应提高了精度。与以前的直接方法相比，我们使用许多(数百)小块而不是少数(数十)大平面块^[18]-[21]。使用许多小面片可以增强鲁棒性，并允许忽略面片法线。提出的用于运动估计的基于稀疏模型的图像对齐算法与基于模型的稠密图像对齐相关^{[8]–[10] [24]}。然而，我们证明了稀疏的深度信息足以获得运动的粗略估计并找到特征对应。一旦建立了特征对应和摄像机姿态的初始估计，该算法将继续只使用点特征，因此名称为“半直接”。这种转换允许我们依赖快速和建立的框架来调整BA（例如^[25]）。

  使用显式建模离群测量值的贝叶斯过滤器来估计特征位置处的深度。仅当相应的深度过滤器已收敛时，才在地图中插入3D点，这需要多次测量。结果是一张地图，只有很少的异常值以及很多可以可靠跟踪的点。

  本文的贡献是:

• 一种新的半直接VO方法，它比目前最先进的 MAV 更快、更准确;

• 集成了一种概率映射方法，对异常值测量具有鲁棒性。

  第二节概述了方法，第三节介绍了一些必要的符号，第四节和第五节解释了提出的运动估计和映射算法，第七节提供了实验结果和比较。

II. 系统概述

  图1 提供了 SVO 的概述。该算法使用两个并行线程（如^[16]），一个用于估计相机的运动，另一个用于在探索环境时进行映射。这种分离允许在一个线程中进行快速和恒定的时间跟踪，而第二个线程扩展映射，与硬实时约束解耦。

  运动估计线程实现了所提出的相对姿态估计的半直接方法。第一步是通过基于 parse 模型的图像对齐进行姿势初始化：通过最小化与相同3D点的投影位置对应的像素之间的光度误差来找到相对于前一帧的相机姿态（见图2）。下一步通过对齐对应的特征片来细化重投影点对应的2D坐标（见图3）。运动估计通过最小化前面特征对齐步骤中引入的重投影误差来细化姿态和结构。

  在建图线程中，为每个要估计相应3D点的2D特征初始化一个概率深度过滤器。只要在图像中很少发现3D到2D对应的区域中选择了新的关键帧，就会初始化新的深度过滤器。过滤器在深度上具有很大的不确定性。在随后的每一帧，深度估计都以贝叶斯方式更新（见图5）。当深度过滤器的不确定性足够小时，将在地图中插入一个新的3D点，并立即用于运动估计。

III. 标记

  在详细介绍算法之前，我们简要定义了本文中使用的符号。

  在时间步长 $k$ 采集的强度图像用 $I_k$ 表示：$\Omega \subset \mathbb{R}^2 \mapsto \mathbb{R}$，在这里 $\Omega$ 是图像域。任意三维点$\mathbf{p}=(x, y, z)^{\top} \in \mathcal{S}$ 在可见场景曲面上 $\mathcal{S}$ 通过摄像机投影模型$\pi ：\mathbb{R}^3 \mapsto \mathbb{R}^2$ 映射到图像坐标 $\mathbf{u}=(u,v)^{\top} \in \Omega$ ： $$ \mathbf{u}=\pi\left({ }_{k} \mathbf{p}\right) {\tag 1} $$

其中 $k$ 表示点坐标是相机参考帧 $k$ 中的。投影 $\pi$ 由通过校准已知的固有摄像机参数确定。给定逆投影函数 $\pi^{-1}$ 和深度 $d_\mathbf{u} \in \mathcal{R}$，可以恢复与图像坐标对应的3D点: $$ {}_k\mathbf{p} = \pi^{-1}(\mathbf{u},d_\mathbf{u}) {\tag 2} $$ 其中 $\mathcal{R} \subseteq \Omega$ 域是深度已知的。

  在时间 $k$ 相机的位置与方向使用刚体变换 $T_{k,w} \in SE(3)$ 表示。它允许我们将一个3D点从世界坐标系映射到相机坐标系：${}_k\mathbf{p} = T_{k,w} · {}_w\mathbf{p}$ 。两个连续帧之间的相对变换可用$T_{k,k−1}=T_{k,w}·T^{−1}_{k−1,w}$ 来计算。在优化过程中，我们需要一个最小的变换表示，因此，在恒等式处使用对应于切空间 $SE(3)$ 的李代数 $\mathfrak{s e}(3)$。我们用 $\xi =(ω，ν)^T \in \mathbb{R}^6$ 表示代数元素，也称为扭转坐标，式中ω称为角速度，ν称为线速度。扭转坐标 $\xi$ 通过指数映射^[26]映射到 $SE(3)$： $$ \mathbf T(\xi) = \exp (\hat{\xi}) {\tag 3} $$

IV. 运动估计

  SVO使用直接方法计算相对摄像机运动和特征对应的初始猜测，并基于特征的非线性重投影误差细化。下面将详细介绍每个步骤，并在 图2 到 图4 中进行说明。

A. 基于稀疏模型的图像对齐

  两次连续的相机的位姿变换 $\mathbf T_{k,k-1}$ 的最大似然估计是最小化强度残差的负对数似然： $$ \mathbf{T}_{k, k-1}=\arg \min _{\mathbf{T}} \iint_{\overline{\mathcal{R}}} \rho[\delta I(\mathbf{T}, \mathbf{u})] d \mathbf{u} {\tag 4} $$ 定义强度残差 $\delta I$ 为对同一3D点的像素之间光度差。它可以通过从之前的图像 $I_{k-1}$ 一个2D点反向投影，然后将其投影到当前的相机视图来计算： $$ \delta I(\mathbf{T}, \mathbf{u})=I_{k}\left(\pi\left(\mathbf{T} \cdot \pi^{-1}\left(\mathbf{u}, d_{\mathbf{u}}\right)\right)\right)-I_{k-1}(\mathbf{u}) \quad \forall \mathbf{u} \in \overline{\mathcal{R}} {\tag 5} $$

其中 $\mathcal{R}$ 是指其深度 $d_{\mathbf u}$ 在 $k-1$ 时间已知，且投影点在当前图像可见的图像域： $$ \overline{\mathcal{R}}=\lbrace\mathbf{u} \mid \mathbf{u} \in \mathcal{R}_{k-1} \wedge \pi\lparen\mathbf{T} \cdot \pi^{-1}\lparen\mathbf{u}, d_{\mathbf{u}}\rparen \rparen \in \Omega_k \rbrace {\tag 6} $$ 为了简单起见，我们在下面假设强度残差是正态分布的，且具有单位方差。负对数似然极小值对应于最小二乘问题：$\rho[.] \hat{=} \frac{1}{2}|\cdot|^{2}$ 。实际上，由于遮挡，分布具有较重的尾部，因此必须应用稳健的成本函数[10]。

  与以前的工作不同，在以前的工作中，图像中的大区域的深度是已知的^{[8]–[10] [24]}，而我们这里只知道稀疏特征位置 $\mathbf u_i$ 的深度 $d_\mathbf {u_i}$。我们用矢量 $\mathbf I(\mathbf u_i)$ 表示特征点周围4×4像素的小块。我们试图找到使所有面片的光度误差最小化的相机位姿（见图2）： $$ \mathbf{T}_{k, k-1}=\arg \min _{\mathbf{T}_{k, k-1}} \frac{1}{2} \sum_{i \in \overline{\mathcal{R}}}\left|\delta \mathbf{I}\left(\mathbf{T}_{k, k-1}, \mathbf{u}_{i}\right)\right|^{2} {\tag 7} $$ 由于方程(7)对 $\mathbf{T}_{k, k-1}$ 是非线性的，我们用迭代高斯-牛顿法求解它。给定一个相对变换的估计 $\hat {\mathbf{T}}_{k, k-1}$，该估计的增量更新 $\mathbf T(\xi)$ 可以用一个小量 $\xi \in \mathfrak{s e}(3)$ 参数化。我们使用强度残差的逆复合公式^[27]，计算时间 $k−1$ 时参考图像的更新步长 $\mathbf T(\xi)$: $$ \delta \mathbf{I}\left(\xi, \mathbf{u}_{i}\right)=\mathbf{I}_{k}\left(\pi\left(\hat{\mathbf{T}}_{k, k-1} \cdot \mathbf{p}_{i}\right)\right)-\mathbf{I}_{k-1}\left(\pi\left(\mathbf{T}(\xi) \cdot \mathbf{p}_{i}\right)\right) {\tag 8} $$ 其中 $\mathbf{p} = \pi^{-1}(\mathbf{u},d_\mathbf{u})$。然后，使用等式（3）将更新步骤的倒数应用于当前估计： $$ \hat {\mathbf T}_{k,k-1} \gets \hat {\mathbf T}_{k,k-1} \cdot \mathbf T(\xi)^{-1} {\tag 9} $$ 注意，我们不会因为计算速度的原因而扭曲补丁。这个假设在帧到帧的小运动和小补丁大小的情况下是有效的。

  为了找到最优更新步长 $\mathbf T(\xi)$，我们计算(7)的导数并设为零: $$ \sum_{i \in \overline{\mathcal{R}}} \nabla \delta \mathbf{I}\left(\xi, \mathbf{u}_{i}\right)^{\top} \delta \mathbf{I}\left(\xi, \mathbf{u}_{i}\right)=0 $$ 为了解决这个系统，我们将当前状态线性化: $$ \delta \mathbf{I}\left(\xi, \mathbf{u}_{i}\right) \approx \delta \mathbf{I}\left(0, \mathbf{u}_{i}\right)+\nabla \delta \mathbf{I}\left(0, \mathbf{u}_{i}\right) \cdot \xi $$ 其中雅克比矩阵 $\mathbf J_i :=\nabla \delta \mathbf{I}\left(0, \mathbf{u}_{i}\right) $ 具有16×6维度，因为面片大小为4×4，并用链式规则计算： $$ \frac{\partial \delta \mathbf{I}\left(\xi, \mathbf{u}_{i}\right)}{\partial \xi}=\left.\left.\left.\frac{\partial \mathbf{I}_{k-1}(\mathbf{a})}{\partial \mathbf{a}}\right|_{\mathbf{a}=\mathbf{u}_{i}} \cdot \frac{\partial \pi(\mathbf{b})}{\partial \mathbf{b}}\right|_{\mathbf{b}=\mathbf{p}_{i}} \cdot \frac{\partial \mathbf{T}(\xi)}{\partial \xi}\right|_{\xi=0} \cdot \mathbf{p}_{i} $$ 通过将（11）插入（10）并将雅可比矩阵叠加在矩阵 $\mathbf J$ 中，我们得到了正规方程： $$ \mathbf J^{\top} \mathbf J \xi = - \mathbf J^{\top} \delta \mathbf I(0) {\tag {12}} $$ 这样可解决更新 $\xi$。注意，通过使用逆合成方法，可以预先计算雅可比矩阵，因为它在所有迭代中保持恒定(参考面片 $\mathbf I_{k-1} (\mathbf u_i)$ 和点 $\mathbf p_i$ 不变)，这将导致显著的加速^[27]。

B. 通过特征对齐实现松弛

  下一步是将相机与前一帧对齐。通过反向投影，找到的相对位姿 $\mathbf T_{k,k−1}$ 隐式地定义了新图像中所有可见3D点的特征位置的初始猜测。由于3D点的位置不准确，因此相机姿势不准确，因此可以改进这种初始猜测。为了减少漂移，相机的位姿应该与地图对齐，而不是与前一帧对齐。

  从估计的相机位姿可以将看到的地图的所有3D点都投影到图像中，从而估计出相应的2D特征位置 $\mathbf u_i^{\prime}$（见图3）。对于每个重投影的点，将具有最近的观察角度识别为关键帧 $r$ 。然后，特征对齐步骤通过最小化当前图像中的补丁相对于关键帧器中的参考补丁的光度误差来单独优化新图像中的所有2D特征位置 $\mathbf u_i$：

$$ \mathbf u_i^{\prime}=\arg \min _{\mathbf u_i^{\prime}} \frac{1}{2}\lVert\mathbf{I}_{k}(\mathbf u_i^{\prime})-\mathbf A_i \cdot \mathbf I_r(\mathbf u_i)\rVert^{2}, \quad \forall i {\tag{13}} $$

这种对齐是用逆合成 Lucas-Kanade 算法^[27]来解决的。与前面的步骤相反，我们对参考补丁应用了一个仿射扭曲 $\mathbf A_i$，因为最近的关键帧通常比前一张图像更远，所以使用了更大的补丁大小（8×8像素）。

  这一步可以理解为一个松弛步骤，它违反了极线约束，以实现特征面片之间更高的相关性。

C. 位姿与结构优化

  在前一步中，我们以违反极线约束为代价，建立了具有亚像素精度的特征对应。特别是，我们生成了重投影误残差 $\lVert \mathbf \delta \mathbf u_i \rVert = \lVert \mathbf u_i - \pi(\mathbf T_{k,w} \enspace {}_w \mathbf p_i )\rVert \neq 0$，平均为 0.3 个像素。在最后一步，我们再一次通过最小化重投影残差来优化相机位姿 $\mathbf T_{k,w}$： $$ \mathbf{T}_{k, w}=\arg \min _{\mathbf{T}_{k, w}} \frac{1}{2} \sum_{i}\left|\mathbf{u}_{i}-\pi\left(\mathbf{T}_{k, w} w \mathbf{p}_{i}\right)\right|^{2} {\tag {14}} $$ 这是一个众所周知的仅运动 BA 问题，并且可以有效地使用迭代非线性最小二乘最小化算法（如高斯牛顿）来解决。

  随后，我们通过最小化重投影误差（structure-only BA）优化观察到的三维点的位置。最后，可以应用局部BA，将所有闭合关键帧的位姿和观察到的三维点共同优化。在算法的快速参数设置中，我们忽略了 BA 步骤（章节VII）。

D. 讨论

  算法的第一个（第IV-A部分）和最后一个（第IV-C部分）优化似乎是多余的，因为两者都优化了相机的6自由度姿态。实际上，我们可以直接从第二步开始，通过 Lucas-Kanade 跟踪所有特征补丁^[27]，建立特征响应，然后进行非线性姿态细化（第IV-C节）。虽然这种方法可行，但处理时间会更长。在大距离上跟踪所有功能（例如，30像素）需要一个更大的补丁和金字塔实现。此外，一些特征可能被跟踪不准确，这将需要异常值检测。然而，在SVO中，特征对齐通过在稀疏图像对齐步骤中优化6个参数（相机姿态）来有效地初始化。稀疏图像对齐步骤隐式地满足极线约束，并确保不存在异常值。

  有人可能会说，第一步（稀疏图像对齐）将足以估计相机的运动。实际上，这是最近为RGB-D相机开发的算法^[10]所做的，然而，通过对齐完整的深度地图而不是稀疏的补丁。我们从经验上发现，与同时使用所有三个步骤相比，使用第一个步骤只会导致更大的漂移。精度的提高是由于新图像相对于关键帧和地图对齐，而稀疏图像对齐仅相对于前一帧对齐新帧。

V. 建图

  给定一幅图像及其位姿 ${ I_k,\mathbf T_{k,w} }$，建图线程估计对应的未知三维点的二维特征的深度。特征的深度估计采用概率分布建模。每个后续观测 ${ I_k,\mathbf T_{k,w} }$ 都用于更新贝叶斯框架中的分布（见图5），如^[28]所示。当分布的方差变得足够小时，使用（2）将深度估计转换为3D点，该点插入地图并立即用于运动估计（见图1）。在下文中，我们在^[28]中报告了基本结果和对原始实现的修改。

  每一个深度过滤器都与一个参考关键帧 $r$ 关联。初始化滤波器在深度上为高度不确定性，并将平均值设置为参考帧中的平均场景深度。对于每个后续观测 ${ I_k,\mathbf T_{k,w} }$，我们在新图像 $I_k$ 的极线上搜索与参考面片具有最高相关性的面片。极线可以从帧 $\mathbf T_{r,k}$ 和穿过 $\mathbf u_i$ 的光线之间的相对姿势计算出来。可通过三角测量找到与深度 $\widetilde {d}_i^k$ 相关最高的相关点 $\mathbf u^{\prime}_i$（见图5）。

  测量 $\widetilde {d}_i^k$ 通过 Gaussian + Uniform 混合模型建模分布：一个好的测量是在真实深度周围正态分布的，而一个异常值测量是在区间 $\lbrack d^{min}_i,d^{max}_i \rbrack$ 内均匀分布的: $$ p\left(\tilde{d}_{i}^{k} \mid d_{i}, \rho_{i}\right)=\rho_{i} \mathcal{N}\left(\tilde{d}_{i}^{k} \mid d_{i}, \tau_{i}^{2}\right)+\left(1-\rho_{i}\right) \mathcal{U}\left(\tilde{d}_{i}^{k} \mid d_{i}^{\min }, d_{i}^{\max }\right) $$ 其中 $\rho_{i}$ 是一个好的测量的概率，$\tau_{i}^{2}$ 是一个好的测量的方差，可以通过假设图像平面中的一个像素的光度差异方差进行几何计算^[29]。

  在^[28]中详细描述了该模型的递归贝叶斯更新步骤。与^[28]相反，我们使用反向深度坐标来处理较大的场景深度。

  本文提出的深度估计方法在仅搜索当前极线深度估计的一小段范围时是非常有效的；在我们的例子中，这个范围相当于当前深度估计的两倍标准差。图6 展示了只需很少的运动就可以显著降低深度的不确定性。与标准的从两个角度对点进行三角测量相比，提出的方法的主要优点是，我们观察到的离群值要少得多，因为每个滤波器都要经过许多测量直到收敛。此外，错误的测量被明确地建模，这允许深度收敛，即使在高度相似的环境。在^[29]中，我们将演示如何将相同的方法用于稠密建图。