SVO詳細解讀

SVO詳細解讀

極品巧克力

前言

接上一篇文章《深度濾波器詳細解讀》。

SVO（Semi-Direct Monocular Visual Odometry）是蘇黎世大學Scaramuzza教授的實驗室，在2014年發表的一種視覺里程計算法，它的名稱是半直接法視覺里程計，通俗點說，就是結合了特征點法和直接法的視覺里程計。目前該算法已經在github上面開源（https://github.com/uzh-rpg/rpg_svo）。賀一家在它的開源版本上面進行改進，形成了SVO_Edgelet（https://github.com/HeYijia/svo_edgelet）。相比原版，SVO_Edgelet增加了一些功能，比如結合本質矩陣和單應矩陣來初始化，把邊緣特征點加入跟蹤等，對SVO的魯棒性有非常大的改善。

雖然SVO已經有論文[1]了，但是論文里面只是講了最核心的算法理論，可以用論文來了解其算法思想。但是具體的實現方法和技巧，都隱藏在源代碼里。要想徹底掌握它，並且在具體實踐中靈活運用它，還是必須要閱讀源代碼才行。

所以我通讀了這2萬多行的源代碼，力求從代碼中反推出所有的具體實現、算法、公式、技巧、作者的意圖。

源碼之下，了無秘密。

在把這2萬多行的代碼全部搞懂之后，我把代碼里面的具體實現方法全都一五一十地還原出來，研究其優缺點、適用情況，探討其可以改進的地方，總結成本文，與各位分享。

本文對應的程序為SVO_Edgelet。

本文目標讀者：對SVO有一定了解的SLAM算法工程師。

流程圖

1.跟蹤

其實，SVO的跟蹤部分的本質是跟ORBSLAM一樣的，TrackWithMotionModel和TrackLocalMap，只是匹配的方法從特征點法改成了灰度值匹配法。

但是，隨后，與ORBSLAM有不同的地方，SVO在優化出相機位姿之后，還有可選項，可以再優化地圖點，還可以再把地圖點和相機位姿一起優化。

1.1初始化

圖像剛進來的時候，就獲取它的金字塔圖像，5層，比例為2。

然后處理第一張圖像，processFirstFrame()。先檢測FAST特征點和邊緣特征。如果圖像中間的特征點數量超過50個，就把這張圖像作為第一個關鍵幀。

然后處理第一張之后的連續圖像，processSecondFrame()，用於跟第一張進行三角初始化。從第一張圖像開始，就用光流法持續跟蹤特征點，把特征像素點轉換成在相機坐標系下的深度歸一化的點，並進行畸變校正,再讓模變成1,映射到單位球面上面。

如果匹配點的數量大於閾值，並且視差的中位數大於閾值。如果視差的方差大的話，選擇計算E矩陣，如果視差的方差小的話，選擇計算H矩陣。如果計算完H或E后，還有足夠的內點，就認為這幀是合適的用來三角化的幀。根據H或E恢復出來的位姿和地圖點，進行尺度變換，把深度的中值調為1。

然后把這一幀，作為關鍵幀，送入到深度濾波器中。（就是送到的深度濾波器的updateSeedsLoop()線程中。深度濾波器來給種子點在極線上搜索匹配點，更新種子點，種子點收斂出新的候選地圖點。如果是關鍵幀的話，就初始化出新的種子點，在這幀圖像里面每層的每個25x25大小的網格里，取一個最大的fast點。在第0層圖像上，找出canny邊緣點。）

之后就是正常的跟蹤processFrame()。

1.2基於稀疏點亮度的位姿預估

把上一幀的位姿作為當前幀的初始位姿。

把上一幀作為參考幀。

先創建n行16列的矩陣ref_patch_cache_，n表示參考幀上面的特征點的個數，16代表要取的圖塊的像素數量。

再創建6行n*16列的矩陣jacobian_cache_。代表圖塊上的每個像素點誤差對相機位姿的雅克比。

