視覺(jué)里程計(jì)（Visual Odometry）

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

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

視覺(jué)里程計(jì)是一種利用連續(xù)的圖像序列來(lái)估計(jì)機(jī)器人移動(dòng)距離的方法。視覺(jué)里程計(jì)增強(qiáng)了機(jī)器人在任何表面以任何方式移動(dòng)時(shí)的導(dǎo)航精度。

視覺(jué)里程計(jì)算法：

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

1、圖像獲?。?jiǎn)文空障鄼C(jī)、雙目照相機(jī)或者全向照相機(jī)；

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

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

（1）、使用相關(guān)性來(lái)度量?jī)煞鶊D像間的一致性，并不進(jìn)行長(zhǎng)時(shí)間的特征跟蹤；

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

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

4、檢查光流場(chǎng)向量是否存在潛在的跟蹤誤差，移除外點(diǎn)；

5、由光流場(chǎng)估計(jì)照相機(jī)的運(yùn)動(dòng)；

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

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

6、周期性的重定位跟蹤點(diǎn)；

視覺(jué)里程計(jì)算法（基本知識(shí)）：

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

1、圖像獲取：?jiǎn)文空障鄼C(jī)、雙目照相機(jī)或者全向照相機(jī)；

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

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

（1）、使用相關(guān)性來(lái)度量?jī)煞鶊D像間的一致性，并不進(jìn)行長(zhǎng)時(shí)間的特征跟蹤；

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

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

4、檢查光流場(chǎng)向量是否存在潛在的跟蹤誤差，移除外點(diǎn)；

5、由光流場(chǎng)估計(jì)照相機(jī)的運(yùn)動(dòng)；

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

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

6、周期性的重定位跟蹤點(diǎn)；

我選擇的視覺(jué)里程計(jì)算法是：“ sift特征匹配點(diǎn)——基本矩陣——R和T”。

第一步：由特征點(diǎn)計(jì)算基本矩陣F。

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

第二步：由基本矩陣計(jì)算R和T

方法1：奇異值分解

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

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

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

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

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

下一步研究sift特征點(diǎn)的自動(dòng)選擇算法。

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

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

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

1×8選取特征點(diǎn)

表不同方向手動(dòng)選取特征點(diǎn)