結構化環境下基于點線面特征融合的SLAM算法

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

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

0 引 言

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

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

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

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

1.1 算法流程

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

圖1 算法整體流程

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

1.2 相關特征的提取與匹配

1.2.1 SuperPoint點特征

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

圖2 結構化紋理環境下提取的SuperPoint點特征

1.2.2 線特征和面特征

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

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

圖3 結構化低紋理環境下提取的線、面特征

1.3 位姿估計

1.3.1 基于曼哈頓世界假設的旋轉估計

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

圖4 基于曼哈頓世界坐標系的旋轉估計

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

RCK+1CK=RCK+1MRCKMT

(1)

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

RCkW=RCkMRMW

(2)

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

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

1.3.2 平移估計

在完成位姿的旋轉估計后,通過最小化聯合點線面的重投影誤差函數來估計位姿中剩余的3自由度平移。首先,將匹配好的參考幀中的三維點重新投影到當前幀中[26],構建關于點的重投影誤差函數:

(3)

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

(4)

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

(5)

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

(6)

因此,參考幀中的平面πj與當前幀中的平面πk之間的誤差函數可表示為:

(7)

(8)

其中,∑p-1、∑l-1、∑π-1分別表示點線面特征在第k幀中的協方差矩陣,ρp、ρl、ρπ分別表示聯合誤差函數中點線面特征對應的魯棒Huber函數。

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

1.4 位姿優化

結構化環境下,平面間呈現垂直和平行的關系。利用這一特性,可將地圖中的平面和當前幀檢測的平面添加垂直和平行的約束,約束函數如下所示:

(9)

(10)

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

2 實 驗

2.1 實驗平臺及數據集介紹

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

2.2 實驗結果與分析

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

(11)

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

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

圖5 幾種算法的旋轉誤差對比

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

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

圖6 幾種算法在TUM數據集序列下的部分軌跡

3 結束語

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