SVO閱讀筆記

一、SVO總體結構

二、追蹤部分：位姿估計

（一）Frame-to-frame位姿估計

1.重投影（上一幀的二維點投影到三維再投影到目前幀的二維坐标中）

2.稀疏直接法求解位姿

（二）Frame-to-Map

1.參考LK光流法的特征對齊Feature Alignment來優化特征比對

2.特征點法BA優化（根據像素位置差優化重投影關系）

三、Mapping部分：深度估計

（一）對極幾何

（二）Mapping總架構

（三）深度估計及融合

1.每一幀的深度估計方法：極線特征比對+三角測量

2.多幀深度值融合

直接法和半直接法：

• 直接法：對整個圖像使用直接比對，整幅圖像的直接比對法常見于RGBD傳感器，因為RGBD傳感器能擷取整幅圖像的深度。
• 半直接：通過對圖像中的特征點圖像塊進行直接比對來擷取相機位姿。

直接法缺點：

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

視覺運動估計方法：

• a） 基于特征的方法：标準的方法是在每個圖像中提取一組稀疏的顯著的圖像特征（例如點、線）；使用不變的特征描述子在連續的幀中進行比對；使用對極幾何魯棒地恢複相機的運動和結構；最後，通過最小化重投影誤差reprojection error來優化姿态和結構。大多數VO算法遵循這個過程，獨立于應用的優化架構。這些方法成功的原因是魯棒的特征檢測器，和即使在大的inter-frame movement也能比對的描述子。基于特征的方法的缺點是依賴于檢測和比對門檻值，必須使用魯棒估計技術來處理錯誤的對應，而且大多數特征檢測器都是針對速度而不是精度進行優化的，是以，需要通過對多個特征測量進行平均來補償運動估計中的漂移。
• b） 直接法：直接方法直接從圖像中的強度值(intensitiy value)估計結構和運動。與基于特征的方法相比，直接法将局部強度梯度的大小和方向用于優化中，而基于特征的方法隻考慮了到某些特征位置的距離。直接法利用到了圖像中的所有資訊，即使是梯度很小的區域，它在少紋理、相機散焦和運動模糊的情況下，魯棒性優于基于特征的方法。光度誤差的計算比重投影誤差更為密集，因為它涉及到大圖像區域的翹曲(warping)和積分。不過，由于直接方法直接對圖像的強度值進行操作，是以節省了特征檢測和不變描述子計算的時間。

一、SVO總體結構

論文： SVO: Fast Semi-Direct Monocular Visual Odometry

結構：

• 位姿估計：是通過direct method來擷取位姿，和feature-method不一樣。但和direct method不同的是，它利用特征塊的配準來對direct method估計的位姿進行優化， “In contrast to previous direct methods, we use many (hundreds) of small patches rather than few (tens) large planar patches.”
• 深度估計：一個貝葉斯濾波器，用顯式模組化異常值測量（explicitly models outlier measurements）來估計特征所在位置的深度。隻有當相應的深度濾波器已收斂時，才會在地圖中插入三維點，這需要多次測量。結果得到一個較少離群點（outliers）和能被可靠地跟蹤的點的地圖。

SVO流程

追蹤部分優缺點：

• 稀疏直接法非常快，關鍵點分布比較均勻。
• 但SVO隻是VO，不是SLAM，沒有閉環。丢失後沒法重定位，丢了基本就挂了。而且開源SVO裡甚少考慮出錯的情況，最開始粗略的位姿估計極其依賴目前幀與上一個追蹤的幀的比較，這必須要求上一個幀是足夠準确的。如果上一個幀由于遮擋、模糊等原因丢失，那麼目前幀也就會得到一個錯誤的結果，導緻和地圖比對不上。又由于這貨是沒法重定位的，是以就挂了。svo檢測和跟蹤特征點的能力比較差，特征點極易丢，是以宋師兄把地闆貼的是花花綠綠。。編隊也隻能這樣了吧，樹莓派跑VINS跑不動啊。
• 并且既然是直接法，SVO就有直接法的所有缺點，怕模糊、大運動和灰階變化。

