【论文翻译】Fast Visual Odometry and Mapping from RGB-D Data

【论文翻译】Fast Visual Odometry and Mapping from RGB-D Data

---

摘要

RGB-D相机是一种传感器，可输出颜色和深度以及有关所观察场景的信息。 在本文中，我们提出了一种用于RGB-D相机的实时视觉测距和制图系统。 该系统在台式机CPU上的单线程中以30Hz或更高的频率运行，而无需GPU加速。 通过将当前RGB-D图像中观察到的稀疏特征与先前特征的模型对齐，我们可以恢复移动摄像机的6自由度轨迹。 该模型具有持久性，并使用卡尔曼滤波器从新观测值动态更新。 我们为滤波阶段基于高斯混合模型制定了稀疏RGD-B特征的新不确定性度量。 我们的定位算法能够在线使室内环境中的小规模回路闭环，而无需任何其他SLAM后端技术。

---

一、简介

RGB-D相机是一种提供两个并发图像流的设备：常规彩色图像和深度图像，其中包含从相机到每个观察像素的距离的度量。 使用设备中内置的红外投影仪和红外摄像头的组合可以恢复深度信息。 近年来，低成本RGB-D相机（例如Microsoft Kinect）已在机器人技术和计算机视觉研究社区中广泛流行。

我们研究了从一系列时间连续的RGB-D图像$I_{0:n}$恢复RGB-D相机的不受约束的轨迹，观察静态场景的问题。 摄像机的轨迹表示为姿势$C_{0:n}$的序列，每个姿势都有6个自由度（位置和方向）。 估计的轨迹可用作控制回路的输入，或使用RGB-D数据构建全局3D地图。

解决此问题的一种常见方法是通过将每个图像与上一个图像对齐来递增计算相机的姿态。 为此，将需要计算最能使$I_t$ 与 $I_{t-1}$对齐的刚性变换$T_t$。 每个相机的姿势因此可以表示为
$$C_t=C_{t-1}{T}_t\text{\hspace{1em}(1)}$$
假定初始摄像机位置为等价变换$\left(C_0=I\right)$。

在机器人技术中，这通常称为视觉里程表（VO），这是Nister等人引入的 [8]。在他们的论文中，他们演示了一种配有立体摄像机的地面机器人的实时系统。该名称适用于任何仅使用单个或多个摄像机输入的轨迹估计算法。 Scaramuzza和Fraundorfer [10]概述了视觉里程计的历史以及最先进的系统。

仅执行最后一对图像之间的对齐（与整个序列相对）的选择将算法的计算复杂度降低到相对于路径长度的恒定时间。这允许视觉里程计系统以高更新频率实时运行。这种设计的缺点是，增量位姿计算中的小误差会随着时间的推移而累积，并在轨迹中产生漂移。减轻这种情况的常用方法是对最后m张图像进行配准。这些称为滑动窗口的技术保留了成对配准的恒定时间复杂度。它们可以大大降低漂移误差，但不能完全解决问题。

在长距离（包括重新定位位置）上获得全局正确轨迹的挑战被称为闭环。 Scaramuzza和Fraundorfer [10]在他们的教程中将这些算法归类为V-SLAM或基于视觉的同时定位和映射。 V-SLAM系统通常依赖视觉里程表作为构建块。

二、研究现状

Steinbrucker等， [13]提出了一种通过最小化密集深度数据空间中的能量函数来进行帧到帧轨迹估计的系统。Dryanovski等，文献[2]提出了一种利用边缘特征进行帧到帧配准的系统，该系统使用高频环路处理稀疏数据，低频环路处理密集数据。亨利等 , [4]提出了一种使用GPU加速的SIFT功能的系统。通过使用稀疏和密集数据，将图像逐帧对齐。全局优化是使用稀疏束调整离线进行的。恩德雷斯等，[3]提出了一个使用稀疏SURF，ORB或GPU加速的SIFT描述符的系统（选择是可配置的）。为了增加鲁棒性，将图像与恒定大小的先前帧的子集对齐。该实现需要多个线程。轨迹的其他细化可以离线进行。最近，Newcombe等， [7]提出了一种用于RGB-D姿态跟踪和映射的系统。他们的方法将密集深度数据与表面模型对齐。该模型增加了新数据。该系统需要配备GPU的计算机。