要優化的是參考幀相對於當前幀的位姿。

把參考幀上的所有圖塊結合地圖點，往當前幀圖像的金字塔圖像上投影。在當前幀的金字塔圖像上，從最高層開始，一層層往低層算。每次繼承前一次的優化結果。如果前一次的誤差相比前前次沒有減小的話，就繼承前前次的優化后的位姿。每層的優化，迭代30次。

要優化的殘差是，參考幀上的特征點的圖塊與投影到當前幀上的位置上的圖塊的亮度殘差。投影位置是，參考幀中的特征點延伸到三維空間中到與對應的地圖點深度一樣的位置，然后投影到當前幀。這是SVO的一個創新點，直接由圖像上的特征點延伸出來，而不是地圖點（因為地圖點與特征點之間也存在投影誤差），這樣子就保證了要投影的圖塊的准確性。延伸出來的空間點肯定也與特征點以及光心在一條直線上。這樣子的針孔模型很漂亮。

SVO的另外一個創新點是，以當前幀上的投影點的像素值為基准，通過優化調整參考幀投影過來的像素點的位置，以此來優化這兩者像素值殘差。這樣子，投影過來的圖塊上的像素值關於像素點位置的雅克比，就可以提前計算並且固定了。（而以前普通的方法是，以參考幀投影過去的像素值為基准，通過優化投影點的位置，來優化這兩者的殘差。）

殘差用公式表示為。

其中，表示半個圖塊的大小。

把擾動加在上，也就是擾動參考幀相對於第幀的位姿。因為一般用左乘擾動，所以這里也用左乘擾動。

所以，殘差關於擾動的雅克比為。

級聯求導，但是，這樣子的話，每次迭代后，雅克比都會發生改變。一般情況下的優化，都會遇到這樣的問題。

所以，采用近似的思想。首先，認為空間點固定不動，只調整參考幀的位姿，所以這個擾動不影響在當前幀上的投影點的位置，只會影響圖塊的內容。然后，參考幀在新的位姿上重新生成新的空間點，再迭代下去。雖然只是近似優化，但每次迭代的方向都是對的，假設步長也差不多，所以最終也可以優化成功。

只是對公式優化的近似，抽象成如下的模型。

 

如果兩個相機是互相正對着的，並且地圖點在兩個相機光心連線的中心的話，那肯定是符合的，跟公式優化的效果是相同的。可以通過平行四邊形定理證明。

兩相機的法線相同；或兩相機的法線相反，地圖點到兩相機光心的距離相同。那這樣子調整，也是跟優化的效果相同的。可以通過相似三角形比例定理來證明，作一個輔助平面出來，地圖點在這個平面上，這個平面過兩個相機光心連線的中點。可以用相似三角形定理證明a等於b。

 

但是，這種模型，只有在這種情況下（兩相機的法線相同；或兩相機的法線相反，地圖點到兩相機光心的距離相同），才跟優化相同。

當不滿足這種情況的時候，就會出現問題。比如，當地圖點距離兩個相機的差別很大的時候。按照公式優化，正確的優化結果應該是第4行，但是，按照這種近似的方法優化，優化的結果是第3行。所以，就與理想優化情況差別蠻大了。

而在SVO中是這樣的情況，兩個相機的法線方向相差不大，並且地圖點離兩個相機的光心都遠大於光心距離，可以近似成地圖點到兩光心的距離是相等的。所以，在SVO中可以使用這個近似方法。

當前的殘差是這樣的。

假設，給參考幀的相機位姿加個擾動，變到的位姿，即

則殘差與擾動的關系可以表達如下。

因為其中有個逆矩陣，這樣子在求導時會不方便，所以，使用來代替，在算出之后再逆過去。至於為什么在這種情況下，在優化的時候可以用另一中形式的變量取代掉，在算出來后再變換回去，參考《優化過程的中間誤差的傳遞》。

所以，原式就可以轉換為。

然后，就可以計算雅克比了。

