視覺里程計的詳細(xì)介紹和算法過程

視覺里程計（Visual Odometry）

在機器人學(xué)與計算機視覺領(lǐng)域，視覺里程計是一個通過分析相關(guān)圖像序列，來確定機器人位置和朝向的過程。

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

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

視覺里程計算法：

大多數(shù)現(xiàn)有的視覺里程計算法都是基于以下幾個步驟：

1、圖像獲?。簡文空障鄼C、雙目照相機或者全向照相機；

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

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

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

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

（3）、構(gòu)建光流場；

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

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

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

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

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

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

大多數(shù)現(xiàn)有的視覺里程計算法都是基于以下幾個步驟：

1、圖像獲?。簡文空障鄼C、雙目照相機或者全向照相機；

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

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

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

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

（3）、構(gòu)建光流場；

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

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

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

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

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

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

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

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

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

方法1：奇異值分解

E = KK'*F*KK; %%這是真實的本質(zhì)矩陣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：非線性優(yōu)化解迭代求解。我嘗試了三種不同的目標(biāo)函數(shù)形式：

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

發(fā)現(xiàn)的問題：特征點的誤匹配問題。

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

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

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

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

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

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

1×8選取特征點

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