SVO原理解析

最近空闲时间在研究Semi-Direct Monocular Visual Odometry(SVO)[1,2],觉得它值得写一写。另外,SVO的运算量相对较小,我想在手机上尝试实现它。

关于SVO的介绍,有两篇博客介绍得非常好,因此我这里只简单提一下大概的思路,重点讲解了一下深度滤波器的原理。

svo: semi-direct visual odometry 论文解析

SVO 代码笔记

一步步完善视觉里程计1——项目框架搭建

姿态估计

估计初始姿态

利用相邻两帧之间的特征点对,计算相对位姿。

  • 计算第\(k\)帧和第\(k-1\)帧中的特征点对的patch的灰度差。特征点对指的是第\(k-1\)帧时深度已知的地图点(3D)在两帧中的投影点(2D)。特征点patch是特征点周围4×4的区域。

  • 利用Gauss-Newton迭代法求解\(\hat{T}_{k,k-1}\)

    • \(k-1\)帧加一个小扰动\(\delta\),通过灰度差优化\(\delta\)。这步叫Inverse compositional formulation。

    • \(\hat{T}_{k,k-1}\leftarrow\hat{T}_{k,k-1}\cdot T(\delta)^{-1}\)

  • 这一步忽略patch的变形,不做warping。因为相邻帧之间的形变很小。

Inverse compositional formulation保证Jacobian在迭代中保持不变,因此可以预先计算,降低计算量。

关于文章中导数的求解,请参考高博的直接法,非常详细。参考文献见[3]。

优化匹配关系

利用初始位姿,寻找更多的地图点(3D)到当前帧投影点(2D)的对应关系。