三、主要贡献

在本文中，我们提出了一种视觉测距系统，该系统不依赖于帧到帧或滑动窗口技术。我们首先计算输入的RGB-D图像中稀疏特征的位置以及它们在相机框架中的对应3D坐标。接下来，我们将这些特征与在固定坐标系中表示的3D特征的全局模型数据集对齐。用ICP算法进行对齐[14]。在计算完转换后，该模型将使用新数据进行扩充。我们将RGB-D图像中的特征与模型中的特征相关联，并使用卡尔曼滤波器框架对其进行更新。图像中所有无法关联的特征都将作为新地标插入模型集中。

随着新数据的积累，模型（开始时是空的）的大小逐渐增大。为了保证恒定时间的性能，我们在模型可以包含的点数上设置了上限。一旦模型增长到超过该大小，就会删除最旧的特征，以腾出空间容纳新功能。

为了能够准确地执行数据关联和过滤，我们开发了一种新颖的方法来估计RGB-D相机深度读取中的不确定性。该方法基于高斯混合模型，并且能够准确捕获问题区域（如对象边缘）中深度的高度不确定性。

通过对持久性模型而不是仅对最后一帧进行对齐，我们能够大幅降低姿态估计的漂移。 我们证明，即使模型尺寸很小，我们也可以准确地绘制出房间或办公室大小的环境，并准确地闭合环路，而无需任何通常与V-SLAM算法关联的其他后端处理技术。

我们以30Hz或更高的速率实时执行轨迹估计（相机以30Hz的速度输出图像，但是我们的算法能够更快地处理它们）。 它使用单个线程，并且不需要GPU。 我们的实施可通过免费的开源许可证公开获得。

四、不确定性模型

在本节中，我们描述如何估计从RGB-D图像检测到的3D坐标和稀疏特征的不确定性。 给定像素$\mathbf{q}=[u,v,d{]}^T$，其中u和v是图像坐标，d是RGB-D相机的原始深度测量，我们可以将其表示为3D点$\mathbf{p}=[x,y,z{]}^T$，在相机坐标系[5]中：
$$\begin{array}{rl}z& =\frac{Z_0}{1+\frac{Z_0}{fb}d}\\ x& =\frac{z}{f}\left(u-c_x\right)\\ y& =\frac{z}{f}\left(v-c_y\right)\end{array}\text{\hspace{1em}(2)}$$

我们有以下参数：
Z0：到用于双目匹配的参考平面的距离（内部设备参数）
b：红外投影仪和照相机之间的基线
f：红外摄像机的焦距
cx，cy：红外图像光学中心
为了获得z的不确定性，Khoshelham和Elberink [5]将d视为具有标准偏差d的随机变量。 他们从等式2a得出
$${\sigma }_z=\frac{1}{fb}{\sigma }_d{\mu }_z^2\text{\hspace{1em}(3)}$$
作者获得的带有校准参数的表达式是
$${\sigma }_z=1.45\times 10^{-3}{\mu }_z^2\text{\hspace{1em}(4)}$$
其中z以米表示。 作者假设每个像素中的深度误差与周围的像素无关。 此外，他们假设u和v读数没有不确定性。

1.通过高斯混合模型的深度不确定性

我们以两种方式扩展模型。 首先，由稀疏特征检测器检测到的特征的位置容易出错。 因此，我们允许在u和v维度中存在不确定性。 其次，我们假设深度不确定性不仅取决于给定像素的深度读数，还取决于局部窗口中的相邻像素。 我们将证明，使用这些假设，我们可以比以前发布的模型更好地预测深度不确定性的大小。

