快速提取特征點的視覺SLAM室內稠密建圖研究

李興州,何 鋒,余國寬

(1.貴州大學 機械工程學院, 貴陽 550025) (2貴州師范大學 機械與電氣工程學院, 貴陽 550025)

0 引言

在未知環境下機器人利用傳感器檢測周邊數據,以完成對周圍環境的定位與構建,稱為即時定位與地圖構建(simultaneous localization and mapping,SLAM)[1-3]。根據所用傳感器類型不同,可分為視覺SLAM與激光SLAM,以成本低廉、建圖色彩豐富等優點著稱的視覺SLAM近幾年得到了快速發展。

通常地,對幀序列圖像特征點的處理是視覺SLAM定位與建圖的第一步,包括特征點的提取與匹配[4]。特征點數量對視覺SLAM性能的影響至關重要,若特征點數量少,會導致周圍環境特征較少,這將嚴重影響視覺SLAM定位與建圖的準確性[5];而特征點數量過多,則可能出現提取和匹配時間長、特征點之間誤匹配的問題,嚴重影響視覺SLAM的實時性和準確性。因此,特征點的提取與匹配一直是視覺SLAM的研究熱點方向,目前主要可以分為下列3種方法:

1)基于光流法(optical flow)的特征提取與匹配。該方法是假設光度不變性來提取特征點,Lucas等[6-7]提出的光流法特征提取最具代表,該方法對關鍵點進行跟蹤并優化,使圖像中的2個對應位置的光度誤差最小,以此估計相機的運動,然而,由于此研究的假設過于保守,實驗結果對圖像的光度變化比較敏感,從而導致建圖與定位的精度差。

2)基于直接法的特征提取與匹配。該方法是基于光流法并對相機估計位姿進行優化。Forster等[8]提出的SVO法(semidirect visual odometry)是其中的代表,該方法利用估計好的位姿對過去已經收斂插入的地圖點進行全局位姿優化,位姿的估計更加準確,然而,該方法單純地使用圖像的梯度求取位姿,由于圖像的非凸性,使最后的結果容易陷入局部最優;同時該方法是基于光度不變性的假設完成的,因此,在實際中無法滿足該假設。

3)基于ORB特征點的特征提取與匹配。該方法是通過FAST角點(features from accelerated segment test)和對其周圍描述子進行快速匹配,提取特征點。Murartal等[9]提出的ORB-SLAM系列是其研究成果的代表,該方法利用對圖像取不同響應值,得到FAST角點,然后利用BRIFE描述子(binary robust independent elementary features)輕量化地完成對關鍵點的信息記錄,利用圖像旋轉不變性和BoW詞袋(bag of words)[10]達到特征點快速提取與匹配。然而,ORB-SLAM在進行特征提取與匹配時,按照固定網格尺寸進行特征點搜索,當面對圖像金字塔圖層時再采用等比例縮放的方式,無法達到特征點最大化快速的提取。

因此采用ORB特征點的特征提取與匹配方法,結合自適應網格劃分,以達到對特征點快速提取的目的,同時保證其定位與建圖的精度要求,縮短特征點提取時間,最后在此自適應網格劃分算法上添加RGB-D稠密建圖線程,完成在室內的稠密建圖。

1 ORB特征提取原理

ORB-SLAM算法[9]由提取特征點和計算描述子兩部分組成,分別是FAST角點檢測法和BRIEF二進制特征描述子法。

如圖1所示,FAST角點的提取檢測法是基于像素灰度差原理完成的。取一點像素點p為圓心,半徑為R的圓上按照順時針順序地選擇16個像素點,并計算p與這些像素點的像素灰度差。如果存在N個連續像素點與p的像素灰度值的差值都大于閾值t,那么就確定p為一個角點。N通常取9,閾值t取p點灰度值的20%[9]。

圖1 FAST角點提取原理示意圖

由于FAST檢測法提取的角點會因為圖像旋轉和尺度不一致,導致后續特征點提取無法正確匹配,為了解決旋轉不變性和尺度不變性的問題,ORB算法采用灰度質心法[11]和構建圖像金字塔解決上述問題。

灰度質心法是通過計算圖像的質心與形心之間的向量作為FAST特征點的主方向,其步驟為:

1) 定義特征點的局部圖像塊的矩為:

(1)

式中:p,q取0或1;I(x,y)表示在該點圖像灰度值;mpq表示圖像的矩。在半徑為R的范圍內,沿x,y坐標軸方向的圖像矩分別為:

(2)

(3)

整個圓形范圍內灰度值總和為:

