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.