图2. RGB-D相机深度读数的不确定度分析。 左：RGB图像，仅用于可视化显示。 右：观察到的（地面真实情况）不确定性$\left({\bar{\sigma }}_z\right)$，是通过拍摄200张静态场景的深度图像并计算每个像素的平均值和标准差得出的。 颜色按不确定度的对数进行缩放。 左中：根据简单模型$\left({\sigma }_z\right)$从单个深度图像预测的不确定性。 中右：根据高斯混合模型$\left({\hat{\sigma }}_z\right)$从同一深度图像预测的不确定性。 我们证明了高斯模型的不确定性可以更准确地预测真实的不确定性，尤其是在物体边缘附近。

让我们开始假设u和v是分别根据正态分布$N\left({\mu }_u,{\sigma }_u\right)$ 和$N\left({\mu }_v,{\sigma }_v\right)$分布的独立随机变量。 此外，让${\sigma }_u$和${\sigma }_v$通知以下大小为3x3的近似高斯核：
$$W=\frac{1}{16}\left[\begin{array}{lll}1& 2& 1\\ 2& 4& 2\\ 1& 2& 1\end{array}\right]\text{\hspace{1em}(5)}$$
假设z是正态分布的，我们可以定义一个随机变量$\hat{z}$，它是z变量在局部窗口 { i : ∈ [ μ u − 1 , μ u + 1 ] , j ∈ [ μ v − 1 , μ v + 1 ] } \left\{i: \in\left[\mu_{u}-1, \mu_{u}+1\right], j \in\left[\mu_{v}-1, \mu_{v}+1\right]\right\} {i:∈[μu​−1,μu​+1],j∈[μv​−1,μv​+1]}中的混合。 根据核$W$选择混合物$w_{ij}$的权重。所得高斯混合分布的均值和方差为
$$\begin{array}{rl}{\hat{\mu }}_z& =\sum _{i,j}{w}_{ij}\left({\mu }_{z_{ij}}\right)\\ {\hat{\sigma }}_z^2& =\sum _{i,j}{w}_{ij}\left({\sigma }_{z_{ij}}^2+{\mu }_{z_{ij}}^2\right)-{\hat{\mu }}_z^2\end{array}\text{\hspace{1em}(6)}$$
在此阶段，我们有两个不确定性的替代模型：${\sigma }_z$，根据等式3中的简单模型进行估算；${\hat{\sigma }}_z$，根据高斯混合模型进行估算。 为了评估哪个模型具有更大的预测能力，我们收集了静态场景的n个深度图像。 对于图像中的每个像素，我们根据以下公式计算度量深度z中的不确定性${\bar{\sigma }}_z$
$$\begin{array}{rl}{\bar{\mu }}_z& =\frac{1}{n}\sum _{k=1}^n{z}_k\\ {\bar{\sigma }}_z^2& =\frac{1}{n-1}\sum _{k=1}^n{\left({\bar{\mu }}_z-z_k\right)}^2\end{array}\text{\hspace{1em}(7)}$$

我们称其为观察到的不确定性。 当n大时（我们使用了200张图像），我们可以假设观察到的不确定度接近RGB-D相机测量的真实不确定度。 接下来，我们拍摄一个深度图像，并生成两个预测的不确定性${\sigma }_z$和${\hat{\sigma }}_z$。 图2显示了根据两种模型观察到的不确定性和预测到的不确定性的比较。 我们注意到，高斯混合模型预测的不确定性比简单模型好得多，尤其是在对象边缘附近。 这是因为Kinect相机会产生边缘像素的读数，这些像素往往会从前景跳到背景。

2.3D不确定性

