Paper——Real-Time Visual Odometry from Dense RGB-D Images

这次带来的一篇文章是一个视觉里程计,针对RGBD的dense Visual Odometry:Real-Time Visual Odometry from Dense RGB-D Images,是一个非常经典的算法,现在依然被广泛使用着。

之前SLAM的文章中介绍了视觉里程计的作用,它用来估计两帧之间的位姿,来给后面的优化提供很好的初始值,这样才能使得优化结果走向最优。对于不同的相机类型有不同的方法,今天看的适用于RGBD-SLAM。

Problem

先定义一下文章中使用的符号,也是对一些基本知识的复习:

上面的式子可以大概明白作者的意思:$\Omega$是二维平面,也就是相机的成像平面,$t$是时刻属于正实数$\mathbb R_+$,$I_{RGB}$指的是RGB颜色的信息,也就是$t$时刻捕获的色彩图($[0,1]^3$指的是一个三维向量,向量的元素范围为0-1,这也是常用的表示颜色的方法,通过乘上scale就可以得到对应的RGB值),$h$指的是捕获的深度图,$x$很明显指的是像素坐标了,是一个二维的点。

通过针孔相机模型,可以得到表面,也就是空间点:

这里的$o_x, o_y$对应的就是相机参数中的$-c_x,-c_y$。

接下来介绍的是刚体运动到李群李代数,之前的文章也有详细的说明,这里简单提一下:
一个刚体运动$g$($4\times 4矩阵$)属于李群SE(3),也就对应到一个李代数se(3)的6维向量$\xi=\left(\omega_{1} \omega_{2} \omega_{3} v_{1} v_{2} v_{3}\right)^{\top} \in \mathbb{R}^{6}$,它对应一个反对称矩阵:

李群李代数的意义在于求Rotation以及Transformation的导数,之前我们通过对于矩阵对$t$求导来引出李群李代数,从而得到微分方程:

根据求解微分方程有,并且假设在很短的时间间隔内,$\xi(t)$不会改变,写成$\xi$:

下面定义符号$G$,表示的是空间点的运动,根据运动矩阵与空间点坐标,有:

通过上面的符号我们得到转换后的空间点,再定义一个重投影的过程:

也就是,我们根据相机内参相机位姿,将空间点重新投影到成像平面。也就是下式,我们成这个过程为warping:

可以看到,它是将一个二维的点映射到另外一个二维点,也就是在第二个相机位姿下相同空间点的投影坐标。通过这样,我们可以根据相机位姿的变化,来计算得到一张理论值,通过与实际拍摄的图片对比,就有了residual,也就有了需要优化的cost function。色彩图以及深度图都考虑进来,这样做有个假设,就是表面的颜色不会变化。实际中,静态重建的环境颜色变化也不会很大,我们称保持颜色一致为Photoconsistency。

Solution

Maximize Photoconsistency

下面要做的就是如何最大化Photoconsistency。现在有两个时刻得到的帧率,很直接的想法就是最小化他们重投影颜色差异:

假设第一帧的位姿为identity,那么可以简化上式:

Linearization of Energy

上述的cost function不是凸函数,实际上对这些函数的形式我们根本不清楚,因此我们需要做Linearization。通过对$t_1$时刻的图像以及对应的warp进行一阶泰勒估计:

由此,可以将能量函数写为:

由于$t_1 - t_0$只是一个放缩指数,我们可以简单得将其设为1,并且假设在这段时间内$I$的导数数不变,那么可以得到:

接下来,根据链式求导法则,可以得到:

由此可以得到:

这里的$\frac{\mathrm{d} g}{\mathrm{d} t}$正是之前的微分方程,可以得到:

在这里,$\widehat{\xi} \cdot g(t)$是一个$4\times 4$矩阵,因此$\frac{\mathrm{d} G}{\mathrm{d} g}$是一个$3\times 4 \times 4$的张量,为了简化标记,作者将$\widehat{\xi} \cdot g(t)$stack在12维向量中,这是因为一个变换矩阵真正有效的信息就是$R$与$t$,自由度为12,stack操作定义为:

通过将$g$写成stacked的版本,我们可以得到最终的cost function的形式:

对于每个pixel,都有一个6维的约束$C(x,t_{0})$,也就是求解$6 \times 6$的正规方程。到了这里,原来的问题已经转换成一个最小二乘问题了,也就很容易求解得到了。

实际上,我们最终希望得到的是关于$\xi$,很巧妙的方法是通过对时间求导而推出来的。

矩阵求导

这篇文章看起来很简单,但是实际上如果你要真正一步步自己推导,还是有点难度的。首先,对于上式中的$\nabla I$,因为它是对像素点位置的泰勒展开,因此实际上是一个二元函数的展开:

因此,

也就是图片的导数,可以使用sobel滤波器得到。

第二点,就是后面的导数:
$\frac{d\pi}{dG}$导数结果是$2\times 3$的矩阵:

因此得到:

第三个,是$\frac{dG}{dg}$,由于$g$是$4 \times 4$矩阵,因此这个求出来很吓人,是$3\times 4 \times 4$的张量。但是$g$的自由度实际上是12,也就有了后面简化为12维度的说话,因为$\hat \xi g$也是只有12个有效的元素。最后$\xi$的大小是$6\times 1$,因此,$M_g$的维度为$12 \times 6$,最后$\hat xi$之前矩阵维度为$(1 \times 2)\cdot (2 \times 3) \cdot (3\times 12) \cdot (12 \times 6) = (1 \times 6)$。

这里给出最后的$C(x,t_{0})$:
定义$c_0,c_1,c_2$

则: