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

Posted on Edited on In

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

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

Problem

先定义一下文章中使用的符号，也是对一些基本知识的复习： IRGB:Ω×ℝ+→[0,1]3,(x,t)↦IRGB(x,t)h:Ω×ℝ+→ℝ+,(x,t)↦h(x,t) \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是二维平面，也就是相机的成像平面，tt是时刻属于正实数ℝ+\mathbb R_+，IRGBI_ {RGB}指的是RGB颜色的信息，也就是tt时刻捕获的色彩图（[0,1]3[0,1]^3指的是一个三维向量，向量的元素范围为0-1，这也是常用的表示颜色的方法，通过乘上scale就可以得到对应的RGB值），hh指的是捕获的深度图，xx很明显指的是像素坐标了，是一个二维的点。

通过针孔相机模型，可以得到表面，也就是空间点： S:Ω→ℝ3,x↦S(x)S(x)=((x+ox)⋅h(x)fx,(y+oy)⋅h(x)fy,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} 这里的ox,oyo_x, o_y对应的就是相机参数中的−cx,−cy-c_x,-c_y。

接下来介绍的是刚体运动到李群李代数，之前的文章也有详细的说明，这里简单提一下：
一个刚体运动gg（4×4矩阵4\times 4矩阵）属于李群SE(3)，也就对应到一个李代数se(3)的6维向量ξ=(ω1ω2ω3v1v2v3)⊤∈ℝ6\xi=\left(\omega_ {1} \omega_ {2} \omega_ {3} v_ {1} v_ {2} v_ {3}\right)^{\top} \in \mathbb{R}^{6}，它对应一个反对称矩阵： ξ̂=(0−ω3ω2v1ω30−ω1v2−ω2ω10v30000) \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的导数，之前我们通过对于矩阵对tt求导来引出李群李代数，从而得到微分方程： dg(t)dt=ξ̂(t)g(t) \frac{\mathrm{d} g(t)}{\mathrm{d} t}=\widehat{\xi}(t) g(t) 根据求解微分方程有，并且假设在很短的时间间隔内，ξ(t)\xi(t)不会改变，写成ξ\xi： g(t1)=exp⁡((t1−t0)ξ̂)g(t0) g\left(t_ {1}\right)=\exp \left(\left(t_ {1}-t_ {0}\right) \widehat{\xi}\right) g\left(t_ {0}\right) 下面定义符号GG，表示的是空间点的运动，根据运动矩阵与空间点坐标，有： G:SE(3)×ℝ3→ℝ3,G(g,P)=RP+T G: S E(3) \times \mathbb{R}^{3} \rightarrow \mathbb{R}^{3}, \quad G(g, P)=R P+T 通过上面的符号我们得到转换后的空间点，再定义一个重投影的过程： π(G)=(G1fxG3−ox,G2fyG3−oy)⊤ \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： wξ:Ω×ℝ+→Ω,(x,t)↦wξ(x,t)wξ(x,t)=π(G(exp((t−t0)ξ̂)g(t0),S(x))) \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(ξ)=∫Ω[I(wξ(x,t1),t1)−I(wξ(x,t0),t0)]2dx 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(wξ(x,t0),t0)=I(x,t0) I\left(w_ {\xi}\left(x, t_ {0}\right), t_ {0}\right)=I\left(x, t_ {0}\right) ### Linearization of Energy