(4)

2) 圖像的質心C為:

(5)

θ=arctan2(cy,cx)=arctan2(m01,m10)

(6)

然后利用二進制描述子快速找到角點。

2 改進的自適應網格劃分算法

在ORB-SLAM2中,采用一種名為Qtree_ORB的算法提取ORB特征點[12]。該算法分為以下4個步驟:① 構建圖像金字塔,將其分成8個等比例的圖層。② 將上述得到的圖像金字塔等比例圖層進行網格劃分,把圖像分成若干個網格,然后提取FAST角點。每次FAST角點提取都要滿足大于初始閾值iniTHFAST的要求,如果在該閾值要求下沒有提取到角點,則按照最小閾值minTHFAST的要求來提取。③ 角點提取使用四叉樹算法均勻提取。首先,初始化節點以獲取四叉樹結構。然后劃分節點區域,如果節點區域中沒有角點,則刪除節點;如果節點區域中只有1個角點,則節點不再劃分;如果該節點區域中有多個角點,則按照上述過程繼續劃分,直到節點數達到期望的數量或無法再進行劃分。④ 對于得到的節點集中保留 Harris 響應值最高節點作為特征點。

2.1 自適應網格劃分

ORB-SLAM算法中是按照固定網格尺寸對圖像進行網格劃分,由于該網格劃分方法無法適應不同的外部環境。因此,采用自適應網格劃分。使用面積法根據每層圖像的面積及特征點數量確定網格大小,以更好地適應不同的環境,具體公式如下:

(7)

式中:X,Y為每層圖像的邊界坐標;w為每層金字塔圖像的寬;h為每層金字塔圖像的高;N為每層金字塔圖像所需的特征點數目。W為劃分網格的尺寸,根據每層特征點的不同,實現自適應劃分。

根據每層金字塔圖像的高和寬的尺寸,計算出實際劃分行與列的數量,如下:

(8)

式中:R為網格劃分的行;C為網格劃分的列。計算網格的實際高和寬并向上取整,如下式:

(9)

hcell為實際自適應劃分網格高;wcell為實際自適應劃分網格寬;ceil為向上取整函數。

自適應網格劃分后,需要在每層金字塔圖像上進行特征點的提取,每一層圖像所提取的特征點數目為:

(10)

式中:Ni為圖像金字塔每層需要提取的特征點數目;N為整個圖像金字塔需要提取特征點總數目;s為每層圖像金字塔之間的縮放系數;m為圖像金字塔總層數,為8;i為第幾層金字塔。

2.2 四叉樹提取法

為了篩選角點并減少冗余,可以使用四叉樹[13]方法。首先,將圖像分割成四叉樹結構并初始化節點進行分割,每個節點中選擇Harris響應值最大的ORB特征點[14]。根據要求的特征點數目對節點進行分裂,每個節點分成4個子節點,直到某個節點不存在特征點或已達到保留數量要求時,停止分裂。如圖2所示(b、c中,彩色圓點為圖像特征點),a為四叉樹原理圖,b、c為仿真試驗效果圖,b、c對比可見,利用四叉樹提取的特征點更加均勻,匹配效果顯著。

圖2 四叉樹原理及仿真效果圖

3 實驗結果與分析

本研究的仿真基于Linux平臺,其中系統版本為Ubuntu18.04,CPU為i5-12490F的處理器、內存為16 GB。為了減少實驗隨機性帶來的誤差,對實驗中的每組數據集分別進行10次仿真測試,最終取平均值作為實驗結果。實驗1進行了6組實驗,分別比對了ORB-SLAM2的MONO、RGB-D和改進算法的MONO和RGB-D在特征點提取時間上的表現。實驗2分別驗證了ORB-SLAM2的MONO、RGB-D和改進算法的MONO和RGB-D在數據集上評估算法的絕對軌跡誤差(absolute trajectory error,ATE)。

3.1 平均每幀特征點提取時間測試

本次實驗基于ORB-SLAM2的MONO、RGB-D,以及改進算法的MONO、RGB-D模式,針對TUM數據集的f1_xyz、f1_desk、f1_plant、f1_rpy、f1_room、f3_long_offices 6組數據,分別進行了10次仿真,并以平均每幀特征點提取時間為評估標準。結果如圖3所示。

圖3 平均每幀特征點提取時間曲線

實驗結果表明,提出的自適應網格劃分特征點提取算法在TUM不同數據集下,無論是在MONO還是RGB-D模式下,都能夠顯著降低特征點提取時間,平均節約時間達到8%～10%。

3.2 絕對軌跡誤差測試