我们可以根据等式2估算点p的3D不确定性$\Sigma$。
$$\Sigma =\left[\begin{array}{ccc}{\sigma }_x^2& {\sigma }_{xy}& {\sigma }_{xz}\\ {\sigma }_{xy}& {\sigma }_y^2& {\sigma }_{yz}\\ {\sigma }_{xz}& {\sigma }_{zy}& {\sigma }_z^2\end{array}\right]\text{\hspace{1em}(8)}$$
式中
$\begin{array}{rl}{\sigma }_x^2& =\frac{{\sigma }_z^2\left({\mu }_u-c_x\right)\left({\mu }_v-c_y\right)+{\sigma }_u^2\left({\mu }_z^2+{\sigma }_z^2\right)}{f_x^2}\\ {\sigma }_y^2& =\frac{{\sigma }_z^2\left({\mu }_u-c_x\right)\left({\mu }_v-c_y\right)+{\sigma }_v^2\left({\mu }_z^2+{\sigma }_z^2\right)}{f_y^2}\\ {\sigma }_{xz}& ={\sigma }_{zx}={\sigma }_z^2\frac{{\mu }_u-c_x}{f_x}\\ {\sigma }_{yz}& ={\sigma }_{zy}={\sigma }_z^2\frac{{\mu }_v-c_y}{f_y}\\ {\sigma }_{xy}& ={\sigma }_{xy}={\sigma }_z^2\frac{\left({\mu }_u-c_x\right)\left({\mu }_v-c_y\right)}{f_x{f}_y}\end{array}$

以上是使用${\mu }_z$ 和 ${\sigma }_z$从简单深度不确定性模型得出的。 然后，我们可以通过分别用${\hat{\mu }}_z$ 和${\hat{\sigma }}_z$替换${\mu }_z$ and ${\sigma }_z$来根据高斯混合模型进行近似。 我们将p近似为均值$\mu ={\left[{\mu }_x,{\mu }_y,{\mu }_z\right]}^T$，协方差$\Sigma$的多元高斯分布协方差。

五、轨迹估计

1.总览

轨迹估计从提取每个传入RGB-D图像$I_t$中的稀疏特征开始。 在RGB图像的强度通道上检测到这些特征。 我们已经尝试了几种特征检测器选择，包括SURF [1]，ORB [9]和Shi-Tomasi [12]关键点。 尽管我们的实现在它们之间提供了可配置的选择，但我们发现Shi-Tomasi功能在鲁棒性和计算速度之间提供了最佳的权衡。 请注意，我们只计算图像中关键点的2D位置； 我们对关键点的本地描述符向量不感兴趣。

接下来，我们根据第四部分中定义的不确定性方程估算每个特征的3D正态分布。 二。 由此，我们生成了一组3D特征 D t = { d i } D_{t}=\left\{\mathbf{d}_{i}\right\} Dt​={di​}。 每个特征$\mathbf{d}=\left\{{\mathbf{\mu }}^{[D]},{\Sigma }^{[D]}\right\}$具有均值和协方差矩阵。 我们将此集标记为数据。 数据集以摄像机参考系表示。

我们有一个类似的模型集， M t = { m j } M_{t}=\left\{\mathbf{m}_{j}\right\} Mt​={mj​}，其中$\mathbf{m}=\left\{{\mathbf{\mu }}^{[M]},{\Sigma }^{[M]}\right\}$ 用固定的参考系表示。 使用ICP将数据集与模型对齐，然后旋转到固定框架中。 接下来，我们建立两组之间的最终对应关系。 使用卡尔曼滤波器更新模型中重新观察到的特征，并将新特征添加到以前的模型中。 本节的其余部分将更详细地描述这些步骤。

2.定位

我们定义了一个距离函数dist，它测量两个特征${\mathrm{f}}_a$，${\mathrm{f}}_b$之间的距离，正态分布为均值${\mu }_a$和${\mu }_b$以及协方差矩阵${\Sigma }_a$和${\Sigma }_b$。
$$\mathrm{dist}\left({\mathbf{f}}_a,{\mathbf{f}}_b\right)=\sqrt{{\Delta }_{{\mathbf{f}}_{\mathbf{a}}{\mathrm{f}}_{\mathbf{b}}}{\left({\Sigma }_a+{\Sigma }_b\right)}^{-1}{\Delta }_{{\mathbf{f}}_{\mathbf{a}}{\mathrm{f}}_{\mathbf{b}}}^T}\text{\hspace{1em}(9)}$$
式中
$${\Delta }_{{\mathrm{f}}_{\mathrm{a}}{\mathrm{f}}_{\mathrm{b}}}={\mu }_a-{\mu }_b\text{\hspace{1em}(10)}$$
距离函数基于从点到分布的马氏距离。