對於上一幀的每一個特征點，都進行這樣的計算，在自己本來的層數上，取那個特征點左上角的4x4圖塊。如果特征點映射回原來的層數時，坐標不是整數，就進行插值，其實，本來提取特征點的時候，在這一層特征點坐標就應該是整數。把圖塊往這一幀的圖像上的對應的層數投影，然后計算雅克比和殘差。計算殘差時，因為投影的位置並不剛好是整數的像素，所以會在投影點附近插值，獲取與投影圖塊對應的圖塊。

最后，得到一個巨大的雅克比矩陣，以及殘差矩陣。但是為了節省存儲空間，提前就轉換成了H矩陣。。

用高斯牛頓法算出擾動。

然后，得到，逆矩陣得到，再更新出。然后，在新的位置上，再從像素點坐標，投影出新的點。

每一層迭代30次。因為這種inverse-compositional方法，用這種近似的思想，雅克比就可以不用再重新計算了。（因為重新投影出新的點的位置，這個過程沒有在殘差公式里面表現出來。）這樣子逐層下去，重復之前的步驟。

對於每一個圖塊的每一個像素，它的雅克比計算如下。

，是這個像素插值點在圖像上的梯度，就是水平右邊的像素點減去水平左邊的像素點，豎直下邊的像素點減去豎直上邊的像素點。如下圖所示。

，算的是投影雅克比。

為了方便計算，雖然，但因為是一個很小的擾動，所以可以認為,。所以，后兩項就可以相乘，統一用來表示了。可以認為。

對於每一個特征點，根據像素位置算出，再根據反投影出來的空間點位置算出上式的左邊項，根據特征點所在的層數算出。然后，相乘，就得到了一行雅克比矩陣。再根據，，加到H矩陣和殘差矩陣上。

最后，在優化出后，應該是這樣更新，。

但是，在程序里面，直接就是，。可能是為了加快計算，認為。在sparse_align.cpp的307行，T_curnew_from_ref = T_curold_from_ref * SE3::exp(-x_); 這個地方，為什么不是 T_curnew_from_ref = T_curold_from_ref * (SE3::exp(x_)).inverse(); 需要以后研究一下。

這樣子，就可以得到當前幀的位姿。

1.3基於圖塊的特征點匹配

因為當前幀有了1.1的預估的位姿。對於關鍵幀鏈表里面的那些關鍵幀，把它們圖像上的分散的5點往當前幀上投影，看是否能投影成功，如果能投影成功，就認為共視。再把所有的共視關鍵幀，按照與當前幀的距離遠近來排序。然后，按照關鍵幀距離從近到遠的順序，依次把這些關鍵幀上面的特征點對應的地圖點都往當前幀上面投影，同一個地圖點只被投影一次。如果地圖點在當前幀上的投影位置，能取到8x8的圖塊，就把這個地圖點存入到當前幀投影位置的網格中。

再把候選地圖點都往當前幀上投影，如果在當前幀上的投影位置，能取到8x8的圖塊，就把這個候選地圖點存入到當前幀投影位置的網格中。如果一個候選點有10幀投影不成功，就把這個候選點刪除掉。

然后，對於每一個網格，把其中對應的地圖點，按照地圖點的質量進行排序（TYPE_GOOD> TYPE_UNKNOWN> TYPE_CANDIDATE> TYPE_DELETED）。如果是TYPE_DELETED，則在網格中把它刪除掉。

遍歷網格中的每個地圖點，找到這個地圖點被觀察到的所有的關鍵幀。獲取那些關鍵幀光心與這個地圖點連線，與，地圖點與當前幀光心連線，的夾角。選出夾角最小的那個關鍵幀作為參考幀，以及對應的特征點。（注意，這里的這種選夾角的情況，是只適合無人機那樣的視角一直朝下的情況的，應該改成ORBSLAM那樣，還要再把視角轉換到對應的相機坐標系下，再篩選一遍）。這個對應的特征點，必須要在它自己的對應的層數上，能獲取10x10的圖塊。

