自动驾驶中的高精度单目SFM与尺度校正

摘要

我们提出了一种实时单目视觉里程计系统，在真实世界自动驾驶应用中实现了高精度。首先，我们展示了鲁棒的单目 SFM（结构与运动恢复），通过利用多线程处理具有大幅运动和快速变化图像的驾驶场景。为了校正尺度漂移，我们使用相机相对于地面平面的已知高度。我们的第二个贡献是一种新颖的数据驱动机制，用于线索融合，该机制基于每帧视觉数据推断出的稀疏特征匹配和帧间稠密立体匹配等多线索的相对置信度，自适应调整观测协方差，从而实现精确的地面平面估计。最后，我们在具有挑战性的KITTI数据集上展示了广泛的基准性能和对比结果，精度可与立体视觉方法相媲美，并超过以往的单目系统。我们的SFM系统在最坏情况下可在50毫秒内输出位姿，平均运行速度超过30帧每秒。我们的框架还显著提升了依赖于地面平面的物体定位等应用的精度。

索引术语

单目运动结构重建，尺度漂移，地面平面估计，对象定位 n F

1 引言

STRUCTURE 运动恢复结构（SFM）在真实世界户外自动驾驶中的应用是近年来备受关注的问题。本文提出了一种基于单目视觉的实时系统，该系统依靠多线程SFM在自动驾驶领域的多项创新技术，能够在跨越数公里真实环境的序列中实现卓越的精度。在具有挑战性的KITTI数据集上[1],，我们的旋转精度达到每米 0.0057度，甚至优于多个最先进的立体视觉系统。我们的平移误差仅为2.53%，这一结果同样可与立体视觉系统相媲美，并且优于此前最先进的单目系统。

尽管立体运动结构重建系统通常能够实现高精度和实时性能，但对于单目系统而言，这一挑战仍然十分艰巨。然而，单目系统对汽车行业具有吸引力，因为它们成本更低，且标定工作量更小。近年来消费级相机的成本持续下降，但用于汽车中实际SFM应用的相机价格仍然昂贵，原因是产量较小，且必须支持高帧率，并能适应极端温度、恶劣天气和抖动，具备良好的鲁棒性。

单目视觉里程计在自动驾驶中的挑战既有基础性，也有实际性。例如，已有研究从实验和理论上观察到，当极点位于图像内部时的前向运动是视觉SFM[3]的“高误差”情况。在室外环境中，车辆速度可能很高，因此即使使用高帧率相机，连续帧之间也可能发生大幅运动。

通过长距离约束和彻底的捆绑调整，且无延迟。自动驾驶中视觉里程计的时序要求同样非常严格。因此，我们的系统针对最坏情况时序场景进行了优化，而不是像大多数传统系统那样进行平均情况优化。例如，传统系统在添加关键帧或执行回环闭合时可能会导致计时出现峰值[4]。特别是，我们的多线程系统无论是否添加关键帧或执行尺度校正，位姿输出最多耗时50毫秒。我们系统的平均帧率要高得多，超过30帧每秒。

基于单目视觉的框架由于成本较低且标定需求较少而具有吸引力。然而，缺乏固定的立体视觉基线会导致不可避免的尺度漂移，这是阻碍单目视觉SFM达到与立体视觉相当精度的主要瓶颈。为了抑制尺度漂移，我们利用先验知识，即相机到地面平面的已知固定高度。因此，对地面平面进行鲁棒且精确的估计对于实现良好性能至关重要。然而，在实际的自动驾驶场景中，地面对应的是快速移动且低纹理的路面，这使得从图像数据中估计地面变得具有挑战性。

我们在第5和第6节中通过两项创新克服了这一挑战。首先，我们融合了多种地面平面估计方法提供的线索；其次，我们利用从训练数据中学习的模型，在一个框架中结合这些线索，并考虑它们的逐帧相对置信度。尽管先前的研究使用稀疏特征匹配进行地面平面估计 [2],[5],[6],，但在实践中明显不足，必须通过其他线索（如第5节中的平面引导的稠密立体匹配）加以补充。因此，在第5节中，我们提出除了稀疏三维点外，还引入连续帧之间的稠密立体线索以及用于物体定位应用的二维检测边界框线索。稠密立体线索显著提升了 SFM性能，而检测线索有助于物体定位。为了融合这些线索，第6节提出了一种新颖的数据驱动框架。在训练过程中，我们学习将每种线索的观测协方差与其底层变量的误差行为相关联的模型。例如，稠密立体线索的底层变量可能是SAD代价。在测试时，通过融合这些模型预测的协方差，使每种线索的贡献能够在逐帧基础上自适应调整，反映对其相对精度的置信度。我们在 KITTI数据集上的实验结果表明，该框架在地面平面估计方面取得了显著改进。进而，这使得在单目SFM和三维物体定位等应用中表现出优异的性能。

本文是我们先前研究[6],[7]的扩展。在KITTI视觉里程计训练集上，我们在[6],[7]和本研究中分别实现了 6.42%、3.37%和2.03%的平移误差。对于KITTI测试基准（其真实值未公开），平移误差从[7]中的3.21%降低到本研究中的2.53%。为了实现这些改进，在第3.2节和第3.3节的单目SFM架构中引入了额外的系统特性以允许在驾驶序列中对快速运动和接近静止的运动进行统一处理。我们多线程设计背后的更多直观解释见第3节。我们在第 7.1和7.2节展示了改进的结果，并说明了我们的系统可能出现的失败情况。此外，我们在第7.4和7.5节提供了与先前最先进方法及其他实现方案的新对比，以及更多解释性图表。

2 相关工作

基于立体的SFM系统现在通常能够在室内[4]和室外环境中实现实时性能。利用GPU能力的视觉立体 SFM并行实现已被证明在室内环境中可实现超过30帧每秒的帧率[4]。此外，还提出了几种方法，使用或结合来自其他采集模式的信息，如全向[9],超声波[10]或深度传感器[11]。

