＜论文阅读＞ DI-FODO — 3D距离传感器的快速视觉里程计

不知多少同学是通过RF2O知道这篇论文的，很多公式RF2O中并没有介绍，读该论文可以很好的答疑。不过其中很重要的一部分Warping 因为也不是本文的贡献，所以没有给出它的数学公式，那么读完这篇就是下一篇：High Accuracy Optical Flow Estimation Based on a Theory for Warping，欢迎讨论～

摘要：

本文提出了一种新的稠密方法来实时计算自由飞行距离传感器的里程计。该方法将距离流约束方程应用于时间流中的感测点，以得出传感器在刚性环境中的线速度和角速度。尽管这种方法适用于任何距离传感器，但我们将其公式具体化以估计距离相机的 3-D 运动。所提出的算法在不同的图像分辨率下进行了测试，并与两种最先进的方法进行了比较：广义迭代最近点 (GICP) 和鲁棒密集视觉里程计 (RDVO) 。实验表明，我们的方法明显优于使用相同几何输入数据的 GICP，而它实现的结果类似于需要几何和光度数据才能工作的 RDVO。此外，还进行了实验以证明我们的方法能够估计在单个 CPU 内核上运行的 60 Hz 的快速运动，这是文献中从未报道过的性能。该算法可在开源许可下在线获得，以便机器人社区可以从中受益。

一、引言

快速准确的 6 自由度 (DOF) 视觉里程计 (VO) 在当前机器人技术中越来越重要，应用程序的要求越来越高。两个明显的例子是必须在不平坦地形上运行的地面车辆和无人机，它们通常需要跟踪它们的 3D 姿态才能自主飞行。在这些情况下，VO 的替代方案是应用基于 IMU 的惯性导航，但由于无法以足够的精度消除重力，因此随着时间的推移，它们会累积过多的误差。另一方面，车轮里程计或 GPS 导航等传统解决方案根本无法替代 VO，因为它们无法提供 3D 姿态估计。 VO 的另一个重要优势是所需的传感数据（由相机或激光扫描仪提供）也被其他机器人模块利用，用于导航（SLAM、避障等）和场景理解。
RGB-D 相机的出现为 VO 带来了新的和充满希望的前景，但也带来了一些挑战。这些传感器能够同时提供 30-60 Hz 的 RGB 和深度图像，这是一个海量的数据。出于这个原因，新颖的 VO 方法难以保持良好的计算性能，同时试图充分利用所有这些传入数据，并且经常倾向于关注 RGB 或深度图像。在任何情况下，真正能够以 30 Hz 运行的 VO 方法并不多，而且很少有人能达到某些 RGB-D 相机提供的 60 Hz 的最大帧速率。
在本文中，我们介绍了一种新的 VO 方法，称为 DI-FODO（DIFerential ODOmetry），它采用 3D 距离图像（或扫描）来估计传感器的线速度和角速度。它的公式基于距离函数的空间和时间线性化（所谓的距离流约束方程），它是按从粗到细的方案逐像素施加的，以便以应对大运动的估计。 DIFODO 的一个显着特点是使用相同的输入数据（距离测量）来获取从粗到细金字塔的每个级别的相机运动，并在级别转换后执行变形。因此，尽管我们在这里专门针对深度相机制定了它的公式，但 DIFODO 可以很容易地适应任何距离传感器。 DIFODO 的另一个关键特性是，与其他 VO 方法相比，它依赖于封闭形式的解决方案，并在单个 CPU 内核上实时运行。因此，DIFODO 能够估计快速运动和更精细的轨迹，因为它可以在最高摄像机帧速率 (60 Hz) 下工作。不利的一面是，它与其他基于几何的 VO 方法具有相同的弱点，即：当对场景的观察缺乏足够的几何信息时，估计问题变得不确定，类似依赖光度数据的 VO 方法在观察低-纹理区域。我们提出的想法来自 5 和 6，它们受到光流概念的启发，提出了一种算法来估计激光扫描的二维运动。
已实现的代码已添加到 MRPT [7] 并在开源许可下可用。可以在此处找到我们方法的说明性视频以及代码：http://mapir.isa.uma.es/mjaimez

