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}leftarrowhat{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 minlimits_{u'_i}frac{1}{2}parallel I_k(u'_i)-A_iI_r(u_i)parallel^2 quadforall 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 minlimits_{T_{k,w}}frac{1}{2}parallel I_k(u_i)-I_k(pi(T_{k,w},p_i))parallel^2quad 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)及不确定性( au)，然后利用贝叶斯概率模型更新(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, au^2)+(1-pi)U(x|Z_{min},Z_{max}) ag{1} ]

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

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

[argmaxlimits_{Z,pi} p(Z,pi|x_1,...,x_N) ag{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) ag{3} ]

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

[q(Z,pi|a_n,b_n,mu_n,sigma_n) riangleq Beta(pi|a_n,b_n)cdot N(Z|mu_n,sigma_n). ag{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}) ag{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, au_n^2)^{y_n}U(x_n)^{1-y_n} ag{6} ]

和

[p(y_n|pi)=pi^{y_n}(1-pi)^{1-y_n} ag{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)}sumlimits_{y_n} p(x_n,Z,pi,y_n) =sumlimits_{y_n} p(y_n|Z,pi)p(x_n|Z,pi,y_n) =sumlimits_{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) =prodlimits_{n=1}^{N}p(x_n|Z,pi,y_n)p(y_n|Z,pi)p(Z|pi)p(pi) =left[prodlimits_{n=1}^{N}p(x_n|Z,pi,y_n)p(y_n|pi) ight]p(Z)p(pi) ag{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 ag{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, au^2)+(1-pi)U(x))q(Z,pi|a,b,mu,sigma^2) ag{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, au^2)N(Z|mu,sigma^2)=N(x|mu,sigma^2+ au^2)N(Z|m,s^2) ]

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

[frac{1}{s^2}=frac{1}{sigma^2}+frac{1}{ au^2}quad m=s^2left(frac{mu}{sigma^2}+frac{x}{ au^2} ight) ]

注意，[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) ag{11} ]

其中

[C_1=frac{a}{a+b}N(x|mu,sigma^2+ au^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.