对每个当前帧能观察到的地图点\(p\)(已经收敛的深度估计),找到观察\(p\)角度最小的关键帧\(r\)上的对应点\(u_i\),优化得到\(p\)在当前帧上的投影\(u'_i\)。优化的目标函数是仿射变换下的灰度差。

\[u'_i=\arg \min\limits_{u'_i}\frac{1}{2}\parallel I_k(u'_i)-A_iI_r(u_i)\parallel^2 \quad\forall i \]

这一步中的patch采用的是8×8邻域,\(A_i\)表示一个仿射变换。这步不考虑极线约束,因为此时的位姿还是不准确的。第二步和第三步需要一定量的地图点,不能在一开始就使用,猜测这是作者强调深度估计收敛快的原因之一。

BA优化

利用第二步建立起的\((p_i,u_i)\)的对应关系,优化世界坐标系下的位姿\(T_{k,w}\) ,标准motion only bundle adjustment。这里\(p_i\)是世界坐标系下的3D坐标。

\[T_{k,w}=\arg \min\limits_{T_{k,w}}\frac{1}{2}\parallel I_k(u_i)-I_k(\pi(T_{k,w},p_i))\parallel^2\quad \forall i \]

根据文章图11,BA优化前投影误差均值为0.3 pixel左右,优化后均值降低了一点点,误差曲线更稳定了。也许是这个原因,作者在程序中提供了一个选项忽略这步。

地图构建

深度估计

当出现新的关键帧\(r\)时,作者在\(r\)上选取若干特征点(即seed),每个特征点对应一个深度估计,其初值为该帧的平均深度,并被赋予极大的不确定性。

后续帧\(\{I_k,T_{k,w}\}\)对它能观测到的seed的深度估计产生贡献。具体而言,对\(r\)上深度还没有确定的点\(\{p,u\}\),根据\(T_{k,r}\)找到\(p\)对应的极线\(L_P\),在极线上寻找和\(u\)最相似的点\(u’\),通过三角测量计算得到深度\(x\)及不确定性\(\tau\),然后利用贝叶斯概率模型更新\(p\)点的估计。当\(p\)的深度估计收敛时,计算其三维坐标,并加入地图。

深度估计的思路

以下内容来源于参考文献[4]

G. Vogiatzis and C. Hern´ andez, “Video-based, Real-Time Multi View Stereo,” Image and Vision Computing, vol. 29, no. 7, 2011.

给定已知相对位姿的两个视角下的图像\(I,I'\)。由两幅图像中的对应点及位姿可以计算得到一个深度值\(x\)。由于重建误差和误匹配的存在,考察实际情况中\(x\)的直方图分布,[4]作者认为,\(x\)的分布可以用高斯分布和均匀分布来联合表示

\[p(x|Z,\pi)=\pi N(x|Z,\tau^2)+(1-\pi)U(x|Z_{\min},Z_{\max}) \tag{1} \]

其中\(\pi\)表示\(x\)为有效测量的概率。下图是一个若干测量的直方图例子。\(x\)轴表示深度测量范围\([-5,5]\)\(y\)轴表示直方图统计。

考虑同一个seed的一系列测量\(x_1,x_2,...,x_n\),假设这些测量是独立的。我们想从\((1)\)式求出\(Z,\pi\)。最直观的做法是通过最大似然估计求解。然而[4]作者认为最大似然估计容易被局部极大值干扰,其结果并不准确。[4]作者选择从最大后验概率求解,等价于

\[\arg\max\limits_{Z,\pi} p(Z,\pi|x_1,...,x_N) \tag{2} \]

上式右边同比与(相对于变量\(Z,\pi\)\(x_i\)的分布和\(Z,\pi\)无关)

\[p(Z,\pi|x_1,...,x_N) \propto p(Z,\pi)\prod \limits_{n} p(x_n|Z,\pi) \tag{3} \]

作者证明,\((3)\)式可以用Gaussian×Beta分布来近似

\[q(Z,\pi|a_n,b_n,\mu_n,\sigma_n)\triangleq Beta(\pi|a_n,b_n)\cdot N(Z|\mu_n,\sigma_n). \tag{4} \]

并给出一个迭代格式

\[q(Z,\pi|a_n,b_n,\mu_n,\sigma_n)\approx p(x_n|Z,\pi)q(Z,\pi|a_{n-1},b_{n-1},\mu_{n-1},\sigma_{n-1}) \tag{5} \]

这里,约等于是因为\((5)\)右端并不是Gaussian×Beta的分布,而是用\(q(Z,\pi|a_n,b_n,\mu_n,\sigma_n)\)去近似右端项。[4]作者实际上利用一阶矩和二阶矩相等来更新参数。根据\((5)\)式,在加入新的测量时,seed的近似后验概率分布也会得到更新。当\(\sigma_n\)小于给定阈值时,认为seed的深度估计已经收敛,计算其三维坐标,并加入地图。

近似分布的推导

[4]作者提供了文档给出了上面推导过程的证明。文档的名字为“Supplementary matterial Parametric approximation to posterior”。这里首先假设\(p(Z,\pi)\)满足独立性公式

\[p(Z,\pi)=p(Z)p(\pi) \]

第一步:引入潜变量(latent variable)\(y_n\)\(y_n=1\)表示\(x_n\)是内点,\(y_n=0\)表示\(x_n\)是外点。那么有

\[p(x_n|Z,\pi,y_n)=N(x_n|Z,\tau_n^2)^{y_n}U(x_n)^{1-y_n} \tag{6} \]

\[p(y_n|\pi)=\pi^{y_n}(1-\pi)^{1-y_n} \tag{7} \]

\((6)\)可以通过直观验证,\(y_n\)为1表明\(x_n\)是内点,满足Gaussian分布,反之满足均匀分布。\((7)\)也类似,当内点概率为\(\pi\)时,\(y_n=1\)的概率为\(\pi\),反之为\((1-\pi)\)。容易证明

\[p(x_n|Z,\pi) =\frac{1}{p(Z,\pi)}\sum\limits_{y_n} p(x_n,Z,\pi,y_n) =\sum\limits_{y_n} p(y_n|Z,\pi)p(x_n|Z,\pi,y_n) =\sum\limits_{y_n} p(y_n|\pi)p(x_n|Z,\pi,y_n) \]

第二步:估计后验概率。令\(\mathscr{X}=\{x_1,...,x_n\},\mathscr{Y}=\{y_1,...y_n\}\)\(\mathscr{X},\mathscr{Y},Z,\pi\)的联合概率密度为

\[p(\mathscr{X},\mathscr{Y},Z,\pi) =\prod\limits_{n=1}^{N}p(x_n|Z,\pi,y_n)p(y_n|Z,\pi)p(Z|\pi)p(\pi) =\left[\prod\limits_{n=1}^{N}p(x_n|Z,\pi,y_n)p(y_n|\pi)\right]p(Z)p(\pi) \tag{8} \]

由于并不知道\(p(\mathscr{Y},Z,\pi|\mathscr{X})\)长什么样,我们想办法去近似它。令\(q(\mathscr{Y},Z,\pi)\)\(p(\mathscr{Y},Z,\pi|\mathscr{X})\)的一个近似推断,且满足以下的因子分解

\[q(\mathscr{Y},Z,\pi)=q_{1}(\mathscr{Y})q_{2}(Z,\pi). \]

由变分推断理论,求解\(p(\mathscr{Y},Z,\pi|\mathscr{X})\)的最佳近似分布等价于最小化\(p\)\(q\)的Kullback-Leibler散度,由此推出\(q_1,q_2\)需要满足

\[\ln q_{2}=E_{\mathscr{Y}}[\ln p(\mathscr{X},\mathscr{Y},Z,\pi)]+const \tag{9} \]

\[\ln q_{1}=E_{Z,\pi}[\ln p(\mathscr{X},\mathscr{Y},Z,\pi)]+const \]

其中\(E_{\mathscr{Y}}\)表示对变量\(\mathscr{Y}\)求期望,\(E_{Z,\pi}\)表示对变量\(Z,\pi\)求期望。以上结论来自于参考文献[5]中的10.1.1章节(变分推断之分解分布)。Git上有人将这本书全文翻译了(膜拜一下),链接点此,也可以参考WIKI Variational_Bayesian_methods 中的Further discussion。

第三步:求解近似表达。这里我们只关心\(Z,\pi\)的估计,将\((6),(7),(8)\)及代入\((9)\),可以证明\(q_2\)满足Gaussian×Beta分布。

这里有一个遗留问题:为什么要引入潜变量?

近似分布的迭代求解

因为\((5)\)右边并不满足Gaussian×Beta分布,[4]作者退而求其次,尝试用另一个Gaussian×Beta分布来近似右端项。令\((5)\)式两端相对于\(Z\)\(\pi\)的一阶矩和二阶矩相等,建立起参数方程,联立求解得到新的参数。

定义Guassian×Beta分布为

\[q(Z,\pi|a,b,\mu,\sigma^2)=N(Z|\mu,\sigma^2)Beta(\pi|a,b) \]

其中\(N\)是Gaussian分布,以及

\[Beta(\pi|a,b)=\frac{\Gamma(a+b)}{\Gamma(a)\Gamma(b)}\pi^{a-1}(1-\pi)^{b-1} \]

\(a\)为整数时,\(\Gamma(a)=(a-1)!\)。根据\((5)\)式,我们想找到\(q(Z,\pi|a',b',\mu',\sigma'^2)\)(记为\(q(')\)),使得\(q(')\)的一阶矩和二阶矩与

\[p(x|Z,\pi)q(Z,\pi|a,b,\mu,\sigma^2) \]

相等。将\(p\)的表达式代入上式,有

\[(\pi N(x|Z,\tau^2)+(1-\pi)U(x))q(Z,\pi|a,b,\mu,\sigma^2) \tag{10} \]

注意到

\[\Gamma(a,b)=\frac{1}{\pi}\frac{a}{a+b}\Gamma(a+1,b)=\frac{1}{1-\pi}\frac{b}{a+b}\Gamma(a,b+1) \]

\[N(x|Z,\tau^2)N(Z|\mu,\sigma^2)=N(x|\mu,\sigma^2+\tau^2)N(Z|m,s^2) \]

第一式用\(\Gamma\)定义即可,第二式分离Gaussian分布乘积中的\(Z\)项和其它项即可。其中

\[\frac{1}{s^2}=\frac{1}{\sigma^2}+\frac{1}{\tau^2}\quad m=s^2\left(\frac{\mu}{\sigma^2}+\frac{x}{\tau^2}\right) \]

注意,[4]补充文档里的\(s^2\)的公式是错误的,应当是上面这个式子。SVO程序里的公式是对的(SVO作者发现了哈)。将以上式子代入\((10)\),就可以得到[4]补充文档(18)式,即

\[C_1N(Z|m,s^2)Beta(\pi|a+1,b)+C_2N(Z|\mu,\sigma^2)Beta(\pi|a,b+1) \tag{11} \]

其中

\[C_1=\frac{a}{a+b}N(x|\mu,\sigma^2+\tau^2)\quad C_2=\frac{b}{a+b}U(x) \]

\(C_1,C_2\)是与\(Z,\pi\)无关的系数。分别计算\(q(')\)\((11)\)关于\(Z\)\(\pi\)的一阶矩和二阶矩,通过一阶矩相等和二阶矩相等得到\(a',b',\mu',\sigma'\)的四个方程。由于\(q\)\(z\)\(\pi\)的联合分布,要先求关于 \(z\) 的边缘分布,再求其期望,结果是\(\mu\)(感谢丁弘毅指正_)。
下面是[4]作者给出的公式

以[4]作者推导为例,那么从(23)式得到\(\mu'\),从(24)式得到\(\sigma'\)。令

\[f=\frac{a'}{a'+b'}\quad e=\frac{a'(a'+1)}{(a'+b')(a'+b'+1)} \]

那么

\[a'=\frac{e-f}{f-e/f}\quad b'=\frac{1-f}{f}a' \]

这样就可以在加入新的测量时更新参数。

一些问题

  • SVO在large scale下能否保持精度?
  • SVO中的位姿估计可以容忍多大的旋转平移?
  • 没有重定位,怎么找回?
  • MAV场景的特殊性在哪里?
参考资料

Git source code https://github.com/uzh-rpg/rpg_svo

参考文献

[1] Forster, Christian, Matia Pizzoli, and Davide Scaramuzza. "SVO: Fast semi-direct monocular visual odometry." 2014 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2014.
[2] M. Pizzoli, C. Forster, and D. Scaramuzza, “REMODE: Probabilistic, Monocular Dense Reconstruction in Real Time,” in Proc. IEEE Int. Conf. on Robotics and Automation, 2014.
[3] S. Baker and I. Matthews, “Lucas-Kanade 20 Years On: A Unifying Framework: Part 1,” International Journal of Computer Vision, vol. 56, no. 3, pp. 221–255, 2002.
[4] G. Vogiatzis and C. Hern´ andez, “Video-based, Real-Time Multi View Stereo,” Image and Vision Computing, vol. 29, no. 7, 2011.
[5] Christopher M. Bishop. Pattern Recognition and Machine Learning (Information Science and Statistics). Springer, 1 edition, October 2007.

posted @ 2016-08-15 17:35  路游侠  阅读(25700)  评论(1编辑  收藏  举报