快速提取特征點(diǎn)的視覺(jué)SLAM室內(nèi)稠密建圖研究

李興州,何 鋒,余國(guó)寬

(1.貴州大學(xué) 機(jī)械工程學(xué)院, 貴陽(yáng) 550025) (2貴州師范大學(xué) 機(jī)械與電氣工程學(xué)院, 貴陽(yáng) 550025)

0 引言

在未知環(huán)境下機(jī)器人利用傳感器檢測(cè)周邊數(shù)據(jù),以完成對(duì)周?chē)h(huán)境的定位與構(gòu)建,稱為即時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)[1-3]。根據(jù)所用傳感器類(lèi)型不同,可分為視覺(jué)SLAM與激光SLAM,以成本低廉、建圖色彩豐富等優(yōu)點(diǎn)著稱的視覺(jué)SLAM近幾年得到了快速發(fā)展。

通常地,對(duì)幀序列圖像特征點(diǎn)的處理是視覺(jué)SLAM定位與建圖的第一步,包括特征點(diǎn)的提取與匹配[4]。特征點(diǎn)數(shù)量對(duì)視覺(jué)SLAM性能的影響至關(guān)重要,若特征點(diǎn)數(shù)量少,會(huì)導(dǎo)致周?chē)h(huán)境特征較少,這將嚴(yán)重影響視覺(jué)SLAM定位與建圖的準(zhǔn)確性[5];而特征點(diǎn)數(shù)量過(guò)多,則可能出現(xiàn)提取和匹配時(shí)間長(zhǎng)、特征點(diǎn)之間誤匹配的問(wèn)題,嚴(yán)重影響視覺(jué)SLAM的實(shí)時(shí)性和準(zhǔn)確性。因此,特征點(diǎn)的提取與匹配一直是視覺(jué)SLAM的研究熱點(diǎn)方向,目前主要可以分為下列3種方法:

1)基于光流法(optical flow)的特征提取與匹配。該方法是假設(shè)光度不變性來(lái)提取特征點(diǎn),Lucas等[6-7]提出的光流法特征提取最具代表,該方法對(duì)關(guān)鍵點(diǎn)進(jìn)行跟蹤并優(yōu)化,使圖像中的2個(gè)對(duì)應(yīng)位置的光度誤差最小,以此估計(jì)相機(jī)的運(yùn)動(dòng),然而,由于此研究的假設(shè)過(guò)于保守,實(shí)驗(yàn)結(jié)果對(duì)圖像的光度變化比較敏感,從而導(dǎo)致建圖與定位的精度差。

2)基于直接法的特征提取與匹配。該方法是基于光流法并對(duì)相機(jī)估計(jì)位姿進(jìn)行優(yōu)化。Forster等[8]提出的SVO法(semidirect visual odometry)是其中的代表,該方法利用估計(jì)好的位姿對(duì)過(guò)去已經(jīng)收斂插入的地圖點(diǎn)進(jìn)行全局位姿優(yōu)化,位姿的估計(jì)更加準(zhǔn)確,然而,該方法單純地使用圖像的梯度求取位姿,由于圖像的非凸性,使最后的結(jié)果容易陷入局部最優(yōu);同時(shí)該方法是基于光度不變性的假設(shè)完成的,因此,在實(shí)際中無(wú)法滿足該假設(shè)。

3)基于ORB特征點(diǎn)的特征提取與匹配。該方法是通過(guò)FAST角點(diǎn)(features from accelerated segment test)和對(duì)其周?chē)枋鲎舆M(jìn)行快速匹配,提取特征點(diǎn)。Murartal等[9]提出的ORB-SLAM系列是其研究成果的代表,該方法利用對(duì)圖像取不同響應(yīng)值,得到FAST角點(diǎn),然后利用BRIFE描述子(binary robust independent elementary features)輕量化地完成對(duì)關(guān)鍵點(diǎn)的信息記錄,利用圖像旋轉(zhuǎn)不變性和BoW詞袋(bag of words)[10]達(dá)到特征點(diǎn)快速提取與匹配。然而,ORB-SLAM在進(jìn)行特征提取與匹配時(shí),按照固定網(wǎng)格尺寸進(jìn)行特征點(diǎn)搜索,當(dāng)面對(duì)圖像金字塔圖層時(shí)再采用等比例縮放的方式,無(wú)法達(dá)到特征點(diǎn)最大化快速的提取。

