一種基于結(jié)構(gòu)約束的點(diǎn)線特征融合SLAM 算法

馬廣飛,邵 巍,李晉屹,劉祥鵬

(青島科技大學(xué) 自動化與電子工程學(xué)院,山東 青島 266061)

SLAM 算法用于估計相機(jī)位姿,同時重建周圍的未知環(huán)境。它們在自主機(jī)器人、自動駕駛汽車和增強(qiáng)、虛擬現(xiàn)實(shí)等方面具有廣泛應(yīng)用價值[1]。特征點(diǎn)SLAM 從圖像中提取特征點(diǎn),通過特征點(diǎn)匹配計算相機(jī)位姿,是視覺SLAM 的主流方法[2]。DAVISON 等[3]提出的MonoSLAM 通過提取每幀特征點(diǎn)估計相機(jī)位姿,但因逐幀對特征點(diǎn)進(jìn)行提取,計算成本很高。ORB-SLAM2[4]是由Mur-Artal和Tardos提出的開源SLAM 算法,該算法以其速度快和精度高成為比較SLAM 算法性能時常用基準(zhǔn)之一。特征點(diǎn)SLAM 算法涌現(xiàn)出許多優(yōu)秀的算法,但這些算法在低紋理環(huán)境中依然面臨嚴(yán)峻挑戰(zhàn)[5]。線特征具有較好的光照不變性和旋轉(zhuǎn)不變性,可以提供更多的幾何結(jié)構(gòu)信息[6]。NEIRA 等[7]首次將直線特征應(yīng)用到單目SLAM 算法中,但該算法因線特征提取精度不高導(dǎo)致定位精度不理想。隨著LSD[8]、EDLines[9]線段提取算法以及LBD[10]線特征描述符等算法的提出,基于線特征的SLAM 算法獲得廣泛研究。然而,這些基于線特征的SLAM 算法仍存在端點(diǎn)位置不準(zhǔn)確、幾何約束不明確等問題,過多的誤匹配反而給系統(tǒng)帶來更多的累積誤差[11]。

研究人員為了得到性能更優(yōu)的SLAM 算法,將點(diǎn)特征與線特征結(jié)合,提出了一些基于點(diǎn)線特征融合的視覺SLAM 算法。GOMEZ-OJEDA 等[12]提出了PL-SLAM 算法,該算法在點(diǎn)特征稀疏的場景性能較好,但其存在數(shù)據(jù)冗余和較大的誤匹配問題。LI等[13]提出一種SLD 線段提取算法,將點(diǎn)線特征加入后端優(yōu)化,有效解決了優(yōu)化中的不穩(wěn)定問題?，F(xiàn)存點(diǎn)線特征SLAM 僅使用線特征的簡單幾何信息,沒有利用線特征的結(jié)構(gòu)性約束優(yōu)化特征位置與相機(jī)位姿,造成了結(jié)構(gòu)約束信息的浪費(fèi)。針對上述問題,本研究提出了一種基于結(jié)構(gòu)性約束的點(diǎn)線融合SLAM(point-line feature fusion based on structural constraints SLAM,PLSC-SLAM)算法。該算法充分利用點(diǎn)特征和線特征的結(jié)構(gòu)性約束的優(yōu)點(diǎn),提高了SLAM 的精度和穩(wěn)定性。具體開展的工作包括:1)利用RANSAC 方法魯棒擬合空間直線,減小空間直線位置誤差。2)將直線分為平行線與垂線,以此作為約束優(yōu)化直線的三維空間位置與相機(jī)位姿。3)利用CAPE[14]算法提取表面法向量,依據(jù)表面法向量估計環(huán)境中的曼哈頓主方向,然后關(guān)聯(lián)關(guān)鍵幀與估計的曼哈頓主方向,并將其作為局部地圖的約束優(yōu)化相機(jī)位姿與地圖。并在TUM RGB-D數(shù)據(jù)集上進(jìn)行PLSC-SLAM 算法評估與對比實(shí)驗(yàn)。

1 PLSC-SLAM 算法總體框架

