霍智豪，苗艷松

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

0 引言

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

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

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

1 相機(jī)模型

■ 1.1 基本相機(jī)模型

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

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

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

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

■ 1.2 畸變矯正

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

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

■ 1.3 深度信息提取

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

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

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

2 特征點(diǎn)提取與匹配

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

■ 2.1 FAST特征點(diǎn)提取與改進(jìn)

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

我們判斷特征點(diǎn)就是對N進(jìn)行比較。

圖4 FAST特征點(diǎn)提取

圖5 改進(jìn)的特征點(diǎn)提取

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

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

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

■ 2.3 BRIEF描述子的改進(jìn)

定義操作Q比較點(diǎn)對之間的灰度值：

BRIEF描述子就是Q值的二進(jìn)制排列[5]。

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

■ 2.4 特征點(diǎn)的匹配

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

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

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

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

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

圖6 RANSAC改進(jìn)的匹配

3 運(yùn)動的估計

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

圖7 優(yōu)化匹配

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

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

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

圖8 相機(jī)運(yùn)動的ICP優(yōu)化

4 系統(tǒng)測試

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

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

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

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

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

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

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

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

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

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

圖11 特征點(diǎn)提取

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

圖2 針孔相機(jī)模型

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

圖13 ATE

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

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

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

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

圖12 追蹤失敗

圖14 RPE

圖15 ATE

圖16 RPE

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

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

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

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

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

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

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

表1 測試誤差比較

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

■ 4.3 真實(shí)場景測試

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

圖17 特征點(diǎn)匹配失敗

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

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

圖18 陰影場景中匹配成功

5 總結(jié)

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