曹一波,趙鵬飛,朱海文,劉 順,張智輝

(華南師范大學(xué) 軟件學(xué)院,廣東 佛山 528200)

0 引 言

近年來,視覺同時定位與地圖構(gòu)建(Simultaneous Localization and Mapping,SLAM)技術(shù)是計算機(jī)視覺和機(jī)器人領(lǐng)域的一大研究熱點,其核心在于幫助機(jī)器人更加準(zhǔn)確高效地認(rèn)知自身位姿和周圍環(huán)境的幾何結(jié)構(gòu)。在基于特征的視覺SLAM中,點特征因其結(jié)構(gòu)簡單且容易表達(dá)最為常用。典型的點特征方法有PTAM[1]、ORBSLAM[2],通過傳統(tǒng)特征檢測算法提取圖像中的顯著角點,再根據(jù)角點附近領(lǐng)域信息計算該點的描述子,最后根據(jù)數(shù)據(jù)關(guān)聯(lián)情況構(gòu)造并求解狀態(tài)變量。隨著近些年深度學(xué)習(xí)的發(fā)展,深度學(xué)習(xí)以其提取特征更豐富、魯棒性更強(qiáng)、精度更高和不需要手工設(shè)計特征等優(yōu)點在圖像分類、目標(biāo)檢測和圖像分割等領(lǐng)域得到了廣泛應(yīng)用[3]。使用深度學(xué)習(xí)方法進(jìn)行特征點及描述子的提取也是該領(lǐng)域研究的一大熱點問題。如SuperPoint_SLAM[4]使用神經(jīng)網(wǎng)絡(luò)提取SuperPoint特征點[5]代替?zhèn)鹘y(tǒng)手工設(shè)計特征,提高了定位的精度和魯棒性?；邳c特征的方法中,特征點在質(zhì)量上和數(shù)量上都對算法的魯棒性和準(zhǔn)確性起著決定性作用。在結(jié)構(gòu)化場景中[6],物體表面、材質(zhì)性能均一,結(jié)構(gòu)及尺寸變化規(guī)律且穩(wěn)定,多帶有顯著的幾何特征和相對規(guī)則的形狀,例如墻壁、桌子、門窗和箱子等等。這種場景以低紋理表面為特征的人造環(huán)境居多,如走廊、室內(nèi)停車場等,難以提取到足夠多的特征點。為了彌補單一點特征提供特征信息的不足,有學(xué)者提出了多種特征相結(jié)合的方法,如Pumarola等[7]在ORBSLAM的基礎(chǔ)上加入線特征,并提出利用線特征進(jìn)行系統(tǒng)初始化的方法。Gomez-Ojeda等[8]提出基于雙目的點線特征SLAM方法,在回環(huán)檢測模塊中加入并權(quán)衡了點線特征。李鐵維等[9]采用一種四參數(shù)的方式表示空間直線,并使用點線特征聯(lián)合圖優(yōu)化去估計相機(jī)位姿,有效改善了因相機(jī)運動過快產(chǎn)生的跟丟問題。Yang Gao等[10]提出的RGB-D相機(jī)下基于點線面特征的視覺里程計,定義了一種平面特征的匹配方法及平面特征的誤差表示方法。Zhang等[11]提出的室內(nèi)環(huán)境下基于點面的RGB-D SLAM,為平面增加了平行和垂直的約束關(guān)系。李海豐等[12]提出的基于點線面特征的PLP-SLAM,其點特征用于對機(jī)器人當(dāng)前位姿進(jìn)行初始估計,線和平面特征用來描述環(huán)境信息,但該方法的本質(zhì)還是依賴點特征實現(xiàn)定位。線、面特征在結(jié)構(gòu)化環(huán)境中較為豐富且對光照變化更加魯棒,將它們和點特征共同用于SLAM系統(tǒng)中,通過增加特征的數(shù)量[13]來提高定位算法的精度。

