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.