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

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

\begin{aligned} &I_{RGB} : \Omega \times \mathbb R_+ \rightarrow [0,1]^3 , (x,t) \mapsto I _{RGB}(x,t)\\ &h: \Omega \times \mathbb R_+ \rightarrow \mathbb R_+, (x,t) \mapsto h(x,t)\\ \end{aligned}

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

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

\begin{array}{l}{S: \Omega \rightarrow \mathbb{R}^{3}, \quad x \mapsto S(x)} \\ {S(x)=\left(\frac{\left(x+o_{x}\right) \cdot h(x)}{f_{x} } \quad, \quad \frac{\left(y+o_{y}\right) \cdot h(x)}{f_{y} } \quad, \quad h(x)\right)^{\top} }\end{array}

这里的$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}$，它对应一个反对称矩阵：

\widehat{\xi}=\left(\begin{array}{cccc}{0} & {-\omega_{3} } & {\omega_{2} } & {v_{1} } \\ {\omega_{3} } & {0} & {-\omega_{1} } & {v_{2} } \\ {-\omega_{2} } & {\omega_{1} } & {0} & {v_{3} } \\ {0} & {0} & {0} & {0}\end{array}\right)

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

\frac{\mathrm{d} g(t)}{\mathrm{d} t}=\widehat{\xi}(t) g(t)

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

g\left(t_{1}\right)=\exp \left(\left(t_{1}-t_{0}\right) \widehat{\xi}\right) g\left(t_{0}\right)

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

G: S E(3) \times \mathbb{R}^{3} \rightarrow \mathbb{R}^{3}, \quad G(g, P)=R P+T

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

\pi(G)=\left(\frac{G_{1} f_{x} }{G_{3} }-o_{x} \quad, \quad \frac{G_{2} f_{y} }{G_{3} }-o_{y}\right)^{\top}

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

\begin{aligned} w_{\xi} &: \Omega \times \mathbb{R}_{+} \rightarrow \Omega, \quad(x, t) \mapsto w_{\xi}(x, t) \\ w_{\xi}(x, t) &=\pi\left(G\left(\exp \left(\left(t-t_{0}\right) \widehat{\xi}\right) g\left(t_{0}\right), S(x)\right)\right) \end{aligned}

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

Solution

Maximize Photoconsistency

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

E(\xi)=\int_{\Omega}\left[I\left(w_{\xi}\left(x, t_{1}\right), t_{1}\right)-I\left(w_{\xi}\left(x, t_{0}\right), t_{0}\right)\right]^{2} \mathrm{d} x

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

I\left(w_{\xi}\left(x, t_{0}\right), t_{0}\right)=I\left(x, t_{0}\right)

Linearization of Energy

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

I\left(w_{\xi}\left(x, t_{1}\right), t_{1}\right) \approx I\left(x, t_{1}\right)+\left(w_{\xi}\left(x, t_{1}\right)-x\right) \cdot \nabla I\left(x, t_{1}\right) w_{\xi}\left(x, t_{1}\right) \approx x+\left.\left(t_{1}-t_{0}\right) \cdot \underbrace{\frac{\mathrm{d}(\pi \circ G \circ g)}{\mathrm{d} t} }_{=\frac{d w}{d t} }\right|_{\left(x, t_{0}\right)}

由此，可以将能量函数写为：

\begin{aligned} E_{l}(\xi)=\int_{\Omega}\left(I\left(x, t_{1}\right)-I\left(x, t_{0}\right)\right.&+\left.\nabla I\left(x, t_{1}\right) \cdot\left(t_{1}-t_{0}\right) \cdot \frac{\mathrm{d} w}{\mathrm{d} t}\left(x, t_{0}\right)\right)^{2} \mathrm{d} x \end{aligned}

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