与之前的实时SFM系统相比，我们的系统架构经过精心设计，以应对高精度和高效单目自动驾驶的挑战。在第2.1节中，我们将讨论该设计的不同之处，以及其如何更适用于该应用场景并易于扩展。

2.1 单目架构

关于实时大规模视觉里程计的早期工作包括Nist´er等人提出的系统，该系统同时提出了立体视觉和单目系统[8]。近年来，一些纯基于视觉的单目系统已实现了良好的定位精度[12],[13],[14],[15]。例如，PTAM是一种优雅的双线程架构，将跟踪与建图部分分开[12]。它针对小范围工作环境设计，侧重于三维重建，并高度依赖对少量三维点的重复观测（“循环浏览运动”）。在我们的测试中，PTAM通常在快速前向移动车辆采集的数据集 （如KITTI）中运行30至50帧后便失效。相比之下，我们的系统旨在适用于大范围室外环境或场景点迅速移出视场的驾驶场景，具有良好的可扩展性。

另一类是基于扩展卡尔曼滤波器（EKF）的 Davison等人提出的V‐SLAM系统[16],[17],， Handa等人通过基于概率框架的主动匹配技术对其进行了改进[18]。Civera等人开发的EKFMonoSLAM系统提出在卡尔曼滤波器中集成单点RANSAC，在 RANSAC模型假设阶段利用EKF提供的先验概率信息 [19]。然而，众所周知，基于光束法平差的方法具有更好的可扩展性[20]。与我们的系统相对比的是Geiger等人在[2]中提出的VISO2‐M系统。该系统依赖于通过基础矩阵估计来匹配并计算每一对连续帧之间的相对位姿，并利用局部平面地面对尺度进行连续校正。然而，众所周知，在窄基线前向运动情况下，双视图估计会导致较大的平移误差[3]。

在第7节中，我们将我们的系统与EKFMonoSLAM和 VISO2‐M以及一个立体运动结构重建系统VISO2‐S[2]进行了比较。结果表明，我们的系统性能与立体视觉系统相当，并优于先前的单目系统。

2.2 尺度漂移校正

用于自主导航的大规模单目系统并不常见，主要原因是存在尺度漂移问题。Strasdat等人[21]最近提出了一种通过回环闭合来处理尺度漂移的单目系统。尽管这种延迟的尺度校正对建图而言是可取的，但对于自动驾驶来说却不可行。通常会利用环境的先验知识来抑制尺度漂移，例如轮式机器人的非完整约束[22],或圆形管道的几何结构[23]。

我们利用相机相对于地面平面的固定高度来处理尺度漂移问题。已有多个先前系统也使用该约束来处理尺度漂移[2],[5],[6]。然而，这些方法通常依赖于来自特征匹配的三角测量或单应性分解，而这些在低纹理路面情况下噪声较大，或者未能提供包含多线索的统一框架。相比之下，我们在一个数据驱动框架中结合了稀疏特征和稠密立体视觉的线索，并通过瞬时视觉数据对观测协方差进行加权，从而实现了更优的结果。

与上述大多数系统不同，我们在真实驾驶基准上展示了出色的单目SFM结果，覆盖了数公里的距离[1]，并报告了相对于真实值的精确的定位性能。

2.3 目标定位

为了定位运动目标，Ozden等人[24]和Kundu等人 [25]采用同时进行运动分割与SFM的方法。另一种方法是多目标跟踪框架，该框架将目标检测与立体视觉 [26]或单目SFM[27],[28]相结合。目标检测能够处理更远距离的目标，并结合地面平面为估计目标尺度提供线索，而传统单目SFM即使具有多个分割的运动[29]也难以解决这一问题。需要注意的是，[26],[28]也联合优化了地面平面和目标位置，但我们引入了更多线索用于地面平面估计，并提出了自适应线索融合框架。我们指出，我们的精确地面平面估计的有效性在任何目标跟踪框架中均可体现，包括[26],[27],[28]。

3 系统架构

与先前的研究类似，通过相对位姿估计[30],三角测量和光束法平差来初始化一组三维点。在正常运行期间，即此处所说的稳态，我们的系统维持一组稳定的三角化三维点，用于估计下一时间点的相机位姿。与先前的研究如[12],[15]专注于小规模环境不同，自主导航等户外应用必须处理在几帧内迅速移出视野的三维场景点。因此，

用于位姿计算的稳定点集必须持续更新，这需要一种新颖的多线程架构。系统架构在稳态操作下的每一帧如图 2所示。

3.1 位姿模块

在稳态下，系统可以获取至少100个稳定的三维点。从典型的户外图像中提取约2000个经过Shi‐Tomasi滤波的 FAST角点[31]。类似于先前的研究[8],[12],，我们通过位姿预测、3D‐2D匹配以及基于RANSAC的PnP位姿估计来计算相机位姿，其中使用模型大小为四个点的EPnP[32]。

3.2 对极更新模块

如图2所示，我们的对极模块在每一帧都运行，这与先前研究中的按需运行方式不同。对极搜索模块在两个线程中并行执行，并在每一帧的位姿估计之后进行。对极搜索机制如图3所示。设最近的关键帧为第0帧。在第 n帧完成位姿计算后，对于位于(x0, y0)处的关键帧中的每个特征 f0，我们在第 n帧中以(x0+ x, y0+ y)为中心设置一个边长为 2r e的窗口，其中 r e与相机速度成正比。引入位移(x,y)是对[6],的改进，使得搜索中心(x0, y0)能够沿对极线移动到更理想的位置。该位移是根据点(x0, y0)到地平线中心的距离计算得出的，而地平线中心是利用第5节中估计的地面平面计算得到的。将 r e和(x,y)适应于速度，有助于在快速变化的高速公路序列中处理远近区域之间显著变化的视差范围。

我们考虑该正方形与一个宽度为 p像素的直线带状区域的交集，该带状区域以对应于帧 n中 f 0的极线为中心。最近的匹配在此交集区域内被找到。该极线匹配过程还通过计算在帧 n 1中 f n 的最近匹配（称为f n 1）来重复进行。仅当 f n 1也匹配 f 0时，才接受该匹配——我们称此为循环匹配，它有助于消除虚假匹配。注意，帧0与 n 1之间的匹配已在帧 n 1处计算完成。由于位姿估计非常

在第 n帧中循环匹配的特征相对于最近的关键帧 （第0帧）进行三角测量。这些三维点作为候选点，当系统在关键帧处需要时可添加到三维点云中。所有候选三维点通过反向投影至所有第1帧,· · · , n 1持续验证，只有在边长为 2rb像素的紧密窗口内找到匹配时才被保留（我们设置 rb= 3）。该方法与第3.3节中的局部光束法平差协同工作，可替代更为精确但计算代价较高的多视角三角化，且效果令人满意，因为对极搜索会产生大量三维点，但仅有最可靠的点可用于位姿估计。

3.3 局部光束法平差模块

为了优化相机位姿和三维点并融合多帧信息，我们在最近的 L帧上实现了滑动窗口局部光束法平差。为了在我们的系统中保持所需的帧率和精度，取值 L= 10已足够。相比[6]的改进在于能够处理微小运动。当相机运动较小时，系统会阻止添加新的关键帧，并强制将前一个关键帧加入局部光束法平差中。这保证了前一个关键帧与当前帧之间的基线不会过短，从而提高了光束法平差的稳定性，即使在近静止状态下也能获得精确的位姿估计。车辆速度测量来自SFM（结构与运动恢复）本身。在光束法平差之后，系统会利用优化后的位姿尝试重新查找丢失的三维点。特征重查找使用半径为3像素的图像窗口。

3.4 关键帧与恢复

该系统无法无限期地保持稳态，因为三维点会由于跟踪失败或移出视野而逐渐丢失。后者是一个重要因素在面向自动驾驶的前向移动系统（与PTAM等浏览系统相对）中，关键帧的作用对于维持系统的持续运行至关重要。关键帧的目的有三个：
- 从对极线程中收集具有长轨迹的三维点，通过局部光束法平差优化它们，并添加到主线程的稳定点集合中。
- 触发一次光束法平差（我们称之为“关键帧光束法平差”），包含最近的 K关键帧，以优化三维点和关键帧位姿。
- 提供新三维点具有匹配关系的帧。

对于我们的应用，对 K= 5前一个关键帧进行光束法平差已足够。有两种原因使得在更大数量的关键帧集（甚至整个地图）上进行更复杂的优化来通过长距离约束优化三维点是不必要的。首先，自动驾驶应用中的图像序列移动迅速且不包含重复场景，因此引入更多的关键帧对光束法平差带来的收益微乎其微。其次，我们的目标是实时输出位姿，而不是构建地图，因此关键帧也无法享受延迟输出的奢侈。这与诸如[4],的并行系统不同，在后者中关键帧可能会导致每帧计时出现明显的峰值。

在极少数情况下，系统可能会遇到一个帧，其中基于位姿的匹配无法跟踪特征并生成足够的3D‐2D匹配点以供PnP鲁棒地工作（由于成像伪影或突然的大运动）。在这种情况下，我们会重新初始化系统，并使用单点RANSAC恢复尺度。通常，在KITTI数据集的每个序列中会遇到0到2次恢复情况。有关关键帧与恢复架构的更多细节见[6]。

3.5 讨论

我们注意到，除了明显的速度优势外，将对极搜索转移到新线程还极大地提升了系统的精度和鲁棒性。依赖 2D‐3D对应关系的系统可能仅在关键帧前的一帧中执行对极搜索，以更新其稳定点集。然而，通过该机制引入的三维点的支持仅限于用于循环匹配和三角测量的三元组，因此这些三维点的质量可能较差。通过在每一帧都执行循环匹配，我们为三维点提供了最长可达前一个关键帧距离的轨迹。此外，这还允许在每一帧中进行重复的验证和外点剔除。显然，在我们的多线程系统中，由对极线程提供的经过充分验证的长轨迹集合更有可能不含外点，同时为更稳定的位姿估计提供更长距离的约束。

我们的多线程架构还具有效率优势。在我们的设计中，对极模块与局部光束法模块并行运行。与大规模多线程光束法平差相比，小规模光束法（例如，包含10个视图和数百个三维点）在多线程下速度提升并不显著。因此，对极更新模块利用了空闲的次级和三级线程，从而实现了更优的三维点。

4 尺度校正的背景

尺度漂移校正是单目SFM的一个关键组成部分。在实践中，它是确保精度最重要的因素。我们通过估计地面平面相对于相机的深度和方向来实现尺度校正。

可以使用多种方法（如连续帧之间稀疏特征匹配的三角测量和稠密立体视觉）来估计地面平面。我们提出了一种系统化方法，将这些线索进行融合，以反映我们对每条线索相对精度的置信度。显然，这种置信度应同时受到特定帧输入以及训练数据观测的影响。我们通过从大量训练数据中学习模型，将每条线索的观测协方差与其底层变量的误差行为关联起来，从而实现这一点。在测试期间，每帧的误差分布利用这些学习到的模型自适应地调整数据融合中的观测协方差。

4.1 地面平面几何结构

如图4所示，相机高度（也称为地面高度） h定义为从光心到地面平面的距离。通常情况下，相机并不完全与地面平面平行，存在一个非零的俯仰角 ✓。对于一个3D点X=(X1, X2, X3) >，地面高度 h和单位法向量n=(n1, n2, n3) >共同定义了地面平面：
n>X+ h= 0. (1)

>定义。)

4.2 单目SFM中的尺度校正

尺度漂移校正是单目SFM的一个关键组成部分。在实践中，它是确保精度的最重要因素。我们根据第5节和第6节所述方法估计地面平面几何结构以进行尺度校正。