建圖部分缺點：

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

二、追蹤部分：位姿估計

（一）Frame-to-frame位姿估計

先把目前幀和上一個追蹤的幀進行比對，擷取粗略的位姿。該問題的基本形式為：已知k-1幀對地圖點的觀測（包括2D投影位置

和深度

），以及目前幀的圖像

，求解目前幀的粗略位姿

1.重投影（上一幀的二維點投影到三維再投影到目前幀的二維坐标中）

基于稀疏模型的特征對齊，尋找同一3D點p在前後兩幀光度差最小的Tk,k-1

• 第1步：根據圖像位置和深度将上一幀像素點逆投影到三維空間：知道

  

  幀中的某個特征在圖像平面的二維位置

  

  ，以及它的深度

  

  ，能夠将該特征投影到三維空間得到坐标

  

  ，即

  

  。該三維空間的坐标系是定義在

  

  錄影機坐标系的；
• 第2步：将三維坐标點旋轉平移到目前幀坐标系下：要将上一幀三維點投影到目前幀

  

  中，需要位姿轉換

  

  ，得到該點在目前幀坐标系中的三維坐标

  

  ；
• 第3步：将目前幀三維坐标點投影回像素系：通過錄影機内參，投影到目前幀

  

  的坐标

  

  ，即

  

  ，完成重投影。

2.稀疏直接法求解位姿

建立殘差損失函數，通過高斯牛頓疊代法不斷疊代最小化殘差損失函數，最小化前一幀和目前幀之間的光度誤差來求得目前幀的（粗略）位姿

殘差函數：

（這裡的1、2、3對應重投影的三步）

位姿求解：

（二）Frame-to-Map

通過第一步的幀間比對能夠得到目前幀相機的位姿，但是這種frame to frame估計位姿的方式不可避免的會帶來累計誤差進而導緻漂移。因而，需要通過已經建立好的地圖模型，和地圖之間進行特征點比對，來對目前幀位姿進一步優化。步驟如下：

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

1.參考LK光流法的特征對齊Feature Alignment來優化特征比對

已知初始估計的粗略位姿

，目标是上一幀的特征塊重投影到目前幀中的，更精确的像素位置

。

可以通過單獨優化每個patch的2D位置，來最小化目前幀與之前關鍵幀ri的相應的patch（藍色）之間的光度誤差

通過上一步獲得的投影過來的藍色方框塊并不完全準确，是以将關鍵幀中的特征patch，和上一步擷取的patch進行比較優化，優化目前幀的特征位置，使其更加準确。灰色的特征塊才是真實位置，幸好他們偏差不大，可以根據最小化光度誤差構造殘差目标函數。和上面直接法類似，不過優化變量不再是相機位姿，而是像素的位置(u′,v′)。基于光度不變性假設，特征塊在以前參考幀中的亮度應該和new frame中的亮度差不多。重新構造一個殘差，通過疊代對特征預測位置進行優化。

優化的目标函數是仿射變換下的灰階差：

注意，光度誤差的前一部分是目前圖像中的亮度值，後一部分不是

而是

，即它是根據投影的3d點追溯到的這個3d點所在的key frame中的像素值，而不是相鄰幀。因為地圖點初次被觀測到的圖像與目前幀的圖像進行比對時，這個3d點所在的key frame可能離目前幀new frame比較遠，并且可能存在旋轉，是以不能單純地假設圖像塊不變。（關鍵幀的選取規則：目前幀距離上一個關鍵幀的平移量超過場景深度的12%。）是以光度誤差和前面不一樣的是還加了一個仿射變換Ai，需要對key frame幀中的特征塊進行旋轉拉伸之類仿射變換後才能和目前幀的特征塊對比。實作當中可能還需要注意一些細節。例如有些地方使用了網格，以保證觀測點的均勻分布。還有Affine Warp當中需要注意特征點所在的金字塔層數，等等。

