能否具體解釋下svo的運動估計與深度估計兩方面？

-------------------------------------------------------------
提綱

1. svo簡介
2. Tracking部分
3. Mapping部分
4. 評述

-------------------------------------------------------------
1. SVO 簡介
SVO 全稱 Semi-direct monocular Visual Odometry（半直接視覺里程計），是蘇黎世大學機器人感知組的克里斯蒂安.弗斯特（Christian Forster，主頁：Christian Forster）等人，於2014年ICRA會議上發表的工作，隨後在github開源：uzh-rpg/rpg_svo 。2016年擴展了多相機和IMU之後，寫成期刊論文，稱為SVO 2.0，預定將在IEEE Trans. on Robotics上發表（視頻見[2]）。SVO 2.0目前未開源（個人認為以後也不會開）。

（克里斯蒂安.弗斯特，圖片來自bing）

SVO主要工作由弗斯特完成，此外他也在喬治亞理工的gtsam組呆過一段時間，參與了gtsam中IMU部分，文章發表在RSS 2015上，亦是VIO當中的著名工作[3]。此文章後續亦有期刊版本，預計也在TRO上發表。不過會議論文中公式推導有誤，而且弗斯特本人似乎只參與了實現部分，沒怎麼管公式推導……當然這些都是八卦，不談了。與SVO相關工作是同一組的REMODE[4]，實現了在SVO定位基礎上的單目稠密建圖（需要GPU），由SVO的二作馬蒂亞（Matia Pizzoli）完成。

SVO，雖然按照作者的理解，稱為「半直接法」，然而按照我個人的理解，稱為「稀疏直接法」可能更好一些。眾所周知，VO主要分為特徵點法和直接法。而SVO的實現中，混合使用了特徵點與直接法：它跟蹤了一些關鍵點（角點，沒有描述子，由FAST實現），然後像直接法那樣，根據這些關鍵點周圍的信息，估計相機運動以及它們的位置。這與特徵點法（以ORB-SLAM為代表）那樣，需要對每張圖像提取特徵點和描述子的實現，是有明顯不同的。所以，作者稱之為「半直接法」。

不過，由於SVO跟蹤的「關鍵點」，亦可以理解成「梯度明顯的像素」。從這個角度來看，它和LSD-SLAM[5]更加相近。只是LSD-SLAM跟蹤了所有梯度明顯的像素，形成半稠密地圖；而SVO只跟蹤稀疏的關鍵點，所以不妨稱之為「稀疏直接法」。這一點，和今年慕尼黑理工丹尼爾.克萊默（Daniel Cremers）組的雅各布.恩格爾（Jakob Engel）提出的DSO[6]是很相似的。（PS：ORB,LSD,SVO幾個作者似乎都去了同一個公司啊……那我還搞啥……）

在視覺里程計中，直接法最突出的優點是非常快（ORB作者勞爾曾認為特徵點比較快，我覺得是不對的）。而在直接法中（包括稀疏的，半稠密的以及稠密的），使用稀疏的直接法，既不必費力去計算描述子，也不必處理像稠密和半稠密那麼多的信息，能夠達到極快的速度。因此，SVO即使在低端計算平台上也能達到實時性，而在PC平台上則可以達到100多幀每秒的速度。在作者後續工作SVO 2.0中，速度更達到了驚人的400幀每秒。這使得SVO非常適用於計算平台受限的場合，例如無人機、手持AR/VR設備的定位。無人機也是弗斯特等人最初開發SVO的目標應用平台。

SVO另一特點是實現了一種特有的深度濾波器（Depth Filter）。這裡一種基於均勻——高斯混合分布的深度濾波器，由弗吉亞茲於2011年提取並推導[7]。由於原理較為複雜，之後再詳細解釋。SVO將這種濾波器用於關鍵點的深度估計，並使用了逆深度作為參數化形式，使之能夠更好地計算特徵點位置。這裡SVO在建圖線程中的主要任務。