在尺度漂移的情况下，任何估计的长度 l在尺度因子 s= l/l ⇤上都是模糊的，其中 l ⇤是真实值长度。尺度校正的目标是计算 s。给定相机相对于地面的标定高度h ⇤，计算表观高度 h即可得到尺度因子 s= h/h ⇤。然后可以将相机平移t调整为tnew=t/s，从而校正尺度漂移。在我们的实现中，根据车速和相机帧率，使用前一帧或前一个关键帧作为尺度漂移校正的原点。在 KITTI数据集中，由于帧率相对较慢（10Hz），使用前一帧作为原点已足够。这一步发生在系统进入局部光束法平差之前，因此校正后的尺度可进一步通过光束法平差进行优化。

5 用于地面平面估计的线索

本节提出了多种方法，例如稀疏特征匹配的三角测量、连续帧之间的稠密立体视觉以及物体检测边界框，用于估计地面平面。在下一节中，这些方法的输出将在一个框架中进行融合，以考虑它们在每帧中的相对有效性。

5.1 平面引导的稠密立体视觉

我们假设前景中的一个感兴趣区域（ROI）（图像底部三分之一的中间五分之一区域）对应于平面地面。对于假设值{h,n} 和帧 k 与 k+ 1之间的相对相机位姿{R,t}，可以计算逐像素单应性映射如下：
G= R+ 1 h tn>. (2)

对于KITTI的10Hz输入帧率，帧 k和 k+ 2之间的感兴趣区域（ROI）通常重叠较少。相反，帧 k和 k+1之间的基线是足够的。对于其他30Hz图像数据，我们相应地调整基线。单应性映射如图5所示。注意，t与真实平移t⇤之间相差一个未知的尺度漂移因子，该因子编码在我们希望估计的 h中。感兴趣区域（ROI）内的像素被映射到帧 k（亚像素精度对良好性能至关重要），并在双线性插值得到的图像强度上计算绝对差之和（SAD）。使用 Nelder‐Mead单纯形算法[34]来估计{h,n}，其表达式为：
min h, n (1 ⇢ SAD ⇤ ), (3)
其中SAD⇤表示在感兴趣区域像素数量上的SAD平均值。我们通过经验选择 ⇢= 1.5，使得在KITTI数据上上述代价的切片接近钟形，该特性在第6节中被利用。注意，优化仅涉及h、 n 1和 n 3，因为 knk= 1。施加范数约束的影响较小，因为标定俯仰角是一个良好的初始化，且代价函数通常在其附近具有明显的局部最小值。(3)的最小化{h,n}即为来自立体线索的估计的地面平面。

5.2 三角化三维点

接下来，我们考虑在上述感兴趣区域（ROI）内计算得到的帧 k与 k+ 1之间的稀疏SIFT[35]描述符匹配结果（我们发现对于低纹理路面而言，SIFT相比ORB是更优的选择，且在较小的感兴趣区域内SIFT可实现实时性能）。为了对三角化三维点进行平面拟合，一种方法是使用三点 RANSAC进行平面拟合来估计{h,n}。然而，在我们的实验中，通过假设相机俯仰角从标定中保持固定，采用[2],的方法可以获得更好的结果。对于每一个三角化三维点，其高度 h通过公式(1)计算得到。每个三维点 i相对于其他每个点 j的高度差hij被计算出来。估计的地面平面高度为对应于最大得分 q的点i的高度，其中
q= max i nX j6=i exp µ h2ij o , with µ= 50. (4)
注意：先前的研究（如[5],[6]）通过分解帧之间的单应性G来获得相机高度[36]。然而，在实践中，这种分解对噪声非常敏感，这是一个严重的问题，因为单应性是通过从低纹理路面提取的含噪声特征匹配计算得到的。

5.3 物体检测线索

我们还可以在目标检测边界框可用时将其作为线索使用，例如在物体定位应用中。地面平面的俯仰角 ✓ 可以通过此线索进行估计。回顾一下， n3= sin ✓，对于地面法向量n=(n1, n2, n3) >。

给定一个二维边界框，我们可以通过地面平面并使用 (10)计算三维物体高度 hb。利用物体高度的先验值 ¯hb，通过求解得到 n3。
min n 3 (hb ¯hb) 2 . (5)
在(10)中使用的地面高度 h被设为标定值，以避免引入 SFM（结构与运动恢复）的尺度漂移，而 n1被设为0，因其对物体高度的影响可忽略不计。

注意：物体边界框线索仅提供地面方向的信息，因此对于SFM尺度漂移校正的影响可忽略不计。然而，对于三维定位等应用，该线索提供了独特的远距离信息，而稠密立体和3D点线索仅考虑车辆附近的感兴趣区域。不准确的俯仰角会导致远处物体出现较大误差。因此，如第??节所示，引入该线索可显著提高远处物体的三维定位精度。

6 自适应线索组合

6.1 使用卡尔曼滤波的数据融合

我们现在提出一种系统化方法，以结合上述线索，同时反映每帧的相对精度每种方法的估计结果。为了融合来自不同方法的估计值，一个自然的框架是卡尔曼滤波器：
xk= Axk 1+ wk 1, p(w) ⇠ N(0, Q), zk= Hxk+ vk 1, p(v) ⇠ N(0, U), (6)
其中x和z分别为状态和观测向量，A和H分别为状态和观测转移矩阵，w和v为过程和观测误差。假设 w和v为零均值高斯分布，其协方差分别为Q和U。在我们的应用中，(6)中的状态变量是地面平面x=(n>, h)>。由于 knk= 1, n2 由n1 和 n3 确定，且我们的观测值为z=(n1, n3, h)>。出于简洁性和实时性考虑，我们假设 n1, n3和 h 相互独立，因此U= diag(un1, un3, uh)。于是，我们的状态和观测转移矩阵分别为
A=  R t 0> 1 > , H= 2 4 1 0 0 0 0 0 1 0 0 0 0 1 3 5. (7)
假设使用方法 i= 1, · · ·, m来估计地面平面，对应的观测协方差为Ui= diag(ui,n1, ui,n3, ui,h)。我们将使用符号i 2{s, p, d}分别表示稠密立体匹配、三维点和目标检测方法。然后，在时间点 k的融合方程为
Uk=( Xm i=1 (Uk i) 1) 1, zk= Uk Xm i=1 (Uk i) 1zik. (8)
显然，这种组合应同时受到特定帧的视觉输入和先验知识的影响。在每一帧中对Uk进行有意义的估计，并为每个线索正确地按比例分配Uk i ，对于有原则的线索组合至关重要。

