**
SVO半直接視覺裡程計 學習與歸納
**
前段時間在一篇文章中看到了關于“單目視覺慣性vio算法的比較與分析”
在最後的結論中提到，svo的算法具有很高的計算效率，并且svo算法具有較強的可拓展性，如下圖所示，svo+gstam可以達到僅次于vins 的測量精度，同時還能保持很高的計算速度。
可以考慮将svo算法與其他算法結合進一步提高精度，可用作組合導航的前端部分。
是以近期，對svo從結構算法到程式實作，進行了較為詳細的學習，對于一些不是很懂的地方也做了标注
svo采用了基于直接法的跟蹤方法，根據灰階不變假設，通最小化像素點的灰階值誤差，以實作位姿與三維空間點的優化。差別于特征點法，svo雖然也提取了特征點，但不需要進行描述子的計算，這能節省很大一部分的計算量，在SVO的實作中，混合使用了特征點與直接法：它跟蹤了一些關鍵點（FAST角點）使用直接法，根據這些關鍵點周圍區塊（4*4）的資訊，估計相機運動以及關鍵點的位置 。由于svo僅需要對圖像中的特征塊進行追蹤，不像傳統直接法那樣需要對圖像中的所有像素點進行跟蹤，是以svo的方法也被稱為稀疏直接法，特征塊提供運動跟蹤所需要的梯度 減少了很多不必要的計算，一定程度上提高了算法效率。
1. svo算法架構
整個架構可以分為追蹤和建圖兩大塊
-上半部分主要完成對目前幀的位姿估計：
- 對比目前幀與上一幀，擷取粗略的位姿估計
- 根據粗略的位姿，将目前幀與地圖進行比對，求得精确的位姿并對可觀測到的地圖點進行優化。
-下半部分為建圖部分：
- 主要是對特征點的深度進行估計；
- “深度濾波器”：利用新來的幀資訊去更新特征點的深度分布。當某點的深度收斂時，将其作為新的地圖點，加入地圖，用于追蹤環節。
2. motion estimation thread
運動估計線程通過三個步驟，實作對位姿的精确估計以及地圖點的優化：
￭1 通過目前幀與上一幀的共視關系：最小化 重投影的光度誤差 來優化位姿變量
￭2 結合目前幀與地圖中的共視點：追溯到特征點在最近一個關鍵幀中的像素值，經過仿射變換，最小化特征點光度誤差優化像素位置
￭3 Pose and Structure Refinement：利用優化後的特征點位置與預測值的位置差，反過來進一步優化相機位姿和特征點的三維空間坐标（x，y，z）
通過最小化相鄰幀之間特征塊的重投影光度誤差，可以實作對相機位姿的初步估計，但由于初始位姿（以上一時刻的位姿作為初值）以及特征點深度都是不确定度很高的值，僅可以實作對位姿的粗略估計
- 在下一步驟，可通過建立好的地圖來進一步限制目前幀的位置
在目前幀Ik中，可以觀察到p1-p4地圖點，如果在之前的關鍵幀Ir2、Ir1中也能看到同樣的地圖點，則關鍵幀與目前幀之間即存在共視關系。
對每個共視的地圖點，選擇與目前幀夾角最小的關鍵幀作為參考幀。
由于是目前幀與關鍵幀的特征塊進行對比，并且3d點所在的關鍵幀可能與目前幀距離較遠，是以需要對KF幀中的特征塊進行旋轉拉伸的仿射變（Affine Warp）之後才能和目前幀的特征塊進行對比。當上式收斂後，即完成目前幀與關鍵幀上的共視點的特征比對，優化的變量為目前幀下的特征點的像素坐标。
比對完成後，得到目前幀上所有優化後的新的特征點 u’ ，這些點分别儲存了各自的資訊：包含像素坐标、圖像層數，并指向對應的共視地圖點
- 在第三步中，利用上一步得到的更加精确的特征點位置u’反過來進一步優化相機位姿和特征點的三維空間坐标
這一步驟包含了兩個部分：
motion-only Bundle Adjustment
structure-only Bundle Adjustment
兩個變量的優化分開進行，構造的誤差函數相同，僅優化變量不同
3. mapping thread
通過上半部分的motion estimation thread，我們可以得到相機運動的姿态估計，以及三維空間地圖點的世界坐标，但是由于單目相機的尺度不确定性，要想恢複地圖點在真實場景中的位置，需要對有效深度進行測量
建圖線程主要工作是計算地圖點的深度，由深度濾波器depth-filter完成，将所有具有深度的地圖點資訊結合拼在一起就構成了環境的三維地圖
depth-filter工作方式：
1.提取關鍵幀上的特征點，每個特征點都對應一個不确定度很高的深度估計初值
2.對後續進來的普通幀，利用可觀測的共視點資訊，更新種子點的深度機率分布
3.若種子點的深度分布已收斂，放入地圖，供追蹤線程使用
3.1初始化種子點
——用以建圖的點都是來自于關鍵幀上的特征點
svo僅在這一步用到了特征提取的方法：對關鍵幀上的點進行特征提取，提取fast角點，并用shi-Tomasi的角點計算方法求值，保留超過門檻值的點作為挑選出來的特征點。
——将所有新選的特征點作為深度待估計的種子點seed
對seed點進行初始化，用高斯分布表示逆深度，深度範圍取目前幀最小深度的倒數，高斯分布的标準差取1/6*depth_min,初值取目前幀的平均深度
3.2極線搜尋尋找seed的比對點
——初始化的seed深度具有極大地不确定性，随着後續不斷添加進入新的普通幀，新的普通幀中同樣可以觀測到部分seed點，對于關鍵幀中深度還不确定的seed點，通過目前幀與關鍵幀之間的位姿變換，将seed點深度射線上最短min到最長max範圍的深度 映射到目前幀的機關深度平面上，進而得到機關平面上的極線線段（？？？這裡不太明白機關平面的具體作用）
——在極線上搜尋目前幀中與關鍵幀seed的最佳比對點，使二者最相似（即 特征塊的光度差最小），進行比對時，同樣要對seed所在的特征塊進行仿射變換後，再與目前幀下的圖塊進行對比。
——在極線上确定了最佳比對的圖塊後，将這兩個比對點通過三角測量計算seed的深度值
3.3更新種子點
——使用深度濾波器，将最新時刻求得的深度觀測值，與上一時刻的深度估計值融合，直到seed深度收斂。
深度測量的模型可了解為高斯+均勻分布的混合模型
有效的深度測量值在真實深度值附近呈現高斯分布，
離群測量值在最大最小深度區間内服從均勻分布，并且 有效測量的機率 與 真實深度值 是獨立分布的
在一些csdn中有關于深度估計機率模型的具體推導過程，不夠有點沒太看明白？？？？？？？？？？？？？？？？？？
（尤其是怎麼從均勻分布變到Beta分布的那部分，麻煩哪位同學有空幫我解答一下！！！！！）
總之，這個很厲害的深度濾波器能夠在每加入一個新的深度測量值之後，就迅速調整機率分布，疊代出一個最佳的深度估計值，直到深度的方差收斂到給定的門檻值，或者是該seed點被判斷為離群點發散，才停止更新。
得到收斂的深度值之後，結合seed點的三維空間坐标，将其作為地圖點，儲存到地圖中。
這就是建圖線程大緻要做的工作。
小結
部落格是依據我個人對svo的了解并且結合已有的一些解析svo的部落格内容寫的，對svo的大緻工作流程都已提及，有些細節部分的内容，還需繼續學習。可能會存在一些了解不對或有誤的地方，歡迎指出。
後續工作
關于後續學習方向，我想在深入了解svo的各個子產品的工作機理的前提下，通過某些方法将imu的測量資料與svo的視覺裡程計結合起來，實作更加魯棒的視覺慣性測量。
個人認為svo是一個很好的組合導航的基礎算法，可以在其基礎上不斷添加新的内容