SVO：Fast Semi-Direct Monocular Visual Odometry论文翻译

简介

SVO: Fast Semi-Direct Monocular Visual Odometry
SVO：半直接法单目视觉里程计

0.摘要

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

1.引言

无人机将在灾害管理、工业检查和环境保护中发挥重要作用，对于这些应用基于GPS的自主导航信息将无法满足要求。精确的完全自主操作需要无人机依赖于其他定位系统。为了将无人机重量和功耗降至最低，建议仅使用单个向下的相机和惯性测量单元[1]-[5]。这种设置允许在室外区域[1]-[3]进行完全自主的航点跟踪，以及MAV和地面机器人之间的协作[4]，[5]。

据我们所知目前所有用于MAV的单目视觉里程计系统都是基于特征点法实现的。对于RGB-D或者双目SLAM系统，基于最小化光度误差的直接法更受欢迎。

在这项工作中我们提出一种半直接法VO，该方法结合了特征点法的优势(跟踪大量的特征，并行跟踪和建图，关键帧选择策略)，同时具备直接法的速度和精度。基于的高帧率VO的MAV有望提高稳健性和更快的飞行动作。

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

同时从视频中恢复摄像机位姿和场景结构的方法可以分为两类：

a）基于特征的方法：
标准的方法是在每个图像中提取一组稀疏的显著的图像特征（例如点、线）；使用不变的特征描述子在连续的帧中进行匹配；使用对极几何鲁棒地恢复相机的运动和结构；最后，通过最小化重投影误差来优化位姿和结构。大多数VO算法[12]遵循此过程，和优化框架之间彼此独立。这种基于特征的方法的优势在于通过鲁棒的特征检测器和描述子使得在运动比较大的图像帧之间也可以匹配成功。基于特征的方法的缺点是依赖于检测和匹配阈值，必须使用鲁棒估计技术来处理错误的特征匹配关系，而且大多数特征检测都是针对速度而不是精度进行优化的，因此，运动估计中的漂移必须通过对多个特征测量进行平均来补偿。

b）直接法：
直接从图像中的强度值（单通道图像像素值的大小）估计场景结构和运动。与仅考虑到某些特征位置的距离的基于特征的方法相比，直接法在优化中使用局部强度梯度大小和方向。利用图像中的所有信息的直接方法，即使是来自梯度很小的区域，在纹理很少的场景中，或者在相机无法聚焦和运动模糊[15]的情况下，在鲁棒性方面优于基于特征的方法[14]。光度误差的计算比重投影误差更为密集，因为它涉及到大图像区域的图像扭曲和积分（warping and integrating）。然而，由于直接方法直接对图像的强度值进行操作，因此可以节省特征检测和不变描述子计算的时间。

B.相关工作

大多数用于无人机[1]、[2]、[7]的单目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]。我们证明了稀疏的深度信息足以得到运动的粗略估计和特征匹配。一旦建立了特征匹配和摄像机姿态的初始估计，该算法将继续只使用点特征；因此，命名为“半直接”。这种转换使我们可以依赖于快速和成熟的框架来进行光束平差（例如，[25]）。

用一个贝叶斯滤波器显式地建模异常测量值，来估计特征位置的深度值。只有当相应的深度滤波器已收敛时，才会在地图中插入三维点，这需要多次测量。在得到深度值之后可以创建一个包含少量外点和特征点的地图，该地图可以被可靠的跟踪。

本文的主要贡献是：（1）一种比现有的无人机定位系统更快、更精确的半直接VO pipeline；（2）集成了一种对异常测量具有鲁棒性的概率建图方法。

本文在第2节介绍了整个系统框架，第3节介绍了所需要的公式，第4节和第5节分别介绍了运动估计方法和建图方法。

2.系统总述

图1展示了SVO算法的系统框架，整个算法包括两个并行的线程，一个是相机运动估计，另一个是当环境被探索到时对当前时刻进行地图构建。这种并行的方式使得系统在一个线程可以保持快速恒定跟踪的同时，在另一个线程可以进行地图更新，无需和跟踪保持同样的实时性。