通常，使用固定协方差来融合线索，但这种方法未考虑线索在视频序列中逐帧变化的有效性。一些方法通过使用自协方差技术[37]来考虑变化的协方差。相比之下，我们提出一种数据驱动机制，基于某些底层变量的误差分布学习模型，以适应每个线索的逐帧协方差Uk i 。这些变量对应于对每条线索准确性置信度的物理依据 （例如稠密立体线索的SAD代价尖锐度）。在测试时，我们学习到的模型能够逐帧调整每条线索的观测协方差。

我们的自适应线索融合性能在第7.3节中展示。假设线索 i的误差行为由一个潜在变量a i 决定： p(v|a i) ⇠ N(0, U i) ，我们训练过程的目标是找到一个将Ui 与a i 关联起来的函数，即Ui = C i( a i) 。正如我们在下文中所看到的，在我们的应用中，每个线索使用线性函数已足够。

6.2 训练

对于稠密立体和3D点线索，我们使用KITTI视觉里程计数据集进行训练，该数据集包含 F= 23201帧。KITTI的序列0到8

给出的 fs，使用稠密立体匹配方法获得地面平面的观测值zsk=(bnk 1, bn k 3, bh k)>。我们固定n1= bnk 1 和 n3= bnk 3，并在[1m,2m],范围内对 h˙ 进行50个均匀采样，根据公式(2)构建从帧 k 到 k+ 1 的单应性映射（注意，R和t已由单目SFM估计出来， 仅差一个尺度因子）。对于每个单应性映射，我们使用公式(3)计算SAD得分 fs( h˙)。类似的过程也应用于 n1和 n3，两者的搜索区间均为[ 0.1, 0.1]。)

模型 As：按照算法1中的步骤3，对 fs( h˙)的分布拟合一个单变量高斯分布 As。其方差 ask,h捕捉了SAD分布的尖锐程度，因此构成了反映帧 k上使用稠密立体匹配估计高度 h的准确性置信度的基础变量。请注意，拟合其他分布（如柯西分布）也可能适用。然而，我们的目的是仅捕捉SAD峰值的锐度，通过实验发现高斯拟合已足够。类似的过程得到了对应于方向变量的方差ak s,n 1和 ak s,n 3。示例拟合结果如图6所示。

错误 v s：接下来，根据算法1中的步骤4，我们计算v k s，h = |bh k h⇤ k| 作为相对于真实值 h⇤ k（KITTI数据集为1.7米）的地面平面高度误差。

模型 Cs：对于KITTI数据集，a k s,h, a k s,n 1 和a k s,n 3 的分布如图7所示。我们按照算法1中的步骤6，将参数a k s,h量化为L= 100个区间。区间中心c l s,h的位置根据 a k s,h的密度进行匹配（即，在每个区间内分布 F/L误差v k s,h）。对 n1和n3也重复类似的过程。现在我们已经得到了区间中心c l s。接下来，我们计算落在以c l误差v k s,h的方差 u l s,h（算法1中的步骤7）。这表示稠密立体匹配方法的观测误差方差，对应于观测变量 h。现在，我们对u l s,h相对于 c l s,h的分布进行曲线拟合，从而得到一个将 h中的观测方差与稠密立体匹配的有效性相关联的模型（算法1中的步骤8）。结果如图8所示，其中每个数据点代表一对观测误差协方差u l s,h和参数 c l s,h。根据经验，我们发现用直线近似足以获得良好的拟合效果。对 n 1 和 n 3 也重复类似的过程。因此，我们为立体视觉方法获得了线性模型 C s （分别为 h、 n 1 和 n 3 各一个）。

6.2.1 稠密立体匹配

稠密立体视觉在两个连续帧之间的误差行为通过与单应性（2）相关的道路区域之间SAD得分的变化来表征，我们在独立改变每个变量 h、 n 1 和 n 3 时观察该方差。

6.2.2 三维点

与稠密立体匹配类似，训练的目标同样是找到一个模型Cp，该模型将三维点方法的观测协方差U p与其底层变量ap关联起来。由(4)可知，唯一需要估计的感兴趣变量是高度zp= h。因此，Up由单个方差up,h给出，我们的目标是将其与一个底层变量ap,h相关联。

观测zp：最优的观测zpk =ˆhk是对应于（4）中最大得分点的高度 q，完成算法1中的第2步。

模型 Ap：我们观察到，由公式(4)中的目标函数 fp定义的分数 q直接反映了利用三维点线索估计的地面平面的准确性置信度。因此，我们可以直接获得参数a k p = q k（算法1中的步骤3），其中 q k是fp在帧 k处的最优值，而无需显式地学习模型 Ap。

错误 v p：相对于真实值计算误差v k p，h = |bh k h⇤ k| （算法1中的步骤4）。

模型 Cp：剩余步骤与立体线索的处理方式相同。上述a k p,h被量化为以c l p,h为中心的 L= 100个区间，计算落入每个区间内误差v k p,h的方差u l p,h。现在可以拟合一个模型 Cp ，以建立每个区间内的观测方差u l p,h与相应量化的底层参数c l p,h之间的关系。如图9所示，直线拟合再次显得合理。

6.2.3 目标检测

我们假设检测器提供了若干候选边界框及其各自的检测分数
（例如，非极大值抑制之前的边界框）。边界框由b=(x, y, w, h b) >表示，其中 x, y为其二维位置， w, h b为其宽度和高度。检测的误差行为通过检测分数 ↵相对于边界框b的变化来量化。