因此采用ORB特征點(diǎn)的特征提取與匹配方法,結(jié)合自適應(yīng)網(wǎng)格劃分,以達(dá)到對(duì)特征點(diǎn)快速提取的目的,同時(shí)保證其定位與建圖的精度要求,縮短特征點(diǎn)提取時(shí)間,最后在此自適應(yīng)網(wǎng)格劃分算法上添加RGB-D稠密建圖線程,完成在室內(nèi)的稠密建圖。

1 ORB特征提取原理

ORB-SLAM算法[9]由提取特征點(diǎn)和計(jì)算描述子兩部分組成,分別是FAST角點(diǎn)檢測(cè)法和BRIEF二進(jìn)制特征描述子法。

如圖1所示,FAST角點(diǎn)的提取檢測(cè)法是基于像素灰度差原理完成的。取一點(diǎn)像素點(diǎn)p為圓心,半徑為R的圓上按照順時(shí)針順序地選擇16個(gè)像素點(diǎn),并計(jì)算p與這些像素點(diǎn)的像素灰度差。如果存在N個(gè)連續(xù)像素點(diǎn)與p的像素灰度值的差值都大于閾值t,那么就確定p為一個(gè)角點(diǎn)。N通常取9,閾值t取p點(diǎn)灰度值的20%[9]。

圖1 FAST角點(diǎn)提取原理示意圖

由于FAST檢測(cè)法提取的角點(diǎn)會(huì)因?yàn)閳D像旋轉(zhuǎn)和尺度不一致,導(dǎo)致后續(xù)特征點(diǎn)提取無(wú)法正確匹配,為了解決旋轉(zhuǎn)不變性和尺度不變性的問(wèn)題,ORB算法采用灰度質(zhì)心法[11]和構(gòu)建圖像金字塔解決上述問(wèn)題。

灰度質(zhì)心法是通過(guò)計(jì)算圖像的質(zhì)心與形心之間的向量作為FAST特征點(diǎn)的主方向,其步驟為:

1) 定義特征點(diǎn)的局部圖像塊的矩為:

(1)

式中:p,q取0或1;I(x,y)表示在該點(diǎn)圖像灰度值;mpq表示圖像的矩。在半徑為R的范圍內(nèi),沿x,y坐標(biāo)軸方向的圖像矩分別為:

(2)

(3)

整個(gè)圓形范圍內(nèi)灰度值總和為:

(4)

2) 圖像的質(zhì)心C為:

(5)

θ=arctan2(cy,cx)=arctan2(m01,m10)

(6)

然后利用二進(jìn)制描述子快速找到角點(diǎn)。

2 改進(jìn)的自適應(yīng)網(wǎng)格劃分算法

在ORB-SLAM2中,采用一種名為Qtree_ORB的算法提取ORB特征點(diǎn)[12]。該算法分為以下4個(gè)步驟:① 構(gòu)建圖像金字塔,將其分成8個(gè)等比例的圖層。② 將上述得到的圖像金字塔等比例圖層進(jìn)行網(wǎng)格劃分,把圖像分成若干個(gè)網(wǎng)格,然后提取FAST角點(diǎn)。每次FAST角點(diǎn)提取都要滿足大于初始閾值iniTHFAST的要求,如果在該閾值要求下沒(méi)有提取到角點(diǎn),則按照最小閾值minTHFAST的要求來(lái)提取。③ 角點(diǎn)提取使用四叉樹(shù)算法均勻提取。首先,初始化節(jié)點(diǎn)以獲取四叉樹(shù)結(jié)構(gòu)。然后劃分節(jié)點(diǎn)區(qū)域,如果節(jié)點(diǎn)區(qū)域中沒(méi)有角點(diǎn),則刪除節(jié)點(diǎn);如果節(jié)點(diǎn)區(qū)域中只有1個(gè)角點(diǎn),則節(jié)點(diǎn)不再劃分;如果該節(jié)點(diǎn)區(qū)域中有多個(gè)角點(diǎn),則按照上述過(guò)程繼續(xù)劃分,直到節(jié)點(diǎn)數(shù)達(dá)到期望的數(shù)量或無(wú)法再進(jìn)行劃分。④ 對(duì)于得到的節(jié)點(diǎn)集中保留 Harris 響應(yīng)值最高節(jié)點(diǎn)作為特征點(diǎn)。

