基于RGB-D相機的視覺里程計設(shè)計

霍智豪，苗艷松

(1.山東大學(xué) 信息科學(xué)與工程學(xué)院，山東青島，266237；2. 魯東大學(xué) 教育科學(xué)學(xué)院，山東煙臺，264025)

0 引言

視覺SLAM在實現(xiàn)過程中，目前公認(rèn)的經(jīng)典框架包含三個主體部分，分別是前端的視覺里程計，后端的非線性優(yōu)化和回環(huán)檢測，如圖1所示 。

圖1 經(jīng)典SLAM框架

在視覺SLAM技術(shù)框架中，傳感器信息是由運動相機采集到的有效幀經(jīng)過預(yù)處理后的可用數(shù)據(jù)；視覺里程計利用這些數(shù)據(jù)估計相機的運動與局部地圖的大致輪廓；后端優(yōu)化對視覺里程計估計的相機位姿進行優(yōu)化，并且接受回環(huán)檢測的重復(fù)信息；本文將會對視覺里程計環(huán)節(jié)進行具體實驗與優(yōu)化。

1 相機模型

■ 1.1 基本相機模型

我們在此次設(shè)計中使用RGB-D 相機，在基本彩色圖像素獲取之外，還可以通過結(jié)構(gòu)光(如Kinect相機)或飛行時間法(Tine-of-Flight，ToF)主動獲取深度信息，彩色圖像素的獲取與深度信息的獲取方式是不同的，我們先來闡述基本的針孔相機模型。

我們得到了相機模型的基本幾何約束：

設(shè)像素坐標(biāo)系下N′點的坐標(biāo)為，在齊次坐標(biāo)系下表示式(1)(2)的約束條件：

我們定義上式中的運算矩陣為K，定義為相機的內(nèi)參矩陣，完整的相機模型[2]敘述為：

■ 1.2 畸變矯正

引入與中心距離有關(guān)的高次多項式函數(shù)進行糾正，得到三維空間點最終映射到像素平面的完整模型：

r表示與中心坐標(biāo)的距離。

■ 1.3 深度信息提取

我們通過結(jié)構(gòu)光(如Kinect相機)或飛行時間法(Tine-of-Flight，ToF)主動獲取深度信息。

圖3 結(jié)構(gòu)光

結(jié)構(gòu)光法基于光學(xué)三角測量原理，根據(jù)攝像頭采集到的光信號變化來計算物體的位置與深度。飛行時間法是計算接收到紅外光的相位差，進而還原出深度信息。

2 特征點提取與匹配

我們將會使用ORB[3](Oriented Fast and Rotate BRIEF)算法進行特征點的處理并提出優(yōu)化算法。

■ 2.1 FAST特征點提取與改進

我們遍歷灰度圖像中的像素點，并將其與圓周上的16個像素點進行像素值的比較，篩選出連續(xù)差值超過規(guī)定閾值的點，作為FAST角點[4]：

我們判斷特征點就是對N進行比較。

圖4 FAST特征點提取

圖5 改進的特征點提取

■ 2.2 FAST特征點的優(yōu)化

我們通過構(gòu)建圖像金字塔來來賦予特征點“尺度不變性”，優(yōu)化后的特征點是每幀縮放圖像中的特征點集。我們通過灰度質(zhì)心法使特征點具有方向：

另外，我們提出將圖像劃分網(wǎng)格的改進思想，共隨機抽取若干個網(wǎng)格進行ORB處理，保證得到的有效特征點既可以滿足匹配的需要，又可以提高運算效率，滿足實時性的要求。

■ 2.3 BRIEF描述子的改進

定義操作Q比較點對之間的灰度值：

BRIEF描述子就是Q值的二進制排列[5]。

我們提出改進的描述子選擇方法，以關(guān)鍵點和特征點的質(zhì)心連線為X軸建立二維取點坐標(biāo)系。

■ 2.4 特征點的匹配

我們在兩幀連續(xù)圖像的點集Qt，Qt+1中計算每兩個特征點的漢明距離：

漢明距離小于指定閾值的特征點是匹配的特征點，這種方法是暴力匹配[6]。

我們提出隨機一致性采樣(RANSAC,Random Sample Consensus)優(yōu)化算法，對匹配之后的點對進行再次篩選。

定義齊次坐標(biāo)下的計算M和S：

我們計算誤差來保留正確匹配和剔除錯誤匹配。

圖6 RANSAC改進的匹配

3 運動的估計

我們對ICP算法提出改進，在全局上描述兩幀之間相機的運動：在使用RANSAC估計[7]的變換矩陣T進行變換之后，計算實際點集與估計點集誤差，由其最小值計算出精確的相機運動參數(shù)。

圖7 優(yōu)化匹配

我們定義該誤差平均數(shù)e：

每一次迭代中，使用變換矩陣nT的估計值去新源數(shù)據(jù)集np中的每一個點：