2.特征點法BA優化（根據像素位置差優化重投影關系）

在一開始的直接法比對中，我們是使用的光度誤差，這裡由于LK優化後的特征位置和之前重投影預測到特征位置存在差異，現在能根據位置差來構造新的優化目标函數。此時是将地圖中的3D點投影到目前幀。

已知上一步建立起的更精确的地圖點投射到目前幀的3D-2D的特征位置(pi,ui)，目标有兩個：

1. 先是隻優化世界坐标系下相機（目前幀）的位姿

   

   (Motion-only BA)；
2. 之後再優化三維點的坐标(x,y,z)(Structure-only BA)，跟蹤過程就結束了。

對錄影機的pose和3Dpoint分别進行優化，目的是使上一步特征對齊過程中的重投影誤差最小化

優化相機姿态

來最小化重投影誤差：

Motion-only BA：上式中誤差變成了像素重投影以後位置的差異（不是像素值的差異），優化變量還是相機位姿，雅克比矩陣大小為2×6(橫縱坐标u，v分别對六個李代數變量求導)。這一步是就叫做motion-only Bundler Adjustment。

Structure-only BA：同時根據根據這個誤差定義，我們還能夠對擷取的三維點的坐标(x,y,z)進行優化，還是上面的誤差像素位置誤差形式，隻不過優化變量變成三維點的坐标，這一步叫Structure -only Bundler Adjustment，優化過程中雅克比矩陣大小為2×3(橫縱坐标u，v分别對點坐标(x,y,z)變量求導)。

三、Mapping部分：深度估計

（一）對極幾何

三維空間中一點P，投影到兩個不同的平面，投影點分别為

，

極線限制就是說同一個點在兩幅圖像上的映射後，兩極線上點的對應關系

• 基線：直線

  

  

  為基線。
• 極點epipole

  

  ：右相機坐标原點

  

  在左像平面上的像。
• 極點

  

  ：左相機坐标原點

  

  在右像平面上的像。
• 極平面 epipolar plane：由兩個相機坐标原點

  

  、

  

  和物點P組成的平面（也可說是

  

  ，

  

  和P組成的平面）。以基線為軸的平面束叫做對極平面束，任何包含基線的平面都稱為對極平面。
• 級線：極平面與兩個像平面的交線。S與左平面的交線L1（圖中虛線）過

  

  、

  

  點，稱之為對應于

  

  的極線；同理S與右平面的交線稱之為對應于

  

  的極線L2（對應于左邊圖像點的極線在右邊圖像上，右邊與之相同）。
• 級線限制：限制了兩極線上點的對應關系。極線限制是指點

  

  一定在對應于

  

  的極線L2上，點

  

  一定在對應于

  

  的極線L1上。

點

，

，

、

，P是5點共面的

（二）Mapping總架構

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

• 如果進來一個關鍵幀，就提取關鍵幀上的新特征點，作為種子點放進一個種子隊列中。（種子seed就是深度未收斂的像素點）
• 如果進來一個普通幀，就用普通幀的資訊，融合更新之前提取的所有種子點的深度值的機率分布。如果某個種子點的深度分布已經收斂，就把它放到地圖中，供追蹤線程使用。

就這樣舊的種子不斷收斂，新的種子不斷增加，不斷進行下去。

（三）深度估計及融合

SVO深度濾波對深度進行漸進式融合（動态融合）。對于給定的一幅參考幀及其姿态

，深度估計要做的是利用一系列的前後幀，完成對指定參考幀上像素深度的求取。

單次比對恢複的深度有誤差，需要依靠多次深度測量值的融合，來找出誤差較小的深度值。多幅後續幀能夠計算出這一個點在r幀的多個深度值，相當于對同一狀态變量進行了多次測量，可以用貝葉斯估計來對多個測量值進行融合，每個後續幀都用于更新貝葉斯架構中的機率分布（圖中深淺綠色的分布圖），使得估計的不确定性減少。當分布的方差足夠小時，使用

