視覺里程計（Visual Odometry）

在機器人學與計算機視覺領域，視覺里程計是一個通過分析相關圖像序列，來確定機器人位置和朝向的過程。

在導航系統中，里程計（odometry）是一種利用致動器的移動數據來估算機器人位置隨時間改變量的方法。例如，測量輪子轉動的旋轉編碼器設備。里程計總是會遇到精度問題，例如輪子的打滑就會導致產生機器人移動的距離與輪子的旋轉圈數不一致的問題。當機器人在不光滑的表面運動時，誤差是由多種因素混合產生的。由于誤差隨時間的累積，導致了里程計的讀數隨著時間的增加，而變得越來越不可靠。

視覺里程計是一種利用連續的圖像序列來估計機器人移動距離的方法。視覺里程計增強了機器人在任何表面以任何方式移動時的導航精度。

視覺里程計算法：

大多數現有的視覺里程計算法都是基于以下幾個步驟：

1、圖像獲取：單目照相機、雙目照相機或者全向照相機；

2、圖像校正：使用一些圖像處理技術來去除透鏡畸變；

3、特征檢測：確定感興趣的描述符，在幀與幀之間匹配特征并構建光流場；

（1）、使用相關性來度量兩幅圖像間的一致性，并不進行長時間的特征跟蹤；

（2）、特征提取、匹配（Lucas–Kanade method）；

（3）、構建光流場；

4、檢查光流場向量是否存在潛在的跟蹤誤差，移除外點；

5、由光流場估計照相機的運動；

（1）、可選方法1：使用卡爾曼濾波進行狀態估計；

（2）、可選方法2：查找特征的幾何與3D屬性，以最小化基于相鄰兩幀之間的重投影誤差的罰函數值。這可以通過數學上的最小化方法或隨機采樣方法來完成；

6、周期性的重定位跟蹤點；

視覺里程計算法（基本知識）：

大多數現有的視覺里程計算法都是基于以下幾個步驟：

1、圖像獲取：單目照相機、雙目照相機或者全向照相機；

2、圖像校正：使用一些圖像處理技術來去除透鏡畸變；

3、特征檢測：確定感興趣的描述符，在幀與幀之間匹配特征并構建光流場；

（1）、使用相關性來度量兩幅圖像間的一致性，并不進行長時間的特征跟蹤；

（2）、特征提取、匹配（Lucas–Kanade method）；

（3）、構建光流場；

4、檢查光流場向量是否存在潛在的跟蹤誤差，移除外點；

5、由光流場估計照相機的運動；

（1）、可選方法1：使用卡爾曼濾波進行狀態估計；

（2）、可選方法2：查找特征的幾何與3D屬性，以最小化基于相鄰兩幀之間的重投影誤差的罰函數值。這可以通過數學上的最小化方法或隨機采樣方法來完成；

6、周期性的重定位跟蹤點；

我選擇的視覺里程計算法是：“ sift特征匹配點——基本矩陣——R和T”。

第一步：由特征點計算基本矩陣F。

一般而言，sift點是存在誤匹配的情況，因此，采用ransac魯棒方法計算基本矩陣F。這個過程已經實現，但是還有一個小問題：同樣的一組sift點，進行兩次基本矩陣計算，得到的基本矩陣差異很大，因此，我在ransac方法的基礎上，根據得到的inliers點，采用常規的8點基本矩陣計算方法，這樣得到的基本矩陣能保持不變

第二步：由基本矩陣計算R和T

方法1：奇異值分解

E = KK'*F*KK; %%這是真實的本質矩陣E

[U,S,V] = svd(E); %奇異值分解。
T_nonscale = U(:,3); %% 不含有刻度因子的平移向量
D= [0 1 0 ;-1 0 0; 0 0 1];
Ra = U*D*V';或者Rb = U*D'*V';

方法2：非線性優化解迭代求解。我嘗試了三種不同的目標函數形式：

% RT_from_E_ydf.m; 物理意義不明顯！精度和速度都不如后面的好。
% RT_point_constraints_ydf.m 速度折中，精度較高
% RT_point_constraints_ydf02.m 速度最快，精度不夠高

發現的問題：特征點的誤匹配問題。

誤匹配對位姿解算結果影響分析：我們將手動選取的匹配點加上一個噪聲后（1-2個像素的噪聲），位姿誤差很大，也就是說，要得到精確的位姿解算結果，噪聲不能大于1個像素，即需要考慮亞像素級別的特征點匹配！！實驗表明，用sift特征是一種比較合理的方法，但是，sift特征容易出現誤匹配點的情況，將其進行剔除是很有意義的一件事情。

目前我采用的是手動選擇匹配好的區域，從而選擇比較好的匹配點，見hand_choose_sift_ydf.m。結果如下：

下一步研究sift特征點的自動選擇算法。

一般而言，針對匹配點的魯棒算法有M-estimators等，我這里選擇的是最小中值法（least-median-squares）。程序見“RT_from_siftpoint_ydf03.m”，該方法運算量非常大，需要進行163（以保證能得到一組正確的樣本，假設sift誤匹配率為40%的情況下）組優化求解計算。Very time consuming

疑問，為什么橫向選取特征點的時候，結算結果會出現很大的誤差呢？

【橫向選取Sift特征點和縱向選取sift特征點，會有不同。能不能進行分析，得到如下結論：如果關心的是航向角，則sift特征點集合呈豎狀比較好，即sift點集合和旋轉軸平行。

1×8選取特征點

表不同方向手動選取特征點