如圖1所示,PLSC-SLAM 算法分為3個主線程:跟蹤、局部地圖、閉環(huán)檢測。該算法是以O(shè)RBSLAM 框架為基礎(chǔ),基于環(huán)境中結(jié)構(gòu)性約束設(shè)計的點(diǎn)線特征融合SLAM 算法。其中每個線程中的藍(lán)色部分是PLSC-SLAM 所做的工作。

圖1 PLSC-SLAM 算法框架Fig.1 Overview of PLSC-SLAM algorithm

跟蹤線程對輸入的每幀圖像進(jìn)行跟蹤與處理,是SLAM 中的基本流程。跟蹤的目的是提取當(dāng)前幀的特征,建立當(dāng)前幀與局部地圖之間的共視關(guān)系及約束關(guān)系,用于優(yōu)化當(dāng)前幀的相機(jī)位姿。SLAM系統(tǒng)完成系統(tǒng)初始化后,對輸入的當(dāng)前幀圖像提取點(diǎn)特征和線特征。在本階段,使用ORB 算法提取和描述點(diǎn)特征,使用LSD 算法和LBD 描述符提取和描述線特征。利用三角化將點(diǎn)特征的三維位置恢復(fù),然后將其映射到局部地圖并進(jìn)行匹配。對于線特征的三維位置,PLSC-SLAM 設(shè)計了平行線和垂直線分類算法,利用二者結(jié)構(gòu)性約束優(yōu)化線特征的三維位置。根據(jù)特征匹配關(guān)系,通過將當(dāng)前幀的匹配特征投影到參考幀,利用非線性優(yōu)化算法優(yōu)化點(diǎn)線特征的投影誤差即可得到當(dāng)前相機(jī)位姿。判斷當(dāng)前幀是否符合關(guān)鍵幀選取條件,然后將滿足條件的新關(guān)鍵幀關(guān)聯(lián)曼哈頓主方向,最后將新關(guān)鍵幀保存到關(guān)鍵幀集合中。

局部地圖線程對關(guān)鍵幀進(jìn)行處理,目的是更新共視圖與完成局部地圖的構(gòu)建。局部地圖由共視關(guān)鍵幀和關(guān)鍵幀對應(yīng)的地圖點(diǎn)線構(gòu)成。在該線程中,通過對新插入的關(guān)鍵幀計算,篩選關(guān)鍵幀和地圖點(diǎn)線。然后進(jìn)行局部BA(bundle adjustment,光束平差法)優(yōu)化,同時優(yōu)化相機(jī)位姿和地圖點(diǎn)線。最后,對插入的關(guān)鍵幀和新的地圖點(diǎn)線進(jìn)行篩選,防止出現(xiàn)冗余關(guān)鍵幀。PLSC-SLAM 設(shè)計了一個曼哈頓主方向估計算法,利用曼哈頓約束優(yōu)化關(guān)鍵幀的點(diǎn)線特征位置與相機(jī)位姿。

閉環(huán)檢測線程分為閉環(huán)檢測和閉環(huán)校正,主要用于消除估計的相機(jī)位姿和三角化后的地圖點(diǎn)累積誤差。閉環(huán)檢測利用詞袋模型檢測關(guān)鍵幀數(shù)據(jù)庫的相似程度來檢測閉環(huán)。閉環(huán)校正包括閉環(huán)融合和圖優(yōu)化,將累計誤差進(jìn)一步消除。

2 結(jié)構(gòu)性約束算法

2.1 垂線與平行線約束

在跟蹤線程中,當(dāng)特征點(diǎn)與特征線提取后,就可以獲得它們在相機(jī)坐標(biāo)系下的三維位置。對于一個特征點(diǎn)p i來說,通過對其進(jìn)行三角化即可得到與二維特征點(diǎn)對應(yīng)的三維地圖點(diǎn)P ci。與特征點(diǎn)相比,特征線更容易受到深度不連續(xù)和遮擋的影響,通過對特征線進(jìn)行三角化可能會導(dǎo)致特征線的三維位置不準(zhǔn)確,因此使用RANSAC 擬合空間直線方法計算直線的三維位置。首先,統(tǒng)計與線段相交的深度值不為零的像素數(shù)量,如果這個數(shù)量超過某個閾值,就通過RANSAC算法估計三維線L ci并去除外點(diǎn),具體流程如圖2所示。