E_{l}(\xi)=\int_{\Omega}\left(\frac{\partial I}{\partial t}+\nabla I\left(x, t_{1}\right) \cdot \frac{\mathrm{d} w}{\mathrm{d} t}\left(x, t_{0}\right)\right)^{2} \mathrm{d} x

接下来，根据链式求导法则，可以得到：

\frac{\mathrm{d} w}{\mathrm{d} t}=\left.\left.\left.\frac{\mathrm{d} \pi}{\mathrm{d} G}\right|_{\pi\left(G\left(g\left(t_{0}\right)\right), S(x)\right)} \cdot \frac{\mathrm{d} G}{\mathrm{d} g}\right|_{\left.G\left(g\left(t_{0}\right)\right), S(x)\right)} \cdot \frac{\mathrm{d} g}{\mathrm{d} t}\right|_{t_{0} }

由此可以得到：

E_{l}(\xi)=\int_{\Omega}\left(\frac{\partial I}{\partial t}+\nabla I \cdot \frac{\mathrm{d} \pi}{\mathrm{d} G} \cdot \frac{\mathrm{d} G}{\mathrm{d} g} \cdot \frac{\mathrm{d} g}{\mathrm{d} t}\right)^{2} \mathrm{d} x

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

E_{l}(\xi)=\int_{\Omega}\left(\frac{\partial I}{\partial t}+\nabla I \cdot \frac{\mathrm{d} \pi}{\mathrm{d} G} \cdot \frac{\mathrm{d} G}{\mathrm{d} g} \cdot \widehat{\xi} \cdot g(t)\right)^{2} \mathrm{d} x

在这里，$\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操作定义为：

\operatorname{stack}(\widehat{\xi} \cdot g(t))=M_{g} \cdot \xi

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

E_{l}(\xi)=\int_{\Omega}\left(\frac{\partial I}{\partial t}+\left.\underbrace{\left(\nabla I \cdot \frac{\mathrm{d} \pi}{\mathrm{d} G} \cdot \frac{\mathrm{d} G}{\mathrm{d} g} \cdot M_{g}\right)}_{=: C\left(x, t_{0}\right)}\right|_{\left(x, t_{0}\right)} \xi\right)^{2} \mathrm{d} x

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

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

矩阵求导

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

\begin{aligned} f(x+\Delta x,y + \Delta y) &= f(x,y) + \Delta x \frac{\partial f(x,y)}{\partial x} + \Delta y \frac{\partial f(x,y)}{\partial y}\\ & = f(x,y) + [\Delta x, \Delta y] \cdot \left[\frac{\partial f(x,y)}{\partial y},\frac{\partial f(x,y)}{\partial y}\right]^{\top} \end{aligned}

因此，

\nabla I = [I_{dx},I_{dy}]

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

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

\begin{bmatrix} \pi^\top \\ 1 \end{bmatrix} = \frac{1}{G_2} \begin{bmatrix} f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1 \end{bmatrix} \cdot \begin{bmatrix} G_0\\ G_1\\ G_2\\ \end{bmatrix}

因此得到：

\frac{d\pi}{dG} = \begin{bmatrix} \frac{f_x}{G_2}&0&-\frac{f_x G_0}{G^2_2}\\ 0&\frac{f_y}{G_2}&-\frac{f_y G_1}{G^2_2} \end{bmatrix}

第三个，是$\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$

c_0 = \frac{I_{dx} f_x}{G_2}\\ c_1 = \frac{I_{dy} f_y}{G_2}\\ c_2 = -(c_0 G_0 + c_1 G_1 )\frac{1}{G_2}

则：

\begin{aligned} &C(x,t_{0})(0) = -G_2c_1 + G_1c_2\\ &C(x,t_{0})(1) = G_2c_0 - G_0c_2\\ &C(x,t_{0})(2) = -G_1 c_0 + G_0 c_1\\ &C(x,t_{0})(3) = c_0\\ &C(x,t_{0})(4) = c_1\\ &C(x,t_{0})(5) = c_2\\ \end{aligned}