二、相关工作

三、从距离流方程导出的速度约束

本节描述了如何通过应用距离流约束及其对观察点速度的限制，从两个连续的帧中估计距离相机的 3-D 运动。在刚性场景的普遍假设下，我们根据相机运动计算点速度，因此，如果考虑足够数量的点，则可以计算后者。
令 $\dot Z : (Ω ∈ \mathbb N^2 ) →\mathbb R$ 是由 3-D 距离相机提供的深度图像，其中 $Ω$ 是图像域。距离相机的距离流约束方程为：
$\dot Z = {∂Z \over ∂t} + {∂Z \over ∂u} \dot u + {∂Z \over ∂v} \dot v + O(Δt, \dot u, \dot v) ⇒(1)$ $\dot Z\approx Z_t+Z_u \dot u+Z_v\dot v \tag{2}$
其中 $w = (u, v)$ 是像素坐标。等式 (2) 反映了距离的全导数可以从光流 $\dot w = (̇\dot u, \dot v)$ 和 $Z$ 关于时间 $t$ 、 $u$ 和 $v$ 的偏导数 ( $Z_t$ , $Z_u$ 和 $Z_v$ ）。由于 (2) 是从一阶泰勒级数展开 (1) 推导出来的，因此只有当高阶项 $\dot u, \dot v)$ 可以忽略不计时，它才是准确的。在实践中，如果连续图像之间的位移很小或观察点属于线性化成立的平面块，则满足此条件。
$Z$ 的三个偏导数可以直接从深度图像中计算出来，但是 $\dot Z$ 、 $\dot u$ 和 $\dot v$ 是未知数，应该用相机速度来表示。令 $ξ = ( v_x , v_y , v_z , ω_x , ω_y , ω_z )^T$ 为摄像机速度， $P_c = ( x, y, z)$ 为相机视野中任意点 P 的空间坐标，两者都在相机的坐标系中进行了描述。 $\dot Z$ 、 $\dot w$ 和 $P_c$ 之间的关系可以从“针孔模型”中推导出来，假设 $P$ 的像素和空间坐标是时变的（见图 1）：
在这里插入图片描述
$f_y{y \over z} + v_m ⇒ \dot v = f_y( {\dot y z − \dot z y \over z^2})\tag{3}$ $f_x{x \over z} + v_m ⇒ \dot v = f_x( {\dot x z − \dot z x \over z^2})\tag{4}$
其中 $u_m , v_m )$ 是图像中心（主点）， $f_x$ , $f_y$ 是焦距值，均以像素表示。
相对于相机坐标系，在刚性和静态场景的假设下，所有观察到的点都以与相机速度相等但符号相反的速度移动。因此，任何 3-D 点相对于相机的速度可以表示为：
$\dot P_c = \begin{pmatrix} \dot x \\ \dot y \\ \dot z \\ \end{pmatrix} = \begin{pmatrix} -v_x-zw_y+yw_z\\ -v_y +zw_x - xw_z\\-v_z -yw_x+xw_y \\ \end{pmatrix}$
作为中间步骤，根据 (3) 和 (4)，我们根据 P 的 3D 速度将 (2) 中的光流表示为：
$−Z_t = − \dot z + Z_u f_x ( {\dot x_z − \dot z_x \over z^2})+ Z_v f_y( {\dot y_z − \dot z_y \over z^2}) \tag{6}$
请注意， $Z$ 已被 $z$ 替换，因为它们是等价的：大写字母已用于表示深度图像，而小写字母直接表示深度的空间坐标。最后，刚性（5）在（6）中被施加为：
$−Z_t = (1 + {xf_x \over z^2} Z_u + {yf_y \over z^2} Z_v)(+v_z + y ω_x −xω_y )+ {f_x \over z} Z_u (−v_x + yω_z −zω_y )+ {f_y \over z} Z_v (−v_y −xω_z + zω_x ) \tag{7}$