运动估计线程主要是使用所提出的半直接法估计相关位姿。第一步是通过基于稀疏模型的图像对齐来初始化位姿：通过最小化相同3D点的投影位置相匹配的像素之间的光度误差来找到相对于前一帧的相机姿势（见图2）。(对于一个3D点，在前一帧图像存在一个特征点p1和其一一对应，同时取该特征点周围的一个4x4像素块patch1,该3D点在当前帧同样有一个对应的投影特征点p2和对应的像素块patch2，通过最小化两个像素块的光度误差为p1找到在当前帧对应的精确的p2的坐标)。

图2：改变当前帧和前一帧之间的相对姿势Tk，k−1隐式地移动新图像u′i中重投影点的位置。稀疏图像对齐找到Tk，k−1，将匹配于同一3D点（蓝色方块）的图像面片之间的光度差减到最小。注意，在所有图中，要优化的参数用红色表示，优化成本用蓝色显示。

在下一步中，通过对齐相应的特征的像素块（参见图3）细化与重投影点相匹配的二维坐标。运动估计是通过优化位姿和结构来完成的，即通过最小化前一个特征对齐步骤中引入的重投影(光度)误差来完成的。

图3：由于3D点和相机位姿估计的不精确性，通过单独优化每个像素块对应的2D位置，可以进一步最小化当前帧中的相应像素块（蓝色方块）与先前关键帧ri匹配特征对应的像素块之间的光度误差。

在最后一个运动估计步骤中，对相机姿态和结构(3d)点进行优化，以最小化在上一个特征对齐步骤中建立的重投影误差。

建图线程为每个2D特征初始化一个概率深度滤波器，以实现对和特征点对应3D点的估计。每当新的关键帧从图像区域中挑选出来，只要有很少的3D到2D匹配关系被找到，新的深度滤波器就会被初始化。始化时深度有很大的不确定性。在随后的每一帧，深度估计都会以贝叶斯方式更新（见图5）。当深度滤波器的不确定性变得足够小时，一个新的3D点被插入地图，并立即用于运动估计。

图5：参考帧r中特征i的概率深度估计diˆ。真实深度处的点投影到两个图像中相似的图像区域（蓝色方块）。因此，根据与参考像素块相关度最高的点u′i计算的三角化深度dki~更新深度估计值。在新图像中，相关性最高的点总是位于极线上。

3.公式符号定义

4.运动估计

SVO使用直接法，通过计算相机运动和特征匹配的初步估计，并以基于特征的非线性重投影误差优化为结论。
以下各节详细说明了每个步骤，并在图2至图4中进行了说明。

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

本文中的图像对齐可以理解为图像帧之间的运动估计,面片即特征点邻域内的4x4像素块。

在系统运行过程中，帧间的匹配是现将上一帧位姿作为当前帧位姿的初始值，然后使用最小光度误差的方式对当前帧特征点的位置和位姿进行迭代。

特征点提取只在创建关键帧时进行，初始化时使用的图像帧会被首先创建为关键帧。

B.通过特征对齐松弛(Relaxation)

这一步可以联想到ORB-SLAM中的局部地图跟踪，主要是将当前地图中当前帧能够观测到的3D点重投影到当前图像帧，光度误差使用参考关键帧与当前帧进行计算。

图片中逆合成光流Inverse Compositional Image Alignment即为逆向光流法，仿射扭曲即为仿射变换。

这里需要理解一下逆向光流法，见链接https://blog.csdn.net/Armergg/article/details/106567744

C.位姿和地图点优化

这一步就是在找到匹配和定义好残差函数之后进行BA求解。

D.讨论

算法的第一个（第四-A节）和最后一个（第四-C节）优化似乎是多余的，因为这两个都优化了相机的6自由度位姿。实际上，我们可以直接从第二步开始，通过Lucas Kanade跟踪所有特征面片[27]来建立特征匹配关系，然后进行非线性位姿优化（第四-C节）。虽然这样可以工作，但处理时间会更长。在大距离（例如，30像素）上跟踪所有特征需要更大的像素块和金字塔实现。此外，有些特征可能会被错误地跟踪，这就需要异常值检测。然而，在SVO中，在稀疏图像对齐步骤中，只需优化摄像机姿态的六个参数即可有效地初始化特征对齐。在稀疏图像对齐步骤隐式地满足了对极约束，并确保没有异常值。