開源版的SVO代碼清晰易讀，十分適合讀者作為第一個SLAM實例進行分析。初學者可以從SVO或ORB開始讀起，弗斯特寫代碼一直比較清楚。

SVO的整體框架如下圖所示：

整個過程分為兩個大模塊：追蹤與建圖（與PTAM類似）。

• 上半部分為追蹤部分。主要任務是估計當前幀的位姿。又分為兩步：
• 先把當前幀和上一個追蹤的幀進行比對，獲取粗略的位姿。
• 然後根據粗略的位姿，將它與地圖進行比對，得到精確的位姿並優化見到的地圖點。隨後判斷此幀是否為關鍵幀。如果為關鍵幀就提取新的特徵點，把這些點作為地圖的種子點，放入優化線程。否則，不為關鍵幀的時候，就用此幀的信息更新地圖中種子點的深度估計值。
• 下半部分為建圖部分。主要任務是估計特徵點的深度。因為單目SLAM中，剛提的特徵點是沒有深度的，所以必須用新來的幀的信息，去更新這些特徵點的深度分布，也就是所謂的「深度濾波器」。當某個點的深度收斂時，用它生成新的地圖點，放進地圖中，再被追蹤部分使用。

整個SVO架構要比ORB簡單一些（ORB有三個線程，且要處理關鍵幀的共視關係和迴環檢測），所以效率也要高一些。下面詳細談這兩個模塊的做法。

-------------------------------------------------------------
2. 追蹤（Tracking）部分
追蹤部分理解難點主要有兩個：

• 如何計算幀與幀之間位姿變換？
• 如何計算幀與地圖之間的位姿變換？

下面分別來說這兩點。

2.1 Frame-to-Frame的位姿變換
追蹤的第一步是將當前幀與上一個追蹤成功的幀進行對比，粗略估計當前幀的位姿。該問題的基本形式為：已知上一幀對地圖點的觀測（包括2D投影位置和深度），以及當前幀的圖像，如何計算當前幀的位姿？用數學語言說，已經幀的位姿，並且知道它的觀測量時，求解。

在SVO里，該問題被稱為 Model-based Image Alignment （帶有相機模型的圖像配准），實際上就是我們平時說的稀疏直接法。直接法的原理在我的博客中給出過比較細的推導：直接法 - 半閑居士 - 博客園 ，此外我也講過一次講座：直接法的原理與實現_高翔_bilibili_演講?公開課_科技_bilibili_嗶哩嗶哩彈幕視頻網 。本質上，它通過最小化前一幀和當前幀之間的光度誤差來求得當前幀的（粗略）位姿：

要理解它，你需要非線性優化的基本知識。同時，為了求目標函數相對於位姿的導數，你需要學習一些李代數的知識。這在我的博客和講座中均有比較詳細的探討：

• 視覺slam第4章_演講?公開課_科技_bilibili_嗶哩嗶哩彈幕視頻網
• 非線性優化與g2o錄像：http://pan.baidu.com/s/1c2qPdle

由於不想在知乎打公式，所以請讀者去看視頻和講座以了解其中原理（因為都已經講過一遍了）。實現當中，SVO自己實現了高斯——牛頓法的迭代下降，並且比較取巧地使用了一種反向的求導方式：即雅可比在k-1幀圖像上進行估計，而不在k幀上估計。這樣做法的好處是在迭代過程中只需計算一次雅可比，其餘部分只需更新殘差即可（即G-N等式右側的）。這能夠節省一定程度的計算量。另一個好處是，我們能夠保證k-1幀的像素具有梯度，但沒法保證在k幀上的投影也具有梯度，所以這樣做至少能保證像素點的梯度是明顯的。

實現當中另一個需要注意的地方是金字塔的處理。這一步估計是從金字塔的頂層開始，把上一層的結果作為下一層估計的初始值，最後迭代到底層的。頂層的解析度最小，所以這是一個由粗到精的過程（Coarse-to-Fine），使得在運動較大時也能有較好的結果。