然后，計算仿射矩陣。首先，獲取地圖點在參考幀上的與光心連線的模。然后它的對應的特征點，在它對應的層數上，取右邊的第5個像素位置和下邊的第5個像素位置，再映射到第0層。再轉換到單位球上，再映射到三維空間中，直到與地圖點的模一樣的長度。把對應的特征點也映射到三維空間中，直到與地圖點的模一樣的長度。然后，再把這3個點映射到當前幀的（有畸變的）圖像上。根據它們與中心投影點的位置變換，算出了仿射矩陣A_cur_ref。A_cur_ref.col(0) = (px_du - px_cur)/halfpatch_size; A_cur_ref.col(1) = (px_dv - px_cur)/halfpatch_size;。（www.cnblogs.com/ilekoaiq）仿射矩陣A，就是把參考幀上的圖塊在它自己對應的層數上，轉換到當前幀的第0層上。（這種把比例變換轉換成矩陣表示的方法，很好）。

然后，計算在當前幀的目標搜索層數。通過計算仿射矩陣A_cur_ref的行列式，其實就是面積放大率。如果面積放大率超過3，就往上一層，面積放大率變為原來的四分之一。知道面積放大率不再大於3，或者到最高層。就得到了目標要搜索的層數。

然后，計算仿射矩陣的逆仿射矩陣A_ref_cur。然后，這樣子，如果以投影點為中心（5,5），取10x10的圖塊，則圖塊上每個像素點的（相對中心點的）位置，都可以通過逆仿射矩陣，得到對應的參考幀上的對應層數圖像上的（相對中心點的）像素位置。進行像素插值。就是，把參考幀上的特征點附近取一些像素點過來，可以組成，映射到當前幀上的對應層數的投影點位置的附近，這些映射到的位置剛好組成10x10的圖塊。

然后，從映射過來的10x10的圖塊中取出8x8的圖塊，作為參考圖塊。對這個圖塊的位置進行優化調整，使得它與目標位置的圖塊最匹配。殘差表達式為。

其中，表示這個像素點對應的殘差，表示在這個像素點對應的當前圖像上的對應圖塊的位置，表示這個像素點在參考圖塊上的位置，表示兩個圖塊的均值差。

在這里，SVO有兩個創新點。

第一個創新的地方是。因為一般情況下，是基於自己圖塊不變，通過優化使得投影位置的圖塊跟自己最接近。而SVO是投影位置的圖塊不變，通過優化使得自己圖塊與投影位置的圖塊最接近。這樣的話，就可以避免重復計算投影位置圖塊像素關於位置的雅克比了。因為自己圖塊是固定的，所以雅克比是固定的，所以只需要計算一次。其實，這個創新點與1.2中的反向創新點一樣，都是用近似優化的方法來。因為，如果是一般的方法的話，計算目標投影位置的圖塊的雅克比，是知道自己參考圖塊重新移動后，會遇到怎樣的目標圖塊。而，這個反向的方法，並不知道重新移動后會遇到怎樣的圖塊，只知道移動后，對當前的目標圖塊可以匹配得更好。也是一種迭代，近似優化的方法，但速度可以塊很多，避免了重復計算雅克比。

第二個創新的地方是。一般情況下，兩圖塊的均值差，都是直接把兩個圖塊的均值相減的。但是，這樣子的話，可能容易受某些極端噪聲的影響。所以，SVO中，直接把也作為優化變量了。

於是，雅克比可以計算如下。

其中，就是參考圖塊上的雅克比。這個點的像素值關於位置（橫坐標，縱坐標）的雅克比，其實就是這個點的右左像素值相減和下上像素值相減，得到的梯度。

對這個圖塊上的所有的像素點都進行這樣的操作。然后用高斯牛頓法進行迭代。最多迭代10次，如果某次調整位置的模小於0.03，就認為收斂了，退出迭代。得到最佳匹配點的位置。認為匹配成功。