圖2 RANSAC擬合空間直線流程圖Fig.2 RANSAC fitting spatial line flow chart

然后,使用場景的結(jié)構(gòu)性約束去優(yōu)化每條檢測的直線位置。對于當(dāng)前圖像中檢測的每個可能的直線對,通過式(1)計算兩個向量之間的夾角余弦值:

如圖3所示,圖中左圖給出了相機(jī)坐標(biāo)系中的特征點(diǎn)和特征線符號及其對應(yīng)的三維點(diǎn)線符號,右圖給出了垂線對誤差和平行線對誤差符號。

圖3 點(diǎn)線二維三維位置示意圖及垂線對、平行線對誤差示意圖Fig.3 Illustration of 2D and 3D position of point-line and illustration of perpendicular error and parallel error

定義L為待優(yōu)化的變量集合,其中至少包含一種關(guān)聯(lián)垂直與平行約束的線。然后,通過最小化以下誤差函數(shù)來優(yōu)化L的線端點(diǎn)位置:

其中ρ是用于減少外點(diǎn)影響的Huber核函數(shù)。

2.2 曼哈頓約束

如圖4所示,曼哈頓世界假設(shè)假定環(huán)境中存在3個相互垂直主方向組成的坐標(biāo)系O M,環(huán)境中存在的平面法向量與其中一個曼哈頓主方向一致。

圖4 曼哈頓世界假設(shè)示意圖Fig.4 Illustration of Manhattan world hypothesis

2.2.1 表面法向量提取

表面法向量使用CAPE[14]算法提取,該算法首先將平面擬合到網(wǎng)格單元,然后通過在這些平面單元上進(jìn)行區(qū)域生長來找到光滑的表面,最后將提取到的法向量表示為n=(n x n y nz)。CAPE 算法在不同場景延遲一致,速度更快。

為了便于將法向量可視化,用(n e+1)×255/2計算法向量對應(yīng)的3通道像素值,把表面法向量轉(zhuǎn)換成3通道圖像。其中n e分別取n x、n y、n z,可視化后的表面法向量如圖5所示。

圖5 表面法向量可視化Fig.5 Visualization of surface normal vector

2.2.2 曼哈頓主方向估計

將曼哈頓世界坐標(biāo)系(下文簡寫為MF)相對于相機(jī)坐標(biāo)系的旋轉(zhuǎn)關(guān)系用旋轉(zhuǎn)矩陣Q·=[q1q2q3]表示,其中q1、q2、q3分別是曼哈頓坐標(biāo)系的x、y、z軸在相機(jī)坐標(biāo)系下的表示。通過式(6)將所有投影到MF的三軸切平面去計算均值偏移:

其中n i表示相機(jī)系下表面法向量,表示MF表面法向量,索引i j表示第i個主方向圓錐截面內(nèi)的法向量。

使用黎曼對數(shù)映射表示切平面中的距離:

其中c表示高斯核寬度。使用黎曼指數(shù)將Mean-Shift算法結(jié)果轉(zhuǎn)換回單位球面:

對MF所有軸重復(fù)以上過程,即可得到更新后的旋轉(zhuǎn)矩陣:

使用Mean-Shift 聚類算法來初始化第一幀MF,從而使跟蹤與局部地圖建立在曼哈頓結(jié)構(gòu)性約束框架下。

將與關(guān)鍵幀k有共視關(guān)系的關(guān)鍵幀記作Kco,被關(guān)鍵幀Kco觀測到的地圖點(diǎn)和地圖線分別記作Pco和Lco,與關(guān)鍵幀k沒有共視關(guān)系但觀測到地圖點(diǎn)Pco和地圖線Lco的關(guān)鍵幀記作Knco。將Pco和Lco地圖點(diǎn)線與關(guān)鍵幀k中特征之間的匹配關(guān)系分別記作匹配集ΩPk和ΩLk,與關(guān)鍵幀k有共視關(guān)系的垂線和平行線集合分別記作將Knco中與曼哈頓主方向有關(guān)聯(lián)的地圖線記作ΩM。定義優(yōu)化變量,j∈Lco,l∈Kco},其中上標(biāo)w表示世界坐標(biāo)系。最后將優(yōu)化問題定義為