值得一提的是，完全可以使用優化庫，例如g2o或ceres來實現所有的步驟。在優化庫中，可以選用更多的優化方式，而且L-M或Dogleg的結果也會比G-N更有保證。我自己就用兩個庫各實現過一遍。

2.2 Frame-to-Map
在2.1求解之後，我們得到了當前幀位姿的粗略估計。因為它是基於上一幀的結果來計算的，所以如果把它當作真實位姿估計的話，將有較大的累積誤差。因此，需要進一步和地圖之間進行特徵點比對，來對當前幀位姿進一步優化。主要步驟如下：

• 遍歷地圖中的所有點，計算在當前幀的投影位置。由於當前幀有粗略的位姿估計，這個投影位置應該與真實位置有少量誤差（2~3個像素）。
• 對每個成功投影的地圖點，比較這些點的初始觀測圖像與當前幀的圖像。通過計算光度的誤差，求取更精準的投影位置。這步類似於光流，在SVO中稱為Refinement。
• 根據更精確的投影位置，進行位姿與地圖點的優化。這一步類似於Bundle Adjustment，但SVO實現中，是把Pose和Point兩個問題拆開優化的，速度更快。
• 判斷是否生成關鍵幀，處理關鍵幀的生成。

這裡理解的難點是，地圖點初次被觀測到的圖像與當前幀的圖像進行比對時，不能直接對兩個圖像塊求差，而需要計算一個仿射變換（Affine Warp）。這是因為初次觀測和當前幀的位移較遠，並且可能存在旋轉，所以不能單純地假設圖像塊不變。仿射變換的計算方式在PTAM論文的5.3節有介紹，似乎是一種比較標準的處理方式。（其實SVO的追蹤部分和PTAM整個兒都挺像。）

實現當中可能還需要注意一些細節。例如有些地方使用了網格，以保證觀測點的均勻分布。還有Affine Warp當中需要注意特徵點所在的金字塔層數，等等。

此後的Bundle Adjustment部分和傳統的區別不大，除了把pose和point分開計算之外。關鍵幀判斷方面，SVO是比較薄弱的（考慮的東西太少），和ORB相比差了不少。

-------------------------------------------------------------
3 Mapping部分
Mapping部分主要是計算特徵點的深度。如前所言，單目VO中，剛剛從圖像中提取的熱乎的關鍵點是沒有深度的，需要等相機位移之後再通過三角化，再估計這些點的深度。這些尚未具備有效深度信息的點，不妨稱之為種子點（或候選點）。然而，三角化的成功與否（以及精度），取決於相機之間的平移量和視線的夾角，所以我們通常要維護種子點的深度分布，而不是單純的一個深度值。

牽涉到概率分布的，往往都是理論一大堆屁話，實際可以操作的只有高斯分布一種——高斯只要在計算機里存均值和協方差即可。在逆深度[8]流行起來之後，用逆深度的高斯分布成了SLAM中的標配。然而SVO卻使用了一種高斯——均勻混合分布的逆深度（由四個參數描述），推導並實現了它的更新方式，稱為Depth Filter。它的工作邏輯大概是這樣的：

• 如果進來一個關鍵幀，就提取關鍵幀上的新特徵點，作為種子點放進一個種子隊列中。
• 如果進來一個普通幀，就用普通幀的信息，更新所有種子點的概率分布。如果某個種子點的深度分布已經收斂，就把它放到地圖中，供追蹤線程使用。

當然實現當中還有一些細節，比如刪掉時間久遠的種子點、刪掉很少被看到的種子點等等。

要理解Depth Filter，請搞清楚這兩件事：

1. 基於高斯——均勻的濾波器，在理論上的推導是怎麼樣的？
2. Depth Filter又是如何利用普通幀的信息去更新種子點的？

