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

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

姿态估计

估计初始姿态

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

• 计算第\(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 中的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

参考文献

[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.