观测zd：根据算法1的第2步，地面平面俯仰观测zd= ˆnk 3通过求解(5)得到。

模型 Ad：我们的模型 Ak d在算法1中是一个高斯混合模型。在每一帧中，我们估计 4 ⇥ 4个满秩协方差矩阵⌃m，以 µm为中心，公式如下：
min A m ,µ m ,⌃ m XN n=1 M X m=1 Ame 1 2 ✏ mn ⌃ 1 m ✏ mn ↵n !2 , (9)
其中 ✏mn= bn µm, M为对象数量， N为候选边界框的数量（为方便起见，省略了对 k的依赖）。图10展示了拟合结果示例。显然，该模型很好地捕捉到了噪声检测器分数的变化 Ak d 。回顾一下，(5)中的目标 fd用于估计 n3。因此，对于我们的应用而言，只有⌃m中对应于 y和 hb的项是重要的。令 y和 h b 分别为⌃m中距离被跟踪的二维框最近的相应对角线元素。我们将它们组合成一个单一的底层参数，记为a k d = y h b y + h b，用以反映对检测线索准确性的置信度。至此，完成了算法1的第3步。

误差 v d ：误差v k d，n 3 = |bn k 3 n⇤k 3 | 是相对于真实值计算的（算法1中的步骤4）。

模型 C d ：其余步骤与立体视觉和三维点线索类似。底层参数a k d 被量化，并与观测误差的相应方差相关联。拟合的线性模型 C d ，用于关联检测线索的观测方差与其预期的底层参数，在图11中展示。

KITTI数据集中目标检测线索的潜在变量ad, n3的分布。参数ad,n3对应于目标检测得分分布中的尖锐度，表示对目标检测线索准确性的置信度。(b)将观测方差ud,n3与量化后的潜在变量cd,n3相关联。)

6.3 测试

测试期间，在帧 k，我们为每个线索 i 2{s, p, d}拟合一个模型 Ak i，并确定其传达预期精度的底层参数ak i。接着，我们使用这些模型 Ci来确定观测方差。

稠密立体匹配 在第k帧处的观测zsk=(bnk 1, bnk 3, bhk)>通过最小化由(3)给出的 fs获得。我们通过对单应性映射的SAD得分拟合一维高斯分布，得到ask,h, ask,n 1和 ask,n 3的值。利用图8中估计的模型 Cs，我们预测对应的方差uk s。稠密立体视觉方法的观测协方差现可表示为Uk 1= diag(u k s,n 1, uk s,n 3, uk s,h)。

三维点 在帧 k，观测值zpk是由(4)式给出的从 fp获得的估计b地面高度 h。由(4)式得到的 qk值直接给出了我们期望的基础参数 ak p。相应的方差 vk p,h是从图9的模型 Cp估计得到的。该线索的观测协方差现在可表示为U k p = uk p,h。

目标检测 在帧 k，观测值z k , m d 是通过最小化 fd（由公式(5)给出）得到的每个对象 m= 1, · · ·, M的地面俯仰角 bn3。对于每个对象 m，我们求解(9)后得到参数 a k , m d 。利用图11的模型 Cd，我们预测相应的误差方差 u k , m d 。该方法的观测协方差现由U k m d = u k ,, m d 给出。

融合 最后，通过结合来自每个对象 m的Uks 、Ukp 以及Uk,m d ，计算得到帧 k 的自适应协方差Uk。然后，使用(8)式，通过组合zks 、 zkp 和zk,m d ，计算得到我们自适应的地面平面估计zk。因此，我们描述了一种地面平面估算法，该算法利用从训练数据中学习的模型，逐帧调整各线索——立体视觉、三维点和检测边界框——的相对重要性。图 12展示了融合框架的概要。

7 实验

我们在KITTI数据集[1],上进行了评估，该数据集包含在 22个序列中近50公里的真实驾驶场景，涵盖了城市、住宅区、乡村和高速公路道路。速度范围从0到90公里/小时，低帧率10Hz频率以及其他车辆频繁出现带来了额外的挑战。KITTI上的评估指标由[1],提供，基于K¨ummerle等人在 [38]中提出的指标的扩展。旋转与平移误差报告为在固定距离下所有相对变换的平均值，以及不同子序列长度和车辆速度下的函数。关于计时，我们的实验在一台配备 IntelCorei72.40GHz处理器、8GBDDR3内存和6M缓存的笔记本电脑上进行。主要模块占用三个线程，如第3节所示，而地面平面估计单独占用两个线程。

考虑到实时性能，仅使用稠密立体和3D点线索进行单目SFM。在目标检测可用的情况下，检测边界框线索用于物体定位应用。注意，当检测线索可用时，它们仅改善地面方向，而不会对SFM产生任何负面影响。物体定位通过目标检测和离线计算的跟踪边界框实现，该过程使用[39]完成。

7.1 在KITTI上的单目SFM基准测试

KITTI中的视觉里程计测试序列编号为11–21，这些序列的真实值未公开。我们的系统在这些序列上的表现可通过评估网页[40],中名为MLM-SFM的结果查看。图 13展示了我们的系统在不同子序列长度和速度下的平均旋转与平移误差。截至2014年8月1日，我们的方法在单目系统中排名第一，总体排名第六（包括立体视觉和激光点系统）。

7.2 单目SFM的精度与鲁棒性

我们的地面平面估计的另一个优势是增强了鲁棒性。作为演示，我们在序列 0 10上对我们的系统、以及与数据集相关的立体视觉和单目系统VISO2‐S和VISO2‐M[2]分别运行了50次试验。相对于真实值的误差使用[1]中的指标进行计算。序列0–10上的平均误差如表1所示。注意，我们相比VISO2‐M有显著的性能提升，且旋转与平移误差甚至优于立体视觉系统VISO2‐S。所有方法在序列1和7上都出现了非常高的误差，因此这两个序列未被纳入本次评估。前者是一个以90公里/小时速度行驶的长距离高速公路序列，具有重复纹理；而后者包含一个大卡车遮挡了超过 70%图像区域的片段（见图14）。我们当前的单目SFM系统仅依赖于低层次特征，然而我们的未来工作将融合车道和目标检测，从而能够更好地应对此类场景。