第1個問題，請參照論文[4],[7],以及[7]的補充材料，以及補充材料的補充材料。相信研究SVO的人應該都推導過，並不很難，靜下心來推一遍即可，我當時也就一塊小白板就推完了。在SLAM群的群文件里有一個depth filter.pdf，也給出了推導過程：

或者請看SVO原理解析 - 路遊俠 - 博客園 。我覺得應該用不著把公式在知乎上再敲一遍……

第2個問題，你需要搞明白極線搜索這件事。由於種子點的深度不確定，它在別的幀里看起來就在一條直線（極線）上：

於是你從這條極線的一個端點走到另一個端點，把每處的圖像塊都和參考的去比較，就可以（可能）找到正確的匹配了。哦別忘了要Affine Warp一下……找到之後，讓depth filter更新其深度分布即可。當然如果位移太小或視線平行性太高，讓深度變得更加不確定也是有可能的。在理想情況下，你可以期待一個地圖點經過不斷觀測之後收斂的過程。

-------------------------------------------------------------
4 評述
以上就是SVO的基本工作原理了。那麼，這樣一套系統實際工作起來效果如何呢？相比於其他幾個開源方案有何優劣呢？

首先要澄清一點的是：開源版本的SVO，是一個比較挫的版本。相比於LSD或ORB，我還很少看到有人能一次性把SVO跑通的。但是從論文上看，開源版本並不能代表SVO的真實水平。所以應該是心機弗斯特開源了一個只有部分代碼的，不怎麼好用的版本，僅供學習研究使用。相比之下，DSO，LSD，ORB至少能夠在自己數據集上順利運行，而ORB、LSD還能在大部分自定義的相機上運行，這點開源版本的SVO是做不到的。

那麼，拋開開源實現，從理論和框架上來說，SVO有何優劣呢？

優點：

• 著實非常快，不愧為稀疏直接法；
• 關鍵點分布比較均勻；

缺點：（不是我嫌棄它，確實有一堆可以吐槽的地方）

1. 首先這貨是VO，不是SLAM，沒有閉環。這意味著丟失後沒法重定位——丟了基本就掛了。
2. 追蹤部分：SVO首先將當前幀與上一個追蹤的幀比較，以求得粗略的位姿估計。這裡存在一個問題：這必須要求上一個幀是足夠準確的！那麼問題就來了：怎麼知道上一個幀是準的呢？開源SVO里甚少考慮出錯的情況。如果上一個幀由於遮擋、模糊等原因丟失，那麼當前幀也就會得到一個錯誤的結果，導致和地圖比對不上。又由於這貨是沒法重定位的，所以就。。。掛了唄。。。
3. 還是追蹤部分。既然是直接法，SVO就有直接法的所有缺點。後面那位同學來背一遍直接法缺點？

• 怕模糊（需要全局曝光相機）
• 怕大運動（圖像非凸性）
• 怕光照變化（灰度不變假設）

4. 地圖部分：

• Depth Filter收斂較慢，結果比較嚴重地依賴於準確的位姿估計。如果統計收斂的種子點的比例，會發現並不高，很多計算浪費在不收斂的點上。
• 相比於純高斯的逆深度，SVO的Depth Filter主要特點是能夠通過Beta分布中的兩個參數a,b來判斷一個種子點是否為outlier。然而在特徵點法中我們也能夠通過描述來判斷outlier，所以並不具有明顯優勢。

--------------------------------------------------------------------------------------------
小結

1. SVO是基於稀疏直接法的視覺里程計，速度非常快。
2. 代碼清晰易讀，研究SVO會有不少啟發。
3. 但是開源實現存在諸多缺點，不實用。論文中效果應該不是這個開源代碼能夠實現的。

   --------------------------------------------------------------------------------------------