傳統(tǒng)的多種特征結(jié)合的方法通過幀間匹配直接計算位姿信息,不可避免地會產(chǎn)生累計誤差,從而導(dǎo)致定位不準(zhǔn)確[14]。一種策略是加入回環(huán)檢測和位姿圖優(yōu)化來糾正誤差,然而這會帶來額外的計算負(fù)擔(dān)。另一種則是根據(jù)特殊的場景結(jié)構(gòu)來減少累計誤差。在結(jié)構(gòu)化場景中,物體多表現(xiàn)為平面性和正交性,大都滿足建立曼哈頓世界[15]的條件,即所處環(huán)境由三個正交方向的幾何結(jié)構(gòu)組成。由于旋轉(zhuǎn)誤差是總誤差的主要來源,并且每幀與曼哈頓世界坐標(biāo)系理論上可以求得無漂移的旋轉(zhuǎn),從而間接得到幀間的旋轉(zhuǎn),減少累計旋轉(zhuǎn)誤差[16]。Joo等[17]在圖像幀中為曼哈頓世界提供了分支定界框架。MVO中[18]提出了一種單位球均值偏移方法,用于尋找曼哈頓世界和相機(jī)坐標(biāo)系之間的旋轉(zhuǎn)矩陣。OPVO中[19]使用平面來估計曼哈頓框架間的旋轉(zhuǎn),在至少具有兩個正交平面的環(huán)境中,通過分別估計相機(jī)旋轉(zhuǎn)和平移運動實現(xiàn)了準(zhǔn)確和低誤差的狀態(tài)估計。Li等[20]提出的RGB-D SLAM系統(tǒng),結(jié)合多種傳統(tǒng)幾何特征并利用曼哈頓世界假設(shè),該系統(tǒng)有效提高了結(jié)構(gòu)化環(huán)境下的定位精度。

綜上所述,為了充分利用結(jié)構(gòu)化環(huán)境下的多種特征信息,以及基于深度學(xué)習(xí)提取特征更豐富、魯棒性更強(qiáng)、精度更高和不需要手工設(shè)計特征等優(yōu)點,提出一種SuperPoint點特征與線面特征融合的SLAM算法。在使用特征數(shù)據(jù)的策略方面,不直接計算幀間的位姿變換,而是利用線面特征,基于曼哈頓世界假設(shè),估計每幀與曼哈頓世界坐標(biāo)系的相對旋轉(zhuǎn)來得到幀間旋轉(zhuǎn),減少了旋轉(zhuǎn)的累計誤差,然后通過求解最小化聯(lián)合點線面的重投影誤差函數(shù)得到平移。將6自由度位姿解耦為3自由度的旋轉(zhuǎn)和平移,降低了算法對特征信息的依賴同時也減少了計算量。位姿優(yōu)化方面,根據(jù)結(jié)構(gòu)化環(huán)境下的特性,添加了平面間的垂直和平行約束關(guān)系,并使用關(guān)鍵幀構(gòu)建的局部地圖優(yōu)化位姿。在TUM公開數(shù)據(jù)集上與現(xiàn)有的主流方法對比表明,該算法有效提升了結(jié)構(gòu)化低紋理環(huán)境下的定位精度。

1 基于點線面特征融合的SLAM算法

1.1 算法流程

算法整體流程如圖1所示。

圖1 算法整體流程

算法主要以基于深度學(xué)習(xí)的SuperPoint點特征和傳統(tǒng)線、面特征相結(jié)合,以及結(jié)構(gòu)化場景特性借助曼哈頓世界假設(shè)來提高定位的精度和魯棒性。算法以RGB-D相機(jī)獲取的RGB-D圖像作為數(shù)據(jù)來源,通過SuperPoint算法提取點特征,LSD算法[21]提取線特征,使用平面分割算法[22]得到面特征。點、線特征的匹配由SuperPoint描述子之間的距離和LBD描述子之間的距離分別得到,平面特征的匹配通過平面到原點的距離、平面間夾角得到。由圖像中提取的線、面構(gòu)建曼哈頓世界坐標(biāo)系,通過求每幀與曼哈頓世界坐標(biāo)系的相對旋轉(zhuǎn)來間接得到幀與幀之間的相對旋轉(zhuǎn)R,然后構(gòu)建聯(lián)合點線面的重投影誤差函數(shù),代入R并求解此函數(shù)即可獲得平移t。根據(jù)結(jié)構(gòu)化場景下平面間相互垂直和平行的特性添加約束函數(shù)優(yōu)化位姿。同時為彌補環(huán)境中出現(xiàn)不嚴(yán)格遵守曼哈頓世界假設(shè)的情況,使用關(guān)鍵幀構(gòu)建的局部地圖投影到當(dāng)前幀進(jìn)一步優(yōu)化位姿。