也有人认为第一步（稀疏图像对齐）足以估计摄像机运动。在事实上，这就是最近为RGB-D相机开发的算法所做的[10]，然而，通过对齐全部深度地图而不是稀疏的像素块。我们从经验上发现，与同时使用所有三个步骤相比，使用第一步只会导致更多的漂移。改进的精度是由于新图像相对于关键帧和地图进行对齐，而稀疏图像对齐仅将新帧与前一帧对齐。

5.建图

[28]中详细描述了该模型的递归贝叶斯更新步骤。与[28]相比，我们使用逆深度坐标来处理大场景深度。当只搜索极线上当前深度估计值周围的小范围时，所提出的深度估计值是非常有效的；在我们的情况下，该范围相当于当前深度估计值标准差的两倍。图6展示了如何减少深度的不确定性所需的运动量。与标准的两视图三角测量方法相比，本文提出的方法的主要优点是，每个滤波器经过多次测量直到收敛，这样观察到的异常值要少得多。此外，错误的测量被显式地建模，这使得深度即使在高度相似的环境中也能收敛。在[29]中，我们演示了如何将相同的方法用于稠密建图。

图6：为了使深度滤波器（如红线所示）的不确定性收敛，无人机（从顶部看）只需要很少的运动。

6.实施细节

基于单应矩阵的初始化，因为论文基于无人机应用出发，且相机视野向下，因此可以认为是平面初始化，后面贺一家的优化版本SVO_Edgelet同时使用基础矩阵和单应矩阵进行初始化。
该算法通过引导（bootstrapped）得到前两个关键帧的位姿和初始地图。和[16]一样，我们假设一个局部平面场景并估计一个单应矩阵。初始地图是从前两个视图三角化的。

应对大运动进行5层图像金字塔处理
为了处理更大的运动，我们采用了由粗到细的稀疏图像对齐算法。对图像进行半采样以创建五个级别的图像金字塔。然后在最粗级别上优化强度残差，直到收敛。随后，在下一个更精细的级别初始化优化。为了节省处理时间，我们在第三级收敛后停止，此时估计值足够精确，可以初始化特征对齐。

关键帧选择有点像滑窗，维持固定的关键帧数量
为了提高效率，该算法在地图中保持固定数量的关键帧，这些关键帧被用作特征对齐和结构细化的参考。如果新帧相对于所有关键帧的欧式距离超过平均场景深度的12%，就被选为关键帧。在地图中插入新的关键帧时，将移除距离相机当前位置最远的关键帧。

特征点的均匀化处理参考了Shi-Tomasi角点
在建图线程中，我们将图像分成固定大小的单元（30×30像素）。除非已经存在二维到三维的匹配关系，否则就在单元中具有最高Shi-Tomasi分数的快速角点[30]处初始化一个新的深度滤波器。这将导致图像中的特征均匀分布。相同的网格也用于在特征对齐之前重投影地图。请注意，我们在图像金字塔的每个级别提取快速角点，以找到与比例无关的最佳角点。

7.实验结果

实验是在一个向下看的相机上记录的数据集和一个手持相机的序列上进行的。视频在笔记本和安装在无人机上的嵌入式平台上处理（参见图17）。注意，算法最多使用2个CPU内核。在消费者笔记本电脑上进行的实验有两个不同的参数设置，一个优化速度，一个优化精度（表一）。在嵌入式平台上，只用到了速度参数。

图17:KMel机器人公司的“Nano+”，定制嵌入式处理器和向下看的摄像头。SVO在平台上以每秒55帧的速度运行，用于稳定和控制。

表一：SVO的两种不同参数设置。
比较了SVO算法与文献[2]中改进的PTAM算法的性能。我们没有与原始版本的PTAM[16]进行比较的原因是它不能处理大环境，并且在高频纹理的场景中不够健壮[2]。文献[2]的版本解决了这些问题，并就我们所知构成了无人机中性能最好的单目SLAM算法。

A、精确度

我们对同样在[2]中使用过的数据集进行精度评估，如图7所示。真实地面（ground-truth）来源于一个运动捕捉系统。飞行轨迹长84米，无人机飞行平均离地高度1.2米。