我们使用ICP将数据与模型对齐。 作为ICP配准的初步猜测，我们使用固定坐标系和摄像机坐标系之间的先前变换$T_{t-1}$。ICP算法有两个有趣的步骤：在两个输入集之间生成对应关系，并计算使对应关系之间的距离最小的变换。 在经典的ICP公式中，对应关系是使用欧几里得空间中的最近邻居生成的； 还可以通过最小化欧几里德距离平方和来估算变换。

我们使用一种改进的ICP算法，其中使用马氏距离建立近似对应关系。 首先，我们利用特征的手段建立模型的kd-tree [6]。 接下来，对于数据中的每个特征d，我们从模型中找到k个最近的欧几里得邻域。 最后，我们遍历所有k个候选对象，并找到马氏距离最小的候选对象。 这使我们能够利用kd-tree的效率，而kd-tree不能直接与非线性函数（如马氏距离）一起使用。 在我们的实现中，我们为k使用较小的大小（例如4）。

ICP算法的其余部分保持不变。 我们注意到，尽管可以将马氏距离优化为最佳转换的目标函数，但我们没有这样做，这是一个可能的改进领域。 实现类似优化（尽管在密集数据的情况下）的算法示例是通用ICP [11]。

3.数据关联和更新

我们通过将数据集$D$旋转到固定的参照系中并将其称为$D^{\prime }$来开始数据关联步骤。 让固定和摄像机坐标系之间的当前变换为$T$，由旋转和平移组成：
$$T=\left[\begin{array}{ll}\mathrm{R}& \mathrm{t}\\ 0& 1\end{array}\right]\text{\hspace{1em}(11)}$$
我们可以根据以下公式转换均值向量和协方差矩阵：
$$\begin{array}{l}{\mu }^{\prime }=\mathrm{R}\mu +\mathrm{t}\\ {\Sigma }^{\prime }=\mathrm{R}\Sigma {\mathrm{R}}^T\end{array}\text{\hspace{1em}(12)}$$

接下来，对于变换后的数据集中的每个点${\mathrm{d}}_i^{\prime }$，我们在模型中找到近似最接近的马氏距离邻近点${\mathbf{m}}_j$。
$\mathrm{dist}\left({\mathbf{d}}^{\prime },\mathbf{m}\right)=\sqrt{{\Delta }_{{\mathbf{d}}^{\prime }\mathbf{m}}{\left({\Sigma }^{[M]}+{\Sigma }^{\left[D^{\prime }\right]}\right)}^{-1}{\Delta }_{{\mathbf{d}}^{\prime }\mathbf{m}}^T}$

如果两个点之间的距离小于阈值$ϵ$，我们认为这两个点是关联的。 典型值包括7.82或11.35。 这两个阈值对应于从给定的模型分布中采样数据点的95％和99％概率测试。

$D^{\prime }$中所有无法关联的要素都将作为M中的新成员插入。模型的大小是有界的，因此，如果超出了允许的最大大小，我们将删除模型中最旧的要素。 这可以通过使用环形缓冲区实现来实现。 对于关联的每个功能，我们执行卡尔曼滤波器更新。 我们将分布$\mathrm{m}$视为先验，将分布$\mathrm{d}$视为观测。 预测分布与时间$t-1$时模型的先前状态相同。

$$\begin{array}{l}{\tilde{\mu }}_t={\mu }_{t-1}^{[M]}\\ {\tilde{\Sigma }}_t={\Sigma }_{t-1}^{[M]}\end{array}\text{\hspace{1em}(13)}$$

图3.轨迹估计的流程。 我们将稀疏特征数据从当前RGB-D帧对齐到持久模型。 数据由具有协方差矩阵的3D点表示。

图4.使用持久性模型（左）与帧间ICP进行轨迹估计的比较。 顶行：侧视图，$xz$ -平面。 最底行：顶视图，$xy$ -平面。 所示的轨迹由5个重复的循环组成，每个循环中处理大约2000张图像。