2.1 自適應(yīng)網(wǎng)格劃分

ORB-SLAM算法中是按照固定網(wǎng)格尺寸對(duì)圖像進(jìn)行網(wǎng)格劃分,由于該網(wǎng)格劃分方法無(wú)法適應(yīng)不同的外部環(huán)境。因此,采用自適應(yīng)網(wǎng)格劃分。使用面積法根據(jù)每層圖像的面積及特征點(diǎn)數(shù)量確定網(wǎng)格大小,以更好地適應(yīng)不同的環(huán)境,具體公式如下:

(7)

式中:X,Y為每層圖像的邊界坐標(biāo);w為每層金字塔圖像的寬;h為每層金字塔圖像的高;N為每層金字塔圖像所需的特征點(diǎn)數(shù)目。W為劃分網(wǎng)格的尺寸,根據(jù)每層特征點(diǎn)的不同,實(shí)現(xiàn)自適應(yīng)劃分。

根據(jù)每層金字塔圖像的高和寬的尺寸,計(jì)算出實(shí)際劃分行與列的數(shù)量,如下:

(8)

式中:R為網(wǎng)格劃分的行;C為網(wǎng)格劃分的列。計(jì)算網(wǎng)格的實(shí)際高和寬并向上取整,如下式:

(9)

hcell為實(shí)際自適應(yīng)劃分網(wǎng)格高;wcell為實(shí)際自適應(yīng)劃分網(wǎng)格寬;ceil為向上取整函數(shù)。

自適應(yīng)網(wǎng)格劃分后,需要在每層金字塔圖像上進(jìn)行特征點(diǎn)的提取,每一層圖像所提取的特征點(diǎn)數(shù)目為:

(10)

式中:Ni為圖像金字塔每層需要提取的特征點(diǎn)數(shù)目;N為整個(gè)圖像金字塔需要提取特征點(diǎn)總數(shù)目;s為每層圖像金字塔之間的縮放系數(shù);m為圖像金字塔總層數(shù),為8;i為第幾層金字塔。

2.2 四叉樹(shù)提取法

為了篩選角點(diǎn)并減少冗余,可以使用四叉樹(shù)[13]方法。首先,將圖像分割成四叉樹(shù)結(jié)構(gòu)并初始化節(jié)點(diǎn)進(jìn)行分割,每個(gè)節(jié)點(diǎn)中選擇Harris響應(yīng)值最大的ORB特征點(diǎn)[14]。根據(jù)要求的特征點(diǎn)數(shù)目對(duì)節(jié)點(diǎn)進(jìn)行分裂,每個(gè)節(jié)點(diǎn)分成4個(gè)子節(jié)點(diǎn),直到某個(gè)節(jié)點(diǎn)不存在特征點(diǎn)或已達(dá)到保留數(shù)量要求時(shí),停止分裂。如圖2所示(b、c中,彩色圓點(diǎn)為圖像特征點(diǎn)),a為四叉樹(shù)原理圖,b、c為仿真試驗(yàn)效果圖,b、c對(duì)比可見(jiàn),利用四叉樹(shù)提取的特征點(diǎn)更加均勻,匹配效果顯著。

圖2 四叉樹(shù)原理及仿真效果圖

3 實(shí)驗(yàn)結(jié)果與分析

本研究的仿真基于Linux平臺(tái),其中系統(tǒng)版本為Ubuntu18.04,CPU為i5-12490F的處理器、內(nèi)存為16 GB。為了減少實(shí)驗(yàn)隨機(jī)性帶來(lái)的誤差,對(duì)實(shí)驗(yàn)中的每組數(shù)據(jù)集分別進(jìn)行10次仿真測(cè)試,最終取平均值作為實(shí)驗(yàn)結(jié)果。實(shí)驗(yàn)1進(jìn)行了6組實(shí)驗(yàn),分別比對(duì)了ORB-SLAM2的MONO、RGB-D和改進(jìn)算法的MONO和RGB-D在特征點(diǎn)提取時(shí)間上的表現(xiàn)。實(shí)驗(yàn)2分別驗(yàn)證了ORB-SLAM2的MONO、RGB-D和改進(jìn)算法的MONO和RGB-D在數(shù)據(jù)集上評(píng)估算法的絕對(duì)軌跡誤差(absolute trajectory error,ATE)。