图7:快速参数设置的SVO（见表一）与PTAM和真实地面对比。放大显示，该算法生成的轨迹比PTAM平滑。
图8和图9说明了随时间变化的位置和姿态误差。为了生成平面图，我们使用[31]将前10帧与实际情况对齐。PTAM的结果与文献[2]中报告的范围相似。由于平面图高度依赖于前10帧的对准精度，我们还报告了以米每秒为单位的偏移表II。

表二：图7中轨迹的相对位姿和旋转误差

图8:SVO的位置漂移，用快速和精确参数设置，并与PTAM进行比较。

总的来说，两个版本的SVO都比PTAM更精确。我们怀疑这一结果的主要原因是[2]的PTAM版本没有在最高分辨率的金字塔级别上提取特征，并且没有对PTAM中的所有特征进行亚像素细化。忽略最高分辨率的图像不可避免地会导致精度降低，这在图7中清晰可见。在文献[2]中，低分辨率图像的使用是因为图像中的高频自相似纹理会产生太多的异常3D点。SVO有效地解决了这一问题，通过使用深度滤波器可以得到很少的异常值。

由于相机只是一个角度传感器，所以不可能通过运动管道（pipeline）从结构中获取地图的比例。因此，在上述评估中，我们还将前10次测量的比例与地面真实情况相一致。建议的管道传播比例，但是有一些漂移，如图10所示。尺度漂移是通过比较相对平移的欧几里德范数与实际情况来计算的。未知的尺度和尺度漂移促使人们需要一个用于无人机控制的相机IMU状态估计系统，如[33]所述。

图10：图7所示轨迹随时间的漂移

图11显示了平均重投影误差。稀疏图像对齐步骤使帧非常接近最终位姿，因为细化步骤仅略微减少了误差。重投影误差是在特征对齐步骤中“生成”的；因此，该图还显示在该步骤中，面片仅移动一小部分的像素。

图11：图7所示轨迹随时间的平均重投影误差。初始误差是在稀疏图像对齐（第四-A节）之后，最终误差是在位姿优化之后（第四-C节）。

快速和精确参数设置之间的精度差异不显著。在每次迭代（快速参数设置）中分别优化位姿和观察到的3D点，对于无人机运动估计而言足够精确。

B、运行时间评估

图13和图14显示了在快速参数设置下，分别在指定的笔记本电脑和嵌入式平台上计算相机运动所需的时间。笔记本电脑能够以每秒300帧（fps）的速度处理，嵌入式平台的处理速度为每秒55帧。PTAM对应的时间分别为91 fps和27 fps。主要的区别是SVO不需要在运动估计过程中提取特征，而是PTAM的主要耗时（笔记本电脑为7ms，嵌入式计算机为16ms）。此外，PTAM跟踪160到220个特征，而在快速参数设置中，该值被限制为120。我们能用较少的特征可靠地跟踪相机的原因是使用了深度滤波器，从而确保跟踪到的特征是有效的。在笔记本电脑上，精确参数设置的运动估计平均需要6毫秒。时间的增加主要是由于局部BA，它在每个关键帧处运行，耗时14ms，建图线程用新帧更新所有深度滤波器所需的时间高度依赖于滤波器的数量。在选定关键帧后，滤波器的数量很高，并且随着滤波器的收敛（converge）而迅速减少。平均而言，建图线程比运动估计线程快，因此它不是限制因素。

C、稳健性

SVO的速度和精度部分归功于深度滤波器，它只产生最少数量的异常点。另外，健壮性也是由于深度滤波器：精确、高帧率跟踪允许滤波器即使在重复和高频纹理的场景（如沥青、草地）中也能收敛，这在本文随附的视频中得到了最好的演示。视频的屏幕截图如图15所示。图16显示了在同一场景中使用PTAM和SVO生成的建图的比较。虽然PTAM生成异常3D点，相比之下，SVO几乎没有异常点，这要归功于深度滤波器的使用。

八、结论

在本文中，我们提出了半直接VO管道（pipeline）“SVO”，它比现有的VO更加精确和快速。速度的提高是由于运动估计不需要特征提取和匹配。取而代之的是一种直接的方法，它直接基于图像的强度。该算法在当前的嵌入式计算机上以每秒50帧以上的速度运行，特别适用于无人机的状态估计。高帧速率运动估计结合抗异常点的概率建图方法，在纹理较少、重复和高频的场景中提高了鲁棒性。