一種SURF-FREAK特征誤差模型視覺(jué)里程計(jì)

劉 軍，柴宏旭，趙 威

(蘭州理工大學(xué) 機(jī)械電子工程學(xué)院，甘肅 蘭州 730050)

0 引 言

近年來(lái)，隨著自動(dòng)化水平的提高，智能機(jī)器人已經(jīng)成為生產(chǎn)生活中的一部分，如掃地機(jī)器人、無(wú)人機(jī)等。無(wú)人設(shè)備在未知環(huán)境下執(zhí)行任務(wù)可通過(guò)自身攜帶的傳感器獲取環(huán)境信息從而完成構(gòu)建導(dǎo)航地圖、路徑規(guī)劃等相關(guān)任務(wù)。而視覺(jué)傳感器不僅能獲得豐富的圖像信息，且具有質(zhì)量輕、價(jià)格低、體積小等優(yōu)勢(shì)，在機(jī)器人導(dǎo)航方面得到了廣泛應(yīng)用。

視覺(jué)里程計(jì)(visual odometry，VO)是智能機(jī)器人同步定位與地圖構(gòu)建(simultaneous localization and mapping，SLAM)的關(guān)鍵技術(shù)之一[1-2]，可實(shí)現(xiàn)移動(dòng)機(jī)器人在未知環(huán)境下的自主定位，為環(huán)境地圖模型的建立與機(jī)器人導(dǎo)航提供位置信息。該技術(shù)起源于二十世紀(jì)八十年代，自2004年David Nister發(fā)表了一篇以視覺(jué)里程計(jì)為標(biāo)題的論文[3]，并于同年成功應(yīng)用于美國(guó)國(guó)家航空航天局(NASA)主導(dǎo)的火星漫游者勇氣號(hào)和機(jī)遇號(hào)上后，掀起了新一輪的研究高潮。經(jīng)過(guò)眾多學(xué)者的努力，當(dāng)前其主流技術(shù)主要發(fā)展為三大類：直接法[4-5]、特征點(diǎn)法[6-7]、半直接法[8]。較典型且已開(kāi)源的A.J.Davison所提MonoSLAM方案的視覺(jué)里程計(jì)技術(shù)[5]即屬于直接法，其技術(shù)特點(diǎn)是直接通過(guò)計(jì)算像素灰度值的差異來(lái)確定相機(jī)運(yùn)動(dòng)。但直接法在過(guò)程中非常容易丟失特征點(diǎn)從而導(dǎo)致相機(jī)定位失敗。特征點(diǎn)法主要基于特征點(diǎn)進(jìn)行圖像匹配，相比于直接法更穩(wěn)定。較典型的如2015年Raul Mur-Artal所提出的ORB-SLAM[7]，以O(shè)RB特征點(diǎn)貫穿于系統(tǒng)的特征處理、匹配、回環(huán)檢測(cè)等方面，在全局優(yōu)化基礎(chǔ)之上可以獲得較準(zhǔn)確、全局一致的相機(jī)運(yùn)動(dòng)軌跡。半直接法將特征點(diǎn)法和直接法相結(jié)合，在關(guān)鍵幀中提取特征點(diǎn)，但在普通幀上通過(guò)計(jì)算灰度值差異進(jìn)行特征跟蹤，如文獻(xiàn)[8]等。該技術(shù)雖具有較快的單幀處理速率以及良好的實(shí)時(shí)性，但在定位精度以及魯棒性上存在不足之處。