1.2 相關(guān)特征的提取與匹配

1.2.1 SuperPoint點特征

SuperPoint是一種先進(jìn)的用于特征點檢測以及計算描述子的自監(jiān)督框架,由MagicPoint和Homographic兩部分組成。前者是用于特征點檢測的基于VGG神經(jīng)網(wǎng)絡(luò),后者是用于生成特征點的描述子。SuperPoint對環(huán)境光照和視點變化的魯棒性使其更加穩(wěn)定,在Hpatches數(shù)據(jù)集[23]上的性能優(yōu)于ORB、SIFT等傳統(tǒng)手工設(shè)計點特征。在SuperPoint_SLAM中,該SLAM系統(tǒng)采用ORBSLAM2[24]的框架,采用SuperPoint點特征替換ORB點特征,其在建筑物多的場景定位表現(xiàn)優(yōu)于ORBSLAM2。文中算法主要應(yīng)用在結(jié)構(gòu)化環(huán)境下,場景中以人造建筑物居多,使用基于深度學(xué)習(xí)的SuperPoint點特征與線面特征相結(jié)合能有效提高SLAM定位的精度。在得到點特征和其描述子之后,通過計算描述子距離完成點特征之間的匹配。在提取圖像中的二維點特征后,利用相機(jī)內(nèi)參和深度圖將其反向投影,獲得對應(yīng)三維空間點。結(jié)構(gòu)化紋理環(huán)境下提取的SuperPoint點特征如圖2所示。

圖2 結(jié)構(gòu)化紋理環(huán)境下提取的SuperPoint點特征

1.2.2 線特征和面特征

結(jié)構(gòu)化場景中,常常存在低紋理表面為特征的人造環(huán)境,點特征提取不到或數(shù)量太少不足以成功跟蹤位姿,所以算法中加入了直線和平面特征。線特征使用LSD算法從圖像中擬合得到,使用LBD算法得到線特征的描述子,根據(jù)描述子距離完成線特征之間的匹配。和點特征一樣,經(jīng)過反投影后得到三維線。但由于RGB-D相機(jī)得到的深度圖在物體邊界處會出現(xiàn)深度不連續(xù)的先天性弱勢,勢必會存在由于深度錯誤導(dǎo)致投影的不準(zhǔn)確情況,為克服這種情況的影響,該文對3D線進(jìn)行濾波,即通過RANSAC的方法對線段上的外點進(jìn)行剔除。

RGB-D圖像能夠提供有序的點云[25],使用算法[22]從點云中分割并提取平面特征。其中平面由hassion的參數(shù)方程來表示,記作π=(nT,d),其中n=(nx,ny,nz)T是平面的單位法向量,d(d>0)是相機(jī)原點到該平面的距離。平面檢測之后,使用文獻(xiàn)[10]中的數(shù)據(jù)關(guān)聯(lián)方法進(jìn)行平面匹配,即兩平面法向量夾角小于10度、原點到平面距離d之差小于0.1米。此外,還將探索局部地圖中的平面和當(dāng)前幀中檢測到的平面之間的關(guān)系,以在優(yōu)化位姿時增加額外的約束。結(jié)構(gòu)化低紋理環(huán)境下提取的線、面特征如圖3所示。

圖3 結(jié)構(gòu)化低紋理環(huán)境下提取的線、面特征

1.3 位姿估計

1.3.1 基于曼哈頓世界假設(shè)的旋轉(zhuǎn)估計

當(dāng)提取到線面特征后,根據(jù)結(jié)構(gòu)化環(huán)境下的正交特性,利用文獻(xiàn)[20]中的方法可將環(huán)境建模為曼哈頓世界,通過球面均值漂移法來估計每幀與曼哈頓坐標(biāo)系之間的旋轉(zhuǎn)關(guān)系RCM[18],如圖4所示。其中,C0、Ck、Ck+1是初始時刻以及第k和k+1時相機(jī)的位姿,其與曼哈頓坐標(biāo)系的旋轉(zhuǎn)關(guān)系分別為RC0M、RCKM、RCK+1M。

圖4 基于曼哈頓世界坐標(biāo)系的旋轉(zhuǎn)估計