其中K=Kco∩Knco,M j∈ΩM,E是誤差。利用Levenberg-Marquardt算法對式(15)進(jìn)行優(yōu)化,即可得到優(yōu)化后的點(diǎn)線三維位置和相機(jī)位姿。

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

3.1 評估標(biāo)準(zhǔn)與實(shí)驗(yàn)環(huán)境

為了驗(yàn)證提出的PLSC-SLAM 算法的有效性,在TUM RGB-D[16]數(shù)據(jù)集上評估了該算法的性能。TUM RGB-D數(shù)據(jù)集提供了幾個圖像序列,包括正常運(yùn)動、快速運(yùn)動、低紋理、無結(jié)構(gòu)等特定場景。這些圖像序列還具有通過外部運(yùn)動捕捉系統(tǒng)獲得的相機(jī)軌跡真值,可滿足實(shí)驗(yàn)者不同的測試要求。

評價SLAM 算法精度指標(biāo)常分為絕對軌跡誤差(absolute pose error,APE)與相對軌跡誤差[17](relative pose error,RPE)。絕對軌跡誤差可以刻畫SLAM 算法估計位姿與真實(shí)位姿的旋轉(zhuǎn)和平移誤差,能夠反映系統(tǒng)精度與軌跡的一致性,故選取絕對軌跡誤差A(yù)PE 作為算法的精度評價指標(biāo)。使用evo SLAM 評估工具,將提出的PLSC-SLAM 算法與多種不同視覺SLAM 算法進(jìn)行比較。

所有實(shí)驗(yàn)均在一臺裝有Ubuntu 18.04 LTS操作系統(tǒng)的電腦上完成,12 GB RAM,處理器為Intel Core i5-7200U(2.50 GHz)。沒有進(jìn)行GPU 并行處理,以節(jié)省SLAM 算法運(yùn)行成本。編程語言為C++、Python3,第三方庫包括OpenCV 3.2、Eigen 3.1、G2O、Pangolin。

3.2 TUM RGB-D數(shù)據(jù)集定位結(jié)果與分析

PLSC-SLAM 對點(diǎn)線特征提取后,將提取的直線分為垂線與平行線,利用垂線與平行線的結(jié)構(gòu)性約束優(yōu)化地圖線位置與相機(jī)位姿,避免了因點(diǎn)特征不足和線特征位置誤差大導(dǎo)致定位失敗及誤差大的問題。fr1_room 圖像序列沿著整個辦公室墻壁走向拍攝,從4張桌子開始,然后圍繞房間的墻壁運(yùn)動,直到回到起點(diǎn)。該圖像序列具有在墻壁、窗戶上紋理稀疏,但墻角和窗戶處存在垂直與平行的邊緣直線的特點(diǎn)。圖6、圖7分別是PLSC-SLAM、ORBSLAM3在fr1_room 圖像序列運(yùn)行時的特征提取對比圖與稀疏地圖對比圖。從圖6(a)可以看出對圖像進(jìn)行點(diǎn)線特征提取后的結(jié)果,并且提取的直線存在明顯的垂線與平行線。從圖7(a)可以看到加入線特征后在局部地圖中加入的地圖線。

圖6 特征提取對比Fig.6 Comparison of feature extraction

圖7 地圖對比Fig.7 Map comparison

如表1所示,選擇了8組不同的TUM RGB-D圖像序列進(jìn)行實(shí)驗(yàn)測試。從命名能夠看出這8組圖像序列的特點(diǎn),其中xyz表示相機(jī)平移運(yùn)動,rpy表示相機(jī)做滾轉(zhuǎn)、俯仰、偏航運(yùn)動;desk、room 分別表示圖像序列的采集環(huán)境在桌子周圍和辦公室內(nèi);str與nstr分別代表圖像序列有無幾何結(jié)構(gòu);tex與ntex分別表示圖像序列有無紋理;near與far分別表示相機(jī)距離物體0.5和1 m。

表1 TUM RGB-D數(shù)據(jù)集上APE RMSE的評估結(jié)果Table 1 Evaluation results of APE RMSE on TUM RGB-D dataset/m