上述視覺(jué)里程計(jì)方案主要基于2D圖像模型，因此容易受到環(huán)境因素的影響，如相機(jī)旋轉(zhuǎn)、圖像模糊、弱紋理區(qū)域等等，易導(dǎo)致定位失敗，因而難以處理復(fù)雜環(huán)境或難以滿足復(fù)雜任務(wù)要求場(chǎng)景的建模要求。而深度相機(jī)近年來(lái)得到了廣泛關(guān)注與應(yīng)用[9]，其除圖像信息之外也提供深度信息，可較好地彌補(bǔ)和解決上述問(wèn)題[10]。目前基于深度相機(jī)的視覺(jué)里程計(jì)有兩種技術(shù)模型：一種為3D-2D點(diǎn)位姿求解，如PnP(perspective-n-point)模型，通過(guò)已知三維空間點(diǎn)和圖像中所對(duì)應(yīng)投影點(diǎn)的位置關(guān)系來(lái)估計(jì)相機(jī)位姿；另一種為3D-3D點(diǎn)云位姿求解，即通過(guò)在不同坐標(biāo)系下觀測(cè)到的同一三維空間點(diǎn)集信息來(lái)估計(jì)相機(jī)位姿，例如Besl、Mckay等人[11]提出的迭代最近點(diǎn)算法等，其算法是基于四元數(shù)來(lái)定義相機(jī)位置，從測(cè)量點(diǎn)確定其對(duì)應(yīng)就近點(diǎn)的點(diǎn)集后，通過(guò)構(gòu)建代價(jià)函數(shù)(cost function)進(jìn)行迭代計(jì)算，直至達(dá)到允許誤差從而結(jié)束迭代過(guò)程。因此，為提高處理效率，許多學(xué)者結(jié)合深度相機(jī)的特點(diǎn)提出了諸多改進(jìn)算法，例如，Siemiatkowska等人在2D圖像上提取特征點(diǎn)進(jìn)行粗配準(zhǔn)，然后將粗配準(zhǔn)得到的變換矩陣作為精配準(zhǔn)的初始值，通過(guò)改進(jìn)算法初始值的方式減小迭代次數(shù)以提高效率[12]。Ji H等人提出了一種ColorICP算法，利用三維點(diǎn)的顏色信息，在尋找最近點(diǎn)的時(shí)候，只在顏色相近的區(qū)域?qū)ふ易罱c(diǎn)作為匹配點(diǎn)，可有效減少計(jì)算量，提高匹配效率[13]。其他圍繞提高計(jì)算效率方面的努力如文獻(xiàn)[14-16]等，但上述改進(jìn)要么效率提升有限，要么往往都以犧牲精度為代價(jià)，存在不同的技術(shù)缺陷，如光照變化影響大、空間旋轉(zhuǎn)尺度難以保證等等，很難兼顧精度與效率，在此不再一一贅述。

基于此，為兼顧精度與效率，受文獻(xiàn)[17]啟發(fā)，該文提出了一種新的視覺(jué)里程計(jì)技術(shù)。該技術(shù)將SURF特征點(diǎn)提取方式與FREAK描述子相結(jié)合，在粗配準(zhǔn)確定特征點(diǎn)時(shí)，為保證精度，采納SURF特征點(diǎn)提取方式以保持空間尺度不變性，同時(shí)采用FREAK描述子，即以先行檢查前128位描述信息而非對(duì)全部描述信息進(jìn)行檢查的方式提高檢查效率。同時(shí)在精配準(zhǔn)時(shí)，在概率與統(tǒng)計(jì)學(xué)意義下，不同于傳統(tǒng)采納歐氏距離對(duì)特征點(diǎn)距離進(jìn)行度量的方式，而是結(jié)合實(shí)際情況將所有特征點(diǎn)的位置誤差也引入到距離函數(shù)中，即提出了一種歐氏、馬氏距離相混合的距離函數(shù)對(duì)特征點(diǎn)距離進(jìn)行度量，該種方式更符合實(shí)際情況，既有效度量了特征點(diǎn)的概率位置，也考慮了其位置方差的影響；并且，為進(jìn)一步增強(qiáng)特征點(diǎn)空間位置準(zhǔn)確性與降低視覺(jué)里程計(jì)累計(jì)誤差，在精匹配后采用卡爾曼濾波對(duì)特征點(diǎn)空間位置進(jìn)行更新，以提高特征點(diǎn)位置與相機(jī)位姿精度。尤其不同于傳統(tǒng)視覺(jué)里程計(jì)技術(shù)僅給出相機(jī)概率位置期望，該文在上述改進(jìn)的基礎(chǔ)上，給出了相機(jī)位置的方差計(jì)算公式。標(biāo)準(zhǔn)數(shù)據(jù)集測(cè)試實(shí)驗(yàn)表明，該技術(shù)能有效兼顧實(shí)時(shí)性和準(zhǔn)確性，并能較好地對(duì)相機(jī)運(yùn)動(dòng)軌跡進(jìn)行估計(jì)。