上述的cost function不是凸函数，实际上对这些函数的形式我们根本不清楚，因此我们需要做Linearization。通过对t1t_1时刻的图像以及对应的warp进行一阶泰勒估计： I(wξ(x,t1),t1)≈I(x,t1)+(wξ(x,t1)−x)⋅∇I(x,t1)wξ(x,t1)≈x+(t1−t0)⋅d(π∘G∘g)dt⏟=dwdt|(x,t0) 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)} 由此，可以将能量函数写为： El(ξ)=∫Ω(I(x,t1)−I(x,t0)+∇I(x,t1)⋅(t1−t0)⋅dwdt(x,t0))2dx \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} 由于t1−t0t_1 - t_0只是一个放缩指数，我们可以简单得将其设为1,并且假设在这段时间内II的导数数不变，那么可以得到： El(ξ)=∫Ω(∂I∂t+∇I(x,t1)⋅dwdt(x,t0))2dx 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 接下来，根据链式求导法则，可以得到： dwdt=dπdG|π(G(g(t0)),S(x))⋅dGdg|G(g(t0)),S(x))⋅dgdt|t0 \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} } 由此可以得到： El(ξ)=∫Ω(∂I∂t+∇I⋅dπdG⋅dGdg⋅dgdt)2dx 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 这里的dgdt\frac{\mathrm{d} g}{\mathrm{d} t}正是之前的微分方程，可以得到： El(ξ)=∫Ω(∂I∂t+∇I⋅dπdG⋅dGdg⋅ξ̂⋅g(t))2dx 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 在这里，ξ̂⋅g(t)\widehat{\xi} \cdot g(t)是一个4×44\times 4矩阵，因此dGdg\frac{\mathrm{d} G}{\mathrm{d} g}是一个3×4×43\times 4 \times 4的张量，为了简化标记，作者将ξ̂⋅g(t)\widehat{\xi} \cdot g(t)stack在12维向量中，这是因为一个变换矩阵真正有效的信息就是RR与tt，自由度为12，stack操作定义为： stack⁡(ξ̂⋅g(t))=Mg⋅ξ \operatorname{stack}(\widehat{\xi} \cdot g(t))=M_ {g} \cdot \xi 通过将gg写成stacked的版本，我们可以得到最终的cost function的形式： El(ξ)=∫Ω(∂I∂t+(∇I⋅dπdG⋅dGdg⋅Mg)⏟=:C(x,t0)|(x,t0)ξ)2dx 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,t0)C(x,t_ {0})，也就是求解6×66 \times 6的正规方程。到了这里，原来的问题已经转换成一个最小二乘问题了，也就很容易求解得到了。

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

矩阵求导

这篇文章看起来很简单，但是实际上如果你要真正一步步自己推导，还是有点难度的。首先，对于上式中的∇I\nabla I，因为它是对像素点位置的泰勒展开，因此实际上是一个二元函数的展开： f(x+Δx,y+Δy)=f(x,y)+Δx∂f(x,y)∂x+Δy∂f(x,y)∂y=f(x,y)+[Δx,Δy]⋅[∂f(x,y)∂y,∂f(x,y)∂y]⊤ \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} 因此， ∇I=[Idx,Idy] \nabla I = [I_ {dx},I_ {dy}] 也就是图片的导数，可以使用sobel滤波器得到。

第二点，就是后面的导数：
dπdG\frac{d\pi}{dG}导数结果是2×32\times 3的矩阵： [π⊤1]=1G2[fx0cx0fycy001]⋅[G0G1G2] \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} 因此得到： dπdG=[fxG20−fxG0G220fyG2−fyG1G22] \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} 第三个，是dGdg\frac{dG}{dg}，由于gg是4×44 \times 4矩阵，因此这个求出来很吓人，是3×4×43\times 4 \times 4的张量。但是gg的自由度实际上是12，也就有了后面简化为12维度的说话，因为ξ̂g\hat \xi g也是只有12个有效的元素。最后ξ\xi的大小是6×16\times 1，因此，MgM_g的维度为12×612 \times 6,最后x̂i\hat xi之前矩阵维度为(1×2)⋅(2×3)⋅(3×12)⋅(12×6)=(1×6)(1 \times 2)\cdot (2 \times 3) \cdot (3\times 12) \cdot (12 \times 6) = (1 \times 6)。

这里给出最后的C(x,t0)C(x,t_ {0}):
定义c0,c1,c2c_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} $$ 则： C(x,t0)(0)=−G2c1+G1c2C(x,t0)(1)=G2c0−G0c2C(x,t0)(2)=−G1c0+G0c1C(x,t0)(3)=c0C(x,t0)(4)=c1C(x,t0)(5)=c2 \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}