陳文佑,章 偉,史曉帆,宋 芳
(上海工程技術(shù)大學(xué) 機(jī)械與汽車(chē)工程學(xué)院,上海 201620)
同時(shí)定位與地圖構(gòu)建(Simultaneous Localization and Mapping,SLAM)[1]是指安裝了特定傳感器的智能體在環(huán)境先驗(yàn)信息未知的情況下,能夠在運(yùn)動(dòng)過(guò)程中建立周?chē)h(huán)境地圖,并同時(shí)估計(jì)自己運(yùn)動(dòng)的一種算法[2]。SLAM技術(shù)可以應(yīng)用在自動(dòng)駕駛、機(jī)器人、無(wú)人機(jī)、VR/AR等領(lǐng)域,是未來(lái)人工智能時(shí)代的關(guān)鍵技術(shù)之一。SLAM按照傳感器的不同,分為激光SLAM和視覺(jué)SLAM。激光SLAM已較為成熟,而視覺(jué)SLAM技術(shù)目前仍還有很多問(wèn)題尚未解決。
自20世紀(jì)80年代以來(lái),視覺(jué)SLAM就已經(jīng)被計(jì)算機(jī)視覺(jué)和機(jī)器人領(lǐng)域的科研人員所關(guān)注。經(jīng)過(guò)幾十年的發(fā)展,目前已形成了基于特征法和直接法的兩種SLAM方法。視覺(jué)SLAM所用到的傳感器分為單目相機(jī)、雙目相機(jī)和RGBD相機(jī)。在目前國(guó)內(nèi)外的視覺(jué)SLAM方案中,比較著名的算法有基于直接法的大范圍單目定位與制圖[3](Large-Scale Direct Monocular SLAM,LSD-SLAM)方法、使用RGB-D 攝像頭的 RGBD-SLAM[4]方法、基于點(diǎn)線(xiàn)特征的VSLAM算法PL-SLAM[5](Points and Line Segments SLAM)以及基于特征點(diǎn)的ORB-SLAM[6](Oriented FAST and Rotated BRIEF-SLAM)方法等。ORB-SLAM算法魯棒性良好,建圖誤差及運(yùn)算復(fù)雜度較低,自提出以來(lái)就一直是研究者們重點(diǎn)關(guān)注的對(duì)象。但是ORB-SLAM算法建立的是稀疏點(diǎn)云圖,只保留了圖像中特征點(diǎn)的一部分作為關(guān)鍵點(diǎn),固定在空間中進(jìn)行定位,難以描繪地圖中障礙物的存在。故本文對(duì)單目ORB-SLAM算法進(jìn)行改進(jìn),在ORB-SLAM框架基礎(chǔ)上,提出一種改進(jìn)的半稠密三維重建算法,并將建立的地圖同ORB-SLAM的建圖效果進(jìn)行對(duì)比。本文利用德國(guó)慕尼黑工業(yè)大學(xué)計(jì)算機(jī)視覺(jué)實(shí)驗(yàn)室發(fā)布的TUM數(shù)據(jù)集進(jìn)行仿真實(shí)驗(yàn),證明本文所提方法能夠改進(jìn)ORB-SLAM的建圖效果。
ORB-SLAM算法最大的特點(diǎn)是所有步驟均使用ORB[7]特征。ORB特征是一種快速的特征提取方法,它改進(jìn)了FAST檢測(cè)子不具有方向性的問(wèn)題,并采用速度較快的二進(jìn)制描述子BRIEF[8],加快了整個(gè)圖像特征的提取。相較于SIFT(Scale-Invariant Feature Transform)[9]和SURF(Speeded Up Robust Feature)[10]特征,ORB特征在特征點(diǎn)的提取速度上有顯著提升,且不需要用GPU加速就可以滿(mǎn)足SLAM所需的實(shí)時(shí)性。同時(shí),使用統(tǒng)一的ORB特征也有助于SLAM算法在特征提取與追蹤、關(guān)鍵幀選取、三維重建、閉環(huán)檢測(cè)等步驟中具有內(nèi)生的一致性。圖1為一張圖像提取ORB特征點(diǎn)的效果圖??梢钥闯?,采用FAST的角點(diǎn)檢測(cè)算法在角點(diǎn)比較密集的地方,例如樹(shù)枝和汽車(chē),可檢測(cè)到較多的特征點(diǎn),點(diǎn)云也更豐富;在角點(diǎn)較稀疏的地方,例如草坪和路面,特征點(diǎn)就比較少,點(diǎn)云稀疏,這就是ORB的主要特點(diǎn)。
圖1 OpenCV提供的ORB特征點(diǎn)檢測(cè)結(jié)果Figure 1.ORB feature point detection results provided by OpenCV
ORB-SLAM共有3個(gè)線(xiàn)程,分別是跟蹤(Tracking)、局部建圖(Local Mapping)和閉環(huán)控制(Loop Closing)。
圖2給出了ORB-SLAM算法建圖的詳細(xì)流程。跟蹤線(xiàn)程主要用ORB算法提取圖像幀中的特征點(diǎn),同時(shí)確定相機(jī)位姿,并決定插入新關(guān)鍵幀的時(shí)機(jī)。局部建圖線(xiàn)程負(fù)責(zé)關(guān)鍵幀的插入,并處理從關(guān)鍵幀中得到的地圖點(diǎn),再進(jìn)行局部BA(Bundle Adjustment),使得重投影誤差最小,最后剔除冗余的關(guān)鍵幀。閉環(huán)控制線(xiàn)程包括閉環(huán)檢測(cè)和閉環(huán)修正:閉環(huán)檢測(cè)負(fù)責(zé)檢測(cè)所有關(guān)鍵幀是否形成閉環(huán)并計(jì)算累積誤差;閉環(huán)修正負(fù)責(zé)減小累積漂移量。詳細(xì)步驟如下:
圖2 ORB-SLAM算法框架Figure 2.ORB-SLAM algorithm framework
步驟1跟蹤線(xiàn)程。ORB算法在8層金字塔模型上提取FAST角點(diǎn),確保特征點(diǎn)均勻分布,設(shè)定角點(diǎn)閾值,根據(jù)保留的FAST角點(diǎn)計(jì)算方向和ORB特征描述子。然后,通過(guò)前一圖像幀估計(jì)相機(jī)的初始位姿,并通過(guò)全局重定位來(lái)初始化位姿。在當(dāng)前幀和局部幀地圖之間找到更多的匹配點(diǎn)對(duì),來(lái)優(yōu)化當(dāng)前幀的位姿。最后,通過(guò)篩選冗余關(guān)鍵幀來(lái)決定當(dāng)前幀是否可以作為關(guān)鍵幀;
步驟2局部建圖線(xiàn)程。首先更新互視圖,添加一個(gè)關(guān)鍵幀節(jié)點(diǎn)Ki,檢查與Ki有共同云點(diǎn)的其他關(guān)鍵幀,用邊線(xiàn)連接。計(jì)算表示該關(guān)鍵幀的詞袋,并利用三角法生成新的地圖云點(diǎn)。三角化的云點(diǎn)必須經(jīng)過(guò)地圖點(diǎn)云篩選測(cè)試,保證留下的云點(diǎn)都是能被跟蹤的。通過(guò)對(duì)互視圖中連接的關(guān)鍵幀Kc中的ORB特征點(diǎn)進(jìn)行三角化實(shí)現(xiàn)新的地圖云點(diǎn)的創(chuàng)建。然后對(duì)當(dāng)前處理的關(guān)鍵幀Ki,以及在互視圖中與Ki連接的其他關(guān)鍵幀Kc進(jìn)行局部BA。為了使重構(gòu)保持簡(jiǎn)潔,構(gòu)建局部地圖時(shí)應(yīng)盡量檢測(cè)并刪除冗余的關(guān)鍵幀;
步驟3閉環(huán)控制線(xiàn)程。閉環(huán)控制線(xiàn)程抽取最后一幀局部地圖關(guān)鍵幀Ki用于檢測(cè)和閉合回環(huán)。為了獲得候選回環(huán),必須檢測(cè)3個(gè)一致的候選回環(huán)(互視圖中相連的關(guān)鍵幀)。得到候選回環(huán)后,需要計(jì)算從當(dāng)前關(guān)鍵幀Ki到回環(huán)關(guān)鍵幀KI的相似變換,以獲得回環(huán)的累積誤差。計(jì)算相似變換也可以作為回環(huán)的幾何驗(yàn)證?;丨h(huán)修正融合重復(fù)的地圖云點(diǎn),在互視圖中插入與回環(huán)相關(guān)的新邊緣。融合過(guò)程中所有的關(guān)鍵幀將會(huì)更新它們?cè)诨ヒ晥D中的邊緣,創(chuàng)建的新邊緣將用于回環(huán)檢測(cè)。為了有效地閉合回環(huán),通過(guò)本質(zhì)圖優(yōu)化位姿圖,將回環(huán)閉合的誤差分散到圖像中去。優(yōu)化過(guò)后,每一個(gè)地圖云點(diǎn)都根據(jù)關(guān)鍵幀的校正進(jìn)行變換。
由于傳統(tǒng)ORB算法使用的是FAST算法來(lái)檢測(cè)圖像中的角點(diǎn),而FAST最大的問(wèn)題在于無(wú)法保證檢測(cè)到的角點(diǎn)具有尺度信息,因此特征點(diǎn)不具有尺度不變性。當(dāng)圖像中特征點(diǎn)的尺度發(fā)生較大的變化時(shí),傳統(tǒng)ORB算法會(huì)產(chǎn)生非常大的誤匹配,匹配效果較差,如圖3所示。單目視覺(jué)SLAM的特征點(diǎn)容易發(fā)生較大的尺度變化,因此為提高特征點(diǎn)匹配精度,需要采取一些措施增加ORB算法的尺度不變性。
圖3 傳統(tǒng)ORB算法在尺度發(fā)生變化時(shí)的特征點(diǎn)匹配狀況Figure 3.Matching status of feature points of traditional ORB algorithm when the scale changes
本文在傳統(tǒng)ORB算法的基礎(chǔ)上,引入SURF算法。SURF算法的一個(gè)重要特性是創(chuàng)建了尺度空間。本文提出的算法結(jié)合SURF算法的尺度空間思想,使得改進(jìn)ORB算法具有一定的尺度不變性。
尺度空間理論是指在處理圖像信息時(shí)引入一個(gè)尺度參數(shù),通過(guò)圖像尺度的變換,獲得不同尺度下的一系列圖像,然后在這一系列圖像上進(jìn)行相應(yīng)的處理[11]。Hessian矩陣是SURF算法的核心,構(gòu)建Hessian矩陣的目的是為了生成圖像穩(wěn)定的邊緣點(diǎn)(突變點(diǎn)),為后面的特征提取做好基礎(chǔ)。每一個(gè)像素點(diǎn)都可以求出一個(gè)Hessian矩陣,如式(1)所示。
(1)
Hessian矩陣的判別式為
(2)
當(dāng)Hessian矩陣的判別式取得局部極大值時(shí),則判定當(dāng)前點(diǎn)是比周?chē)徲騼?nèi)其他點(diǎn)更亮或更暗的點(diǎn),由此來(lái)定位關(guān)鍵點(diǎn)的位置。
在SURF算法中,圖像像素l(x,y)即為函數(shù)值f(x,y)。但是由于特征點(diǎn)需要具備尺度無(wú)關(guān)性,所以在進(jìn)行Hessian矩陣構(gòu)造前,需要對(duì)其進(jìn)行高斯濾波。本文選用二階標(biāo)準(zhǔn)高斯函數(shù)作為濾波器。
L(x,t)=G(t)·I(x,t)
(3)
通過(guò)特定核間的卷積計(jì)算二階偏導(dǎo)數(shù),即可得出H矩陣的3個(gè)矩陣元素Lxx、Lxy、Lyy,從而計(jì)算出H矩陣。
(4)
由于高斯核是服從正態(tài)分布的,從中心點(diǎn)向外,系數(shù)越來(lái)越低。為了提高運(yùn)算速度,SURF使用了盒式濾波器來(lái)近似替代高斯濾波器。盒式濾波器(Boxfilter)將圖像的濾波轉(zhuǎn)化成計(jì)算圖像上不同區(qū)域間像素和的加減運(yùn)算問(wèn)題,只需要查找積分圖就可以完成。
每個(gè)像素的Hessian矩陣行列式的近似值為
(5)
在Dxy上乘以一個(gè)加權(quán)系數(shù)0.9,目的是為了平衡因使用盒式濾波器近似所帶來(lái)的誤差。
本文在ORB-SLAM算法基礎(chǔ)上應(yīng)用單目高精度半稠密建圖算法[12],在局部建圖線(xiàn)程后添加半稠密建圖線(xiàn)程,與閉環(huán)控制線(xiàn)程同步進(jìn)行,建立較為直觀(guān)的基于特征法的SLAM半稠密三維點(diǎn)云地圖。算法增加部分的框架如圖4所示。半稠密建圖線(xiàn)程的詳細(xì)步驟如下:
圖4 半稠密建圖線(xiàn)程框架Figure 4.Semi-dense mapping thread framework
步驟1在N個(gè)相鄰關(guān)鍵幀上,沿極線(xiàn)搜索高梯度區(qū)域的每一個(gè)像素,得到N個(gè)逆深度假設(shè);
步驟2每個(gè)逆向深度假設(shè)采用高斯分布表示,并在匹配過(guò)程中考慮圖像噪聲、時(shí)差和模糊度;
步驟4由文獻(xiàn)[13]提出,在逆深度地圖上應(yīng)用一個(gè)過(guò)渡的步驟,以便于一個(gè)像素可以和它臨近的像素進(jìn)行平均。如果一個(gè)像素的逆深度分布與它臨近的像素不兼容,則剔除該像素;
步驟5在相鄰關(guān)鍵幀計(jì)算出它們各自的逆深度地圖后,跨越相鄰關(guān)鍵幀來(lái)檢查每個(gè)像素深度的一致性以剔除異常值。最后通過(guò)優(yōu)化使深度更加精確。
本文將圖2 ORB-SLAM算法框架中跟蹤線(xiàn)程的ORB特征提取部分改為上文中具有尺度不變性的ORB特征提取算法。SURF算法通過(guò)改變模板的大小來(lái)獲得不同的尺度,因此本文改進(jìn)的ORB算法結(jié)合了SURF的特征。本文采用積分圖像和盒氏濾波器進(jìn)行卷積,通過(guò)高斯核函數(shù)構(gòu)建尺度空間并求解快速Hessian響應(yīng)提取特征點(diǎn),并在特征點(diǎn)的3×3×3鄰域內(nèi),將Hessian矩陣處理的每個(gè)像素與其三維空間內(nèi)鄰域的26個(gè)點(diǎn)進(jìn)行比較,若為最大值或最小值,則保留。然后,采用三維線(xiàn)性插值法[14]得到亞像素級(jí)別的特征點(diǎn),此時(shí)的特征點(diǎn)具備尺度不變性。最后,使用ORB描述子對(duì)具備尺度不變性和旋轉(zhuǎn)不變性的特征點(diǎn)進(jìn)行描述,得到二進(jìn)制描述符。
通過(guò)上述步驟,機(jī)器人在運(yùn)動(dòng)過(guò)程中就可以使用單目攝像頭獲取周?chē)h(huán)境的圖像,并通過(guò)對(duì)圖像進(jìn)行改進(jìn)ORB特征檢測(cè),獲取具備尺度不變性的特征點(diǎn)作為環(huán)境特征信息,將更加穩(wěn)定、精確的特征點(diǎn)輸入到半稠密ORB-SLAM算法中。
本文使用TUM RGBD數(shù)據(jù)集基準(zhǔn)[15]測(cè)試改進(jìn)算法的性能,進(jìn)行室內(nèi)環(huán)境下改進(jìn)ORB-SLAM算法三維建圖的仿真。TUM RGBD數(shù)據(jù)集能夠客觀(guān)評(píng)價(jià)算法的科學(xué)性,并對(duì)比不同算法之間的性能。為驗(yàn)證算法的一致性,本文選用了具有代表性的數(shù)據(jù)集freiburg1_xyz和freiburg2_xyz。在freiburg1_xyz中,Kinect相機(jī)(實(shí)驗(yàn)中未使用該相機(jī)深度通道所提供的信息)沿著相機(jī)主軸平移運(yùn)動(dòng),方向保持固定。該數(shù)據(jù)集運(yùn)行時(shí)間短,數(shù)據(jù)量小,較為簡(jiǎn)便,比較適用于算法調(diào)試以排除故障。在freiburg2_xyz中,Kinect相機(jī)在地面坐標(biāo)系x、y、z3個(gè)方向上沿著相機(jī)主軸緩慢移動(dòng)。相機(jī)的緩慢移動(dòng)消除了數(shù)據(jù)中的運(yùn)動(dòng)模糊和滾動(dòng)快門(mén)現(xiàn)象,能夠提高建圖效果。
本文仿真實(shí)驗(yàn)平臺(tái)為一臺(tái)Intel(R)Core(TM)i5-7300U CPU @ 2.60 GHz,8 GB內(nèi)存的計(jì)算機(jī),安裝有Ubuntu16.04版本Linux操作系統(tǒng)。在ROS(機(jī)器人操作系統(tǒng))環(huán)境下運(yùn)用C++編程語(yǔ)言實(shí)現(xiàn)改進(jìn)的半稠密算法。原ORB-SLAM算法與本文提出的改進(jìn)算法均廣泛使用了計(jì)算機(jī)的線(xiàn)程并行計(jì)算能力,加快了算法的運(yùn)行速度。
圖5為本實(shí)驗(yàn)中數(shù)據(jù)集freiburg1_xyz與freiburg2_xyz的建圖效果。圖中第1、第2行分別為在數(shù)據(jù)集freiburg1_xyz和freiburg2_xyz中的實(shí)驗(yàn)結(jié)果。在第1列圖片中,采用具有尺度不變性的ORB特征點(diǎn)檢測(cè)算法檢測(cè)動(dòng)態(tài)圖像的角點(diǎn)。第2列圖片為ORB-SLAM算法根據(jù)檢測(cè)到的特征點(diǎn)建立的稀疏三維點(diǎn)云圖。第3列圖片為改進(jìn)ORB-SLAM算法建立半稠密三維點(diǎn)云圖。通過(guò)第2列和第3列圖片的對(duì)比可以看出,ORB-SLAM算法能基本建立小場(chǎng)景環(huán)境的三維點(diǎn)云地圖,但是點(diǎn)云非常稀疏,而且沒(méi)有顏色特征,難以分辨地圖的真實(shí)形態(tài)。本文提出的改進(jìn)ORB-SLAM算法使用文獻(xiàn)[12]中提出的半稠密建圖方法,并引入具有尺度不變性的ORB算法,使得改進(jìn)ORB-SLAM算法建立的三維點(diǎn)云圖能明顯分辨出物體的形狀特征,生成更加直觀(guān)的三維地圖。
圖5 數(shù)據(jù)集freiburg1_xyz與freiburg2_xyz的建圖效果Figure 5.The mapping effect of freiburg1_xyz and freiburg2_xyz datasets
本實(shí)驗(yàn)將ORB-SLAM算法、半稠密建圖算法和改進(jìn)的ORB-SLAM 算法分別在數(shù)據(jù)集freiburg1_xyz與freiburg2_xyz上運(yùn)行。實(shí)驗(yàn)結(jié)果表明,ORB-SLAM算法在freiburg1_xyz中運(yùn)行時(shí)長(zhǎng)為34.96 s,比該序列本身時(shí)長(zhǎng)多4.87 s;半稠密算法由于在ORB-SLAM的基礎(chǔ)上增加了半稠密建圖線(xiàn)程,相比ORB-SLAM的時(shí)長(zhǎng)增加了1.59 s;改進(jìn)ORB-SLAM算法既包括了半稠密建圖線(xiàn)程,改進(jìn)了ORB特征匹配,相比半稠密算法的時(shí)長(zhǎng)增加了0.68 s,時(shí)長(zhǎng)增加量在可接受范圍內(nèi)。時(shí)長(zhǎng)對(duì)比如表1所示。
在特征匹配精度方面,ORB-SLAM算法和半稠密算法包含相同的跟蹤線(xiàn)程,即具有相同的ORB特征提取方法,故特征匹配精度一致,在序列freiburg1_xyz中精度均為83.8%。改進(jìn)ORB-SLAM算法的ORB特征匹配具有尺度不變性的特點(diǎn),精度達(dá)到88.9%。在實(shí)驗(yàn)中,數(shù)據(jù)集freiburg1_xyz與freiburg2_xyz的主要區(qū)別在于前者時(shí)長(zhǎng)短,運(yùn)行速度快,檢測(cè)到的正確匹配點(diǎn)較少,而后者時(shí)長(zhǎng)較長(zhǎng),相機(jī)運(yùn)動(dòng)緩慢,能夠檢測(cè)到更多的正確匹配點(diǎn),所建的地圖場(chǎng)景更加豐富,點(diǎn)云更加稠密。由表1和表2可以看出,兩個(gè)序列的實(shí)驗(yàn)結(jié)果有相同規(guī)律,證明了本文改進(jìn)算法的一致性。
表1 實(shí)時(shí)運(yùn)行時(shí)長(zhǎng)對(duì)比Table 1. Comparison of real-time running time /s
表2 特征匹配精度對(duì)比Table 2. Comparison of feature matching accuracy /%
數(shù)據(jù)集仿真實(shí)驗(yàn)表明,相比傳統(tǒng)ORB-SLAM算法,本文提出的改進(jìn)ORB-SLAM算法能夠建立效果更好的半稠密三維點(diǎn)云地圖,并提高ORB特征點(diǎn)的匹配精度,且運(yùn)行時(shí)長(zhǎng)的增加量在可接受范圍內(nèi)。但是本文所研究的半稠密三維點(diǎn)云地圖仍存在很多缺陷,并不能完全復(fù)現(xiàn)真實(shí)環(huán)境中的物體。稠密三維點(diǎn)云地圖更加貼近于真實(shí)環(huán)境,也有助于后期語(yǔ)義SLAM[16]的研究,這也是今后的研究重點(diǎn)。