在图19中，我们展示了我们的单目SFM结合自适应地面平面估计、VISO2‐M的单目系统以及VISO2‐S的立体视觉系统在除图1所示两个序列外的其他八个KITTI序列上的重建轨迹。所有轨迹以蓝色显示，真实值以红色显示。可以看出，我们的单目系统相对于真实值具有很高的精度，可与立体视觉系统相媲美，且明显优于以往的单目方法。同时注意到，我们的旋转误差低于立体视觉系统，这对长距离位置误差具有显著影响。这一性能得益于我们的系统架构创新和地面平面估计，该方法结合了多线索，并利用从训练数据中学习的模型，根据每帧视觉数据的不确定性自适应调整各线索的相对权重。

7.3 地面平面估计精度

结合线索并采用严格的卡尔曼滤波器以及自适应计算融合协方差的地面平面估计是实现我们鲁棒性性能的关键。图 15展示了使用三维点和立体线索分别进行估计时，地面平面高度相对于真实值的误差示例，以及我们线索融合后的输出结果。请注意，虽然单独的方法噪声很大，但我们的线索融合能够实现比任何单一方法都更准确的估计。

接下来，我们通过第6节中使用自适应协方差的数据驱动框架，展示线索融合的优势，相较于具有固定协方差的传统卡尔曼滤波器。在本实验中，卡尔曼滤波器的固定协方差由整个训练集上各变量的误差方差确定（我们通过交叉验证验证了这是一种合理的选择）。

在图16中，仅使用稀疏特征匹配会导致明显较差的性能（黑色曲线）。稠密立体视觉表现更好（青色曲线）。在具有固定协方差的卡尔曼滤波器中加入额外的稠密立体线索后有所改进（蓝色曲线）。然而，使用第6节中的训练机制根据各线索的相对置信度调整每帧观测协方差，可进一步将误差降低近1%（红色曲线）。图1 6(b)显示，我们在所有序列中以75 – 100%的比率达到正确尺度，远高于其他方法。特别是将我们的输出（红色曲线）与仅使用三维点的结果（黑色曲线）进行比较。这代表了本文相较于先前的研究（如[2],[5],[6]，仅使用路面的稀疏特征匹配）所取得的改进。

序列0‐10 中地面平面估计的平均误差。(b)高度误差小于7%的帧所 占百分比。请注意，我们的方法在误差上远低于单独使用 任一方法，在鲁棒性上则远高于单独使用任一方法。)

7.4 地面平面估计的有效性

在本节中，我们通过将地面平面估计与其他公开的单目 SFM系统VISO2‐M集成，展示了其有效性。该系统依赖于通过基础矩阵估计计算所有连续帧对之间的相对位姿，并针对局部平面地面进行连续尺度校正。这种系统架构具有简洁性的优势。鲁棒性。理论上，它不会崩溃，因为它并不试图构建长的特征轨迹，但在第7.2节中已显示出其精度较低的缺点。在本节中，我们展示了我们的地面平面估计可以显著提高 VISO2‐M的精度，表明了我们改进其他单目SFM系统的潜力。

我们将VISO2‐M的地面平面估计替换为我们的方法（第5和第6节），其余部分保持不变。使用KITTI训练数据集进行测试。同样，相对于真实值的误差通过[1]中的指标计算。在表2中显示了使用KITTI训练数据集的误差率。将VISO2‐M的地面平面估计替换为我们的方法命名为“VISO2‐M+我们的地面平面估计模块”（右栏）。注意平移误差从10.43%降低到6.62%。将“VISO2‐ M+我们的地面平面估计模块”的误差与表1中我们系统的误差进行比较，也证明了我们单目系统架构的有效性。“VISO2‐M+我们的地面平面估计模块”在KITTI测试数据集上的性能可通过KITTI评估网页[40],上名为VISO2-M+ GP的结果查看。平移误差从原始方法VISO2-M的11.94%降低至我们的7.46%。

Seq	Frms	VISO2‐M Rot平移 （度/米） (%)	VISO2‐M + 我们的地面平面估计模块 Rot平移 （度/米） (%)
0	4540	0.0209 11.9	0.0206 6.57
2	4660	0.0114 3.33	0.0114 2.73
3	800	0.0197 10.7	0.0192 5.67
4	270	0.0093 7.40	0.0087 1.49
5	2760	0.0328 12.7	0.0333 7.63
6	1100	0.0157 4.74	0.0156 4.47
8	4070	0.0203 13.9	0.0203 6.64
9	1590	0.0143 4.04	0.0145 3.04
10	1200	0.0388 25.2	0.0379 21.3
Avg		0.0204 10.43	0.0202 6.62

表2 ：通过将VISO2‐M的地面平面估计模块替换为我们的地面平面估计模块，验证了我们方法的有效性。新方法“VISO2‐M+我们的地面平面估计模块”在平移误差上提高了超过4%。

7.5 我们SFM架构的有效性

为进一步证明我们在第3节中讨论的单目SFM架构的有效性，我们对比了我们的原始SFM性能（未进行尺度校正）与另一个著名的SFM系统 EKFMonoSLAM[19]进行比较。再次使用KITTI里程计数据集的训练序列00‐10以及[1]中的指标。然而， EKFMonoSLAM仅成功完成了三个相对较短的序列03、 04和06。误差率如表3所示。中间一列“我们的系统+无 GP”显示了我们的系统未启用基于第5和第6节地面平面估计的尺度漂移校正时的误差数值。与第三列中的完整系统相比，平移误差较高，但仍比EKFMonoSLAM好10%。此外，我们提供了实验对比，以证明第3.2节中提出的特征匹配机制的有效性。我们在其他所有系统组件保持不变的情况下，将我们的系统使用提出的方法与更常见的链式匹配（将帧 i与 i+ 1、 i+ 1与 i+ 2等进行特征匹配）进行比较。如表4所示，对于KITTI训练集，我们的方法使旋转误差降低至近一半，平移误差减少了近1%。