重新計算新的源數(shù)據(jù)集 1np+與目標(biāo)數(shù)據(jù)集Q之間的誤差ne，當(dāng)滿足設(shè)定的閾值時，結(jié)束迭代，并將此時的nT作為相機運動的最優(yōu)解。

圖8 相機運動的ICP優(yōu)化

4 系統(tǒng)測試

■ 4.1 相機內(nèi)參的標(biāo)定

我們使用張氏標(biāo)定法[8]，對所使用的的相機進行參數(shù)的標(biāo)定。

圖9，圖10分別是相機坐標(biāo)系和世界坐標(biāo)系的視圖。

圖9 相機坐標(biāo)系

圖10 世界坐標(biāo)系

最終標(biāo)定得到的相機內(nèi)參：

對標(biāo)定得到的相機內(nèi)參矩陣K進行誤差分析可知，每個角點的像素誤差均不超過±0.5，標(biāo)定所得的內(nèi)參正確。

■ 4.2 數(shù)據(jù)集測試

4.2.1 測試walking.xyz數(shù)據(jù)集

該場景中，相機的運動主要是平移和較小的旋轉(zhuǎn)，干擾項是視頻中兩位走動的研究者（見圖11）。

圖11 特征點提取

在相機的連續(xù)拍攝畫面中，特征點的匹配完成度較好，大部分幀場景下不會出現(xiàn)特征點丟失的現(xiàn)象。但是因為研究者的走動，過程中出現(xiàn)了追蹤失敗的現(xiàn)象（見圖2）。

圖2 針孔相機模型

使用數(shù)據(jù)集包含的evaluate_ate工具對本算法的位姿估計值進行誤差分析，如圖13~14所示。

圖13 ATE

ATE誤差的值較小， RPE誤差卻可以保持在1以內(nèi)，說明本算法可能會存在地圖的誤差，但是相機的運動估計比較準(zhǔn)確。

4.2.2 測試sitting_static數(shù)據(jù)集

在該測試場景中，兩位研究者不運動，相機也沒有較大幅度的運動，特征點提取壓力相對較小。

由圖15~16中的實驗現(xiàn)象，我們發(fā)現(xiàn)，在相機運動幅度很小并且視頻中畫面相對變化不大的情況下，本算法的卷對誤差與相對誤差都較小，說明本算法在寬容條件中的準(zhǔn)確性較高。

圖12 追蹤失敗

圖14 RPE

圖15 ATE

圖16 RPE

4.2.3 測試walking_halfsphere數(shù)據(jù)集

該場景中，相機的運動主要是大幅度的旋轉(zhuǎn)和較小的平移，同樣兩位研究者的移動會給特征點的提取帶來壓力。

測試的結(jié)果為；RPE的值偏大，可知本算法對旋轉(zhuǎn)情況下的估計性能略差與平移情況下的性能，我們需要對運動的估計做出優(yōu)化。

4.2.4 測試walking_static數(shù)據(jù)集

在該場景中，相機的位姿基本固定，畫面中兩位研究者不斷運動，給特征點的匹配帶來壓力。

該項測試的結(jié)果量誤差都是較小的值，可知在相機運動幅度較小的情況下，本算法的誤差較小。

比較四個數(shù)據(jù)集測試得到的均方根誤差、均值誤差、中值誤差和標(biāo)準(zhǔn)差，如表1所示。

表1 測試誤差比較

從表格數(shù)據(jù)可以看出，當(dāng)相機與拍攝的圖像變化不大時，該算法誤差較小，但是當(dāng)相機運動較大特別是在旋轉(zhuǎn)場景下時，該算法的誤差較大。

■ 4.3 真實場景測試

如圖17所示，在光影變化顯顯著的場景下，以灰度值為特征的ORB算法不穩(wěn)定甚至失效。

圖17 特征點匹配失敗

但是，當(dāng)拍攝者走進光影變化較小的場景中，本算法就可以正常的提取特征點，以完成位姿的估計。

總的說來，在缺少后端優(yōu)化[9]的條件下，盡管存在著無法應(yīng)對光影對比顯著場景的缺陷，視覺里程計的運動估計功能基本實現(xiàn)。

圖18 陰影場景中匹配成功

5 總結(jié)

本設(shè)計基于RGB-D相機，完成了SLAM框架的前端視覺里程計的設(shè)計，研究并優(yōu)化了傳統(tǒng)的SLAM算法，分別實現(xiàn)了ORB特征點提取的“網(wǎng)格化處理”、特征點匹配的RANSAC改進以及相機位姿的ICP優(yōu)化等，最終通過PCL輸出局部點云地圖；室外環(huán)境下表現(xiàn)較弱，光線陰影變化的地方特征點的提取和匹配算法會出現(xiàn)較大的誤差。后續(xù)研究可以結(jié)合深度信息提取與語義信息對特征點提取與跟蹤進行優(yōu)化。