表1 分別給出了ORB-SLAM3、SP-SLAM、LSD-SLAM 及PLSC-SLAM 在8組圖像序列測試的絕對軌跡誤差結(jié)果。從結(jié)果可以看出,PLSCSLAM 獲得的APE RMSE結(jié)果在不同程度上低于其他三種SLAM 算法。從表1可以看出在fr3_str_tex_far、fr3_str_tex_near兩個具有結(jié)構(gòu)性的RGBD數(shù)據(jù)集中,相比ORB-SLAM3,PLSC-SLAM 的精度分別提高了1.07%和20.30%,在剩余的多數(shù)數(shù)據(jù)集中,PLSC-SLAM 的定位精度都高于ORBSLAM3。而且PLSC-SLAM 在8組圖像序列上都能穩(wěn)定運(yùn)行,完整估計相機(jī)軌跡,并沒有出現(xiàn)跟蹤丟失以及SLAM 算法定位失敗的情況。

圖8、圖10 給出了PLSC-SLAM 與ORBSLAM3算法在fr1_room 圖像序列上的APE 誤差結(jié)果對比。從圖中可以更直觀地看出本工作PLSCSLAM 算法精度取得了顯著的提高。究其原因,該算法對線特征三維位姿進(jìn)行優(yōu)化,提高當(dāng)前幀與局部地圖共視關(guān)鍵幀的匹配精度,降低了對相機(jī)位姿估計的誤差。而加入結(jié)構(gòu)性約束,依靠對曼哈頓主方向的估計,為后端優(yōu)化提供了更多的約束條件,使得估計軌跡更接近真實(shí)軌跡。

圖8 fr1_room 數(shù)據(jù)集絕對軌跡誤差在估計軌跡上的映射對比Fig.8 Comparison of absolute pose error mapped onto estimated trajectory of fr1_room dataset

圖9 fr1_xyz數(shù)據(jù)集絕對軌跡誤差在估計軌跡上的映射對比Fig.9 Comparison of absolute pose error mapped onto estimated trajectory of fr1_xyz dataset

圖10 PLSC-SLAM 與ORB-SLAM3在數(shù)據(jù)集fr1_room 上APE對比Fig.10 APE comparison between PLSC-SLAM and ORB-SLAM3 on fr1_room dataset

圖9、圖11是PLSC-SLAM 與PL-SLAM 算法在fr1_xyz圖像序列上的APE 誤差結(jié)果對比。兩種算法都對點(diǎn)線特征進(jìn)行提取,但PL-SLAM 沒有利用環(huán)境中的結(jié)構(gòu)性約束對跟蹤過程的點(diǎn)線位置進(jìn)行優(yōu)化,也沒有利用環(huán)境中的結(jié)構(gòu)性約束優(yōu)化局部地圖,而PLSC-SLAM 充分考慮了環(huán)境中的結(jié)構(gòu)性約束,并利用該約束優(yōu)化相機(jī)位姿與局部地圖。從二者對比結(jié)果可以看出,利用環(huán)境中的結(jié)構(gòu)約束可以提高SLAM 系統(tǒng)的定位精度,減小估計軌跡與真實(shí)軌跡的誤差。

圖11 PLSC-SLAM 與PL-SLAM 在數(shù)據(jù)集fr1_xyz上APE對比Fig.11 APE comparison between PLSC-SLAM and PL-SLAM on fr1_xyz dataset

4 結(jié)語

針對目前存在的稀疏特征點(diǎn)SLAM 算法在低紋理環(huán)境下難以提取足夠多的特征點(diǎn)、定位精度低等問題,提出一種基于結(jié)構(gòu)約束的點(diǎn)線特征融合PLSC-SLAM 算法。通過提取環(huán)境中的點(diǎn)線特征,充分利用環(huán)境中的結(jié)構(gòu)性約束信息對直線三維位置與相機(jī)位姿進(jìn)行優(yōu)化,有效解決了點(diǎn)特征稀疏與定位精度低的問題。最后在TUM RGB-D 數(shù)據(jù)集上進(jìn)行實(shí)驗(yàn)測試,依據(jù)絕對軌跡誤差指標(biāo)對本算法進(jìn)行評估,結(jié)果表明本算法因加入線特征和結(jié)構(gòu)性約束,提高了弱紋理環(huán)境中的定位精度與穩(wěn)定性。