1 視覺(jué)里程計(jì)問(wèn)題描述

視覺(jué)里程計(jì)是通過(guò)視覺(jué)方式還原出相機(jī)運(yùn)動(dòng)軌跡。該文在考慮視覺(jué)深度信息誤差的基礎(chǔ)之上，提出了一種新型視覺(jué)里程計(jì)技術(shù)，以解決當(dāng)前基于深度相機(jī)的視覺(jué)里程計(jì)技術(shù)在小型自主移動(dòng)設(shè)備上使用時(shí)的精度與效率協(xié)調(diào)問(wèn)題。通過(guò)對(duì)相機(jī)采集到的圖像進(jìn)行處理，計(jì)算出相機(jī)每一時(shí)刻運(yùn)動(dòng)位姿，即公式(1)，從而構(gòu)建出相機(jī)運(yùn)動(dòng)軌跡。

令世界坐標(biāo)系(用上標(biāo)w表示)是一個(gè)固定的參照坐標(biāo)用來(lái)描述相機(jī)和物體的位置；相機(jī)坐標(biāo)系(用上標(biāo)c表示)是相機(jī)視角下建立的坐標(biāo)系；像素坐標(biāo)(u,v)是在相機(jī)采集的圖像上建立的二維坐標(biāo)。相機(jī)初始位姿坐標(biāo)定義為世界坐標(biāo)，相機(jī)坐標(biāo)后續(xù)不斷變化。相機(jī)間后續(xù)位姿變化用一個(gè)4×4的變換矩陣表示：

(1)

每一時(shí)刻在不同位姿處，空間特征點(diǎn)與相機(jī)特征點(diǎn)的轉(zhuǎn)換關(guān)系通過(guò)公式(2)呈現(xiàn)。設(shè)相機(jī)坐標(biāo)系下的特征點(diǎn)為PC，采集圖像投影點(diǎn)為p，像素坐標(biāo)記為ui，此坐標(biāo)可由變換矩陣T轉(zhuǎn)換到世界坐標(biāo)系下：

(2)

2 一種融合SURF-FREAK特征點(diǎn)誤差模型視覺(jué)里程計(jì)技術(shù)

傳統(tǒng)的視覺(jué)里程計(jì)建立圖像噪聲擾動(dòng)模型，但未考慮Kinetic相機(jī)采集深度數(shù)據(jù)時(shí)存在噪聲，會(huì)導(dǎo)致深度值d存在誤差，對(duì)位姿估計(jì)的準(zhǔn)確性有較大的影響，甚至?xí)?dǎo)致機(jī)器定位失敗。因此，受文獻(xiàn)[17]啟發(fā)，該文在其基礎(chǔ)上采取高斯誤差模型描述Kinect深度值的誤差；并且為了快速獲得準(zhǔn)確的相機(jī)位姿，先行通過(guò)圖像配準(zhǔn)環(huán)節(jié)進(jìn)行粗配準(zhǔn)，為后續(xù)精配準(zhǔn)環(huán)節(jié)提供初始值，減少精配準(zhǔn)目標(biāo)函數(shù)迭代次數(shù)，提高效率。