以下是时间t处新状态的更新（校正）方程式：
$$\begin{array}{rl}{K}_t& ={\tilde{\Sigma }}_t{\left({\tilde{\Sigma }}_t+{\Sigma }_t^{[D]}\right)}^{-1}\\ {\mu }_t^{[M]}& ={\tilde{\mu }}_t+K_t\left({\mu }_t^{[D]}-{\tilde{\mu }}_t\right)\\ {\Sigma }_t^{[M]}& =\left(I-K_t\right){\tilde{\Sigma }}_t\end{array}\text{\hspace{1em}(14)}$$

六、实验

1、视觉里程计

我们使用在室内环境中记录的RGBD数据评估轨迹估计流程。 摄像机沿循环移动，然后放回其起点。 图像以QVGA分辨率流式传输。 我们将数据复制5次以模拟完全相同的循环。 图4显示了由持久性模型生成的轨迹（左）与由帧间ICP生成的轨迹（右）的对比。 我们展示了我们的方法能够正确解决循环问题，而无需任何其他后端优化。 请注意，我们并不能声称可以解决任意长度的循环。 但是，如果需要，我们的方法可以与现有技术（例如姿态图优化或稀疏束调整）结合使用。

图5.顶部：每个传入图像的处理持续时间。 横轴：图像索引号。 垂直轴：特征检测（红色），对齐和模型更新（蓝色）所需的时间。 下：数据和模型集的大小。 垂直线标记每个重复循环的开始。

图5（顶部）显示了视觉里程表的运行时间。 我们将过程分为两部分：特征检测和注册。 特征检测包括检测Shi-Tomasi角并计算分布。 注册包括ICP运行时间和模型更新。 结合起来，它们占据了每个RGB-D图像的整个处理时间。 我们实现了16.1毫秒的平均处理时间，最大30.5毫秒，标准偏差为3.8毫秒。

图5（底部）显示了随时间变化的数据和模型集的大小。 我们每帧检测约100个特征。 随着新空间的探索，模型也在不断发展。 闭环后，模型中插入的新功能将大大减少。 我们成功关闭了模型中少于1500个功能的循环。

2、建图

我们实现了一个映射应用程序，该应用程序使用视觉里程表的输出并随时间聚集密集点云数据。 使用八叉树实时对数据进行下采样，以消除冗余点。 它在单独的线程中运行。 输出图如图6所示。

图6.用我们的系统生成的基于八进制的3D地图。 底行显示了密集3D地图的正交视图（左），以及最终的模型集（右）。

七、结论

在本文中，我们介绍了用于RGB-D相机的视觉里程计系统。该系统使用稀疏特征，这些稀疏特征针对有界大小的持久模型进行注册。该模型通过概率卡尔曼滤波器框架进行更新。为了实现这一目标，我们基于局部图像窗口中读数的高斯混合模型，为RGB-D图像中的稀疏特征开发了3D不确定性的公式。

我们已经显示了实验结果，这些结果证明了不确定性模型的预测能力，该模型能够比该领域以前发布的公式更好地估计物体边缘周围的不确定性。 我们还提供了实验结果，这些结果证明了我们的视觉里程表的速度和准确性。

与许多其他现有技术系统不同，我们的算法不使用密集数据，特征描述符向量，RANSAC对齐或基于关键帧的包调整。 通过避免这些计算，我们能够实现60Hz的平均性能。 我们的视觉里程表只需要一个线程，不需要GPU。 使用持久性模型而不是逐帧技术可以显着降低漂移。 我们证明了我们的方法具有足够的准确性，可以自然地在房间环境中关闭循环，而无需进行任何后端后期处理。

有几个领域仍有待改进。 首先，我们的概率框架用于最近的邻居关联和模型更新，而不用于实际的ICP错误最小化步骤。 其次，我们假设深度图像具有零均值噪声。 但是，我们注意到该假设不成立，并且更换相机会影响视觉里程表的性能。 因此，我们需要进一步研究估计和校正深度图像中系统误差的方法。 最后，我们想探索如何结合经典的闭环技术来应用我们的方法，以便能够解决大规模的闭环问题。