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$的一阶矩（实际上就是期望），作者认为等于$\mu'$，实际上是不是应当等于$\mu'Beta(\pi|a',b')$？这一点我并没有完全想清楚。
下面是[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.