摘要

本文的目标是针对RGB-D图像流提供一种快速和准确的方法来估计相机运动。我们的方法通过最小化光度误差，将两个连续的RGB-D图形进行配准。我们使用非线性最小化结合由粗到精策略来估计相机的运动。为了考虑图像数据中的噪声和异常值，我们提出使用鲁棒误差函数来减少大残差的影响。此外，我们的公式允许包括一个运动模型，它可以基于先验知识，时间滤波或额外的传感器如IMU。我们的方法对于计算资源有限的机器人很有吸引力，因为它在单核CPU上实时运行，并且占用很少的内存。在基准数据集和合成数据上进行了大量的实验，证明了我们的方法比以前的方法更加准确和鲁棒。我们在开源许可证下提供我们的软件。

1 介绍

在没有外部参考系统(如GPS)的环境中，视觉里程计是机器人控制和导航的一种重要传感方式。尤其是在杂乱的室内环境中运行的四轴飞行器，需要快速更新姿态以进行位置控制。与此同时，它们只能携带重量和功耗有限的传感器和处理器。近年来出现的轻型商用RGB-D相机非常适合这样的应用场景。例如，华硕Xtion Pro Live传感器在视频帧速率下提供VGA分辨率的3D几何图形和视觉外观。该传感器重量仅为77克，功耗不到2.5瓦特，非常适合在室内飞行机器人上应用。

在我们的工作中，我们对估计RGB-D相机运动的方法感兴趣，并将这些估计用于局部导航和位置控制。考虑到这个应用场景，挑战是在低延迟的高帧率下计算运动更新，并使它们对异常值鲁棒和对故障尽可能地安全。然而，到目前为止，只有少数几种方法被提出， 它们充分利用了RGB-D传感器提供的灰度和深度信息。

基于视觉特征(如SIFT或SURF)的里程计方法运行得太慢，不适合于低延迟应用。基于patch的方法，如KLT跟踪器或PTAM跟踪器是快速的，但忽略了图像的主要部分，因此不能最佳地利用传感器数据。最近，有潜力的方法已经被提出，它直接从稠密的RGB-D图像中计算相机的运动。这些方法的优点是，它们给出了精确的运动估计，并且足够快，可以在单核CPU上实时运行。

在本文中，我们提出了一种基于稠密RGB-D图像的鲁棒实时里程计方法。我们对齐如图1所示的两幅连续的RGB-D图像，并通过最小化它们之间的光度误差来计算相机运动。与基于稀疏特征的方法相比，我们使用了两幅图像的所有颜色信息和第一幅图像的深度信息。我们的方法是对最近工作的一种扩展和推广。我们方法的代码和视频可以通过如下网址获得：

vision.in.tum.de/data/software/dvo

本文的主要贡献是：

• 基于RGB-D数据的直接运动估计的概率公式
• 从真实数据推导出的鲁棒传感器模型
• 融合了时间先验
• 一个开源实现，在单核CPU上实时运行(30Hz)
• 对基准数据的严格评估证明了我们方法的高准确性和鲁棒性

2 相关工作

视觉里程计方法通常跟踪单目或双目图像上的特征并估计它们之间的相机运动。通过RANSAC剔除错误或不一致的特征匹配，来实现鲁棒性。为了提高精度，需要在多帧图像之间跟踪特征，这就是所谓的同时定位和建图(SLAM)或运动结构恢复(SfM)问题。通常，光束调整被用来改进位姿估计。在过去几年中，各种基于这种方法的良好运行的系统已经被提出，包括我们最近的工作。然而，所有这些方法都忽略了图像的大部分内容，因为特征只是稀疏地被提取，即在图像中的几个(通常是50-500)兴趣点。通过这个预选步骤，很多有价值的信息会被丢失。

相比之下，稠密方法旨在利用整个图像信息。Köser等人表明，给定一个有纹理的3D模型，通过最小化观测图像和合成图像之间的光度误差，可以有效地估计相机位姿。该方法被Comport等人扩展用于双目相机。原理上，他们的方法可以被视为二维Lukas-Kanade跟踪器到三维的扩展。最近，Steinbrücker等人和Tykkälä等人都扩展了这种方法，将它用来配准从微软Kinect传感器获得的RGB-D图像，并证明了该方法在静态场景下可以获得高精度的视觉里程计。

除了对齐图像，我们还可以对齐3D点云。通常，应用迭代最近点(ICP)算法的变体，但这也是非常耗时的，因为在每次迭代中两个点云之间的最近邻居必须确定。文献[21]将RGB-D相机的深度和颜色信息合并到八叉树模型中，使用特征描述子找到两个八叉树之间的对应关系，并运行ICP将它们进行对齐。