2.1 基于SURF-FREAK圖像特征匹配

在粗配準(zhǔn)環(huán)節(jié)，該文結(jié)合SURF特征點(diǎn)提取方式和FREAK描述信息的優(yōu)勢(shì)，提高特征點(diǎn)準(zhǔn)確性和效率。相較于傳統(tǒng)的FAST提取方式，因通過(guò)不同尺度的盒式濾波(box filter，BF)與圖像卷積構(gòu)建出多尺度空間，在圖像中穩(wěn)定的特征點(diǎn)進(jìn)行檢測(cè)篩選；并且SURF特征點(diǎn)描述子在匹配時(shí)計(jì)算量大，結(jié)合FREAK描述子通過(guò)二進(jìn)制描述符，以先行匹配128位描述信息而非對(duì)全部描述信息進(jìn)行檢查的方式提高檢查效率。

2.1.1 SURF特征提取

SURF特征檢測(cè)是通過(guò)計(jì)算Hessian矩陣行列式，以局部曲率極大值來(lái)檢測(cè)特征點(diǎn)的區(qū)域，并通過(guò)盒式濾波(box filter)構(gòu)建多尺度空間，對(duì)特征點(diǎn)和原始圖像做卷積，進(jìn)行不同尺度空間的極值檢測(cè)，從而生成圖像穩(wěn)定的邊緣點(diǎn)(突變點(diǎn))，用于特征點(diǎn)提取，使其具備多尺度不變性。經(jīng)過(guò)Hessian矩陣處理的每個(gè)像素點(diǎn)與其圖像域和尺度域的所有相鄰點(diǎn)進(jìn)行比較尋找極值點(diǎn)，再通過(guò)盒式濾波器構(gòu)建多尺度空間，濾除掉不具備多尺度不變性的極值點(diǎn)，篩選出最終穩(wěn)定的特征點(diǎn)。傳統(tǒng)SURF特征點(diǎn)在匹配時(shí)，建立特征點(diǎn)主方向，通過(guò)在特征點(diǎn)周圍選取一個(gè)正方形框，框的邊長(zhǎng)為20倍特征點(diǎn)像素大小。然后將該框平分為16個(gè)子區(qū)域，在每個(gè)子區(qū)域計(jì)算25個(gè)像素的水平方向和垂直方向的haar小波特征，并對(duì)水平方向和垂直方向計(jì)算絕對(duì)值之和，構(gòu)造描述向量。以此作為圖像特征區(qū)域描述信息，對(duì)圖像進(jìn)行匹配。

SURF特征點(diǎn)匹配如圖1所示。

圖1 SURF特征點(diǎn)匹配

SURF特征點(diǎn)可以通過(guò)提高閾值來(lái)提高匹配準(zhǔn)確性，但SURF特征需要計(jì)算每個(gè)尺寸的haar小波響應(yīng)，計(jì)算量較大；此外圖像配準(zhǔn)時(shí)，通過(guò)計(jì)算特征點(diǎn)周圍描述向量間的歐氏距離，確定特征點(diǎn)匹配度。SURF特征點(diǎn)描述子雖然精度高，但存在計(jì)算量大、效率低等問(wèn)題?；诖?，該文通過(guò)FREAK二進(jìn)制描述代替SURF特征描述的方式來(lái)提高效率。

2.1.2 FREAK描述子及匹配

圖像通過(guò)SURF完成特征提取，通過(guò)二進(jìn)制描述符FREAK描述子代替SURF描述子，以降低計(jì)算量，提高效率。相較于SURF特征描述子，一方面通過(guò)二進(jìn)制編碼來(lái)代替haar小波響應(yīng)，以提高運(yùn)算速度；另一方面FREAK描述子模擬人眼，根據(jù)采樣點(diǎn)與特征點(diǎn)的距離遠(yuǎn)近，采用了不同大小的高斯核函數(shù)進(jìn)行平滑處理，通過(guò)重疊的感受野，可以獲得更多的信息，使最終的描述符更具獨(dú)特性和可辨識(shí)性。