在上述得到第k和k+1幀與曼哈頓坐標(biāo)系之間的相對旋轉(zhuǎn)關(guān)系后,根據(jù)公式(1),就可以估計出k到k+1幀的旋轉(zhuǎn)RCK+1CK。

RCK+1CK=RCK+1MRCKMT

(1)

根據(jù)世界坐標(biāo)系與曼哈頓坐標(biāo)系之間的旋轉(zhuǎn)關(guān)系RMW,由公式(2),就可以得到每一時刻相機(jī)與世界坐標(biāo)系下的旋轉(zhuǎn)情況。

RCkW=RCkMRMW

(2)

其中,RMW由曼哈頓世界初始化時得到。

通過借助曼哈頓世界假設(shè)將位姿解耦,可單獨求解3自由度的旋轉(zhuǎn),不僅減少了計算量,而且旋轉(zhuǎn)估計不會受到上一幀或參考幀姿態(tài)的影響,誤差將不會累計,有效降低了旋轉(zhuǎn)累計誤差。

1.3.2 平移估計

在完成位姿的旋轉(zhuǎn)估計后,通過最小化聯(lián)合點線面的重投影誤差函數(shù)來估計位姿中剩余的3自由度平移。首先,將匹配好的參考幀中的三維點重新投影到當(dāng)前幀中[26],構(gòu)建關(guān)于點的重投影誤差函數(shù):

(3)

其中,Π(.)為投影函數(shù),Pj是參考幀中的三維點,pk是當(dāng)前幀的像素坐標(biāo),Rk,j是參考幀到當(dāng)前幀的旋轉(zhuǎn)矩陣并在上節(jié)方法中已經(jīng)得到,只需求解參考幀到當(dāng)前幀的平移矩陣tk,j即可。對于線,使用端點pstart和pend來獲得歸一化的線段,其描述如下所示:

(4)

然后,通過參考幀中的線段端點到當(dāng)前幀直線的距離來定義線的重投影誤差函數(shù)[27-28],誤差函數(shù)可以表示為:

(5)

對于平面,為了避免平面表示方法的過參,可將其最小參數(shù)化,表示為q(π)=(φ,ψ,d),其中φ和ψ是法線的方位角和仰角。平面表示方法如下:

(6)

因此,參考幀中的平面πj與當(dāng)前幀中的平面πk之間的誤差函數(shù)可表示為:

(7)

(8)

其中,∑p-1、∑l-1、∑π-1分別表示點線面特征在第k幀中的協(xié)方差矩陣,ρp、ρl、ρπ分別表示聯(lián)合誤差函數(shù)中點線面特征對應(yīng)的魯棒Huber函數(shù)。

最后使用LM(Levenberg-Marquart)算法最小化聯(lián)合誤差函數(shù)Et求解得到平移。

1.4 位姿優(yōu)化

結(jié)構(gòu)化環(huán)境下,平面間呈現(xiàn)垂直和平行的關(guān)系。利用這一特性,可將地圖中的平面和當(dāng)前幀檢測的平面添加垂直和平行的約束,約束函數(shù)如下所示:

(9)

(10)

其中,qn(π)=(φ,ψ)。對于互相垂直的平面,將其平面法線旋轉(zhuǎn)90度(R⊥)以構(gòu)建誤差函數(shù),同時這兩個誤差函數(shù)在優(yōu)化時被合并到(8)中建立聯(lián)合誤差函數(shù)。結(jié)構(gòu)化環(huán)境下,相機(jī)運動到不嚴(yán)格遵守曼哈頓世界的時刻,會使得定位精度下降。因此,該文采用ORBSLAM2的策略選擇關(guān)鍵幀并建立含有點、線、面的局部地圖,基于局部地圖投影到當(dāng)前幀的策略來優(yōu)化位姿,提高定位精度。

2 實 驗

2.1 實驗平臺及數(shù)據(jù)集介紹

實驗所用的電腦軟硬件配置:CPU為Inter Core i5-9400F,6核處理器,主頻2.9 GHz,內(nèi)存8 GB,GPU為1050Ti,系統(tǒng)為Ubuntu16.04。使用TUM RGB-D公開數(shù)據(jù)集的四個結(jié)構(gòu)化場景的序列進(jìn)行實驗測試,其中fr3-structure-texture-far、fr3-structure-texture-near為結(jié)構(gòu)化紋理環(huán)境,fr3-structure-notexture-far、fr3-structure-notexture-near為結(jié)構(gòu)化低紋理環(huán)境,以下簡稱為st-far、st-near、snot-far和snot-near[29]。