最近的研究表明，稠密对齐算法可以通过匹配当前图像与场景模型而不是最近的图像来提高精度。在相机跟踪过程中，模型不断用新图像来更新。这些方法的例子是KinectFusion和稠密跟踪和建图(DTAM)。KinectFusion使用一种ICP变体来对齐图像，而DTAM使用类似[10]和[11]中的光度误差。DTAM和KinectFusion都实现了实时性能，但需要最先进的GPU进行计算。为了实现对异常值的鲁棒性，许多方法采用二值阈值分割方法将图像分割为内点和外点，或者采用鲁棒统计的连续方法。鲁棒误差函数在其它应用领域也很常见，比如光束调整。

几种视觉里程计方法使用非均匀先验的运动估计，以引导优化指向真实解。这些先验可以从运动的假设中推导出来，例如小运动或匀速运动。或者，可以来自其它传感器(如IMU)的测量或滤波器(如卡尔曼滤波器)的预测。

与之前所有关于稠密直接运动估计的工作相比，我们提供了模型的概率推导。这个公式允许我们根据应用选择合适的传感器和运动模型。特别地，我们提出了基于分布的鲁棒传感器模型，以及基于匀速模型的运动先验。

3 直接运动估计

在本节中，我们介绍利用RGB-D数据进行直接运动估计的方法。这种方法是我们近期研究的一个广义版本。与之前的工作相比，我们在这里首次提供了一个概率推导，并说明如何使用这个公式轻松地集成运动先验和传感器噪声。

我们的目标是通过对齐从RGB-D图像获得的两个连续的灰度图像和以及相应的深度图和来估计相机运动。我们的方法是基于光度一致性假设，如图2所示。假设两个相机观测到的世界点在两幅图像中产生相同的亮度，

其中，是warping函数，给定相机运动，它将第一张图像中的像素坐标映射至第二张图像。我们的目标是找到在所有像素上最满足光度一致性约束的相机运动。这种方法是对[27]从手持相机构建实时稠密几何的补充。在这两种情况下，光度一致性误差都是最小的。在[27]中，几何是用已知的相机位姿来估计的，而在这里，我们利用已知的几何来估计相机位姿。

在本节的其余部分中，我们推导了warping函数，指定了基于所有像素的误差函数，并使用由粗到精的方案提供了一个有效的最小化策略。随后，我们在第4节中扩展了这种方法，通过为每个像素添加一个权重以及合并一个运动先验。

A warping函数

我们这样构造warping函数。首先，我们利用投影函数的反函数来重建像素对应的3D点，


其中是像素的深度，和是针孔相机模型的焦距，和是针孔相机模型的光心。在第二个相机的坐标系中，点根据刚体运动进行旋转和平移。刚体运动包含由正交矩阵表示的旋转和由向量表示的平移组成。相应的，在第二个相机坐标系中的点为

为了得到的最小参数化表示，我们使用扭转(twist)坐标，

其中是运动的线速度，是运动的角速度。利用从李代数到李群的指数映射，从计算的旋转矩阵和平移向量：

存在一种计算矩阵指数的闭式解。关于李代数和指数映射的更多细节，有兴趣的读者可以参考Ma等人的书。

当第二个相机观察变换后的点时，，我们得到warp后的像素坐标，

综上所述，完整的warpping函数为，

B 似然函数

目前，我们假设(1)中所述的光度一致性假设对图像中所有个像素都是相等的。我们将第个像素的残差定义为第一幅图像和第二幅warp后图像之间的亮度差，

在图1(a)和图1(b)中显示了两个示例性输入图像。它们的残差图像如图1©所示，其中较亮的像素表示较大的误差。理想情况下，残差为零，然而，由于传感器噪声，残差将按照概率传感器模型分布。假设所有像素的噪声独立同分布，则观察整个残差图像的似然为，

给定残差图像，利用贝叶斯法则，我们可以获得相机运动的后验概率，

注意表示相机运动的先验分布。可能的选择包括均匀先验(所有相机的运动都是等可能的)，来自额外传感器(如IMU)的运动先验，或卡尔曼滤波器的预测。

C 最大后验(MAP)估计

我们现在求取一个相机运动，它使得后验概率最大，

代入公式(12)和(11)，去掉项，因为它与无关，可以得到，

通过最小化负对数似然，我们可以等价地写成，

为了避免符号混乱，我们在本节的其余部分中删除运动先验。我们将在下一节对它进行更详细的讨论。

通过求取对数似然的导数，并将其设为零来得最小值，

通过定义，可得，

这可以转换为加权最小二乘问题，

其中为权重函数，它描述了在优化过程中考虑某个特定残差的强度。注意，如果是正态分布，那么是常值，则该问题转换为常规的最小二乘问题。对于非高斯误差模型，我们将在下一节中考虑，权重函数将是变化的。图3(a)给出了不同权重函数的加权二次误差。这个公式的优点是，其它传感器模型也可以被应用，它允许非高斯噪声(例如，异常值)。为了解决这个优化问题，我们应用迭代重加权最小二乘(IRLS)算法，其中权重的计算和的估计是交替的，直到收敛。