圖2 FREAK描述子

FREAK描述子[18]是通過(guò)模擬人眼視網(wǎng)膜對(duì)特征點(diǎn)周圍的圖像信息進(jìn)行處理。如圖2(a)所示，人眼的視網(wǎng)膜根據(jù)感光細(xì)胞的密度分成了三個(gè)區(qū)域，即中央凹(fovea)、旁中央凹(parafoveal)、周邊區(qū)(perifoveal)。fovea區(qū)域感光細(xì)胞密度大，能夠獲得較高的分辨率，主要用于細(xì)節(jié)識(shí)別；perifoveal區(qū)域感光細(xì)胞密度較少可獲得低分辨率的圖像，可用于輪廓識(shí)別。FREAK描述子就是借鑒了人類視覺(jué)分區(qū)獲取信息而提出的一種算法，F(xiàn)REAK描述子采樣像素分布在多個(gè)同心圓，采樣像素的分布是按照指數(shù)形式的分布密度分布的，越接近同心圓的分布密度越大，如圖2(b)所示。

二進(jìn)制編碼FREAK特征描述算法，編碼過(guò)程見(jiàn)公式(3):

(3)

最終，根據(jù)計(jì)算得到了512 bit的二進(jìn)制描述符，該描述符的列是方差由高到低的排列，而高方差表征了模糊信息，低方差表征了細(xì)節(jié)信息。因此，選取前128 bit即16 bytes進(jìn)行匹配，若兩個(gè)待匹配的特征點(diǎn)前16 bytes距離小于設(shè)定的閾值，則再用剩余的比特位進(jìn)行匹配。第一步匹配可以剔除掉90%的不相關(guān)匹配點(diǎn)，很大程度上提高了匹配的速度。通過(guò)融合SURF-FREAK特征提取描述算法，對(duì)室內(nèi)區(qū)域進(jìn)行環(huán)境匹配，如圖3所示。

圖3 SURF-FREAK圖像匹配

至此相機(jī)通過(guò)圖像匹配技術(shù)完成粗匹配，結(jié)合圖像間對(duì)極幾何約束預(yù)估相機(jī)初始位姿，為后續(xù)精匹配環(huán)節(jié)提供了一個(gè)較好的初值，有效縮短了迭代次數(shù)，降低了計(jì)算復(fù)雜度。

2.2 基于高斯分布的特征點(diǎn)云幀間配準(zhǔn)模型

傳統(tǒng)的視覺(jué)里程計(jì)只考慮圖像噪聲擾動(dòng)模型，但未考慮RGB-D相機(jī)采集深度數(shù)據(jù)時(shí)存在噪聲，導(dǎo)致深度值d存在誤差，對(duì)位姿估計(jì)的準(zhǔn)確性有較大的影響。因此，該文采取高斯誤差模型描述RGB-D相機(jī)深度采集圖像的誤差，以便更好地估計(jì)相機(jī)的位姿變化。由于深度相機(jī)采集的數(shù)據(jù)存在噪聲，會(huì)導(dǎo)致像素坐標(biāo)(u,v)及深度值d存在誤差，現(xiàn)作如下假設(shè)：

(1)u、v相互獨(dú)立，且服從正態(tài)分布，即u～N(μu,σu)，v～N(μv,σv)；

(2)相機(jī)相鄰時(shí)刻觀測(cè)值相互獨(dú)立；

(3)同一時(shí)刻圖像提取不同特征點(diǎn)相互獨(dú)立；

像素(u,v)處z的均值和方差為：

i∈[μu-1,μu+1],j∈[μv-1,μv+1]

(4)

空間點(diǎn)P(x,y,z)的位置期望為：

μ=(μx,μy,μz)T

(5)

且μz滿足協(xié)方差矩陣，可表示為：