Seq	EKFMonoSLAM Rot平移 （度/米） (%)	我们的SFM+无GP Rot平移 （度/米） (%)	我们的系统 Rot平移 （度/米） (%)
3	0.014 16.4	0.002 9.66	0.002 3.37
4	0.010 11.6	0.003 2.40	0.002 1.43
6	0.040 27.0	0.013 14.4	0.008 2.09
Avg	0.027 21.2	0.008 11.2	0.005 2.48

表3 ：通过将原始SFM性能（未使用地面平面估计的尺度校正）与最先进的SFM系统EKFMonoSLAM[19] 进行比较，展示了我们单目SFM架构的有效性。我们的原始平移误差比EKFMonoSLAM低10%。

Seq	Frms	链式匹配 Rot平移 （度/米） (%)	提出的方法 Rot平移 （度/米） (%)
0	4540	0.0198 5.42	0.0048 2.04
2	4660	0.0045 1.99	0.0035 1.50
3	800	0.0026 3.58	0.0021 3.37
4	270	0.0022 0.63	0.0023 1.43
5	2760	0.0046 2.65	0.0038 2.19
6	1100	0.0133 3.10	0.0081 2.09
8	4070	0.0042 2.31	0.0044 2.37
9	1590	0.0064 1.48	0.0047 1.76
10	1200	0.0040 2.40	0.0085 2.12
Avg		0.0087 3.02	0.0045 2.03

表4 ：通过使用提出的方法与更常用的链式匹配方法进行SFM性能比较，展示了第3.2节中特征匹配机制的有效性。

7.6 实时性能

为了说明我们的系统以平均30帧每秒的速度返回实时位姿，且最坏情况下每帧耗时50毫秒，图17提供了该系统在两个序列上的时间图。特别是请注意，插入关键帧、触发光束法平差或纠错机制并不会导致我们的计时出现显著的尖峰，这与若干当代实时系统不同。

还可以观察到，在序列08和05中，分别大约每5帧和6帧插入一次关键帧。这是预料之中的，因为快速移动的车辆会频繁地需要对极更新模块提供新的三维点。这不会影响我们系统的性能，因为在关键帧插入后触发的关键帧光束法平差会在下一帧位姿计算完成之前结束，并与其并行运行。事实上，关键帧插入为优化过程引入长距离约束提供了机会（只要对极更新模块能够返回足够长的轨迹）。因此，为了确保速度和精度，一个多线程SFM系统不仅需要设计良好的关键帧架构，还需要其各个模块（如位姿估计、对极搜索以及各种光束法平差）彼此之间以最优方式协同工作。

7.6.1 背景

设K为相机内参标定矩阵。将二维边界框的底部[26], [27],[28], b=(x, y,1) >（以齐次坐标表示）通过地面平面{h,n}反投影到3D空间中。
B=(Bx , B y , B z) > = hK 1 b n > K 1 b , (10)
类似地，也可以使用估计的地面平面和2D边界框高度来获取物体高度。给定2D物体轨迹，可以估计最优拟合3D边界框。对象的俯仰角和横滚角由地面平面确定（见图4）。对于车辆，初始偏航角假设为其运动方向，并对其长宽比施加先验。根据(10)给出的初始位置，可通过最小化其重投影与跟踪的2D边界框之间的差异来计算3D边界框。详细的单目物体定位描述超出了本文范围[41]。这里，我们仅指出两点：首先，无论采用何种实际的定位框架，精确的地面平面显然是实现精确单目定位的关键；其次，将检测边界框中的线索融入地面平面估计中，构成了SFM与物体定位之间一种优雅的反馈机制。

7.6.2 物体定位精度

现在我们展示第6节中自适应地面平面估计在三维物体定位中的优势。KITTI未提供定位基准，因此我们使用跟踪训练数据集与真实值进行评估。我们使用序列1‐8进行训练，序列9‐20用于测试。评估所采用的指标是物体位置的百分比误差。为了说明问题，我们仅考虑车辆物体，并将其分为“近距”和“远距”两类，其中远距物体距离超过10米。我们剔除所有不在道路上的物体。用于训练目标检测线索的候选边界框来自[42]。

图18对比了使用数据驱动的线索组合（红色曲线）、固定协方差估计（蓝色）以及标定得到的固定地面平面（黑色）进行物体定位的结果。顶行使用真实物体轨迹，底行使用[39]跟踪器生成的轨迹。在每种情况下，均可观察到使用我们的线索融合方法在定位上的显著提升。此外，从图18(b)、(d)中可以看出，在地面平面估计中引入检测线索后，远距物体的定位误差显著降低。图1展示了我们的定位输出示例。

8 结论

我们提出了一种新型的多线程实时单目SFM（结构与运动恢复），在真实世界自动驾驶中实现了卓越的精度。我们证明了合理的多线程设计能够提升处理复杂道路条件时的速度和精度。该系统经过优化，可在每一帧实时输出位姿，无需因关键帧插入或优化而产生延迟。我们已经证明，精确的地面平面估计能够使基于单目视觉的系统实现高精度和鲁棒性。特别是，我们表明包含多线索是有益的，并提出了一种数据驱动机制，在反映这些线索逐帧相对置信度的框架中融合这些线索。我们展示了除了稀疏三维点之外，引入稠密立体线索可通过鲁棒尺度改善单目SFM性能漂移校正，而进一步加入物体边界框线索则提高了三维对象定位的精度。我们提出的鲁棒且精确的尺度校正方法在缩小单目与双目SFM之间的差距方面迈出了重要一步。我们相信，这对自动驾驶应用具有重要意义。未来工作将利用提出的单目SFM与物体定位技术，面向碰撞避免、场景识别和可行驶路径规划等实时应用。