3.1 平均每幀特征點(diǎn)提取時(shí)間測(cè)試

本次實(shí)驗(yàn)基于ORB-SLAM2的MONO、RGB-D,以及改進(jìn)算法的MONO、RGB-D模式,針對(duì)TUM數(shù)據(jù)集的f1_xyz、f1_desk、f1_plant、f1_rpy、f1_room、f3_long_offices 6組數(shù)據(jù),分別進(jìn)行了10次仿真,并以平均每幀特征點(diǎn)提取時(shí)間為評(píng)估標(biāo)準(zhǔn)。結(jié)果如圖3所示。

圖3 平均每幀特征點(diǎn)提取時(shí)間曲線

實(shí)驗(yàn)結(jié)果表明,提出的自適應(yīng)網(wǎng)格劃分特征點(diǎn)提取算法在TUM不同數(shù)據(jù)集下,無(wú)論是在MONO還是RGB-D模式下,都能夠顯著降低特征點(diǎn)提取時(shí)間,平均節(jié)約時(shí)間達(dá)到8%～10%。

3.2 絕對(duì)軌跡誤差測(cè)試

本實(shí)驗(yàn)中,使用ORB-SLAM2算法以及改進(jìn)后的算法分別在TUM數(shù)據(jù)集上進(jìn)行仿真測(cè)試。并以ATE作為仿真測(cè)試的評(píng)價(jià)標(biāo)準(zhǔn)(ATE是用于評(píng)估SLAM系統(tǒng)估計(jì)位姿精度的一種標(biāo)準(zhǔn),它計(jì)算真實(shí)位姿與SLAM系統(tǒng)估計(jì)位姿之間的誤差)。為了進(jìn)行評(píng)估,需要先通過(guò)時(shí)間戳將真實(shí)值和估計(jì)值進(jìn)行對(duì)齊,然后計(jì)算每對(duì)位姿之間的誤差值[15]。最終,ATE以圖表形式輸出,以便更加直觀地呈現(xiàn)評(píng)估結(jié)果。

1) 對(duì)其真實(shí)軌跡與估計(jì)軌跡,得到兩者軌跡之間的最小二乘解變換矩陣S,絕對(duì)軌跡誤差定義如下:

(11)

式中:Pi表示SLAM系統(tǒng)估計(jì)位姿;Qi表示軌跡真實(shí)位姿。

2) 整個(gè)時(shí)間戳范圍內(nèi)位姿的均方根誤差RMSE為:

(12)

式中:trans為SLAM系統(tǒng)得到的平移誤差。

在TUM數(shù)據(jù)集f1_xyz、f1_desk、f1_plant、f1_rpy、f1_room、f3_long_offices上分別對(duì)ORB-SLAM2和改進(jìn)后的算法進(jìn)行仿真,統(tǒng)計(jì)MONO和RGB-D兩種模式下的絕對(duì)軌跡誤差,如表1所示。

表1 絕對(duì)軌跡誤差評(píng)估 mm

根據(jù)表1的實(shí)驗(yàn)結(jié)果顯示,自適應(yīng)網(wǎng)格劃分特征點(diǎn)提取算法在絕對(duì)軌跡誤差方面表現(xiàn)明顯優(yōu)于ORB-SLAM2算法。在6組不同的數(shù)據(jù)集中,無(wú)論是MONO,還是RGB-D模式下,該算法的精度均提高了5%以上。尤其在某些大型室內(nèi)場(chǎng)景下,改進(jìn)算法的精度提高幅度最高可達(dá)50%。以f1_plant和f1_fesk兩組數(shù)據(jù)的MONO和RGB-D模式下的軌跡路線及x-y-z軸方向上的誤差曲線為例,如圖4、圖5所示。在圖中,groundtruth為真實(shí)軌跡,old_trajectory為ORB-SLAM2算法得到的軌跡路線,new_trajectory表示改進(jìn)后的自適應(yīng)網(wǎng)格劃分ORB算法得到的軌跡路線。從圖中可以看出,改進(jìn)后的算法無(wú)論在軌跡匹配上還是在絕對(duì)誤差方面的精度都要高于ORB-SLAM2算法。