将深度估計值轉換為三維點，将該點插入地圖并立即用于運動估計。

1.每一幀的深度估計方法：極線特征比對+三角測量

跟蹤部分的比對，是靠特征對齊完成的，不需要極線搜尋，這是因為在深度和位姿都比較準确的情況下，用特征對齊可以完成比對。但是，深度估計卻不能這樣幹，它需要極線搜尋，因為深度未知或者深度不确定性太大。是以，上一個關鍵幀的特征點在這一個關鍵幀上找比對點的方法，是用極線搜尋，尋找亮度差最小的點，最後再用深度濾波器把這個地圖點準确地濾出來。

1. 初始化：第一幀的初始化方法為假設前兩個關鍵幀所拍到的特征點在一個平面上(四軸飛行器對地面進行拍攝)，然後估計單應性H矩陣，并通過三角化來估計初始特征點的深度值。後續每個關鍵幀的初始化方法為，每個一個參考的關鍵幀r都與一個深度濾波器相關聯，初始化時具有很大的深度不确定性，它的平均值設定為參考幀r中的平均場景深度。
2. 極線計算：對于每一個新圖像幀

   

   ，與參考patch

   

   相關性最高的patch

   

   總是位于新圖像

   

   的極線（藍綠色線）上。是以，從

   

   幀和穿過

   

   的線之間的相對姿态關系來構造對極平面，計算出

   

   對應的極線。
3. 極線搜尋特征比對：确定了特征出現的極線段位置，就可以在極線上搜尋最佳的特征比對了。如果極線段很短，小于兩個像素，那直接使用上面求位姿時提到的Feature Alignment光流法就可以比較準确地預測特征位置。如果極線段很長，那分兩步走，第一步在極線段上間隔采樣，對采樣的多個特征塊一 一和參考幀中的特征塊比對，用Zero mean Sum of Squared Differences 方法對各采樣特征塊評分，哪個得分最高，說明他和參考幀中的特征塊最比對。第二步就是在這個得分最高點附近使用Feature Alignment得到次像素精度的特征點位置。
4. 三角測量恢複深度：種子以及種子在目前幀上的比對像素點的位置确定了，就可以三角化計算深度了，即通過三角測量找到

   

   對應的深度估計

   

   。

此圖示意了r坐标系中對特征i的深度di的機率估計diˆ。方法：将真實深度處的點di投影到兩幅圖像中得出相應的圖像區域(藍色方塊)，然後根據與ui相關度最高的ui'點估計出的三角化深度dik~來更新深度估計值。由于種子點的深度不确定，它在新圖像幀中看起來就在一條直線（極線）上，即，相關性最高的點ui'總是位于ui對應的極線上。

對于深度估計的不确定性的計算：多次三角測量的深度是為了融合得到種子較準确的深度。那麼既然有融合，不同測量值肯定有不同權重。不确定性就是用來計算權重的。在SVO中，深度的不确定被認為是，在比對時，誤比對一個像素所帶來的最大深度誤差。REMODE對由于特征定位不準導緻的三角化深度誤差進行了分析。（REMODE實作了在SVO定位基礎上的單目稠密建圖（需要GPU），關鍵幀上選取像素點SVO是fast角點，REMODE是梯度點）

2.多幀深度值融合

SVO的融合是不斷利用最新時刻深度的觀測值，來融合上一時刻深度最優值，直至深度收斂。

用高斯均勻混合模型分布（Gaussian + Uniform）對測量值

進行模組化。極線上的點對目前深度的估計值，良好的深度測量值服從高斯分布在真實值

附近，假設會有一定的機率出現錯誤的深度估計值，錯誤值則均勻分布在

（這是叫做Beta分布麼？）。這個機率模型就是一個高斯分布加上一個設定在最小深度dmin和最大深度dmax之間的均勻分布：

得到一個新的深度估計值以後，用貝葉斯機率模型對深度值遞歸更新。

參考

高博大神 https://www.zhihu.com/question/39904950

https://blog.csdn.net/u011178262/article/details/90142834