(6)

將圖像特征點(diǎn)誤差模型通過(guò)高斯表達(dá)，位置期望μ表示特征點(diǎn)位置理想情況，而協(xié)方差矩陣表達(dá)特征點(diǎn)位置在期望附近分布模型。在精配準(zhǔn)環(huán)節(jié)，通過(guò)建立累積誤差模型，并對(duì)其優(yōu)化降低協(xié)方差矩陣數(shù)值，提高位置準(zhǔn)確性。

3 點(diǎn)云圖像幀間精配準(zhǔn)模型

本節(jié)根據(jù)圖像局部特征匹配為精配準(zhǔn)環(huán)節(jié)中ICP算法提供初值，結(jié)合RGB-D相機(jī)圖像高斯誤差模型，改進(jìn)傳統(tǒng)ICP算法的距離函數(shù)，構(gòu)建出新型的ICP算法目標(biāo)函數(shù)，并推導(dǎo)出相機(jī)運(yùn)動(dòng)位置的期望和協(xié)方差。

3.1 ICP算法幀間配準(zhǔn)計(jì)算

首先，將已配準(zhǔn)特征點(diǎn)帶入ICP模型計(jì)算。使用特征描述向量x，匹配到新一幀與歷史模型集中的相似特征點(diǎn)，進(jìn)行狀態(tài)估計(jì)。

該文結(jié)合2.2節(jié)世界坐標(biāo)系下特征點(diǎn)位置方差的描述，引入Kinect相機(jī)位姿方差，使其局部特征點(diǎn)位置不確定性模型具有更加完整的描述：

(7)

然后，將相鄰關(guān)鍵幀的特征匹配點(diǎn)集帶入ICP算法進(jìn)行配準(zhǔn)。其中ICP匹配算法中的傳統(tǒng)歐氏距離不能體現(xiàn)特征點(diǎn)位置的空間方差，而馬氏距離是一種有效計(jì)算相互匹配的未知樣本集合之間的相似度計(jì)算方法。用馬氏距離作為ICP算法配準(zhǔn)準(zhǔn)則時(shí)，對(duì)微小的擾動(dòng)非常敏感，會(huì)夸大擾動(dòng)或誤匹配帶來(lái)的影響，且缺乏描述空間距離的幾何意義和直觀性。因此，ICP算法的配準(zhǔn)特征點(diǎn)之間的距離函數(shù)為：

(8)

其中，

3.2 視覺(jué)里程計(jì)累計(jì)誤差優(yōu)化模型

根據(jù)公式(8)，構(gòu)造世界坐標(biāo)系下相機(jī)位姿累計(jì)誤差模型，并求出最優(yōu)的相機(jī)位置期望及協(xié)方差。

設(shè)當(dāng)前場(chǎng)景和模型集中參與ICP配準(zhǔn)的特征點(diǎn)一共為n對(duì)，其中k時(shí)刻的第i個(gè)特征點(diǎn)的位置期望和協(xié)方差矩陣分別用μPi,k、ΣPi,k表示。

根據(jù)ICP算法，相機(jī)位置期望μPi,k應(yīng)滿足：

(9)

通過(guò)對(duì)上式用μtk求偏導(dǎo)，計(jì)算期望極值點(diǎn)：

(10)

整理可得，相機(jī)位置期望μtk應(yīng)滿足：

(11)

根據(jù)協(xié)方差的定義，相機(jī)位置協(xié)方差陣Σtk應(yīng)滿足：

Σtk=E[(tk-μtk)(tk-μtk)T]

(12)

(13)

3.3 基于EKF的特征點(diǎn)位置一致化更新

考慮到觀測(cè)噪聲的存在，采用卡爾曼濾波更新特征點(diǎn)空間位置的一致性，降低視覺(jué)里程計(jì)的累計(jì)誤差。首先，k-1時(shí)刻W坐標(biāo)系下的坐標(biāo)表示k時(shí)刻特征點(diǎn)位置的預(yù)測(cè)值，即：