4 稠密建圖

為了生成稠密的點(diǎn)云地圖,在上述自適應(yīng)網(wǎng)格劃分算法框架中,增加稠密點(diǎn)云構(gòu)建線程。該線程使用RGB-D相機(jī)測(cè)量深度信息,與RGB圖像進(jìn)行匹配,利用前端估算的相機(jī)位姿計(jì)算出所有像素的空間坐標(biāo),并生成點(diǎn)云圖。通過(guò)RGB圖像信息上色[16],可以得到一個(gè)由離散的彩色點(diǎn)組成的地圖。該線程會(huì)隨著新增關(guān)鍵幀的不斷加入,進(jìn)行點(diǎn)云拼接和全局優(yōu)化,實(shí)現(xiàn)稀疏到稠密的轉(zhuǎn)變。具體流程如圖6所示。遍歷匹配好的RGB圖像和深度圖,提取出像素坐標(biāo)(u,v)和深度值d,根據(jù)針孔相機(jī)成像模型:

(13)

式中:Z為深度值和實(shí)際空間距離的比例因子;K為相機(jī)內(nèi)參矩陣;R、t為相機(jī)的旋轉(zhuǎn)矩陣和平移矩陣;Pw為像素點(diǎn)在世界坐標(biāo)系下的三維坐標(biāo)。由于相機(jī)已進(jìn)行標(biāo)定,因此內(nèi)參矩陣K為已知量,跟蹤線程計(jì)算相機(jī)位姿,從而確定了R、t,因此可以確定RGB圖像中每個(gè)像素的空間坐標(biāo)。利用PCL點(diǎn)云庫(kù)(point cloud library)來(lái)處理計(jì)算出的點(diǎn)云數(shù)據(jù)。點(diǎn)云Pi的坐標(biāo)可由下式計(jì)算得出:

圖6 稠密建圖具體流程框圖

(14)

式中, depthScale是深度值。

再利用RGB圖像三通道的色彩信息給點(diǎn)云上色,然后,讀取下一個(gè)關(guān)鍵幀進(jìn)行點(diǎn)云拼接。以TUM下的f1_desk、f1_plant、f1_room、f3_long_office四組數(shù)據(jù)集為例進(jìn)行室內(nèi)稠密建圖,效果如圖7所示。由圖7可知,該稠密建圖效果顯著,室內(nèi)物體輪廓清晰,顏色比對(duì)正確,因此,在原有基礎(chǔ)上添加稠密建圖線程可以滿足室內(nèi)稠密建圖的需求。

圖7 稠密建圖

5 結(jié)論

1) 自適應(yīng)網(wǎng)格劃分算法能夠根據(jù)圖像金字塔層數(shù)的不同,自動(dòng)調(diào)整網(wǎng)格大小和密度,更好地捕捉數(shù)據(jù)的局部特征信息,使得自適應(yīng)網(wǎng)格劃分算法進(jìn)行特征點(diǎn)提取時(shí),具有更高的時(shí)間效率和更高的絕對(duì)軌跡誤差精度。

2) 采用外點(diǎn)去除濾波和體素網(wǎng)格濾波方法對(duì)室內(nèi)場(chǎng)景進(jìn)行稠密建圖,外點(diǎn)去除濾波通過(guò)排除異?；驘o(wú)效特征點(diǎn),提高了場(chǎng)景重建的精度和可靠性;體素網(wǎng)格濾波方法利用分層表示和空間降采樣技術(shù),在保持場(chǎng)景結(jié)構(gòu)完整性的同時(shí),有效降低了噪聲和采樣密度不均勻性對(duì)建模結(jié)果的影響,顯著增強(qiáng)了室內(nèi)建筑和紋理的建模效果。