一、視覺里程計(VisualOdometry)介紹

目前，有不止一種方式可以確定移動機器人的軌跡，這裡將重點強調“視覺里程計”這種方法。在這種方法中，單個相機或者雙目相機被用到，其目的是為了重構出機器人6-DOF的軌跡。

視覺里程計通過分析一系列影象序列，來確定機器人的朝向和位置。

二、單目(monocular)與雙目(stereo)視覺里程計比較

雙目VO的優勢在於，能夠精確的估計運動軌跡，且具有確切的物理單位。在單目VO中，你僅僅只知道物體在x/y方向上移動了1個單位，而雙目VO則可明確知道是移動了1cm。但是，對於距離很遠的物體，雙目系統則退化成為單目系統，如果說我們的機器人很小，那麼雙目看起來似乎與單目無異，所以現在也有很多人只關注、研究單目VO

三、雙目視覺里程計數學原理

Input:左右相機影象序列，時刻t和t+1時採集的影象表示為I^t和I^t+1，相機內參檔案已提前標定好。

Output:針對不同幀影象對，需要估算出兩幀影象之間的旋轉矩陣R和平移向量T。

演算法提綱：

1.捕捉影象對I^t，I^t+1；

2.對以上影象進行畸變校準；

3.計算時刻t和t+1的視差圖；

4.使用FAST演算法檢測影象I^t中的特徵，並與影象I^t+1中的特徵匹配；

5.利用深度圖，計算上一步中匹配特徵點的3D座標值，此時，獲得兩組點雲資料；

6.選取點雲資料中的一部分子集，使得所有的匹配都是互相相容的

7.估算上一步檢測中的內點(inliers)。

Undistortion,Rectification

在計算深度圖之前，為了補償鏡頭帶來的畸變，通過相機標定以及校準等過程，可以得到去畸變的影象對，影象對通過校準後，同一目標在左右影象中的水平位置是對齊的。

Stereoimages overlayed from KITTI dataset, notice the feature matches arealong parallel (horizontal) lines

DisparityMap Computation

關於深度圖重建方法此處不贅述，前面開發的SGBM與BM演算法皆能夠較好實現深度圖重建。

Adisparity map computed on frames from KITTI VO dataset

FeatureDetection

使用高效的FAST特徵代替SIFT做特徵檢測。假設需要檢測一個點是不是角點，那麼我們就在這個畫素附近畫一個圓，判斷圓周上面的16個畫素點的強度，如果有連續的一組畫素的強度大於原始畫素點的強度，另外有一組連續的畫素點強度小於原始畫素點的強度，那麼，可標記該原始點為角點。一種啟發式的方法用來拒絕非角點，只需通過判斷1,5,9,13畫素強度，至少有3個點的強度大於或小於原始畫素值，那麼就有可能是角點。

Image from the original FAST feature detection paper

為了整幅圖上都有特徵表達，選取一種“桶”特徵的方式對全圖進行表示，例如，將整幅影象分割成100x100pixel大小的網格，在每個網格塊中，獲取至少20個以上特徵，這樣整幅影象中就能夠維持一個比較均勻的特徵分佈。

Feature Description andMatching

使用KLT方法跟蹤上一步計算得到的角點特徵，通過尋找每個角點進行跟蹤。

Triangulation of 3D PointCloud

同一目標點的萬維座標計算也不必多說，利用標定引數檔案，重投影矩陣Q即可計算結果。

The Inlier Detection Step

該演算法不同於其他演算法的是，不存在異常值檢測步驟，但是存在正常值檢測步驟。從原始的點雲資料中，希望能夠選取出最大的一個子集合，裡面所有的點都是一致的，那麼這個問題等價於最大團問題

Computation of rotation matrixR and translation vector T

目前最常用的求解R,T的思路是先用RANSAC方法求出內點的集合，即把誤匹配的外點(離群點)篩除，然後對內點的集合，採用線性最小二乘法去求得R,T的初值，代入初值並利用L-M非線性優化方法求得經過優化後的RT，作為最終值。