(14)

(15)

當(dāng)用以表示特征點(diǎn)空間位置誤差的協(xié)方差矩陣較小時(shí)，可以用式(16)對(duì)相機(jī)位姿的期望及協(xié)方差進(jìn)行近似，相對(duì)于原式(11)、(13)一定程度上降低了算法的運(yùn)算量：

(16)

至此，通過(guò)公式(16)可以得到相機(jī)當(dāng)前時(shí)刻的位置期望及協(xié)方差，通過(guò)記錄每一時(shí)刻相機(jī)位置期望，可以得到相機(jī)運(yùn)動(dòng)軌跡。

4 實(shí)驗(yàn)與分析

實(shí)驗(yàn)軟硬件配置如下：

深度相機(jī)：Kinect v2；

計(jì)算機(jī)硬件配置：AMD A8處理器，3.0 GHz主頻，不使用GPU加速，4 G內(nèi)存；

計(jì)算機(jī)系統(tǒng)為Ubuntu16.04，運(yùn)行平臺(tái)ROS kinetic。

為了驗(yàn)證算法的有效性和可對(duì)比性，采用TUM標(biāo)準(zhǔn)數(shù)據(jù)集，通過(guò)對(duì)辦公桌前相機(jī)簡(jiǎn)單平行運(yùn)動(dòng)frg1_xyz數(shù)據(jù)集在保證精度的基礎(chǔ)上測(cè)試算法實(shí)時(shí)性；此外在復(fù)雜室內(nèi)辦公室場(chǎng)景frg1_room數(shù)據(jù)集測(cè)試其準(zhǔn)確性。同時(shí)作為比較，使用基于非線性PnP模型BA算法、基于SVD-ICP算法作為對(duì)比。

4.1 算法實(shí)時(shí)性測(cè)試

測(cè)試數(shù)據(jù)集frg1_xyz，主要用于程序?qū)崟r(shí)性測(cè)試。基于此，該數(shù)據(jù)集將著重測(cè)試程序?qū)崟r(shí)性，其中真實(shí)軌跡與估計(jì)軌跡對(duì)比見(jiàn)圖4，非線性PnP-BA算法、SVD-ICP算法與文中算法的實(shí)時(shí)性對(duì)比見(jiàn)圖5，表1記錄了不同算法下的誤差數(shù)據(jù)。

圖4 真實(shí)軌跡與估計(jì)軌跡對(duì)比

圖5 不同算法實(shí)時(shí)性圖像

表1 不同算法誤差 mm

從圖4可以看出真實(shí)軌跡與估計(jì)軌跡的誤差情況，相機(jī)在做直線運(yùn)動(dòng)的情況下，沒(méi)有產(chǎn)生誤差，但在旋轉(zhuǎn)過(guò)程中產(chǎn)生少許誤差。圖5所展示的是不同算法處理采集圖像所使用的時(shí)間，PNP-BA模型算法處理精度最高但時(shí)間最慢，原因是PNP-BA算法采用了全局非線性優(yōu)化，通過(guò)構(gòu)造全局誤差模型并對(duì)其優(yōu)化，降低了整體誤差；文中算法通過(guò)圖像匹配技術(shù)結(jié)合對(duì)極幾何約束提供初值，相較于SVD-ICP算法不但提高了精度而且縮短了時(shí)間?？梢源鍵CP算法在輕量級(jí)設(shè)備上計(jì)算相機(jī)運(yùn)動(dòng)軌跡。表1中數(shù)據(jù)表明，文中方法誤差不超25 mm，雖然PNP-BA算法誤差很小，但是實(shí)時(shí)性較差，對(duì)處理器有較高的要求，不適用于小型化設(shè)備。

4.2 準(zhǔn)確性對(duì)比實(shí)驗(yàn)