而，如果是對於那些邊緣上的點。則只在梯度方向上進行調整，只調整這1個維度，即梯度方向上的長度。右左下上像素的變化，映射到梯度方向上，得到在梯度方向上的像素變化。最后優化完后，再從這個維度上映射出橫縱坐標。最后，也得到最佳匹配點的位置。

上面的優化，必須在1.2估算出的位姿較准確的情況下，才能使用這樣的方法，在預測的投影點位置用像素梯度來優化出最佳匹配點位置。

如果是一個TYPE_UNKNOWN類型的地圖點，它找匹配失敗的次數大於15次，就把它變為delete類型的點。如果是一個TYPE_CANDIDATE類型的點，它匹配失敗的次數大於30次，就把它變為delete類型的點。

如果匹配成功的話，就在當前圖像上，新生成一個特征點（包括坐標和層數），特征點指向那個地圖點。如果對應的參考幀上的特征點是邊緣點的話，則新的特征點的類型也設為邊緣點，把梯度也仿射過來，歸一化后，作為這個新特征點的梯度。

每個網格里，只要有一個地圖點匹配成，就跳出這個網格的遍歷循環。如果有180個網格匹配成功了，直接跳出所有網格的循環。循環結束后，如果成功匹配的網格的數量小於30個，就認為當前幀的匹配失敗。

1.4進一步優化位姿

然后，對於1.3中的，當前幀上的所有的新的特征點 ，如果它指向的是地圖點的話，通過優化當前幀的相機位姿，使得地圖點的在對應的層數上的預測投影位置和最佳匹配位置的殘差最小。注意，是在對應層數上的殘差。

雅克比為。

上式結果中的。

程序里為了計算方便，優化公式的左右兩邊都約掉第0層的，所以就只剩下1.0 / (1<<(*it)->level)了，右邊也只需要算到單位平面再乘以1.0 / (1<<(*it)->level)就可以了。

如果是邊緣點的話，則把重投影誤差映射到梯度方向上。

使用了核函數TukeyWeightFunction，根據誤差的模來調整誤差的權重。Tukey's hard re-descending function，http://en.wikipedia.org/wiki/Redescending_M-estimator。

用高斯牛頓法來優化。

然后，程序里，通過誤差平方和的值是否變大，來判斷這次優化是否有效。（但是，這個誤差平方和是在優化之前的，程序里可能寫錯了，應該在優化之后再算誤差平方和。）

總共優化迭代10次，如果某次優化量約等於0，則跳出優化循環。

 

優化結束后，接下來，要算這個算出來的位姿的協方差，即增加的擾動的協方差，就是對應的高斯分布里面的那個協方差。這里，可以通過高斯分布，轉換出位姿的協方差。因為，參考卡爾曼濾波的狀態轉移方程，協方差，也是會隨着狀態轉移矩陣而改變的。假設，在對應的層數上，測量值的協方差都為1個像素，即測量值滿足方差為1的高斯分布。即（如果是其它方差的話，改成，同樣代入下面的公式即可），要求。

根據，得出，

最后，如果有些點的的重投影誤差，映射到第0層上，模大於2個像素的話，則把這個特征點指向地圖點的指針賦值為空。如果最后剩下的匹配成功點的數量大於20個，就認為優化成功。

1.5優化地圖點

就是optimizeStructure。在程序里，用nth_element找出前20個，最近一次優化幀的id，離當前幀id較遠的，地圖點。

針對每個地圖點，優化地圖點的三維位置，使得它在它被觀察到的每個關鍵幀上的重投影誤差最小。每個地圖點優化5次。如果這次優化的誤差平方和小於上次優化的誤差平方和，就接受這次的優化結果。（注意，這里的平方和也是在優化之前算的，其實應該在優化之后算）。如果是邊緣點的話，則把重投影誤差映射到梯度方向上，成為梯度方向上的模，就是與梯度方向進行點積。相應的，雅克比也左乘對應的梯度方向。相當於是，優化重投影誤差在梯度方向上的映射。

