svo： semi-direct visual odometry 半直接視覺裡程計
本博文github位址
svo代碼注釋
SVO代碼分析 較細緻
svo： semi-direct visual odometry 論文解析
SVO原了解析
SVO 代碼筆記
SVO 代碼筆記
SVO 代碼筆記
SVO代碼分析（一）結構
項目首頁
ssvo類似代碼
一步步完善視覺裡程計1——項目架構搭建
1.1為什麼叫半直接法？
我們知道，VSLAM有直接法和特征點法兩大類。直接法和特征點法，在幀間VO階段的不同在于，
直接法：提取梯度紋理特征明顯的像素，幀間VO是靠圖像對齊，即通過
最小化像素灰階差函數來優化幀間位姿。
特征點法：提取特征點（通常是角點），幀間VO靠PNP，即縮小在後幀圖像上，
重投影點與特征比對點距離誤差，來優化幀間位姿。
而SVO是這樣幹的：
提取稀疏特征點（類似特征點法），幀間VO用圖像對齊（類似于直接法），
SVO結合了直接法和特征點法，是以，稱它為半直接法。
SVO主要幹了兩件事，
<1>跟蹤
<2>深度濾波
深度濾波是我們常說的建圖（Mapping）部分。
1.2.1跟蹤部分
跟蹤部分幹的事情是：初始化位姿，估計和優化位姿(分别對應于幀間VO和局部地圖優化)。
初始化位姿：
用KLT光流法找比對，然後恢複H矩陣。初始化思想是這樣的，
第一幀上提取的特征點，作為關鍵幀，後來的幀不斷用KLT與第一幀比對，
直到比對到的特征點平均視差比較大，就認為初始化成功，計算對應特征點的深度，
與此對應的幀作為第二幀。之後進入正常工作模式，即估計和優化位姿。
正常工作模式：
首先，通過和上一幀進行對齊，求取初始位姿；
然後，建立局部地圖，通過優化局部地圖和目前幀的投影灰階塊誤差，來優化目前位姿；
最後，判斷此幀是否是關鍵幀，如果為關鍵幀就提取新的特征點。
經過以上四個步驟，完成一幀的處理。如果在CMakeLists裡打開了HAVE_G2O的選項，
代碼隔一段時間還會BA,不過非常慢。
1.2.2深度濾波部分
深度濾波部分主要任務是完成估計特征點的深度資訊。
深度濾波和跟蹤部分互相依賴，因為深度濾波是以相機位姿已知為前提進行的，
而跟蹤部分又依靠深度濾波的結果（較準确的三維點），完成位姿的估計。
乍一看，這是個雞生蛋，蛋生雞的問題，既然兩者循環疊代，總得有個起始點。
其實，單目的slam在啟動時需要初始化，而這個初始化就提供粗糙的幀間位姿，
以便于深度濾波和跟蹤部分的疊代。
當深度可以用後（稱之為收斂），把它放入地圖裡，用于跟蹤。
1.2.3為什麼叫VO
這個得從SVO幹的事來說，它既沒有閉環檢測，也沒有重定位（SVO的重定位太。。。），
它幹的事隻要是定位,比較符合視覺裡程計(VO)的特點。
ORBSLAM算是目前性能最好的開源算法，這些功能它都有，是以算一個比較完整的VSLAM算法。
1.2.4 svo怎麼樣
優點：是比較快，如果你跑代碼的時候發現很容易跟丢，可以修改這幾個配置參數：
quality_min_fts：比對到的最小特征點。
quality_max_drop_fts：容許相鄰幀比對到特征點的最大落差。
缺點：缺點是比較明顯的
和ORBSLAM相比。
<1>由于位姿的估計和優化全是靠灰階比對，這就導緻了系統對光照的魯棒性不足。
<2>對快速運動魯棒性性不足。直接法都似這個樣子。。可以加入IMU改善。
<3>沒有重定位和閉環檢測功能。
如果把此工程修改成RGBD的模式後，魯棒性和精度明顯提高，近似于ORBSLAM的RGBD模式。
半直接法解析
SVO 從名字來看，是半直接視覺裡程計，
所謂半直接是指通過對圖像中的特征點圖像塊進行直接比對來擷取相機位姿，
而不像直接比對法那樣對整個圖像（根據灰階梯度大小篩選需要比對的點）使用直接比對。
整幅圖像的直接比對法常見于RGBD傳感器，因為RGBD傳感器能擷取整幅圖像的深度。
雖然semi-direct方法使用了特征，但它的思路主要還是通過direct method來擷取位姿，這和特征點法feature-method不一樣。
同時，semi-direct方法和direct method不同的是它利用特征塊的配準來對direct method估計的位姿進行優化。
和正常的單目一樣，SVO算法分成兩部分: 位姿估計，深度估計
直接法
使用圖像中具有灰階梯度大的點 使用極線搜尋(線段比對)獲得比對點對，
參考幀根據深度資訊建立3d點，
按照3d-2d比對，
将3d點按照變換矩陣以及相機内參數投影到目前幀，擷取亞像素灰階值，
和原有的比對點做差，得到灰階內插補點，使用權重LM算法進行優化，得到變換矩陣[R t]。
半直接法
利用特征塊的配準來對direct method估計的位姿進行優化。
特征點法
使用ORB等特征提取方法，确定兩幅圖像中的比對點對(特征點快比對)，對應同一個實體空間中的點
使用 單應矩陣H / 或者本質矩陣F求解 變換矩陣[R t]
2D-2D點三角化 得到對應的 三維點坐标
也可轉化到 相機歸一化平面下的點 x1 x2
p1 = k × [R1 t1] × D k逆 × p1 = [R1 t1] × D x1 = T1 × D x1叉乘x1 = x1叉乘T1 × D = 0
p2 = k × [ R2 t2] × D k逆 × p2 = [R2 t2] × D x2 = T2 × D x2叉乘x2 = x2叉乘T2 × D = 0
式中：x1 = k逆 × p1 ，x2 = k逆 × p2 ， T= [R, t] 已知
可以求解D
D是3維齊次坐标，需要除以第四個尺度因子 歸一化
特征點比對
在講解恢複R,T前，稍微提一下特征點比對的方法。
常見的有如下兩種方式：
1. 計算特征點，然後計算特征描述子，通過描述子來進行比對，優點準确度高，缺點是描述子計算量大。
2. 光流法：在第一幅圖中檢測特征點，使用光流法(Lucas Kanade method)對這些特征點進行跟蹤，
得到這些特征點在第二幅圖像中的位置，得到的位置可能和真實特征點所對應的位置有偏差。
是以通常的做法是對第二幅圖也檢測特征點，如果檢測到的特征點位置和光流法預測的位置靠近，
那就認為這個特征點和第一幅圖中的對應。
在相鄰時刻光照條件幾乎不變的條件下（特别是單目slam的情形），
光流法比對是個不錯的選擇，它不需要計算特征描述子，計算量更小。
單應矩陣H 恢複變換矩陣 R, t
單目視覺slam 基礎幾何知識
p2 = H12 * p1 4對點 A*h = 0 奇異值分解 A 得到 單元矩陣 H ， T = K 逆 * H21*K
展開成矩陣形式：
u2 h1 h2 h3 u1
v2 = h4 h5 h6 * v1
1 h7 h8 h9 1
按矩陣乘法展開：
u2 = (h1*u1 + h2*v1 + h3) /( h7*u1 + h8*v1 + h9)
v2 = (h4*u1 + h5*v1 + h6) /( h7*u1 + h8*v1 + h9)
将分母移到另一邊，兩邊再做減法
-((h4*u1 + h5*v1 + h6) - ( h7*u1*v2 + h8*v1*v2 + h9*v2))=0 式子為0 左側加 - 号不變
h1*u1 + h2*v1 + h3 - ( h7*u1*u2 + h8*v1*u2 + h9*u2)=0
寫成關于 H的矩陣形式：
0 0 0 0 -u1 -v1 -1 u1*v2 v1*v2 v2
u1 v1 1 0 0 0 0 -u1*u2 -v1*u2 -u2 * (h1 h2 h3 h4 h5 h6 h7 h8 h9)轉置 = 0
h1~h9 9個變量一個尺度因子，相當于8個自由變量
一對點 2個限制
4對點 8個限制 求解8個變量
A*h = 0 奇異值分解 A 得到 單元矩陣 H
cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// 奇異值分解
H = vt.row(8).reshape(0, 3);// v的最後一列
單應矩陣恢複 旋轉矩陣 R 和平移向量t
p2 = H21 * p1 = H21 * KP
p2 = K( RP + t) = KTP = H21 * KP
T = K 逆 * H21*K
本質矩陣F求解 變換矩陣[R t] p2轉置 * F * p1 = 0
基本矩陣的獲得
空間點 P 兩相機 像素點對 p1 p2 兩相機 歸一化平面上的點對 x1 x2 與P點對應
p1 = KP
p2 = K( RP + t)
x1 = K逆* p1 = P
x2 = K逆* p2 = ( RP + t) = R * x1 + t
消去t(同一個變量和自己叉乘得到0向量)
t 叉乘 x2 = t 叉乘 R * x1
再消去等式右邊
x2轉置 * t 叉乘 x2 = 0 = x2轉置 * t 叉乘 R * x1
得到 ：
x2轉置 * t 叉乘 R * x1 = x2轉置 * E * x1 = 0 本質矩陣
也可以寫成：
p2轉置 * K 轉置逆 * t 叉乘 R * K逆 * p1 = p2轉置 * F * p1 = 0 基本矩陣
幾何知識參考
對極幾何原理：
兩個錄影機的光心C0、C1，三維空間中一點P，在兩幅圖像中的位置為p0、p1（相當于上面的 x1, x2）。
像素點 u1，u2
p0 = inv(K) * u0
p1 = inv(K) * u1
x0 (u0x - cx) / fx
p0 = y0 = (u0y - cy) / fy
1 1
x1 (u1x - cx) / fx
p1 = y1 = (u1y - cy) / fy
1 1
如下圖所示：
由于C0、C1、P三點共面，得到：
這時，由共面得到的向量方程可寫成：
p0 *(t 叉乘 R * p1)
其中，
t是兩個錄影機光心的平移量;
R是從坐标系C1到坐标系C0的旋轉變換，
p1左乘旋轉矩陣R的目的是把向量p1從C1坐标系下旋轉到C0坐标系下(統一表示)。
一個向量a叉乘一個向量b，
可以表示為一個反對稱矩陣乘以向量b的形式，
這時由向量a=(a1,a2,a3) 表示的反對稱矩陣(skew symmetric matrix)如下:
0 -a3 a2
ax = a3 0 -a1
-a2 a1 0
是以把括号去掉的話，p0 需要變成 1*3的行向量形式
才可以與 3*3的 反對稱矩陣相乘
p0轉置 * t叉乘 * R * P1
我們把 t叉乘 * R = E 寫成
p0轉置 * E * P1
本征矩陣E的性質：
一個3x3的矩陣是本征矩陣的充要條件是對它奇異值分解後，
它有兩個相等的奇異值，并且第三個奇異值為0。
牢記這個性質，它在實際求解本征矩陣時有很重要的意義 .
計算本征矩陣E、尺度scale的來由:
将上述矩陣相乘的形式拆開得到 ：
上面這個方程左邊進行任意縮放都不會影響方程的解：
(x0x1 x0y1 x0 y0x1 y0y1 y0 x1 y1 1)*E33(E11/E33 ... E32/E33 1) = 0
是以E雖然有9個未知數，但是有一個變量E33可以看做是縮放因子，
是以實際隻有8個未知量，這裡就是尺度scale的來由，後面會進一步分析這個尺度。
AX=0，x有8個未知量，需要A的秩等于8，是以至少需要8對比對點,應為有可能有兩個限制是可以變成一個的，
點可能在同一條線上，或者所有點在同一個面上的情況，這時候就存在多解,得到的值可能不對。
對矩陣A進行奇異值SVD分解，可以得到A
p2轉置 * F * p1 = 0 8點對8個限制求解得到F
* f1 f2 f3 u1
* (u2 v2 1) * f4 f5 f6 * v1 = 0
* f7 f8 f9 1
按照矩陣乘法展開：
a1 = f1*u2 + f4*v2 + f7;
b1 = f2*u2 + f5*v2 + f8;
c1 = f3*u2 + f6*v2 + f9;
得到：
a1*u1+ b1*v1 + c1= 0
展開：
f1*u2*u1 + f2*u2*v1 + f3*u2 + f4*v2*u1 + f5*v2*v1 + f6*v2 + f7*u1 + f8*v1 + f9*1 = 0
寫成矩陣形式：
[u1*u2 v1*u2 u2 u1*v2 v1*v2 v2 u1 v1 1]*[f1 f2 f3 f4 f5 f6 f7 f8 f9]轉置 = 0
f 9個變量，1個尺度因子，相當于8個變量
一個點對，得到一個限制方程
需要8個點對，得到8個限制方程，來求解8個變量
A*f = 0
是以F雖然有9個未知數，但是有一個變量f9可以看做是縮放因子，
是以實際隻有8個未知量，這裡就是尺度scale的來由，後面會進一步分析這個尺度。
上面這個方程的解就是矩陣A進行SVD分解A=UΣV轉置 後，V矩陣是最右邊那一列的值f。
另外如果這些比對點都在一個平面上那就會出現A的秩小于8的情況，這時會出現多解，會讓你計算的E/F可能是錯誤的。
A * f = 0 求 f
奇異值分解F 基礎矩陣 且其秩為2
需要再奇異值分解 後 取對角矩陣 秩為2 後在合成F
cv::Mat u,w,vt;
cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// A = w * u * vt
cv::Mat Fpre = vt.row(8).reshape(0, 3);// F 基礎矩陣的秩為2 需要在分解 後 取對角矩陣 秩為2 在合成F
cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
w.at<float>(2)=0;// 基礎矩陣的秩為2，重要的限制條件
F = u * cv::Mat::diag(w) * vt;// 在合成F
從基本矩陣恢複 旋轉矩陣R 和 平移向量t
F = K轉置逆 * E * K逆
本質矩陣 E = K轉置 * F * K = t 叉乘 R
從本質矩陣恢複 旋轉矩陣R 和 平移向量t
恢複時有四種假設 并驗證得到其中一個可行的解
本征矩陣的性質：
一個3x3的矩陣是本征矩陣的充要條件是對它奇異值分解後，
它有兩個相等的奇異值，
并且第三個奇異值為0。
牢記這個性質，它在實際求解本征矩陣時有很重要的意義。
計算本征矩陣E的八點法，大家也可以去看看wiki的詳細說明
本征矩陣E的八點法
有本質矩陣E 恢複 R,t
從R,T的計算公式中可以看到R,T都有兩種情況，
組合起來R,T有4種組合方式。
由于一組R,T就決定了錄影機光心坐标系C的位姿，
是以選擇正确R、T的方式就是，把所有特征點的深度計算出來，
看深度值是不是都大于0，深度都大于0的那組R,T就是正确的。
以上的解法 在 orbslam2中有很好的代碼解法
orbslam2 單目初始化
尺度問題
IMU+視覺消除尺度漂移問題
這個圖簡單明了的示範了這種平移縮放作用。
從圖中也可以看出，由于尺度scale的關系，
不同尺度的F得到不同的t，決定了以後計算點P的深度也是不同的，
是以恢複的物體深度也是跟尺度scale有關的，
這就是論文中常說的結構恢複structure reconstruction，
隻是恢複了物體的結構架構，而不是實際意義的物體尺寸。并
且要十分注意，每兩對圖像計算E并恢複R,T時，他們的尺度都不是一樣的，
本來是同一點，在不同尺寸下，深度不一樣了，這時候地圖map它最頭痛了，是以這個尺度需要統一。
那麼如何讓scale之間統一呢？如果你一直采用這種2d-2d比對計算位姿的方式，那每次計算的t都是在不同尺度下的，
一種方法使得相鄰位姿間的不同的尺度s經過縮放進行統一。
我們已經知道出現尺度不一緻是由于每次都是用這種計算本征矩陣的方式，而尺度就是在計算E時産生的。
是以尺度統一的另一種思路就是後續的位姿估計我不用這種2d-2d計算本征E的方式了。
也就說你通過最開始的兩幀圖像計算E恢複了R,T，
并通過三角法計算出了深度，那我就有了場景點的3D坐标，
後續的視訊序列就可以通過3Dto2d(opencv裡的solvePnp)來進行位姿估計
，這樣後續的視訊序列就不用計算本征矩陣進而繞過了尺度，
是以後續的視訊序列的位姿和最開始的兩幀的尺度是一樣的了。
但是，這樣計算又會産生新的問題–scale drift尺度漂移。
因為，兩幀間的位姿總會出現誤差，這些誤差積累以後，
使得尺度不再統一了，
如下圖所示：
随着相機位姿誤差的積累，地圖中的四個點在第二幀的位置相對于第一幀中來說像是縮小了一樣。
位姿誤差累計導緻尺度漂移這一點，對照上面講尺度不确定問題時的那個圖就很容易了解。
關于如何糾正這個scale drift的問題很多單目slam裡都提到了，是以這裡不再深入。
Visual SLAM Tutorial
Features特征, Tracking跟蹤, Essential Matrix本質矩陣, and RANSAC随機采樣序列一緻性
Stereo Visual Odometry 雙目視覺裡程計
BundleAdjustmen 集束優化 李群 李代數
DealingWithScale 尺度問題
Incremental 濾波器優化 非線性最小二乘優化等
LoopClosing閉環檢測 移動裝置VSLAM
大尺度地圖高效建構
稠密VO
ptam svo 光流
Dense mapping: KinectFusion and DTAM
Kintinuous: Reconstruction of an Apartment ICP+RGBD
SLAM++
和正常的單目一樣，SVO算法分成兩部分: 位姿估計，深度估計
1. 位姿估計 motion estimation
svo 方法中motion estimation的步驟可以簡單概括如下:
a) 相鄰幀 2d-2d--->3d-2d比對，最小化重投影誤差計算R，t
對稀疏的特征塊使用direct method 配準，擷取相機位姿；
SE3跟蹤詳見lsd_slam中的方法。
參考幀2d點由逆深度得到3d點，進行3d-2d點像素誤差比對，3d點反投影到目前幀像素平面，和2d點的灰階誤差
使用誤差權重LM算法優化得到位姿。
b) 3d點反向追蹤，擷取參考關鍵幀像素位置--->放射變換----> 目前幀像素位置內插補點--->優化特征塊在 目前幀中的位置
優化 3d-2d特征塊比對關系
通過擷取的 位姿預測參考幀 中 的特征塊在 目前幀中的位置，
由于深度估計的不準導緻擷取的位姿也存在偏差，進而使得預測的特征塊位置不準。
由于預測的特征塊位置和真實位置很近，是以可以使用牛頓疊代法對這個特征塊的預測位置進行優化。
c) 使用優化後的 3d-2d特征塊比對關系， 再次使用直接法， 來對相機位姿(pose)以及特征點位置（structure）進行反向優化。
特征塊的預測位置得到優化，說明之前使用直接法預測的有問題。
利用這個優化後的特征塊預測位置，再次使用直接法，
對相機位姿(pose)以及特征點位置（structure）進行優化。
a) 使用直接法 最小化圖像塊重投影殘差 來擷取位姿。 sparse model-based image alignment
南國楓葉 SVO深度解析(二)之跟蹤部分
如圖所示：其中紅色的Tk,k−1為位姿，即優化變量
其過程如下，
1. 找到前幀（K-1）看到的地圖點p1,p2,p3，對應着2d像素點。
2. 使用變換關系投影至後幀（k）二維圖像上，也對應着2d點。
3. 然後最小化灰階誤差函數（這是一個最優化過程，又稱圖像對齊），得到位姿，over。
直接法具體過程如下：
step1. 準備工作。
假設目前相鄰幀之間的位姿Tk,k−1的初始值已知，
一般初始化為上一相鄰時刻的位姿或者假設為機關矩陣[I,0]。
通過之前多幀之間的特征檢測以及深度估計，
我們已經知道 第k-1幀 (參考幀)中 特征點位置 以及 它們的 深度(一般為逆深度)。
step2. 重投影。
擷取參考幀3d點：
知道Ik−1中的某個特征點在圖像平面的位置(u,v)，
以及它的深度dd，能夠将該特征投影到三維空間pk−1 = dd * k逆 * (u,v,1)，
該三維空間的坐标系是定義在Ik−1錄影機坐标系的。
投影到 目前幀2維像素平面，計算亞像素灰階值：
是以，我們要将它投影到目前幀Ik中，需要位姿轉換Tk,k−1=[R, t]，
得到該點在目前幀坐标系中的三維坐标pk= T*pk−1 = R*pk−1 + t
最後通過錄影機内參數，投影到Ik的圖像平面(u′,v′) = k * pk，
得到的是浮點數坐标，需要根據周圍的4個點按照距離權重得到亞像素灰階值。
完成重投影。
step3. 疊代優化更新位姿 。
按理來說對于空間中同一個點，被極短時間内的相鄰兩幀拍到，它的亮度值應該沒啥變化。
使用參考幀特征點(u,v)灰階值 - 投影點(u′,v′) 在目前幀亞像素灰階值 得到誤差。
但由于位姿是假設的一個值，是以重投影的點不準确，導緻投影前後的亮度值是不相等的。
不斷優化位姿使得這個殘差最小，就能得到優化後的位姿Tk,k−1。
這裡使用 權重LM優化算法得到 位姿Tk,k−1。
将上述過程公式化如下：通過不斷優化位姿Tk,k−1Tk,k−1最小化殘差損失函數。
其中,2d->3d->3d->2d
1. 公式中第一步為根據參考幀圖像特征點位置和深度逆投影到三維空間,
pk−1 = dd * k逆 * (u,v,1)
2. 第二步将三維坐标點旋轉平移到目前幀坐标系下，
pk= T*pk−1 = R*pk−1 + t
3. 第三步再将三維坐标點投影回目前幀圖像坐标。
(u′,v′) = k * pk，
這裡得到是浮點數坐标，
需要根據周圍的4個點按照距離權重得到亞像素灰階值。
當然在優化過程中，殘差的計算方式不止這一種形式：
有前向(forwards)，
逆向(inverse)之分，
并且還有疊加式(additive)和
構造式(compositional)之分。
這方面可以讀讀光流法方面的論文，
Baker的大作《Lucas-Kanade 20 Years On: A Unifying Framework》。
選擇的方式不同，在疊代優化過程中計算雅克比矩陣的時候就有差别，一般為了減小計算量，
都采用的是inverse compositional algorithm(逆向組成算法)。
上述最小化的誤差方程式非凸的，時非線性最小化二乘問題，
可以用高斯牛頓疊代法GN求解,通常會使用期更新版LM列文伯格馬爾誇克算法，
再者為了減小外點的影響，會根據投影誤差的大小确定誤差的權重，使用權重LM算法優化求解。
位姿的疊代增量ξξ(李代數)可以通過下述方程計算：
J轉置 * J * se3 = -J * err
其中雅克比矩陣J 為圖像殘差對李代數的求導，可以通過鍊式求導得到：
這中間最複雜的部分是位姿矩陣對李代數的求導。
很多文獻都有提到過，比如DTAM作者Newcombe的博士論文，
gtsam的作者Dellaert的數學筆記。
不在這裡展開(有兩篇部落格的篇幅)，
可以參看清華大學王京的李代數筆記。
到這裡，我們已經能夠估計位姿了，但是這個位姿肯定不是完美的。
導緻重投影預測的特征點在Ik中的位置并不和真正的吻合，也就是還會有殘差的存在。
如下圖所示：
圖中灰色的特征塊為真實位置，藍色特征塊為預測位置。
幸好，他們偏差不大，可以構造殘差目标函數，
和上面直接法類似，不過優化變量不再是相機位姿，
而是像素的位置(u′,v′)(u′,v′)，通過疊代對特征塊的預測位置進行優化。
這就是svo中提到的Feature Alignment。
最小二乘優化算法簡介 L-K算法（或稱光流法）本質上是一種基于梯度下降的優化方法。
模闆 I 和 T
位置x 變換關系W 變換參數R
誤差函數 E = SUM(I(W(x,R)) - T(x))^2
= SUM(I(W(x,R+detR)) - T(x))^2
求解R使得誤差函數E最小：
首先 要是E最小，需要使得其導數E'=0可以得到E的局部最小值
對E進行一階泰勒展開
E= SUM(I(W(x,R)) + E'*detR - T(x))^2
再求導 E'= 2*SUM(E'*(I(W(x,R)) + E'*detR - T(x))) = 0
得到 ：
detR = - (E'轉置*E')逆 *E'轉置* (I(W(x,R)) - T(x))
= - H逆 * J * err ， H逆 = (J轉置 * J)逆
H* detR = -J * err
這裡也可以加入一個權重，根據err的大小計算的一個權重w
H* w* detR = -J * w* err
寫成線性方程組形式：
A* detR = b
可使用多種 矩陣分解方法求解該線性方程組，得到 更新變量 detR
se3下的 detR 指數映射到SE3 通過 李群乘法直接 對 變換矩陣R 左乘更新
把H=J轉置J叫作Hessian矩陣，它是對J求僞逆過程的一個副産品，和表達二階偏導的Hessian陣不同。
不過H的作用很重要，它能反映圖像資料的統計特性。
如果不幸H是奇異的，也就是說無法求逆矩陣，
那麼說明這個圖像模闆包含的各個梯度方向的資訊不足以進行跟蹤應用。
J逆 = (J轉置*J)逆 * J轉置 = J逆 * (J轉置)逆 * J轉置 = J逆
(J轉置*J)逆 * J轉置 又稱為 僞逆
前向加性算法
I <---- T
T - I(W(Tx, R)) 像素誤差
計算 I(W(Tx, R))中各個點在I中的梯度 DetI
T中各個像素點的坐标對應到 ----->I的梯度圖像 DetI 中各個點的坐标
逆向組成算法
b) 最小二乘優化特征塊的預測位置 更新3d-2d特征塊比對關系 Relaxation Through Feature Alignment
通過第一步的幀間比對能夠得到目前幀相機的位姿，
但是這種frame to frame估計位姿的方式不可避免的會帶來累計誤差進而導緻漂移。
是以，應該通過已經建立好的地圖模型，來進一步限制目前幀的位姿。
地圖模型通常來說儲存的就是三維空間點，
因為每一個Key frame通過深度估計能夠得到特征點的三維坐标，
這些三維坐标點通過特征點在Key Frame中進行儲存。
是以SVO地圖上儲存的是Key Frame
以及還未插入地圖的KF中的已經收斂的3d點坐标（這些3d點坐标是在世界坐标系下的），
也就是說地圖map不需要自己管理所有的3d點，它隻需要管理KF就行了。
當新的幀new frame和 相鄰關鍵幀KF的平移量超過場景深度平均值的12%時(比如四軸上升，機器人向前運動)，
new frame就會被當做KF，它會被立即插入地圖。
同時，又在這個新的KF上檢測新的特征點作為深度估計的seed，
這些seed會不斷融合新的new frame進行深度估計。
但是，如果有些seed點3d點位姿通過深度估計已經收斂了，怎麼辦？
map用一個point_candidates來儲存這些尚未插入地圖中的點。
是以map這個資料結構中儲存了兩樣東西，以前的KF 以及新的尚未插入地圖的KF 中已經收斂的3d點。
通過地圖我們儲存了很多三維空間點，很明顯，每一個new frame都是可能看到地圖中的某些點的。
由于new frame的位姿通過上一步的直接法已經計算出來了，
按理來說這些被看到的地圖上的點可以被投影到這個new frame中，即圖中的藍色方框塊。
上圖中分析了，所有位姿誤差導緻這個方框塊在new frame中肯定不是真正的特征塊所處的位置。
是以需要Feature Alignment來找到地圖中特征塊在new frame中應該出現的位置，
根據這個位置誤差為進一步的優化做準備。
基于光度不變性假設，特征塊在以前參考幀中的亮度(灰階值)應該和new frame中的亮度差不多。
是以可以重新構造一個殘差，對特征預測位置進行優化。
注意這裡的優化變量是像素位置，這過程就是 光流法跟蹤,
并且注意，
光度誤差的前一部分是目前圖像中的亮度值 Ik，
後一部分不是前一個參考幀 Ik−1 而是Ir（3d點所在的關鍵幀），
即它是根據投影的3d點追溯到的這個3d點所在的key frame中的像素值，而不是相鄰幀。
由于是特征塊對比并且3d點所在的KF可能離目前幀new frame比較遠(觀察的角度變換了，觀察的形狀不一樣了)，
是以光度誤差和前面不一樣的是還加了一個仿射變換，
需要對KF幀中的特征塊進行旋轉拉伸之類仿射變換後才能和目前幀的特征塊對比。
這時候的疊代量計算方程和之前是一樣的，隻不過雅克比矩陣變了，這裡的雅克比矩陣很好計算：
J=[∂r/∂u′
∂r/∂v′] = [∂I(u′,v′)/∂u′
∂I(u′,v′)/∂v′]
這不就是圖像橫縱兩個方向的梯度嘛.
通過這一步我們能夠得到優化後的特征點預測位置，
它比之前通過相機位姿預測的位置更準，
是以反過來，我們利用這個優化後的特征位置，
能夠進一步去優化相機位姿以及特征點的三維坐标。
是以位姿估計的最後一步就是Pose and Structure Refinement。
c) 使用優化後的 3d-2d特征塊比對關系， 再次使用直接法， 來對相機位姿(pose)以及特征點位置（structure）進行反向優化。 Pose and Structure Refinement
在一開始的直接法比對中，我們是使用的光度誤差(灰階誤差)，
這裡由于優化後的特征位置 和 之前預測的特征位置存在差異，這個能用來構造新的優化目标函數。
注意上式中誤差變成了像素重投影以後位置的差異（不是像素值的差異），
優化變量還是相機位姿(r,p,y,tx,ty,tz)，雅克比矩陣大小為2×6(橫縱坐标u，v，分别對六個李代數變量求導)。
這一步是就叫做motion-only Bundler Adjustment。
同時根據根據這個誤差定義，我們還能夠對擷取的三維點的坐标(x,y,z)進行優化，
還是上面的像素位置誤差形式，
隻不過優化變量變成三維點的坐标，這一步叫Structure -only Bundler Adjustment，
優化過程中雅克比矩陣大小為2×3(橫縱坐标u，v分别對點坐标(x,y,z)變量求導)。
2. 深度估計 depth估計
最基本的深度估計就是三角化，
這是多視角幾何的基礎内容(可以參看聖經Hartly的《Multiple View Geometry in Computer Vision》
中的第十二章structure computation.
用單應變換矩陣H或者本質矩陣E 求解得到相鄰兩幀的變換矩陣R,t後就可是使用類似雙目的
三角測距原理來得到深度。
上圖中的實體實體中的點 P =[X,Y,Z,1]
在兩相機歸一化平面下的點 x1 x2 [x,y,z]
在兩相機像素平面上的點 p1, p2，比對點對 [u,v,1]
p1 = k × [R1 t1] × P
左乘 k逆 × p1 = [R1 t1] × P
得到 x1 = T1 × P
計算 x1叉乘x1 = x1叉乘T1 × P = 0
這裡 T1 = [I, 0 0 0] 為機關矩陣。
p2 = k × [R2 t2] × P
左乘 k逆 × p2 = [R2 t2] × P
得到 x2 = T2 × P
消去 x2叉乘x2 = x2叉乘T2 × P = 0
式中：
x1 = k逆 × p1 ，
x2 = k逆 × p2 ，
T2= [R, t]為幀1變換到幀2的 已知
得到兩個方程
x1叉乘T1 × P = 0
x2叉乘T2 × P = 0
寫成矩陣形式 A*P = 0
A = [x1叉乘T1; x2叉乘T2]
這又是一個要用最小二乘求解的線性方程方程組 ,和求本征矩陣一樣，
計算矩陣A的SVD分解，然後奇異值最小的那個奇異向量就是三維坐标P的解。
P是3維齊次坐标，需要除以第四個尺度因子 歸一化.
[U,D,V] = svd(A);
P = V[:,4] 最後一列
P = P/P(4)；// 歸一化
以上也是由本質矩陣E = t 叉乘 R 恢複變換矩陣R,t的時候，有四種情況，但是隻有一種是正确的。
而判斷正确的标準，就是按照這個R,t 計算出來的深度值都是正值，因為相機正前方為Z軸正方向。
三角法求深度（triangulation）
我們知道通過兩幀圖像的比對點就可以計算出這一點的深度值，
如果有多幅比對的圖像，那就能計算出這一點的多個深度值。
一幅圖像對應另外n幅圖像，可以看作為n個深度傳感器，
把得到點的深度資訊的問題看作是有噪聲的多傳感器資料融合的問題，
使用Gaussian Uniform mixture model(高斯均值混合模型)來對這一問題進行模組化，
假設好的測量值符合正态分布，好的測量值的比例為pi,
噪聲符合均勻分布，噪聲比例為1-pi 使用貝葉斯方法不斷更新這個模型，
讓點的深度（也就是正态分布中的均值參數）和好的測量值的比例pi收斂到真實值。
這就像對同一個狀态變量我們進行了多次測量，
是以，可以用貝葉斯估計來對多個測量值進行融合，
使得估計的不确定性縮小。
如下圖所示：
一開始深度估計的不确定性較大(淺綠色部分)，
通過三角化得到一個深度估計值以後，
能夠極大的縮小這個不确定性(墨綠色部分)。
極線搜尋比對點三角化計算深度 機率方法分析圖像點的深度資訊
深度估計的思路論文 Video-based, Real-Time Multi View Stereo
深度估計的思路 參考部落格
在這裡，先簡單介紹下svo中的三角化計算深度的過程，主要是極線搜尋确定比對點。
在參考幀Ir中，我們知道了一個特征的圖像位置ui，假設它的深度值在[dmin,dmax]之間，
那麼根據這兩個端點深度值，我們能夠計算出他們在目前幀Ik中的位置，
如上圖中草綠色圓圈中的藍色線段。
确定了特征出現的極線段位置，就可以進行特征搜尋比對了。
如果極線段很短，小于兩個像素，
那直接使用上面求位姿時提到的Feature Alignment光流法就可以比較準确地預測特征位置。
如果極線段很長，那分兩步走，第一步在極線段上間隔采樣，
對采樣的多個特征塊一一和參考幀中的特征塊比對，
用Zero mean Sum of Squared Differences （零均值差平方和）方法對各采樣特征塊評分，
哪個得分最高，說明他和參考幀中的特征塊最比對。
第二步就是在這個得分最高點附近使用Feature Alignment得到次像素精度的特征點位置。
像素點位置确定了，就可以三角化計算深度了。
得到一個新的深度估計值以後，用貝葉斯機率模型對深度值更新。
在LSD slam中，假設深度估計值服從高斯分布，
用卡爾曼濾波(貝葉斯的一種)來更新深度值。
這種假設中，他認為深度估計值效果很棒，
很大的機率出現在真實值(高斯分布均值)附近。
而SVO的作者采用的是Vogiatzis的
論文《Video-based, real-time multi-view stereo》提到的機率模型：
一幅圖像中的像素點為中心産生一個patch，
在另一幅圖像對應的極線上搜尋patch，
使用NCC（NormalizedCrossCorrelation 歸一化的交叉相關性）來分析兩個patch間的相似程度，
NCC（NormalizedCrossCorrelation 歸一化的交叉相關性部落格參考
圖像總大小M x N，mxn表示帶比對的視窗大小，
這樣的計算複雜度就為O(m x n x M x N)。
其中f為其中一幅圖像上的視窗
r為另一幅圖像上的視窗
ur，uf為對應序列槽内像素的均值
通過積分圖像建立起來視窗下面的待檢測圖像與模闆圖像
的和
與平方和
以及他們的交叉乘積
五個積分圖索引之後，這樣就完成了整個預計算生成。
依靠索引表查找計算結果，
NCC就可以實作線性時間的複雜度計算，
而且時間消耗近似常量跟視窗半徑大小無關，
完全可以滿足實時對象檢測工業環境工作條件。
在深度[X(min),X(max)]區間内進行搜尋比對，求的，
這個區間内NCC的計算結果會産生一系列局部極值。
把上述計算過程看作是一個深度傳感器的計算過程，
其輸出結果為一系列NCC的局部極值對應的點的深度資訊。
它們是有噪聲的資料。
這個機率模型是一個 高斯分布 加上一個設定
在最小深度dmin和最大深度dmax之間的 均勻分布。
這個均勻分布的意義是假設會有一定的機率出現錯誤的深度估計值。
其中pi π 表示x為有效測量(高斯分布)的機率,而(1-pi)為噪聲出現錯誤的機率
一幅圖像對應另外n幅圖像，可以看作為n個深度傳感器，
把得到點的深度資訊的問題看作是有噪聲的多傳感器資料融合的問題，
使用Gaussian Uniform mixture model
（高斯均勻混合模型,GUMM,另外GMM高斯混合模型）來對這一問題進行模組化，
假設好的測量值符合正态分布，
噪聲符合均勻分布，
好的測量值的比例為pi，
使用貝葉斯方法不斷更新這個模型，
讓點的深度（也就是正态分布中的均值參數）和好的測量值的比例pi收斂到真實值。
實際實作的時候一個seed對應與圖像中一個像素，這個像素是想要求得其深度資訊的點。
一個seed還關聯五個參數，
其中四個參數是模型中的參數:
1、好的測量值和
2、壞的測量值的個數，
3、深度估計中高斯分布的均值和
4、方差。
最後一個參數是以這個像素點為中心的一個正方形patch，
計算這個patch和其他圖像中的對應極線上的patch的NCC值，
此處因為計算量的關系不會搜尋整個極線，隻會搜尋以目前模型中高斯分布的均值（深度）
對應的極線上的像素點為中心的一個固定範圍。
同時，有關這個機率模型遞推更新的過程具體可以看Vogiatzis在論文中
提到的Supplementary material，論文中告知了下載下傳位址。
知道了這個貝葉斯機率模型的遞推過程，程式就可以實作深度值的融合了，
結合supplementary material去看svo代碼中的updateSeeds(frame)這個程式就容易了，
整個程式裡的那些參數的計算遞歸過程的推導，我簡單截個圖，
這部分我也沒細看(公式19是錯誤的，svo作者指出了)，
現在有幾篇部落格對該部分進行了推導.
每次新進一幀，依照這樣的過程：
1. 産生新的seed（seed的數目固定，上次疊代結束時會去掉一些seed）
2. 對每個seed進行更新：
在此幀上進行相關seed的對極搜尋；
檢測極線上點對應patch與seed中的patch的NCC分數的局部極大值(越大越相似)；
用這些局部最大值更新seed中的四個參數。
3. 去掉那些好的測量值的個數低于門檻值的，
如果好的測量值的個數高于門檻值而且方差低于一個門檻值，
把這個seed對應的點的三位坐标算出來并加到地圖中。
svo的Supplementary matterial 推導過程
深度估計 濾波2 解析
在深度估計的過程中，除了計算深度值外，這個深度值的不确定性(權重值)也是需要計算的，
它在很多地方都會用到，如極線搜尋中确定極線的起始位置和長度，
如用貝葉斯機率更新深度的過程中用它來确定更新權重(就像卡爾曼濾波中協方差矩陣扮演的角色)，
如判斷這個深度點是否收斂了，如果收斂就插入地圖等等。
SVO的作者Forster作為第二作者發表的
《REMODE: Probabilistic, Monocular Dense Reconstruction in Real Time》
中對由于特征定位不準導緻的三角化深度誤差進行了分析，
如下圖：
最後，簡單說下SVO的初始化過程：
它假設前兩個關鍵幀所拍到的特征點在一個平面上(四軸飛行棋對地面進行拍攝)，
1. 然後估計單應性H矩陣，p2 = H * p1 ,恢複R，t矩陣
2. 并通過三角化來估計初始特征點的深度值。
SVO初始化時triangulation的方法具體代碼是vikit/math_utils.cpp裡的triangulateFeatureNonLin()函數，
使用的是中點法，關于這個三角化代碼算法的推導見github issue。
還有就是SVO适用于攝像頭垂直向下的情況（也就是無人機上,垂直向上也可以,朝着一面牆也可以），
為什麼呢？
1.初始化的時候假設的是平面模型
2.KF的選擇是個極大的限制，除了KF的選擇原因外攝像頭水準朝前運動的時候，
SVO中的深度濾波做的不好，這個讨論可以看看github issue，
然而在我的測試中，不知道修改了哪些參數，稍微改動了部分代碼，發現前向運動，
并且對着非平面SVO也是很溜的。
同時我也對svo的代碼加了一些中文注釋，後續會放到github上，希望幫助大家加快了解svo。最
後，祝大家好運，一起分享知識。
SVO代碼
SVO也是将一幀圖像處理成多層圖像金字塔形式，跟蹤過程，也就是通過灰階殘差最小來算相機位姿(直接法)，
這部分都在FrameHandlerMono類的addImage方法中，addImage方法完成初始化過程和相機位姿跟蹤的部分，
跟蹤部分中存在四個系統狀态：
第一幀關鍵幀 (處理第一幀到第一個關鍵幀之間的，fast角點特征比對，單應變換求取位姿變換)，
第二幀關鍵幀 (光流法金字塔多尺度跟蹤上一幀，單應性矩陣求取位姿變換，局部BA優化，深度值濾波器濾波)，
其他幀 (上幀位姿初始值，直接法最小化重投影誤差優化變換矩陣，比對點,3d點，深度值濾波器濾波)，
重定位模式（relocalizing 跟蹤上一幀的參考幀 ），
一二幀是初始化（算單應矩陣分解求位姿），
其他幀就是有了初始地圖之後的跟蹤過程，具體函數為FrameHandlerMono類的processFrame函數，
這個函數中的跟蹤過程與論文中基本一樣，
也包括幾個步驟：
圖像配準（alignment）求位姿，
優化特征點的二維坐标，
同時優化位姿和特征點坐标（BA）。
建圖部分的主要函數在depthfilter類中，流程是首先在ros節點調用的時候會調用這個類的startThread函數，
新開線程，線程裡運作depthfiler類中的 updateSeedsLoop方法，
大緻過程也就是等tracking部分向一個vector或者list中填充它選擇的關鍵幀，
如果有，就進行機率化建圖，這個部分還沒仔細看。
對應SVO論文中跟蹤部分的三個過程，
一是通過目前幀和上一幀的特征點對應的patch之間的灰階殘差最小來求目前幀的位姿（即SE(3) T），
一個函數完成，這是一個粗略估計，有了這個粗略估計就可以進行下一步：
把地圖中已有的在目前幀可見的特征點重投影到目前幀，一個重投影函數完成，
這個重投影過程完成之後會分析重投影的品質，也就是比對到的特征點對的個數，
當點對太少的時候就會直接把目前幀的位姿設定為上一幀的位姿，并報錯比對特征點過少，
這也是我常遇到的問題。
最後是優化兩個部分：
一是之前粗略估計出的T，這一步叫做 pose優化；
二是投影到目前幀的點在世界坐标系下的坐标點，這一步叫做structure優化。
最後是可選的BA算法同時對位姿和三維點坐标進行優化。
以下分析均在rpg_svo檔案夾下：
first：在svo_ros/src中vo_node檔案中包含了了節點類vo_node的相關聲明，
并包含一個main函數
vo_node.cpp的main中:
1.包含一些ros節點的基本操作，在vo_node的構造函數中，
執行個體化了一個FrameHandlerMono的對象vo_，并運作vo_->start
2.使用subscribe(cam_topic, 5, &svo::VoNode::imgCb, &vo_node)，
每次獲得圖像時使用VoNode中的imgCb方法處理
3.在imgCb方法中，運作了FrameHandlerMono對象vo_的成員函數addImage，
此方法中其他的部分是關于顯示的，即Visualizer類的一些操作。
3.當ros::ok()為真，且vo_node.quit_為假時，循環ros::spinOnce()
----------------------------------------------------------------------------------------------
FrameHandlerMono類繼承自FrameHandlerBase類：
1.其構造函數中運作FrameHandlerMono.initialize()
2.initialize()函數中執行個體化DepthFilter類的一個對象，
運作此對象的startThread()函數，此函數使用boost的thread方法建立了一個線程，
并在此線程中運作DepthFilter.updateSeedsLoop函數。
3.在updateSeedsLoop函數中，
會檢查frame_queue_隊列中是否有FramePtr存在，
FramePtr在global.h中定義為boost::shared_ptr
====frame_handler_mono.cpp =======
processFirstFrame(); // 作用是處理第1幀并将其設定為關鍵幀；
processSecondFrame();// 作用是處理第1幀後面所有幀，直至找到一個新的關鍵幀；
processFrame(); // 作用是處理兩個關鍵幀之後的所有幀；
relocalizeFrame(=); //作用是在相關位置重定位幀以提供關鍵幀
startFrameProcessingCommon(timestamp) 設定處理第一幀 stage_ = STAGE_FIRST_FRAME;
processFirstFrame(); 處理第一幀，直到找打第一個關鍵幀==============================================
1. fast角點特征，單應變換求取位姿變換，特點點數超過100個點才設定第一幀為為關鍵幀，
2. 計算單應性矩陣（根據前兩個關鍵幀）來初始化位姿，3d點
3. 且設定系統處理标志為第二幀 stage_ = STAGE_SECOND_FRAME
processSecondFrame(); 處理第一幀關鍵幀到第二幀關鍵幀之間的所有幀(跟蹤上一幀) ========================
1. 光流法 金字塔多尺度 跟蹤關鍵點,根據跟蹤的數量和門檻值，做跟蹤成功/失敗的判斷
2. 前後兩幀計算單應性矩陣，根據重投影誤差記錄内點數量，根據門檻值，判斷跟蹤 成功/失敗,計算3d點
3. 集束調整優化, 非線性最小二乘優化,
4. 計算場景深度均值和最小值
5. 深度濾波器對深度進行濾波，高斯均值混合模型進行更新深度值
6. 設定系統狀态 stage_= STAGE_DEFAULT_FRAME；
processFrame(); 處理前兩個關鍵幀後的所有幀(跟蹤上一幀) ===================================
1. 設定初始位姿, 上幀的位姿初始化為目前幀的位姿
2. 直接法 最小化 3d-2d 重投影 像素誤差 求解位姿 LM算法優化位姿)
3. 最小化 3d-2d 重投影像素誤差 優化特征塊的預測位置 更新3d-2d特征塊比對關系
4. 使用優化後的 3d-2d特征塊比對關系， 類似直接法，最小化2d位置誤差，來優化相機位姿(pose)
5. 使用優化後的 3d-2d特征塊比對關系， 類似直接法，最小化2d位置誤差，來優化3D點(structure)
6. 深度值濾波
relocalizeFrame(); 重定位模式（跟蹤參考幀）=======================================================
1. 得到上一幀附近的關鍵幀作為參考幀 ref_keyframe
2. 進行直接法 最小化 3d-2d 重投影 像素誤差 求解目前幀的位姿
3. 若跟蹤品質較高(比對特征點數大于30) 将參考幀設定為上一幀
4. 直接法跟蹤上一幀參考幀進行processFrame()處理( 其實就是跟蹤參考幀模式 )
5. 如果跟蹤成功就設定為相應的跟蹤後的位姿，否者設定為參考幀的位姿
項目代碼結構
rpg_svo
├── rqt_svo 為與 顯示界面 有關的功能插件
├── svo 主程式檔案，編譯 svo_ros 時需要
│ ├── include
│ │ └── svo
│ │ ├── bundle_adjustment.h 光束法平差（圖優化）
│ │ ├── config.h SVO的全局配置
│ │ ├── depth_filter.h 像素深度估計（基于機率） 高斯均值混合模型
│ │ ├── feature_alignment.h 特征比對
│ │ ├── feature_detection.h 特征檢測 faster角點
│ │ ├── feature.h（無對應cpp） 特征定義
│ │ ├── frame.h frame定義
│ │ ├── frame_handler_base.h 視覺前端基礎類
│ │ ├── frame_handler_mono.h 單目視覺前端原理(較重要)==============================
│ │ ├── global.h（無對應cpp） 有關全局的一些配置
│ │ ├── initialization.h 單目初始化
│ │ ├── map.h 地圖的生成與管理
│ │ ├── matcher.h 重投影比對與極線搜尋
│ │ ├── point.h 3D點的定義
│ │ ├── pose_optimizer.h 圖優化（光束法平差最優化重投影誤差）
│ │ ├── reprojector.h 重投影
│ │ └── sparse_img_align.h 直接法優化位姿（最小化光度誤差）
│ ├── src
│ │ ├── bundle_adjustment.cpp
│ │ ├── config.cpp
│ │ ├── depth_filter.cpp
│ │ ├── feature_alignment.cpp
│ │ ├── feature_detection.cpp
│ │ ├── frame.cpp
│ │ ├── frame_handler_base.cpp
│ │ ├── frame_handler_mono.cpp
│ │ ├── initialization.cpp
│ │ ├── map.cpp
│ │ ├── matcher.cpp
│ │ ├── point.cpp
│ │ ├── pose_optimizer.cpp
│ │ ├── reprojector.cpp
│ │ └── sparse_img_align.cpp
├── svo_analysis 未知
├── svo_msgs 一些配置檔案，編譯 svo_ros 時需要
└── svo_ros 為與ros有關的程式，包括 launch 檔案
├── CMakeLists.txt 定義ROS節點并指導rpg_svo的編譯
├── include
│ └── svo_ros
│ └── visualizer.h
├── launch
│ └── test_rig3.launch ROS啟動檔案
├── package.xml
├── param 攝像頭等一些配置檔案
├── rviz_config.rviz Rviz配置檔案（啟動Rviz時調用）
└── src
├── benchmark_node.cpp
├── visualizer.cpp 地圖可視化
└── vo_node.cpp VO主節點=======================================