使用測(cè)試數(shù)據(jù)集frg1_room，對(duì)文中算法進(jìn)行測(cè)試。文中算法真實(shí)軌跡與估計(jì)軌跡對(duì)比見(jiàn)圖6，SVD-ICP算法真實(shí)軌跡與估計(jì)軌跡對(duì)比見(jiàn)圖7，表2記錄了不同算法模型下的誤差。

圖6 文中算法真實(shí)軌跡與估計(jì)軌跡對(duì)比

圖7 SVD-ICP算法真實(shí)軌跡與估計(jì)軌跡對(duì)比

表2 算法誤差對(duì)比 mm

用TUM的測(cè)試數(shù)據(jù)集frg1_room，測(cè)試室內(nèi)環(huán)境下相機(jī)定位準(zhǔn)確性。從圖6可以看出，在相機(jī)進(jìn)行直線運(yùn)動(dòng)時(shí)產(chǎn)生的誤差較小，但當(dāng)相機(jī)出現(xiàn)旋轉(zhuǎn)運(yùn)動(dòng)時(shí)會(huì)出現(xiàn)一定的誤差，因?yàn)樾D(zhuǎn)運(yùn)動(dòng)時(shí)相機(jī)采集的圖像信息會(huì)發(fā)生劇烈的變化，所提取的特征點(diǎn)會(huì)有一部分失效，此時(shí)深度圖像估計(jì)運(yùn)動(dòng)軌跡比重增大，所以程序主要的誤差都是從相機(jī)旋轉(zhuǎn)運(yùn)動(dòng)中產(chǎn)生。

相較于SVD-ICP算法，圖7展示的真實(shí)軌跡與估計(jì)軌跡存在較大差異，一方面SVD-ICP算法未對(duì)其建立高斯分布的特征點(diǎn)誤差模型，會(huì)存在深度圖的噪音干擾；另一方面，文中設(shè)計(jì)的距離函數(shù)，既有效度量了特征點(diǎn)的概率位置，也考慮了其位置方差的影響，因而提高了軌跡精度。表2可以通過(guò)數(shù)據(jù)展示不同算法估計(jì)軌跡產(chǎn)生的誤差值，其中室內(nèi)環(huán)境下相機(jī)定位誤差不超過(guò)30 mm。文中算法可以滿足相機(jī)定位精度和實(shí)時(shí)性需求，能夠較好地實(shí)現(xiàn)嵌入式機(jī)器視覺(jué)定位的需求。

5 結(jié)束語(yǔ)

視覺(jué)里程計(jì)作為SLAM重要環(huán)節(jié)之一，對(duì)減少機(jī)器運(yùn)動(dòng)軌跡誤差、提高地圖精度至關(guān)重要。該文提出一種基于FREAK-SURF特征點(diǎn)深度誤差視覺(jué)里程計(jì)算法，旨在輕量級(jí)設(shè)備滿足實(shí)時(shí)性要求的基礎(chǔ)上提高相機(jī)運(yùn)動(dòng)軌跡精度，為構(gòu)造地圖提供準(zhǔn)確的位置信息。首先，融合SURF特征提取算法和FREAK描述子完成特征匹配，提高匹配效率；其次，ICP配準(zhǔn)環(huán)節(jié)設(shè)計(jì)出一種新型的距離代價(jià)函數(shù)，融合馬氏距離和歐氏距離的特點(diǎn)；然后，根據(jù)特征點(diǎn)和相機(jī)空間位置誤差模型，建立視覺(jué)里程計(jì)累計(jì)誤差并通過(guò)擴(kuò)展卡爾曼濾波器優(yōu)化。對(duì)比實(shí)驗(yàn)表明，該算法在實(shí)時(shí)性上強(qiáng)于非線性PNP模型BA優(yōu)化算法和SVD-ICP算法，在精度上高于傳統(tǒng)的SVD-ICP算法。