對於普通點，把擾動加在三維坐標上，重投影誤差為，

雅克比為，

所以，根據，用高斯牛頓法來進行計算。在程序里，為了計算方便，的左右兩邊，都約去了對應層數的，所以，右邊就只需要算到深度為1的平面上的殘差就可以。

如果是邊緣點的話，則上式的左右兩邊都要乘以梯度的轉置。。也用高斯牛頓法來算。

1.6 BA

SVO里面有個選項，可以開啟使用g2o的BA功能。

如果開啟使用這個功能的話，則在一開始的兩張圖像初始化之后，兩張圖像以及初始化出來的地圖點，會用BA來優化。用的是g2o里面的模板。

另外，會在1.5優化完地圖點后，對窗口里的所有的關鍵幀和地圖點，或者全局關鍵幀和地圖點，進行優化。用的是g2o里面的模板。

1.7對畸變圖像處理的啟發

SVO的跟蹤都是在畸變的魚眼圖像上跟蹤的，沒有對圖像進行校正，這樣子可以盡可能地保留圖像的原始信息。

又因為在1.2中的逆向圖塊雅克比的方法，除了可以加快計算外，還避免了對畸變參數的雅克比計算。因為如果用正向圖像雅克比的話，在計算雅克比的時候，必須要把畸變參數也考慮進來。

而在1.3中，圖塊匹配就是用畸變的圖塊取匹配的，保證了准確性。為了避免對畸變參數的雅克比計算，在匹配完成后，把投影點位置和匹配點位置都從畸變的圖像上，轉換到了單位平面上。以后在畸變圖像上，計算重投影誤差，就用這樣的方法。

2.創建地圖點

特征點提取的方法，放在了地圖線程里。因為與ORBSLAM不同的是，它跟蹤的時候，不需要找特征點再匹配，而是直接根據圖塊亮度差匹配的。

而如果是vins的話，也可以參考這個方法，把特征點提取放到地圖線程里，連續幀之間的特征點用光流匹配。但光流要求幀與幀之間不能差別太大。

而在SVO中，后端的特征點是只在關鍵幀上提取的，用FAST加金字塔。而上一個關鍵幀的特征點在這一個關鍵幀上找匹配點的方法，是用極線搜索，尋找亮度差最小的點。最后再用depthfilter深度濾波器把這個地圖點准確地濾出來。

選取30個地圖點，如果這30個地圖點在當前幀和最近一個關鍵幀的視差的中位數大於40，或者與窗口中的關鍵幀的距離大於一定閾值，就認為需要一個新的關鍵幀。然后把當前幀設置為關鍵幀，對當前幀進行操作。

2.1初始化種子

當關鍵幀過來的時候，對關鍵幀進行處理。在當前圖像上，划分出25像素*25像素的網格。

首先，當前幀上的這些已經有的特征點，占據住網格。

在當前幀的5層金字塔上，每層頭提取fast點，首先用3x3范圍的非極大值抑制。然后，對剩下的點，全部都計算shiTomasi分數，有點像Harris角點里面的那個分數。再全部映射到第0層的網格上，每個網格只保留分數最大的，且大於閾值的那個點。

找邊緣點的話，都只在第0層上面找。同樣也是畫網格，然后再每個網格中找canny線，然后對於網格中的在canny線上的點，計算它的梯度的模，保留模梯度最大的那個點，作為邊緣點。梯度方向是二維的，就是這個點的右左下上梯度。程序里用了cv::Scharr結合cv::magnitude來快速算出所有點的橫縱方向的梯度。

然后，對於所有的新的特征點，初始化成種子點。用高斯分布表示逆深度。均值為最近的那個點的深度的倒數。深度范圍為當前幀的最近的深度的倒數，即1.0/depth_min。高斯分布的標准差為1/6*1.0/depth_min。

2.2更新種子，深度濾波器