等式（7）是一个线性限制，场景（相对于相机）的一个点的速度必须满足，因此，对相机速度施加了限制。因此，如果至少有六个线性独立的限制可用，我们就可以建立一个可解的代数系统。请注意，并非每个点都会向系统添加新信息，并且正如将在第五节中描述的那样，根据场景点的空间分布，问题可能是不合时宜的。
为了推导距离流方程的线性化假设深度图像的可微性，以及场景的小位移或恒定的距离梯度。因此，首先，必须排除边缘上的点，因为深度场在对象边界处不可微分。其次，深度图像梯度可能不是恒定的（具有高阶导数），这意味着在最一般的情况下，（7）仅适用于小位移。在我们的公式中，“小位移”意味着投影到图像平面上的场景点的运动小于计算图像梯度的邻域，通常：
$|\dot uΔt|< 1 \\ | \dot vΔt|< 1 \tag{8}$ 其中 $Δ t$ 是连续帧之间的时间间隔。因此，小位移的假设涉及图像分辨率和点的实际 3-D 运动。
Brox 等人提出了“小位移”限制的解决方案。包括利用从粗到细的方案。在这种策略中，为输入图像构建了一个高斯金字塔，光流从粗到细求解，在最粗的层次上捕获大位移，然后在整个金字塔中进行细化。在每个级别，使用先前获得的解决方案来 Warping 其中一个图像与另一个图像，导致图像对呈现出比原始对更少的差异，并且满足小位移的假设。另一种策略也可以应用于估计相机运动。在这种情况下，变形不是在图像平面中而是在 3D 空间中执行，并且利用距离传感器捕获的几何数据来根据空间变换对给定的图像或测量值进行变形。这就是为什么[2]和[23]中提出的方法需要几何和光度数据才能工作的原因：它们强加了光度一致性来估计运动，但它们需要场景的几何形状来执行变形。在我们的研究中，我们采用与 [2] 或 [23] 相同的 Warping 策略，但由于 DIFODO 仅依赖几何约束来估计相机运动，因此可以将其视为纯粹基于几何的方法，并且可以推广到工作与任何距离传感器。
Warping 已广泛应用于计算机视觉中，这不是本文的贡献。因此，这里没有给出它的数学公式；更多信息和细节可以在 [2]、[23] 或 [30] 中找到（又要读论文了，套娃！）。

四、求解相机运动

A. 最小二乘解

如前所述，至少需要六个带有线性独立限制的点来计算金字塔每一层的相机速度。然而，在实践中，更多的点 $(N)$ 可以使解决方案对噪声和错误更具有鲁棒性，这导致了一个可以通过加权最小二乘法求解的超定线性系统：
$(A^T W A )^{−1} A^T W B = M B \tag{9}$
其中 $∈\mathbb R^{N ×6}$ 包含乘以 (7) 中的 $ξ$ 的系数， $∈\mathbb R^{N ×1}$ 包含每个像素的深度时间导数（倒置符号）， $∈\mathbb R^{N ×N}$ 是包含权重的对角矩阵与每个方程的不确定性有关。（9）中的 $∈\mathbb R^{6×6}$ 矩阵是对称的和正定/半定的，因此，可以使用 Cholesky LDLT 分解来计算解。由于这种分解适用于固定大小的矩阵 $（ 6 \times 6 ）$ ，它不会限制我们方法的计算成本。尽管还没有详细介绍算法的所有步骤（参见第 VI 节），但复杂度与点数成二次方的唯一操作是乘积 $A^T WA$ 和 $A^T W B$ （使用稠密代数）。尽管如此，代数系统 (9) 可以重写为不需要计算这些乘积：通过将 (7) 的两边乘以相应权重的平方根，并用伪逆矩阵：
$A^w ξ = B^w →ξ = ((A^w )^T A^w )^{−1} (A^w )^T B^w = MB \tag{10}$ $A^w = \begin{pmatrix} \sqrt{w_1a_{11}} & \cdots &\sqrt{w_1a_{16}} \\\vdots &\ddots & \vdots\\\sqrt{w_Na_{N1}} & \cdots &\sqrt{w_Na_{N6}} \end{pmatrix} \qquad B^w = \begin{pmatrix} \sqrt{w_1b_{1}}\\ \vdots \\ \sqrt{w_Nb_{N}} \\ \end{pmatrix} \tag{11}$ 因此，新公式允许我们恢复运动参数时，计算时间仅随点数线性增长。

B. 加权函数

根据与距离流方程相关的不确定性或误差，权重对于调整每个点对整体运动估计的贡献是必要的。不失一般性，该误差可以表示为添加两项：

测量误差，测量传感器噪声如何影响距离方程，
以及来自 (2) 中的一阶近似的线性化误差。

前一项通常由零均值高斯分布建模，并且可以通过将测量误差传播到整个方程来计算。后者不遵循高斯模型，并且通常估计更复杂，因为它涉及研究距离的二阶导数以评估（1）中被忽略的项实际上有多重要。
在这项研究中，我们解决了两种误差源的分析，并提出了一个加权函数，该函数包含有关相机的信息和场景的几何形状，据此对图像像素进行相应的加权。
为了估计测量误差，我们需要考虑（7）中出现的每个随机变量或参数。假设相机的参数是完全已知的，(7) 可以重新排列并表示为所有容易产生噪声的变量的函数，即依赖于距离的变量：
$Z_t , Z_u , Z_v ) = 0 \tag{12}$ $x, y, z$ 与给定像素线性相关。
为了将这些与距离相关的变量的误差传播到距离流方程，必须先建立它们的协方差矩阵 $Σ_d ∈\mathbb R^{6×6}$ ：
$Σ_d = \begin{pmatrix} Σ_{xyz} & Σ_{(xyz)(Z_{t,u,v})} \\ Σ^T_{(xyz)(Z_{t,u,v})} & Σ_{Z_{t,u,v}} \\ \end{pmatrix}$
其中 $Σ_{xyz}$ , $Σ_{(xy z)(Z_{t , u , v} )}$ , $Σ_{Z_{t , u , v}} ∈ \mathbb R^{3×3}$ 。知道 $Σ d$ ，与测量误差相关的 (7) 的方差可以计算为：
$σ^2_m = ∇R ·Σ_d ·∇R^T \tag{14}$
其中 $\nabla R$ 是 $R$ 相对于 $x、y、z、Z_t 、Z_u$ 和 $Z_v$ 的梯度。鉴于 $R$ 还取决于 $ξ$ ，使用在前一个时间估计的 $ξ$ 计算 $\nabla R$ 的近似值。
另一方面，为了分析（1）中的线性化产生的误差，我们必须重写该方程，包括二阶项：
$Z_t + Z_u \dot u + Z_v \dot v + Z_2 (Δt, \dot w) + O (Δt^2 , \dot w), \\ Z_2 (Δt, \dot w) = {Δt \over 2} (Z_{tt} + Z_{tu} \dot u +Z_{tv} \dot v + Z_{uu} \dot u^2 + Z_{vv} \dot 2 + 2 Z_{uv} \dot u \dot v) \tag{15}$

可以观察到，忽略三阶或更高阶项，误差是所有二阶导数和光流的函数。为了估计（15）如何偏离线性，我们可以从距离图像中近似距离二阶导数，但事先不知道光流。这个问题的一种可能的解决方案是计算前一个 $ξ$ 时刻的光流。然而，当必须将这种策略应用于高斯金字塔的更精细级别时，这种策略就失去了意义，其中之前的解决方案没有为我们提供关于扭曲图像的光流的信息。出于这个原因，我们选择一个二次表达式，对所有像素均匀地惩罚二阶导数：
$δ^2_l = k_l[Δt^2 (Z^2_{tu} + Z^2_{tv}) + Z^2_{u u} + Z^2_{v v} + Z^2_{u v} ] \tag{16}$
常数 $k_l$ 加权线性化误差与测量误差，时间增量乘以时间导数，使所有项具有相同的数量级（距离差异）。在实践中，可以注意到二阶导数的高惩罚可能会丢弃场景中的一些点或区域，尽管其不准确，但对约束运动估计很有用。可以对场景几何进行更深入的研究，以预先检测每个点的距离方程如何约束速度估计并相应地调整 $k_l$ 。然而，这个过程会显着增加算法的计算成本，因此在本研究中没有采用。此外，在（16）中没有考虑 $Z$ 的二阶时间导数，因为它不能在不同的金字塔级别进行估计。除了最粗略的级别之外，其他都涉及到不受时间变化的扭曲图像，并且二阶时间导数的概念没有意义。
如果考虑构建 (9) N 个点，对于每个点 $i ， i \in 1, 2 . . N$ ，其对应的权重与距离流方程相关的不确定性成反比：
$W_{ii} = {1 \over \sigma^2_{m,i}+δ^2_{l,i}} \tag{17}$