D 线性化

为了找到最优的相机运动，我们必须求解公式(17)。由于残差对于是非线性的，我们利用高斯-牛顿法迭代地建立和求解一个线性方程组。为此，我们需要计算的一阶泰勒近似。


其中是第个像素对于六自由度相机运动的雅克比矩阵。代入公式(17)，将所有约束条件用矩阵表示，得到了正规方程，

其中是所有的堆叠，是对角权重矩阵，。图1(d)给出了两幅示例图像的雅可比矩阵的第一列，对应于沿相机轴的导数。公式(21)中的线性方程组可以有效地被求解，例如使用Cholesky分解。在高斯-牛顿算法的第次迭代中，我们利用公式(21)计算出增量，并利用来更新运动估计。注意，我们不需要在每次迭代时都重新线性化公式(17)，因为我们利用当前的估计将图像warp到上。因此，我们只需要单位矩阵处的雅可比矩阵。

由于线性化只对小的有效，我们应用一个由粗到精的方案。首先，我们构建一个图像金字塔，每层图像分辨率减半。然后，我们在上面描述的金字塔中的每一层估计相机运动，并使用它作为下一层的初始化。这样，即使是大的平移运动和旋转运动也可以被处理。

4 鲁棒运动估计

在第3节中描述的方法是非常灵活的，因为它允许我们选择合适的传感器模型和运动先验。例如，运动估计的鲁棒性可以通过使用允许异常值的传感器模型来提高。此外，如果相机运动的额外信息是可用的，它将其代入到运动模型中。

A 传感器模型

在文献[10]中，不使用权重函数。这等价于假设误差服从正态分布。然而，在我们最近的研究中，发现这一假设经常被违背。如图3(b)所示，图中描述了fr1/desk序列的一个典型的残差直方图(灰色条)。从这张图可以看出，正态分布(红色曲线)并不能较好地拟合数据。文献[11]最近提出使用M估计器来更鲁棒地拟合高斯分布(绿色曲线)。然而，在我们的分析中，我们发现正态分布并不能很好地表示残差。问题是，正态分布分配给大的和非常小的残差的概率太低，而分配给介于两者之间的概率太高。因此，我们遵循[29]和[30]，并假定误差分布为分布，其中除了均值和方差外，还可以指定该分布的自由度。由于分布的重尾以较低的概率覆盖了异常值，因此适合用于建模有异常值的数据。从图中可以看出，拟合的分布(蓝色曲线)可以很好地匹配残差分布。

由分布推导出的权重函数为，

根据我们的实验，我们确定自由度。在IRLS算法的每次迭代，我们利用下式计算方差，

这个方程必须通过迭代求解，因为它是递归的，但它迭代几次就收敛了。

加权的影响如图4所示，其中手移动的方向与相机的不同，导致残差中的异常值(参见图4(b))。在图4©中可以看到，较暗的像素表示较低的权重。

B 运动先验

采用上述方法，当RGB-D图像包含足够的纹理和结构时，可以准确地估计相机的运动。然而，输入图像特征较差、运动模糊或动态物体可能导致运动估计噪声增加，甚至发散。特别是在我们之前的实施中，我们对所有运动都一视同仁。因此，我们偶尔会观察到如图5所示的情况发生，其中估计轨迹出现跳跃。

我们假设匀速模型和正态分布，例如，其中是上一个时刻相机的速度，是对角协方差矩阵，它定义了改变的快慢。

当我们从公式(15)推导出包含运动先验的类似于公式(21)的正规方程时，我们得到

其中是第步第次迭代后的运动估计。从这个方程可以看出，协方差矩阵越大，运动先验对图像残差的影响越小，反之亦然。

5 评估

在本节中，我们通过一系列实验证明，鲁棒传感器模型和时间运动模型能大大提高估计里程计的准确性。我们使用TUM RGB-D基准和具有真值的合成数据集来评估我们的方法。对于每个实验，我们计算了以米每秒为单位的漂移的均方根误差(RMSE)。此外，还记录了匹配一对图像所需的运行时间。所有的计时结果都是在Intel i5 670 CPU(3.46GHz)和4 GB RAM的个人电脑上获得的。需要注意的是，计算只使用了一个单核CPU，内存占用小于输入图像大小的四倍(用于存储图像金字塔和每层图像的梯度)。

为了比较，我们使用之前版本和文献[11]中使用Tukey权重函数重新实现的版本计算了相机的轨迹。我们运行了前面的实现，在表中记为reference。

A 合成序列