如果新來一個關鍵幀，或者是當前的普通的幀，或者之前的關鍵幀，用於更新種子點。對於每個種子點，通過正負1倍標准差，確定逆深度的搜索范圍。這些參數都是對應種子點在它自己被初始化的那一幀。

然后把深度射線上的最短和最長的深度，映射到當前幀的單位深度平面上，其實就得到的在單位平面上的極線線段。然后，再把逆深度的均值對應的深度，映射到當前幀，就是跟1.3中的同樣的方法，得到圖塊仿射矩陣，和最佳搜索層數。

（對於邊緣點，如果把梯度仿射過來后，梯度的方向與極線方向的夾角大於45度，就認為沿着極線找，圖塊像素也不會變化很大，就不搜索了，直接返回false。）

把極線線段投影到對應的層數上，如果兩個端點間的像素距離小於2個像素，就直接進行優化位置。用的是1.3中的找圖塊匹配的方法，把對應的圖塊映射過來。找到最佳匹配位置后，進行三角定位。三角定位的方法參考《視覺SLAM十四講》的三角定位，矩陣分塊計算。

如果兩個端點間像素距離大於2個像素，就在極線上進行搜索。首先，確定總步長數，以兩端點間的距離除以0.7，得到總步長數n_steps。然后，把單位深度平面上的極線線段分n_steps段，從一個端點開始往另外一個端點走，每走一步，就把位置投影（包括畸變）到對應層數的圖像上，坐標取整后，獲取圖塊。（這里可以改進，不應該對坐標進行取整，而應該改成插值）。然后，計算投影過來的圖塊與投影位置圖塊的相似度，相似度的計算公式如下，其中有消除均值的影響。

如果分數小於閾值，就認為兩個圖塊是相似的。在當前位置，再進行優化位置，用的是1.3中的找圖塊匹配然后優化位置的方法。然后再進行三角定位。

接下來，計算這個三角定位出來的深度值的協方差。用的是《視覺SLAM十四講》的深度濾波。假設，在圖像上的測量協方差為1個像素，則這個協方差的傳遞到深度上的過程如下。這個傳遞的，都還是標准差，而不是。

再把這個協方差傳遞到逆深度上。假設這時候三角定位出來的深度值為，則在逆深度上的標准差為，

所以，這個測量出來的深度，滿足的分布為。然后，就是更新種子點的深度分布了，參考《深度濾波器詳細解讀》。但是在DepthFilter.cpp的486-490行對系數進行平均了。這里與depthfilter的論文里推導的不一樣。可能這里程序寫錯了，應該改成和論文里面一樣。

如果種子點的方差，小於深度范圍/200的時候，就認為收斂了，它就不再是種子點，而是candidate點。candidate點被成功觀察到1次，就變成UNKNOW點。UNKNOW被成功觀察到10次，就變成GOOD點。如果多次應該觀察而沒有被觀察到，就變成DELETE點。

3.重定位

SVO中重定位，實現很簡單，就是在跟丟之后，仍然假設當前幀的位姿和前一幀一樣，往這個位姿上投地圖點，用第1部分中的方法去優化計算，如果優化成功，就重定位回來，如果優化不成功，就繼續下一幀。所以，在跟丟后，只能再回到跟丟時的位置，才能重定位回來。

這樣子實現重定位的方法很簡單，可重定位的效果就很差了。這地方可以進行改進。

4.總結

SVO的定位很好，抖動很小。尤其在重復紋理的環境中，表現得比基於特征點法的ORBSLAM2要出色。

將來可以在上面增加更魯棒的重定位，回環閉環，全局地圖的功能，來滿足更多的實際應用場景，比如室內機器人、無人機、無人車。

 

5.求贊賞

 

您覺得，本文值多少？ 

 

 

 

6.參考文獻

1. Forster C, Pizzoli M, Scaramuzza D. SVO: Fast semi-direct monocular visual odometry[C]// IEEE International Conference on Robotics and Automation. IEEE, 2014:15-22.
2. 高翔.視覺SLAM十四講[M].北京:電子工業出版社,2017.