2.2 實驗結(jié)果與分析

為了驗證提出的基于深度學(xué)習(xí)的SuperPoint點特征與傳統(tǒng)線面特征相結(jié)合能夠有效提升結(jié)構(gòu)化環(huán)境下SLAM定位算法的精度,將所提算法與基于ORB點特征的ORBSLAM2、基于SuperPoint點特征的SuperPoint_SLAM[4]、基于傳統(tǒng)點面特征的PP_SLAM[11]、基于傳統(tǒng)點線面特征的PLANAR_SLAM[20]作對比分析。絕對軌跡誤差(Absolute Trajectory Error,ATE)直接計算相機(jī)位姿的真實值與SLAM定位算法的估計值之差,代表軌跡的全局一致性,此標(biāo)準(zhǔn)非常適合評估SLAM系統(tǒng)的性能,如式(11):

(11)

其中,Testi,i是估計軌跡,Tgt,i是真實軌跡,i=1,2,…,N。首先單獨比較了幾種算法在絕對軌跡中的旋轉(zhuǎn)誤差。

如圖5所示,除st-near序列,所提算法在其他序列上的旋轉(zhuǎn)誤差值明顯低于對比算法,表明借助曼哈頓世界假設(shè)去求解位姿中的旋轉(zhuǎn)能夠有效減少旋轉(zhuǎn)的累計誤差。

圖5 幾種算法的旋轉(zhuǎn)誤差對比

對于絕對軌跡誤差進(jìn)行分析,就要得到相機(jī)傳感器在數(shù)據(jù)集各個序列上的運動軌跡。圖6是幾種算法在各個序列上的運動軌跡和真實軌跡的對比, 可以看出所提算法在結(jié)構(gòu)化低紋理的環(huán)境中比其他算法更貼近于真值,表明所提算法在這些環(huán)境下的定位精度更高。表1數(shù)據(jù)是幾種算法絕對軌跡誤差的具體數(shù)值,代表各算法對相機(jī)位姿的估計值與真實值之間的差異,數(shù)值越小,算法定位精度越高。從表1數(shù)據(jù)可知,在snot-far、snot-near序列中,ORBSLAM2和SuperPoint_SLAM過于依賴點特征,在上述序列因點特征提取不到或提取數(shù)量過少導(dǎo)致跟蹤丟失。所提算法基于點線面多種特征,彌補了單一點特征在低紋理環(huán)境下跟蹤丟失的問題,并且絕對軌跡誤差也保持在較小的水平,表明所提算法在結(jié)構(gòu)化低紋理環(huán)境下能夠有效提高SLAM的定位精度。在st-far、st-near序列中,點特征比較豐富,且ORBSLAM2、SuperPoint_SLAM、PP_SLAM中含有全局BA模塊,使得所提算法在這兩個序列中稍遜一籌,但是比之同樣沒有全局BA模塊的PLANAR_SLAM,所提算法的誤差更小,也體現(xiàn)了基于SuperPoint點特征與線面特征結(jié)合相對于傳統(tǒng)點線面特征結(jié)合的優(yōu)勢。

表1 幾種算法的絕對軌跡誤差對比 cm

圖6 幾種算法在TUM數(shù)據(jù)集序列下的部分軌跡

3 結(jié)束語

提出了一種基于點線面特征融合的SLAM定位算法,多種特征相結(jié)合充分利用了環(huán)境中的特征信息,解決了純點特征在低紋理環(huán)境下跟蹤丟失的問題。將基于深度學(xué)習(xí)的SuperPoint點特征與傳統(tǒng)線面特征相結(jié)合,利用結(jié)構(gòu)化環(huán)境特性,借助曼哈頓世界假設(shè)減少了累計誤差。實驗表明,相較于傳統(tǒng)多種特征融合的方法,該算法有效提高了結(jié)構(gòu)化低紋理環(huán)境下的定位精度。在未來的研究中,將考慮加入全局位姿優(yōu)化以及回環(huán)檢測模塊,以進(jìn)一步提升算法定位的準(zhǔn)確性和魯棒性。