葉培楚,李 東,章 云
(廣東工業(yè)大學(xué) 自動(dòng)化學(xué)院,廣東 廣州 510006)
自1980年至今,SLAM(即時(shí)定位與地圖構(gòu)建)[1-3]一直都是計(jì)算機(jī)視覺(jué)和機(jī)器人領(lǐng)域的熱門(mén)研究方向,廣泛應(yīng)用于需要實(shí)時(shí)定位的設(shè)備比如移動(dòng)機(jī)器人、無(wú)人機(jī)[4]、自動(dòng)駕駛汽車(chē)[5-6]以及增強(qiáng)現(xiàn)實(shí)/虛擬現(xiàn)實(shí)[7]等。現(xiàn)有的SLAM算法中涵蓋了多種不同類(lèi)型的傳感器,比如相機(jī)、激光雷達(dá)、GPS或者慣性測(cè)量單元(Inertial Measurement Unit, IMU)等。由于相機(jī)具有成本低、普及率高并且可被動(dòng)地獲取豐富的環(huán)境信息等優(yōu)點(diǎn),因此基于相機(jī)的視覺(jué)SLAM研究成為熱點(diǎn)。
視覺(jué)SLAM主要由前端和后端兩部分組成:前端實(shí)現(xiàn)對(duì)相機(jī)的跟蹤,根據(jù)當(dāng)前地圖全局定位相機(jī)[7-10],也可以根據(jù)視覺(jué)里程計(jì)本地定位相機(jī)[11-12]。后端創(chuàng)建和維護(hù)關(guān)鍵幀地圖并通過(guò)閉環(huán)檢測(cè)和地圖優(yōu)化減少全局誤差。
現(xiàn)有的視覺(jué)SLAM算法可以分為間接法和直接法,其中間接法通過(guò)對(duì)圖像提取特征點(diǎn),實(shí)現(xiàn)對(duì)相機(jī)的定位和環(huán)境地圖的構(gòu)建,但只用少量的特征點(diǎn)代替一整幅圖像導(dǎo)致重建的環(huán)境地圖非常稀疏,無(wú)法用于機(jī)器人等設(shè)備導(dǎo)航[8-9]。盡管ORB-SLAM3[10]采用IMU和相機(jī)緊耦合的方式提高了定位精度[8-9],并且利用新的回環(huán)方式減少系統(tǒng)整體的累積誤差,但是其純雙目版本的性能并無(wú)改善。此外,特征點(diǎn)的SLAM方法會(huì)耗費(fèi)大量時(shí)間進(jìn)行特征提取,并且對(duì)于弱紋理場(chǎng)景不具備很好的魯棒性。鑒于特征點(diǎn)這些缺點(diǎn),文獻(xiàn)[12]提出了一種直接法的SLAM算法,直接基于圖像所有像素點(diǎn)和邊緣進(jìn)行處理,實(shí)現(xiàn)對(duì)相機(jī)的定位和環(huán)境地圖構(gòu)建,并且構(gòu)建出非常稠密的環(huán)境地圖。然而,重建非常稠密的環(huán)境地圖需要消耗大量的計(jì)算資源,不適用于一般的移動(dòng)設(shè)備,比如增強(qiáng)現(xiàn)實(shí)、虛擬現(xiàn)實(shí)、無(wú)人機(jī)等。因此,文獻(xiàn)[13]提出一種基于直接法的稀疏里程計(jì)方法(Direct Sparse Odometry, DSO),通過(guò)提取圖像中梯度較大的點(diǎn)進(jìn)行相機(jī)定位,在后端采用滑動(dòng)窗口和邊緣化技術(shù)對(duì)相機(jī)位姿和環(huán)境點(diǎn)云進(jìn)行優(yōu)化,實(shí)現(xiàn)相對(duì)稠密的環(huán)境重建以及定位,同時(shí)達(dá)到較高的幀率。然而,采用直接法、滑動(dòng)窗口和邊緣化技術(shù)的直接稀疏里程計(jì)目前仍然存在許多挑戰(zhàn),比如在室內(nèi)環(huán)境中運(yùn)行時(shí)無(wú)法產(chǎn)生具有絕對(duì)尺度的定位結(jié)果和環(huán)境重建,在室外場(chǎng)景中運(yùn)行時(shí)會(huì)產(chǎn)生嚴(yán)重的尺度漂移問(wèn)題。雙目DSO通過(guò)引入雙目約束,解決了單目DSO中的尺度不確定性和大尺度環(huán)境下漂移的問(wèn)題,但其深度初始化效率低,且并未在運(yùn)動(dòng)估計(jì)初值時(shí)引入雙目約束,無(wú)法得到較好的絕對(duì)尺度信息[14]。
針對(duì)上述問(wèn)題,本文提出一種基于雙目強(qiáng)約束的直接稀疏視覺(jué)里程計(jì)。主要的工作如下:
(1) 提出一種新的雙目初始化方案,通過(guò)對(duì)梯度點(diǎn)提取快速描述子,采用絕對(duì)誤差和算法(Sum of Absolute Differences, SAD)在極線(xiàn)方向搜索最優(yōu)匹配點(diǎn),利用歸一化積相關(guān)算法(Normalized Cross Correlation, NCC)進(jìn)行驗(yàn)證,實(shí)現(xiàn)了快速的雙目初始化。
(2) 基于雙目相機(jī)固定基線(xiàn)長(zhǎng)度設(shè)置閾值,根據(jù)梯度點(diǎn)的初始深度值將所有點(diǎn)劃分為近點(diǎn)和遠(yuǎn)點(diǎn)兩種類(lèi)型。對(duì)遠(yuǎn)點(diǎn)和近點(diǎn)分別賦予不同的深度范圍,深度濾波器在幀間濾波時(shí)可以快速收斂到其真實(shí)值。
(3) 在估計(jì)移動(dòng)機(jī)器人運(yùn)動(dòng)初值時(shí),引入雙目約束,增強(qiáng)運(yùn)動(dòng)估計(jì)階段的絕對(duì)尺度信息,使得移動(dòng)機(jī)器人在定位精度上得到顯著提高。
(4) 與現(xiàn)有方法相比,本文所提出的方法無(wú)需提取特征點(diǎn),對(duì)于弱紋理場(chǎng)景具有較好的魯棒性。優(yōu)化目標(biāo)函數(shù)包含所有遠(yuǎn)點(diǎn)和近點(diǎn),極大提高了移動(dòng)機(jī)器人定位的旋轉(zhuǎn)準(zhǔn)確率。實(shí)驗(yàn)結(jié)果表明,本文提出的方案在直接法SLAM中,如Stereo DSO與LSD-SLAM2,定位精度上達(dá)到了先進(jìn)水平,而相比特征點(diǎn)法的SLAM方案,本文方案達(dá)到了與ORB-SLAM3相近的水平。
本文提出的基于雙目強(qiáng)約束的直接稀疏視覺(jué)里程計(jì)系統(tǒng)流程如圖1所示。
圖1 本文系統(tǒng)流程圖Fig.1 The proposed system flow
系統(tǒng)的輸入是雙目圖像對(duì),通過(guò)對(duì)左幀圖像網(wǎng)格化,并在每個(gè)網(wǎng)格中計(jì)算梯度信息,得到期望的關(guān)鍵點(diǎn)。結(jié)合SAD以及NCC雙步驟驗(yàn)證的方式進(jìn)行雙目圖像初始化,得到每個(gè)關(guān)鍵點(diǎn)的初始深度值。
基于圖像雙目固定基線(xiàn)長(zhǎng)度設(shè)置參考閾值,根據(jù)初始深度值將所有關(guān)鍵點(diǎn)劃分成近點(diǎn)和遠(yuǎn)點(diǎn)。由于雙目相機(jī)的固定基線(xiàn)長(zhǎng)度較短,對(duì)于近距離的點(diǎn)深度估計(jì)較為準(zhǔn)確,因此近距離的點(diǎn)初始深度范圍被限制得比較小。遠(yuǎn)距離的點(diǎn)深度直接由雙目相機(jī)估計(jì)會(huì)產(chǎn)生較大的誤差,需要融合較多相鄰幀的信息做深度優(yōu)化,因此遠(yuǎn)距離的點(diǎn)初始化深度范圍設(shè)置得相對(duì)比較大。
在估計(jì)相對(duì)運(yùn)動(dòng)初值時(shí),引入雙目相機(jī)約束,從而更準(zhǔn)確地估計(jì)絕對(duì)尺度。
在系統(tǒng)后端,所有模型參數(shù)包括相機(jī)固有參數(shù)、所有關(guān)鍵幀的相機(jī)位姿以及所有關(guān)鍵點(diǎn)的深度值,都在一個(gè)滑動(dòng)窗口中聯(lián)合優(yōu)化。為了在滑動(dòng)窗口中保持固定數(shù)量的關(guān)鍵幀和激活點(diǎn),系統(tǒng)采用了邊緣化的操作,以保持?jǐn)?shù)據(jù)關(guān)聯(lián)的一致性。
為了獲得跟蹤階段需要的初始深度圖像,文獻(xiàn)[14]采用NCC的雙目匹配方式[15]實(shí)現(xiàn)雙目初始化。對(duì)于左幀圖像IL中檢測(cè)到的每個(gè)關(guān)鍵點(diǎn)p,沿著極線(xiàn)方向在右?guī)瑘D像IR計(jì)算NCC匹配分?jǐn)?shù),搜索最優(yōu)匹配點(diǎn)q。對(duì)于每一個(gè)3 ×5的圖像塊M,NCC度量相似性的計(jì)算方法如式(1)所示。
其中,Mp和Mq分 別表示左幀圖像中的點(diǎn)p和右?guī)瑘D像中的點(diǎn)q對(duì) 應(yīng)的3 ×5的 匹配塊,IL[pi]和IR[qj]分別表示左幀圖像塊Mp中的點(diǎn)pi與右?guī)瑘D像塊Mq中的點(diǎn)qj對(duì) 應(yīng)的圖像光度值,ILavg[p]和IRavg[q]分別表示對(duì)應(yīng)圖像塊光度平均值。
由于每一次匹配,NCC都必須計(jì)算公式(1),顯然是非常耗時(shí)的。為了提高雙目初始化的效率,并保持與Stereo DSO[14]深度濾波器誤差方程的一致性,本文提出Two-stages的雙目初始化方式。采用圖2所示的描述子[16]對(duì)SAD進(jìn)行搜索匹配[17],如式(2)所示。
圖2 雙目初始化采用的描述子Fig.2 The descriptor used for stereo initialization
其中,Qp和Qq分別表示左幀圖像中的點(diǎn)p和右?guī)瑘D像中的點(diǎn)q對(duì)應(yīng)的描述子。
顯然,本文采用的描述子的大小為1 6維,與NCC匹配塊大小接近,但度量相似性的計(jì)算步驟更簡(jiǎn)單,可以極大提高匹配效率。為了保證匹配結(jié)果可靠性,NCC被用于驗(yàn)證SAD的匹配結(jié)果。通過(guò)這種雙步驟驗(yàn)證的方式,降低雙目初始化的計(jì)算量,在不損失定位精度的情況下提高了匹配速度。
由于雙目相機(jī)固定基線(xiàn)的長(zhǎng)度固定,距離相機(jī)中心比較遠(yuǎn)的關(guān)鍵點(diǎn)深度值無(wú)法準(zhǔn)確測(cè)量,因此得到的初始深度圖像并不完全可靠。
如圖3所示,假設(shè)世界坐標(biāo)系下有兩個(gè)點(diǎn)P和Q,它們?cè)谧蟆⒂規(guī)瑘D像中的投影分別用p1、p2和q1、q2表示,根據(jù)多視圖幾何約束,可以通過(guò)三角化得到其對(duì)應(yīng)空間深度值。值得注意的是,由于測(cè)量存在誤差,對(duì)于距離相機(jī)中心較近的點(diǎn),當(dāng)測(cè)量存在一個(gè)像素誤差時(shí),估計(jì)的深度值誤差為eP,而距離相機(jī)中心較遠(yuǎn)的點(diǎn),一個(gè)像素誤差導(dǎo)致的深度值誤差為eQ,顯然eP<eQ。距離相機(jī)中心越遠(yuǎn)的點(diǎn),其深度值不確定性會(huì)越大,即測(cè)量誤差越大。
圖3 三角化測(cè)量不確定性Fig.3 Uncertainty in triangulation
在本文中,參考文獻(xiàn)[9]將區(qū)分近點(diǎn)和遠(yuǎn)點(diǎn)的閾值設(shè)置為40倍雙目相機(jī)固定基線(xiàn)長(zhǎng)度。初始深度值小于40倍基線(xiàn)的點(diǎn)被定義為近點(diǎn),否則定義為遠(yuǎn)點(diǎn)。Stereo DSO[14]采用深度濾波器[18]對(duì)關(guān)鍵點(diǎn)的深度進(jìn)行估計(jì),但其對(duì)所有的點(diǎn)都采用同樣的深度范圍,并未考慮三角測(cè)量不確定性。本文通過(guò)區(qū)分近點(diǎn)和遠(yuǎn)點(diǎn),對(duì)近點(diǎn)賦予較小的深度范圍,對(duì)遠(yuǎn)點(diǎn)賦予較大的深度范圍。通過(guò)相鄰幀約束,使關(guān)鍵點(diǎn)的深度范圍快速收斂到真實(shí)值。
在跟蹤線(xiàn)程中,滑動(dòng)窗口中的所有激活點(diǎn)被投影到最新的關(guān)鍵幀構(gòu)建參考幀深度圖。通過(guò)將參考幀中的點(diǎn)反投影到當(dāng)前幀并優(yōu)化能量函數(shù)(3),可以獲得當(dāng)前幀Ic相對(duì)于參考幀Ir的相對(duì)運(yùn)動(dòng)。
其中,Pr表示參考幀圖像中的點(diǎn),Np采用與DSO[13]一樣的圖像塊,ωp表示點(diǎn)的權(quán)重,并且Np中所有的點(diǎn)均采用同樣的權(quán)重, ‖ ·‖γ為核函數(shù),防止畸形點(diǎn)破壞優(yōu)化結(jié)構(gòu)。 (ar,br)和 (ac,bc)分別表示參考幀和當(dāng)前幀的光度仿射參數(shù),用于建模光度變化。對(duì)于投影點(diǎn)q~,由公式(4)計(jì)算得出。
其中,π (·)表示將相機(jī)坐標(biāo)系的三維點(diǎn)投影到相機(jī)平面,同理,π-1(·)表示將相機(jī)平面的點(diǎn)反投影到相機(jī)坐標(biāo)系中。Tcr表 示參考幀到當(dāng)前幀的相對(duì)位姿,dp為點(diǎn)p的逆深度,Np中的所有點(diǎn)均采用同樣的深度值。
實(shí)際上,具有固定基線(xiàn)的雙目圖像可以為絕對(duì)尺度估計(jì)提供更多的約束,但Stereo DSO[14]并未引入這個(gè)約束。因此,在本文中,通過(guò)引入雙目約束,將能量函數(shù)(3)拓展為公式(5)。
通過(guò)最小化公式(5),當(dāng)前幀的參數(shù)包括相機(jī)位姿、左右?guī)瑘D像的光度仿射參數(shù)被有效初始化,隨后傳入后端滑動(dòng)窗口[19]中進(jìn)一步優(yōu)化。
本文使用KITTI數(shù)據(jù)集[20]對(duì)所提出的算法進(jìn)行驗(yàn)證與分析。KITTI數(shù)據(jù)集是車(chē)載雙目相機(jī)在街區(qū)和高速公路等地區(qū)同步采集,并經(jīng)過(guò)校正得到的高分辨率雙目圖像,是視覺(jué)SLAM測(cè)試算法在大場(chǎng)景環(huán)境下的定位性能常用的數(shù)據(jù)集。為了驗(yàn)證本文算法的有效性,在該數(shù)據(jù)集上進(jìn)行了定位與建圖實(shí)驗(yàn),同時(shí)與Stereo DSO[14]、LSD-SLAM2[21]以及ORBSLAM3[10]分別進(jìn)行了對(duì)比,其中Stereo DSO、LSDSLAM2是目前公開(kāi)性能最優(yōu)的直接法SLAM方案,ORB-SLAM3是目前公開(kāi)性能最優(yōu)的特征點(diǎn)法SLAM方案。本文算法的實(shí)驗(yàn)平臺(tái)為:Intel i5 4200H處理(2.8 GH),12 GB RAM,64位Ubuntu16.04操作系統(tǒng)。
由于本文所提出的方案是基于直接法的SLAM,因此為了驗(yàn)證本文方案的性能,首先將本文方案與基于直接法的視覺(jué)SLAM方案(LSD-SLAM2以及Stereo DSO)在位移誤差、旋轉(zhuǎn)誤差上進(jìn)行定位精度對(duì)比。由于Stereo DSO并未開(kāi)源代碼,我們無(wú)法直接與其進(jìn)行對(duì)比實(shí)驗(yàn)。通過(guò)復(fù)現(xiàn)Stereo DSO提出的方法,與本文方案進(jìn)行對(duì)比。從表1的實(shí)驗(yàn)結(jié)果可以看出,本文提出的基于雙目強(qiáng)約束的直接稀疏里程計(jì)系統(tǒng)通過(guò)增強(qiáng)雙目約束條件,實(shí)現(xiàn)了比Stereo DSO與LSD-SLAM2更好的定位效果。特別是,本文的平均位移誤差和平均旋轉(zhuǎn)誤差分別為0.89%和0.23%,而LSD-SLAM2為1.14%和0.4%,Stereo DSO為0.95%和0.25%,均是本文提出的方案最優(yōu)。相比LSD-SLAM2和Stereo DSO,本文采用快速雙目初始化的策略,提高梯度點(diǎn)深度值初始化效率,同時(shí)根據(jù)初值深度值區(qū)分近遠(yuǎn)點(diǎn),為深度濾波器提供較為準(zhǔn)確的深度初始范圍,使梯度點(diǎn)可以快速收斂到其真實(shí)值。在估計(jì)運(yùn)動(dòng)初始階段,引入雙目約束,可以有效克服尺度誤差,提高定位精度。
表1 本文算法與LSD-SLAM2以及Stereo DSO定位精度對(duì)比Table 1 Comparison of accuracy on KITTI between our proposed and LSD-SLAM2 as well as Stereo DSO
為了進(jìn)一步驗(yàn)證本文算法的可靠性,還與特征點(diǎn)法SLAM方案ORB-SLAM3進(jìn)行比較。由于本文算法是純雙目視覺(jué)里程計(jì),因此為了公平比較,關(guān)閉了ORB-SLAM3的回環(huán)模塊與全局優(yōu)化模塊等功能。實(shí)驗(yàn)結(jié)果如表2所示,顯然本文的直接法SLAM與基于特征點(diǎn)法的ORB-SLAM3取得相近的性能,在平均位移誤差上,本文算法弱于ORB-SLAM3,主要原因是KITTI數(shù)據(jù)集幀間距離較遠(yuǎn),相鄰幀圖像同一位置的像素偏差較大,對(duì)直接法SLAM方案挑戰(zhàn)較大,此外,光照強(qiáng)度變化較大時(shí)也會(huì)對(duì)直接法的定位精度產(chǎn)生一定影響。特別是,在KITTI序列03和序列09,本文提出的方案在位移誤差上與ORB-SLAM3相差較大,原因在于這兩個(gè)序列的光照強(qiáng)度較大,強(qiáng)弱變化也非常頻繁,直接法很難對(duì)這樣的光照變化進(jìn)行非常準(zhǔn)確的建模。但在旋轉(zhuǎn)誤差上,本文算法幾乎完全優(yōu)于ORB-SLAM3。這是由于ORB-SLAM3在估計(jì)運(yùn)動(dòng)時(shí)只考慮近點(diǎn),而本文算法同時(shí)考慮遠(yuǎn)點(diǎn)和近點(diǎn)兩類(lèi)點(diǎn),其中近點(diǎn)可以提高位移估計(jì)的準(zhǔn)確度,而遠(yuǎn)點(diǎn)可以更好地估計(jì)旋轉(zhuǎn)變化,特別是表2中的序列01和序列10,旋轉(zhuǎn)誤差明顯遠(yuǎn)小于ORB-SLAM3。從實(shí)驗(yàn)結(jié)果也可以明顯看出,同時(shí)融合遠(yuǎn)點(diǎn)和近點(diǎn)的策略可以極大提高旋轉(zhuǎn)估計(jì)的精度。在絕對(duì)軌跡誤差上,本文算法在大部分序列中都優(yōu)于ORB-SLAM3,本文算法的絕對(duì)軌跡誤差為3.21 m,而ORB-SLAM3的絕對(duì)軌跡誤差為3.44 m。從綜合結(jié)果來(lái)看,本文提出的算法與ORB-SLAM3性能相近,為無(wú)人駕駛汽車(chē)/移動(dòng)機(jī)器人提供一個(gè)較優(yōu)的解決方案,特別是在弱紋理以及窄基線(xiàn)的場(chǎng)景環(huán)境中。
表2 本文算法與ORB-SLAM3定位精度對(duì)比Table 2 Comparison of accuracy on KITTI between our proposed and ORB-SLAM3
圖4是本文算法在KITTI 02序列估計(jì)的軌跡與真實(shí)軌跡的對(duì)比圖,可以看出本文提出的算法定位精度是非常高的。
圖4 本文算法在KITTI-02的估計(jì)軌跡與真實(shí)軌跡對(duì)比Fig.4 Comparison between the proposed and the ground-truth on KITTI 02
圖5是本文算法在KITTI 00序列重建的環(huán)境地圖,相比于基于特征點(diǎn)法的ORB-SLAM3,本文算法重建的環(huán)境地圖更加稠密,能更好地實(shí)現(xiàn)機(jī)器人/無(wú)人駕駛汽車(chē)的導(dǎo)航功能。
圖5 本文算法在KITTI-00的重建效果Fig.5 3D reconstruction of the proposed on KITTI 00
由上述實(shí)驗(yàn)結(jié)果可知,本文通過(guò)增強(qiáng)雙目約束,在公開(kāi)的直接法SLAM方案中定位精度達(dá)到了先進(jìn)水平,并實(shí)現(xiàn)了與最優(yōu)的特征點(diǎn)法SLAM系統(tǒng)ORBSLAM3相近的定位精度。
本文提出了基于雙目強(qiáng)約束的直接稀疏視覺(jué)里程計(jì)系統(tǒng)進(jìn)行SLAM應(yīng)用研究,為直接法SLAM提供一種改進(jìn)方案。通過(guò)引入SAD與NCC雙步驟的雙目匹配方案,實(shí)現(xiàn)了快速的雙目初始化。基于雙目相機(jī)固定基線(xiàn)長(zhǎng)度設(shè)置閾值條件,將觀(guān)測(cè)點(diǎn)劃分成遠(yuǎn)點(diǎn)和近點(diǎn),由于三角化不確定性隨觀(guān)測(cè)點(diǎn)的觀(guān)測(cè)深度變化而變化,因此對(duì)兩類(lèi)點(diǎn)分別初始化不同的深度范圍,后續(xù)深度濾波估計(jì)時(shí)可以快速收斂到其真實(shí)值。在估計(jì)相對(duì)運(yùn)動(dòng)初值時(shí),添加雙目約束可以為估計(jì)絕對(duì)尺度信息提供更多約束信息。本文分別與目前性能最優(yōu)的直接法SLAM系統(tǒng)Stereo DSO和LSDSLAM2以及性能最優(yōu)的特征點(diǎn)法SLAM系統(tǒng)ORBSLAM3在KITTI數(shù)據(jù)集11個(gè)序列上進(jìn)行了對(duì)比試驗(yàn),實(shí)驗(yàn)結(jié)果驗(yàn)證了雙目強(qiáng)約束的有效性并證明本文提出的方案在直接法SLAM中達(dá)到了先進(jìn)水平,相比于目前最優(yōu)的基于特征點(diǎn)法的ORB-SLAM3,本文方案實(shí)現(xiàn)了與其相近的定位精度。
本文提出的基于雙目強(qiáng)約束的直接稀疏視覺(jué)里程計(jì),僅是一個(gè)直接法視覺(jué)SLAM前端,隨著系統(tǒng)長(zhǎng)時(shí)間長(zhǎng)距離的運(yùn)動(dòng),會(huì)產(chǎn)生累積誤差,導(dǎo)致定位精度下降。因此,為了使本文提出的方案在大場(chǎng)景環(huán)境下更加魯棒,未來(lái)需要進(jìn)一步為系統(tǒng)添加回環(huán)檢測(cè)和全局優(yōu)化模塊。