本實驗中,使用ORB-SLAM2算法以及改進后的算法分別在TUM數據集上進行仿真測試。并以ATE作為仿真測試的評價標準(ATE是用于評估SLAM系統估計位姿精度的一種標準,它計算真實位姿與SLAM系統估計位姿之間的誤差)。為了進行評估,需要先通過時間戳將真實值和估計值進行對齊,然后計算每對位姿之間的誤差值[15]。最終,ATE以圖表形式輸出,以便更加直觀地呈現評估結果。

1) 對其真實軌跡與估計軌跡,得到兩者軌跡之間的最小二乘解變換矩陣S,絕對軌跡誤差定義如下:

(11)

式中:Pi表示SLAM系統估計位姿;Qi表示軌跡真實位姿。

2) 整個時間戳范圍內位姿的均方根誤差RMSE為:

(12)

式中:trans為SLAM系統得到的平移誤差。

在TUM數據集f1_xyz、f1_desk、f1_plant、f1_rpy、f1_room、f3_long_offices上分別對ORB-SLAM2和改進后的算法進行仿真,統計MONO和RGB-D兩種模式下的絕對軌跡誤差,如表1所示。

表1 絕對軌跡誤差評估 mm

根據表1的實驗結果顯示,自適應網格劃分特征點提取算法在絕對軌跡誤差方面表現明顯優于ORB-SLAM2算法。在6組不同的數據集中,無論是MONO,還是RGB-D模式下,該算法的精度均提高了5%以上。尤其在某些大型室內場景下,改進算法的精度提高幅度最高可達50%。以f1_plant和f1_fesk兩組數據的MONO和RGB-D模式下的軌跡路線及x-y-z軸方向上的誤差曲線為例,如圖4、圖5所示。在圖中,groundtruth為真實軌跡,old_trajectory為ORB-SLAM2算法得到的軌跡路線,new_trajectory表示改進后的自適應網格劃分ORB算法得到的軌跡路線。從圖中可以看出,改進后的算法無論在軌跡匹配上還是在絕對誤差方面的精度都要高于ORB-SLAM2算法。

4 稠密建圖

為了生成稠密的點云地圖,在上述自適應網格劃分算法框架中,增加稠密點云構建線程。該線程使用RGB-D相機測量深度信息,與RGB圖像進行匹配,利用前端估算的相機位姿計算出所有像素的空間坐標,并生成點云圖。通過RGB圖像信息上色[16],可以得到一個由離散的彩色點組成的地圖。該線程會隨著新增關鍵幀的不斷加入,進行點云拼接和全局優化,實現稀疏到稠密的轉變。具體流程如圖6所示。遍歷匹配好的RGB圖像和深度圖,提取出像素坐標(u,v)和深度值d,根據針孔相機成像模型:

(13)

式中:Z為深度值和實際空間距離的比例因子;K為相機內參矩陣;R、t為相機的旋轉矩陣和平移矩陣;Pw為像素點在世界坐標系下的三維坐標。由于相機已進行標定,因此內參矩陣K為已知量,跟蹤線程計算相機位姿,從而確定了R、t,因此可以確定RGB圖像中每個像素的空間坐標。利用PCL點云庫(point cloud library)來處理計算出的點云數據。點云Pi的坐標可由下式計算得出:

圖6 稠密建圖具體流程框圖

(14)

式中, depthScale是深度值。

再利用RGB圖像三通道的色彩信息給點云上色,然后,讀取下一個關鍵幀進行點云拼接。以TUM下的f1_desk、f1_plant、f1_room、f3_long_office四組數據集為例進行室內稠密建圖,效果如圖7所示。由圖7可知,該稠密建圖效果顯著,室內物體輪廓清晰,顏色比對正確,因此,在原有基礎上添加稠密建圖線程可以滿足室內稠密建圖的需求。

圖7 稠密建圖

5 結論

1) 自適應網格劃分算法能夠根據圖像金字塔層數的不同,自動調整網格大小和密度,更好地捕捉數據的局部特征信息,使得自適應網格劃分算法進行特征點提取時,具有更高的時間效率和更高的絕對軌跡誤差精度。

2) 采用外點去除濾波和體素網格濾波方法對室內場景進行稠密建圖,外點去除濾波通過排除異?；驘o效特征點,提高了場景重建的精度和可靠性;體素網格濾波方法利用分層表示和空間降采樣技術,在保持場景結構完整性的同時,有效降低了噪聲和采樣密度不均勻性對建模結果的影響,顯著增強了室內建筑和紋理的建模效果。