五、场景几何、协方差分析和速度过滤

根据用于构建代数系统 (10) 的点的空间分布，问题可能是好的也可能是病态的。如果这些点包含关于场景在空间的三个方向上如何变化的足够信息，则 $M$ 矩阵将是正定的；否则，M 矩阵将是秩不足的（或接近）。排除某些旋转表面的退化奇异情况（即，球体中心的相机），如果观察到的表面的法线可以构成 3-D 矢量基础，则可以保证足够的信息，即
$([n_1 , n_2 , . . . , n_N ]) = 3 \tag{18}$
其中 $n_i ∈\mathbb R^{3×1}$ 是给定点 $i$ 处的表面法线向量。这个条件的不满足会导致众所周知的 ICP [32] 滑动问题，并且当整个点云来自一个或两个平面（墙壁、地板等）时，经常会发生这种问题。
如果 $M$ 矩阵是正定的，则场景的点提供了足够的几何约束来估计运动的 6 DOF。相反，如果 $M$ 没有满秩，则无法估计一些线速度或角速度项，求解器为这些变量提供的解是没有意义的。可以通过分析与最小二乘解相关的协方差矩阵 $Σ_ξ ∈ \mathbb R^{6×6}$ 来检测这种猜想
$Σ_ξ = {1 \over N −6}∑^N_{i=1}r^2_i((A^w )^T A^w )^{−1} \tag{19}$
其中 $r_i$ 是最小二乘解的残差。
协方差矩阵的对角元素反映了相机坐标系中估计的 $ξ$ 分量的方差。一般来说，以对角线形式表示矩阵 $Σ_ξ$ 更有意义，其中特征值指示哪些运动组合（特征向量）是受约束的，哪些是未确定的。这样，由于几何信息不佳而可能出现的速度估计中的任何不确定性都将被揭示：其相应的特征值将反映这种不确定性的高低，而其特征向量将包含受其影响的速度项。这个信息可以用来忽略那些方差（特征值）太高的速度项。因此，如果来自其他来源（如 IMU 或车轮里程计）的数据可用，它们可以与 VO 估计融合以生成更稳健的解决方案。如果不是这种情况，合适的选项包括根据当前和先前的估计应用平滑滤波器，如下所述：
令 $E = {ev_1 , . . . , ev_6 }$ 是一个正交基，包括 $Σ_ξ$ 的特征向量 $(ev_{1...6} ∈ \mathbb R^6 )$ ，而 $∈\mathbb R^{6×6}$ 是包含相关特征值的协方差对角矩阵。在给定的时间间隔 $[t, t + 1]$ 中，求解器 $ξ^{t,s}$ 提供的解和先前的速度估计 $ξ^{t−1}$ 必须在基 $E^t$ 中表示，该基总是从最后一个协方差矩阵 $Σ^{tξ}$ 计算得出，然后，获得基础 $E^t$ 中的滤波速度 $ξ^t_E 作为当前估计值和先前速度的加权和
$k_2 D^t ] ξ^t_E = ξ^{t,s}_E + (k_1 I + k_2 Dt ) ξ^{t−1}_E \tag{20}$
等式（20）表示具有动态平滑的低通滤波器，其中 $k_1$ 有助于软化估计的轨迹， $k_2$ 控制 D 中的特征值如何影响最终估计。 $k_2$ 的高值意味着那些具有不确定性的速度项将近似于它们之前的值（区间 $[t - 1, t]$ ），而 $k_2$ 低时当前估计值（区间 $[t, t + 1]$ ）将具有更高的重要性。

六、 DIFODO 框架和实施

