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$ 上的对应点ui，优化得到 $p$ 在当前帧上的投影u′i。优化的目标函数是仿射变换下的灰度差。

u′i=argminu′i12∥Ik(u′i)−AiIr(ui)∥2∀i$$$$

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

BA优化

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

Tk,w=argminTk,w12∥Ik(ui)−Ik(π(Tk,w,pi))∥2∀i

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

地图构建

深度估计

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

后续帧{Ik,Tk,w}对它能观测到的seed的深度估计产生贡献。具体而言，对 $r$ 上深度还没有确定的点{p,u}，根据Tk,r找到 $p$ 对应的极线LP，在极线上寻找和 $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,π)=πN(x|Z,τ2)+(1−π)U(x|Zmin,Zmax)(1)

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

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

argmaxZ,πp(Z,π|x1,...,xN)(2)

上式右边同比与（相对于变量Z,π，xi的分布和Z,π无关）

p(Z,π|x1,...,xN)∝p(Z,π)∏np(xn|Z,π)(3)

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

q(Z,π|an,bn,μn,σn)≜Beta(π|an,bn)⋅N(Z|μn,σn).(4)

并给出一个迭代格式

q(Z,π|an,bn,μn,σn)≈p(xn|Z,π)q(Z,π|an−1,bn−1,μn−1,σn−1)(5)

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

近似分布的推导

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

p(Z,π)=p(Z)p(π)

第一步：引入潜变量（latent variable）yn。 yn=1表示 $x_n$ 是内点，yn=0表示xn是外点。那么有

p(xn|Z,π,yn)=N(xn|Z,τ2n)ynU(xn)1−yn(6)

和

p(yn|π)=πyn(1−π)1−yn(7)

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

p(xn|Z,π)=1p(Z,π)∑ynp(xn,Z,π,yn)=∑ynp(yn|Z,π)p(xn|Z,π,yn)=∑ynp(yn|π)p(xn|Z,π,yn)$$$$

第二步：估计后验概率。令X={x1,...,xn},Y={y1,...yn}，X,Y,Z,π的联合概率密度为

p(X,Y,Z,π)=∏n=1Np(xn|Z,π,yn)p(yn|Z,π)p(Z|π)p(π)=[∏n=1Np(xn|Z,π,yn)p(yn|π)]p(Z)p(π)(8)$$\begin{array}{}\end{array}$$

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

q(Y,Z,π)=q1(Y)q2(Z,π).$$$$

由变分推断理论，求解p(Y,Z,π|X)的最佳近似分布等价于最小化 $p$ 和 $q$ 的Kullback-Leibler散度，由此推出q1,q2需要满足

lnq2=EY[lnp(X,Y,Z,π)]+const(9)

和

lnq1=EZ,π[lnp(X,Y,Z,π)]+const$$$$

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

第三步：求解近似表达。这里我们只关心 $Z,\pi$ 的估计，将(6),(7),(8)及代入(9)，可以证明q2满足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$ 为整数时，Γ(a)=(a−1)!。根据(5)式，我们想找到q(Z,π|a′,b′,μ′,σ′2)（记为q(′)），使得q(′)的一阶矩和二阶矩与

p(x|Z,π)q(Z,π|a,b,μ,σ2)$$$$

相等。将 $p$ 的表达式代入上式，有

(πN(x|Z,τ2)+(1−π)U(x))q(Z,π|a,b,μ,σ2)(10)$$\begin{array}{}\end{array}$$

注意到

Γ(a,b)=1πaa+bΓ(a+1,b)=11−πba+bΓ(a,b+1)$$\mathrm{}$$

N(x|Z,τ2)N(Z|μ,σ2)=N(x|μ,σ2+τ2)N(Z|m,s2)$$$$

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

1s2=1σ2+1τ2m=s2(μσ2+xτ2)$$\frac{\mathrm{}}{}$$

注意，[4]补充文档里的s2的公式是错误的，应当是上面这个式子。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}$$

其中

C1=aa+bN(x|μ,σ2+τ2)C2=ba+bU(x)

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

以[4]作者推导为例，那么从(23)式得到μ′，从(24)式得到σ′。令

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

那么

a′=e−ff−e/fb′=1−ffa′$$$$

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

一些问题

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