我们生成了两个具有真值的合成序列，一个在静态场景中(静态)，另一个在其中有一个小的、移动的物体(移动)。为了生成图像，我们在fr1/desk序列的真实RGB-D图像上模拟了一个沿着采样轨迹移动的相机。相机平面内的运动在米之间的均匀分布采样，模拟一个与文献[31]中相机运动速度相似的相机，即0.3米/秒。此外，围绕相机轴的旋转从度之间的均匀分布采样。对于移动序列，我们沿着独立于模拟相机运动的轨迹移动原始图像的一小块。

此外，我们使用两个参数集来运行我们的代码，一个适合实时性能，另一个适合最大精度。两个参数集之间最显著的区别是，一个只使用分辨率为像素的图像，而另一个使用的是的全分辨率。其它参数控制算法的迭代次数。

表1和表2展示了结果。对于静态序列，所有方法的性能几乎都一样好，特别是当我们允许大的迭代次数和在全图像分辨率(适合最大精度)下操作时。对于移动序列，这将发生巨大的变化：即使有尽可能多的迭代，最小二乘也会产生5.0厘米/秒的漂移。Tukey变体产生2.7厘米/秒的漂移，而分布只有1.3厘米/秒。在所有情况下，分布总是优于未加权的变体。

需要注意的是，在实时设置下，加权版本(56毫秒和47毫秒)的计算时间大约是未加权版本(30毫秒)的两倍。当允许更多的迭代时，对于非加权版本，计算时间甚至增加到130毫秒，而对于分布，则增加到508毫秒。

由此，我们得出以下结论：(1)在存在噪声和(可能一致)异常值时，鲁棒的加权函数特别有用；(2)分布比高斯分布更适合。

B TUM RGB-D基准序列

另外，我们在TUM RGB-D基准的几个序列上评估了我们稳鲁棒的视觉里程计方法。在这些实验中，我们关注的是实时参数设置，因为它更适合我们在四轴飞行器上的应用。

请注意，我们以米/秒为单位来测量漂移，而早期的文章则是测量每帧的漂移。我们发现这种测量是不准确的，因为运动捕捉系统的噪声具有相同的量级。此外，我们测量的不是中值漂移，而是RMSE漂移，它更容易受到较大误差的影响。因此，RMSE漂移越低，跟踪质量越高。

对于评估，我们选择了四个desk数据集，因为它们包含了具有结构和纹理的真实世界的布景，不同的速度剖面(profile)，并且该数据集已被其他研究者用于评估视觉里程计的性能。

单个序列的结果在表3中给出，并在表4中总结。基于分布的变体与合成数据集相似，表现最好。与合成数据集相比，加权版本在静态场景数据集上的性能优于非加权版本。我们的结论是，真实的数据即使来自静态的真实场景，也包含大量的噪声和异常值，需要处理。通过检查，我们发现这些异常值部分源于遮挡和反射，这违反了光强一致性假设。在desk序列中，我们发现时间先验并不能进一步提高我们方法的准确性。我们认为这是因为desk序列特征足够丰富，使得残差最小化是驱动(driving)因素。

我们还研究了sitting和walking序列的所有变体的行为，其中两个人在一个桌子前的视野，而手持相机是沿着不同的轨迹移动。在sitting序列上，我们发现分布结合时间运动先验的表现最好，见表5。这是一个预期的结果，因为先验为最小化提供了一个有价值的初始化，并将其引导到正确的方向。然而，在轨迹的转弯处超调(overshooting)会降低整体的改善。然而，当输入数据(如这些序列)中存在许多异常值(由于运动物体)时，先验尤其有用。

在walking序列中，异常点占主导地位的图像，因为大多数图像的实质部分由移动的人组成。在这些数据集上，我们发现所有讨论的方法都失败了。特别是，即使是最好的方法(Tukey权重函数)仍然产生0.25米/秒的漂移，这比平均相机速度还要大。因此，在大的动态物体存在的情况下，计算精确的运动估计仍然是一个开放的挑战。注意，对于这项工作，我们明确地关注于无状态视觉里程计，但在这种情况下，可能需要估计类似文献[22]的静态背景的(局部)地图。

总之，我们在实验中表明，分布在广泛的真实序列和合成序列上显著提高了精度。此外，我们还证明了时间先验在异常点数量多、光度信息少或相机速度快的场景中具有积极的作用。

6 结论

本文推导了一个从RGB-D图像中直接估计相机运动的概率公式。我们的公式允许使用自定义的概率分布的传感器和运动模型。对于我们的应用，我们证明了分布能够更好地匹配观测残差，并提升准确性。此外，在最小化问题中加入一个运动先验，即便存在运动物体，也能够指导和稳定运动估计。在真实数据和模拟数据的详尽实验中，我们证明了我们的方法是高度准确和鲁棒的，并优于以往的方法。在不久的将来，我们的目标是将我们的算法应用到四轴飞行器上，以补偿短期的运动漂移。我们的软件在开源许可下可用。

参考文献

略