參考文獻
[1] Foster et al., SVO: Fast semi-direct monocular visual odometry, ICRA 2014.
[2] SVO 2.0
[3] Foster et al., IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation, RSS 2015.
[4] Pizzoli, Matia and Forster, Christian and Scaramuzza, Davide, REMODE: Probabilistic, monocular dense reconstruction in real time, ICRA 2014.
[5] Engel, Jakob and Schops, Thomas and Cremers, Daniel, LSD-SLAM: Large-scale direct monocular SLAM, ECCV, 2014.
[6] Engel, Jakob and Koltun, Vladlen and Cremers, Daniel, Direct sparse odometry, 2016.
[7] George Vogiatzis and Carlos Hernández, Video-based, real-time multi-view stereo, Image and Vision Computing, 2011.
[8] Civera, Javier and Davison, Andrew J and Montiel, JM Martinez, Inverse depth parametrization for monocular SLAM, IEEE transactions on robotics, 2008.

附：SVO相關中文博客、材料

• 馮兵的博客|內外兼修 &<一步步完善視覺里程計&>是對SVO比較完整的介紹。
• 白巧克力亦唯心的博客 - 程序園 賀一家的博客，有幾篇關於SVO的介紹。
• 路遊俠 - 博客園 這個是誰。。。知道的回復我一下。。。
• ORB-LSD-SVO比較-劉浩敏_bilibili_演講?公開課_科技_bilibili_嗶哩嗶哩彈幕視頻網 浩敏師兄講的一次ppt

嗯，大概就這些，別嫌我啰嗦……

曾經大改過SVO，也用SVO跑過各種數據集。 高博的答案非常好，我再補充幾點我在改的過程中的一些收穫。

1，開源svo實現中，每次檢測到新的feature， 加入到depth filter之後，要下一次再有關鍵幀來才更新。 這樣導致很多seeds收斂慢。開源版本很容易出現一種情況，跟蹤系統掛了，feature不夠， 但是depth filter裡面還有一堆seeds嗷嗷待哺，既沒有被判定為Outlier，也還沒活到收斂。 解決這個問題有一個很簡單的辦法，就是每次新加一個seed進入到depth filter之後，先用前面的關鍵幀先更新這個seed，因為很高概率新進來的特徵點，之前的幾幀也能看到，這個trick可以大大增加系統的魯棒性。

2，svo是畫格子檢測特徵點，這個策略在有些環境裡面確實不好，比如自動駕駛(參考kitti數據集)，比較平的地方會有大塊圖是天空，完全沒有特徵點，這個時候系統很容易陷入到特徵點不足的困境之中。 比較好的策略還是kt樹，或者其他的數據結構，優先考慮均勻分布的足夠多的特徵點，但是當找不到足夠多的feature的時候，還是要繼續找不那麼均勻的特徵點，保證數量夠。vo系統裡面特徵點的數量非常關鍵，不夠的時候很容易誤差積累導致跑不下去。

3，開源svo明顯有很多的保留，比如edgelet特徵，開源代碼裡面沒有刪乾淨，還是能看到一些痕迹的。

4，svo在理論上很優雅，depth filter， lk 光流， 整個系統很乾凈。但是工程上真的還有很多優化的空間。orb剛好相反，理論上幾乎沒有啥創新。但是工程化方面用了很多tricks，開源版本裡面有很多簡單粗暴的做法，也有很多神奇參數或者多層優化變參。

5，只要你的相機幀率不太低，連續兩幀之間運動不要太誇張，svo改改完全可以又快又好的跑起來。kitti數據集其實對直接法已經很不友好了，車跑得又快，幀率又低。但是我當時改改之後，kitti所有路線都可以用一套參數跑下來，當然有幾條漂得比較厲害。特別是高速路那幾條。

在當時的ppt裡面找到一張彙報的圖，這是kitti第一條路，稍微有點漂。

6，svo2.0 放出來了，雖然沒開源，但是有執行文件，可以試試看，效果不錯。