总体而言，该算法执行算法 1 中描述的一系列步骤。它接收新的深度图像、先前的深度图像金字塔和最后的运动估计作为输入，并在期间产生平均线性和角相机速度最后一个时间间隔，将从该时间间隔更新相机姿势。下面解释该算法的重要方面和实现细节。

A. 高斯金字塔

高斯金字塔是通过使用标准 $5 \times 5$ 高斯核对深度图像进行下采样而构建的。然而，纯高斯平滑会在深度图像中产生伪影，因为它会在对象边界处混合非常不同的深度值。因此，改为应用双边滤波器来保留场景的几何形状。在我们的研究中，高斯金字塔开始以 QVGA 分辨率（240 × 320）构建。正如将在第 VII 节中讨论的那样，为了估计快速运动，以 120 × 160 (QQVGA) 的较低分辨率运行 DIFODO 可能是有利的，在这种情况下，应该从该分辨率开始构建图像金字塔，从而节省计算时间。

B. Warping

在高斯金字塔的每个新级别，两个距离图像中的一个必须相对于另一个进行变形。为了执行变形，必须整合来自先前级别的所有运动估计以获得累积到当前级别的整体变换。当然，一个特殊情况是不需要变形的第一级。在我们的公式中，新帧总是相对于旧帧变形，并且每个级别的运动估计都在同一个参考帧中表示：相机的最后一个已知姿势。

C. 深度图像梯度和权重

DIFODO 的实现需要一个离散的公式来估计两个连续深度帧之间的平均相机速度。与场景的颜色相反，其梯度对于观察场景的所有视角几乎保持恒定，深度梯度随着相机的移动而变化。出于这个原因，不应从给定时间间隔的初始或最终深度图像计算深度梯度。作为替代方案，我们证明了近似深度梯度的更好选择包括应用梯形解决方案，该解决方案平均来自两个图像的值。

D. 过滤速度和更新姿势

速度估计必须在金字塔的每一层进行过滤，因为每一层都可能缺乏几何特征。然而，最后的速度估计不能直接用于过滤每个级别的解，因为所有先前的级别已经估计了一些应该从中减去的运动，

根据我的理解说一下，这里因为上一层已经估计出了粗略的转换关系和速度，并且已经把更精确的一层的激光点做了 warp 变化，因此需要把之前估计的速度加上，才能得到该帧最终的速度。

过滤器在金字塔的每一层执行的顺序步骤如下。

计算总速度估计 $ξ^{t,acu}_i$ 累积到当前层 $i$ 。
从最后的速度估计 $ξ^{t−1}$ 中减去 $ξ^{t,acu}_i$ 得到 $ξ^{t,sub}_i$ 。
计算协方差矩阵 $Σ^t_{ξ ,i}$ 。
计算特征值 $D^t_i$ 和特征向量 $E^t_i$ 。
在基 $E^t_i$ 中表示最小二乘解 $ξ^{t,s}_i$ 和 $ξ^{t,sub}_i$ 。
以 $ξ^{t,s}_{i,E}$ 和 $ξ^{t,sub}_{i,E}$ 作为输入应用(20)，得到 $ξ^t_{i,E}$ 。
将 $ξ^t_{i,E}$ 变换为相机的坐标系

当该过程在所有金字塔层中完成时，计算最终运动估计 $ξ^t$ 并在最后一个时间间隔内积分以更新相机位姿。
尽管每个级别都遵循相同的过滤过程，但有一个重要方面需要考虑：最粗略的级别捕获运动的趋势，而最后的级别对其进行细化以获得准确的估计。因此，最粗略的水平可能会得到与相机先前速度相似的解决方案，而更精细水平的估计可能完全独立于它。因此，我们的过滤器应该为最后的运动估计赋予一个权重，该权重从粗到细的水平递减。为此，为了平滑轨迹估计，我们根据经验选择了以下加权函数
$k_1= 0.5e^{−(l−1)}, \quad k_2= 0.05e^{−(l−1)}\tag{30}$
其中 $l$ 是金字塔级别，范围从 1（最粗）到所考虑的级别数。这些函数已被启发式地证明是在估计轨迹的平滑度和适应各种场景中的运动变化的能力之间的良好折衷。