一種改進的Qtree_ORB算法

楊世強，白樂樂，趙 成，王國棟，李德信

西安理工大學 機械與精密儀器工程學院，西安 710048

同時定位與建圖算法（Simultaneous Localization And Mapping，SLAM）是指移動機器人在沒有任何先驗環境的情況下同時完成自身的定位與周圍環境地圖的構建[1-2]。SLAM根據傳感器的不同可以分為激光SLAM和視覺SLAM，雖然視覺SLAM相比于激光SLAM精度較差，但是由于其價格低廉得到了廣泛的應用。

視覺SLAM根據利用圖像信息的不同可以分為基于特征的SLAM方法和直接SLAM方法?；谔卣鞯囊曈XSLAM方法是對每幀圖像進行特征提取，并構建描述符，然后通過特征點的匹配來進行位姿和特征點三維坐標優化[3]。直接SLAM方法不需要進行特征提取，通過計算局部像素的梯度和方向值，優化出相機的位姿和三維點?；谔卣鞯腟LAM方法由于其對復雜場景魯棒性高的優點得到了廣泛的研究。

目前在視覺SLAM中由于ORB[4]（Oriented Fast and Rotated Brief）特征點速度快的優點而被廣泛應用。ORB算法雖然有著較高的提取實時性，但是其提取的特征點不均勻，容易提取到過多重疊的特征點。Sun等人[5]提出了一種基于多探針LSH的A-ORB算法，雖然魯棒性有一定的提高，但是提取的特征點較少，仍存在特征點分布不均勻，容易產生簇集的現象。

針對標準ORB算法分布不均勻的問題，Mur-Artal等人[6-7]所提出的ORB-SLAM算法中采用四叉樹來對特征點進行均勻化處理，同時采用了自適應閾值的方法對ORB算法進行了改進，提高了算法的均勻度，但存在迭代次數多、過均勻的問題。在ORB-SLAM2的基礎上還提出了很多優秀的SLAM系統，如Yu等人[8]提出的DS-SLAM，Bescos等人[9]提出的DynaSLAM算法，這些SLAM系統并沒有對特征點提取模塊進行改進，仍采用傳統的四叉樹結構來進行特征點的提取，特征點過均勻和迭代次數多的問題并沒有得到改善，影響了SLAM系統的精度。伍錫如等人[10]提出一種改進的基于四叉樹的ORB特征提取方法，但是仍采用傳統的四叉樹結構管理特征點，所提取的特征依然存在過均勻的問題。

針對ORB-SLAM2中過均勻、迭代次數過多的問題，本文以ORB-SLAM2為基礎，提出了一種改進的Qtree_ORB算法。首先通過在圖像金字塔層自適應劃分網格提取特征點；然后對四叉樹劃分深度進行限制，設定特征點響應值的最小閾值；最后對本文所提出的方法進行均勻度和匹配性能的實驗，并將該方法融合到ORB-SLAM2視覺里程計前端，并使用TUM中的RGBD數據集進行測試，實驗表明，本文方法可以有效地改善ORB-SLAM2的性能。

1 ORB算法原理

ORB算法[4]分為特征點的提取和特征描述符計算兩部分，分別是基于FAST[11]的oFAST（oriented FAST）特征點檢測算法和基于BRIEF[12]的rBRIEF（rotated BRIEF）二進制特征描述符計算算法，有效解決了FAST特征點算法不具備旋轉不變性的缺陷。

1.1 oFAST角點提取

如圖1所示，FAST角點提取時通過檢測以像素點p為圓心，以3為半徑的圓上16個像素點與圓心p的灰度差來提取角點。如果圓周上有N個連續像素點的灰度值與圓心p的差大于某一閾值t，則將圓心像素點p作為候選特征點。其中N一般取12，稱為FAST-12，常用的還有FAST-9、FAST-11。閾值一般為p點灰度值的20%。

圖1 FAST角點提取圖Fig.1 FAST feature point extraction diagram

針對FAST角點不具備旋轉不變性和尺度不變性的缺點，ORB算法通過構建圖像金字塔來實現尺度不變性，而特征的旋轉不變性通過灰度質心法（Intensity Centroid）來實現。

灰度質心法主要用來給FAST特征點確定一個主方向，其主要步驟為：

（1）在特征點的局部圖像塊B中，定義圖像塊的矩為：

（2）通過B的矩找到圖像塊的質心：

（3）連接圖像塊的幾何中心O與質心C得到方向向量OC，定義為特征點的主方向為：

1.2 rBRIEF描述子提取原理

BRIEF是一種二進制描述符，其構成了一個由0和1組成的描述向量，可以將此描述向量表述為：

其中，P(x)、P(y)是以特征點為中心的圖像塊，在x、y像素點處的灰度值。

選取M對測試點(x m,y m)就能產生一個二進制描述向量。二進制描述符可以表述為：

為了使BRIEF算法具有旋轉不變性，rBRIEF算法通過對圖像塊中任意M對點(x m,y m)生成的m維二進制序列，定義一個2×m的矩陣S：

由特征點的主方向θ，確定旋轉向量Rθ，對矩陣S進行旋轉，可得到如下式的描述矩陣Sθ：

通過將描述矩陣旋轉到主方向上，改進了BRIEF算法不具有旋轉不變性的缺點。旋轉后的BRIEF描述符可以表征為：

2 改進的Qtree_ORB算法

在ORB-SLAM2[6]中采用一種基于四叉樹來提取ORB特征點的算法，稱為Qtree_ORB算法。Qtree_ORB算法主要分為以下幾個部分：（1）構造圖像金字塔。通過構建出8層圖像金字塔，為圖像增加了尺度信息，解決了ORB算法中尺度不變性的問題。（2）對每一層圖像金字塔進行網格劃分。（3）用初始閾值iniTHFAST對每個網格進行FAST角點的提取，如果在網格內提取不到特征點則降低閾值用最小閾值minTHFAST來提取FAST角點。（4）基于四叉樹均勻的選取所需要的FAST角點。首先初始化節點，得到初始的四叉樹結構。其次劃分節點，若節點中無特征點則刪除該節點，若節點中的個數為1則不再劃分節點，若節點中個數大于1則繼續劃分節點，若節點數大于期望的特征點數則不再進行劃分節點。最后在每個節點中保留具有最高Harris響應值的特征點作為最終的特征點。以上標準的Qtree_ORB算法會存在所提取特征點過均勻和迭代次數多的問題，基于此本文對四叉樹的劃分深度、特征點的保留策略、金字塔網格的劃分進行改進。

2.1 網格劃分

在構造圖像金字塔后對每層圖像進行網格劃分，原ORB-SLAM2算法中對每層圖像根據工程經驗值劃分為30×30網格，不能很好地適應外部環境。所以本文對于網格劃分采取自適應處理，根據每一層圖像所需特征點數量采用面積法來對每一層圖像進行網絡劃分。

其中，maxBordeX、minBordeX、maxBordeY、minBordeY為每層圖像的邊界坐標，width為每層金字塔圖像的寬，height為每層金字塔圖像的高，ε為比例系數，經過實驗驗證ε值為1.8，N為所需的特征點數目。W為劃分網格的寬與高，在不為整的情況下對其進行取整運算。

由每層圖像金字塔的寬和高來計算出實際劃分的行與列，在不為整的情況下進行向上取整運算，如下式：

其中，nCols為網格劃分的列，nRows為網格劃分的行。網格的實際寬和高為：

其中，wCell為劃分網格的實際寬，hCell為劃分網格的實際高，round為取整函數。

在劃分網絡后需要在每層上進行特征點的提取，每一層圖像所提取的特征點數目為：

其中，N i每層所需要的特征點數目，InvS為每層圖像縮放尺度因子的倒數。上式構造了一個等比數列，比例系數為InvS，其和為N，為所需要的總的特征點數目。

在網格內提取特征點時首先用初始閾值iniTHFAST對每個網格進行FAST角點得提取，如果在網格內提取不到特征點則降低閾值用最小閾值minTHFAST來提取FAST角點。最后采用非極大值抑制算法減少重疊特征點的輸出。

2.2 劃分四叉樹

通過上述劃分網格所提取的角點存在著大量的冗余，需要用四叉樹來對其進行進一步的篩選。其主要思想是把具有ORB特征的圖像分割為四叉樹節點，然后根據Harris響應值保留節點中響應值最大的特征。

傳統的四叉樹劃分在有些情況下會導致四叉樹分割次數過多，降低算法效率，其次所提取到的特征點存在過均勻的問題，針對這些問題，本文對四叉樹的劃分重新進行限制，首先根據每層金字塔圖像所需特征點數目來設定最大劃分深度Dmax，四叉樹劃分深度間的關系可以表達為：

其中，d為四叉樹當前劃分深度，Dmax為最大劃分深度。每層節點數量與劃分深度的關系表達為：

其中，Nodes i當前劃分深度下的節點數目。

如圖2所示，在劃分為四叉樹節點之后是對四叉樹節點中特征點的保留，首先對每個節點中的特征點數目進行判斷，若該節點中沒有特征點則刪除該節點；若該節點中只有一個特征點，則保留該節點；若節點中有兩個或者兩個以上的特征點時，根據該節點中所有特征點的Harris響應值對其進行排序，保留節點中具有Harris響應值最高的特征點并刪除掉其他的特征。最后對Harris響應值的最小值進行限定，若提取到的特征點的Harris響應值小于其最小的閾值，則不再提取該節點中的特征。圖2中nNum為節點中的特征點，maxKP為Harris響應值最大的特征點，minH為Harris響應值的最小閾值。

圖2 四叉樹劃分流程圖Fig.2 Flow chart of quadtree division

2.3 特征點的匹配

在提取完特征點之后，需要進行特征點的匹配。特征點匹配是指兩幅圖像之間特征像素點的對應關系，從而確定兩幅圖像的位姿關系。在SLAM中通過特征匹配確定當前路標與之前觀測到路標的對應關系，然后對機器人的位姿進行推測。本文所采取的特征點匹配方法如下：

（1）對兩幀圖像的特征點進行暴力匹配[13]（Brute-Force Matcher），得到兩幀圖像上的初始匹配的點對信息。

（2）暴力匹配后的點對存在著很多的誤匹配，需要進一步的篩選。所以通過漢明距離來進行進一步的篩選，記所有匹配點對描述符間的最小距離為Dmin，當兩個特征點間的漢明距離小于30與2×Dmin的最大值時，認為該點對為粗匹配點對。

（3）在完成粗匹配后利用隨機抽樣一致算法[14]（Random Sample Consensus，RANSAC）對粗匹配點對中的誤匹配進行進一步的濾除，都得到最終的匹配點對。

通過以上步驟可以得到正確的匹配點對信息，為后續機器人的位姿計算提供了依據。

3 實驗結果與分析

本文所有實驗都是在一臺處理器為i7-2600，內存8 GB，操作系統為Ubuntu14.04的臺式計算機上運行，對實驗中的數據均測試15次取平均值來避免實驗的隨機性。在第一個實驗中對提取到的特征點均勻性、準確率和運行效率進行測試。在第二個實驗中，把改進算法融入ORB-SLAM2中的視覺里程計中，測試改進算法的絕對軌跡誤差（Absolute Trajectory Error，ATE）與相對軌跡誤差（Relative Posture Error，RPE），并與標準ORBSLAM2進行比較，驗證其應用在SLAM系統上的可行性。

經過多次實驗測定，四叉樹劃分深度在圖像金字塔第一層至第四層的最大劃分深度取3，第五層至第六層的最大劃分深度為2，第7層至第8層的最大劃分深度為1。Harris響應值的最小閾值取18。這樣一方面可以減少冗余計算提高速率，另一方面可以使特征點的均勻性更好。

3.1 特征點均勻度和匹配性能測試

本實驗采用Mikolajczyk數據集[15]來進行均勻度和匹配性能的測試。實驗中取每組數據集中前兩張圖像作為實驗對象進行特征點的提取與匹配，分別對不同算法在模糊、視角變化、縮放、旋轉、光照變化下進行性能測試并進行對比。

將改進的Qtree_ORB算法與基于四叉樹的Qtree_ORB算法以及OpenCV 2.4.11中提供的傳統ORB算法進行對比，所提取的特征點數目設置為1 000，其余均為默認參數。記本文改進的算法為Improved_Qtree_ORB，除本文設置的參數外，其余均為Qtree_ORB算法的原始參數值。

3.1.1 圖像均勻度測試

用圖像特征點的分布均勻性[15]來判別圖像中的特征點是否分布均勻，對待檢測的圖像進行區域劃分，把圖像從豎直、水平、中心和四周、45°和135°進行劃分。然后分別計算各個區域內的特征點數目，并將其構建為一個向量。計算特征點區域統計分布的標準差，將標準差定義為特征點分布的均勻度S。標準差越小表明特征點數量的波動越小，分布越均勻，反之則分布越不均勻。

其中，x1,x2,…,x10分別為劃分的10個區域內的特征點數量，為其平均值。

表1為三種算法在不同數據中的均勻度情況。傳統ORB算法在提取到的特征點的均勻度最大，這是由于傳統ORB算法所提取的特征點過于集中所導致的；Qtree_ORB算法所提取到的特征點的均勻度相比較傳統ORB算法的均勻度有很大的提升；Improved_Qtree_ORB算法的均勻度最低，這是因為其對四叉樹的劃分深度進行了限制，在每個四叉樹節點中提取的特征點的數量基本相同。Improved_Qtree_ORB的平均均勻度為38.74，相比于Qtree_ORB算法的65.98提升了41.3%。

表1 Mikolajczyk數據集特征點分布均勻度Table 1 Distribution uniformity of feature points in Mikolajczyk dataset

圖3為三種算法的均勻度比較圖，圖4是三種算法在尺度變化數據集bark下所提取到的特征點?？梢钥闯鰝鹘yORB算法所提取到的ORB特征點有簇集現象，分布并不均勻。Qtree_ORB算法所提取到的特征點雖然沒有簇集現象，但是出現了過均勻的情況，在一些梯度變化不明顯的地方也提取了一些Harris響應值較小的特征點。Improved_Qtree_ORB算法的均勻化效果最好。

圖3 特征點均勻度比較Fig.3 Comparison of feature point uniformity

圖4 特征點提取比較Fig.4 Feature point extraction comparison

3.1.2 特征點的匹配測試

用特征點的匹配正確率C來評價兩幀圖像特征點匹配的準確度，定義為：

其中，mc為正確匹配點對，m為所有匹配點對。

圖5為三種算法在不同數據下特征點匹配正確數量的對比圖，圖6為三種算法在不同數據下匹配正確率的對比?？梢园l現Improved_Qtree_ORB算法在匹配數量方面明顯優于Qtree_ORB算法，但是比ORB算法略少一些。在匹配正確率方面，Improved_Qtree_ORB算法比Qtree_ORB算法在尺度變化數據集bark、視角變化數據集graf、光照變化數據集leuven、旋轉變化數據集boat下有優勢，Improved_Qtree_ORB算法與ORB算法準確率基本相當。從整體來看，Improved_Qtree_ORB算法更加穩定，整體優于其他兩種算法。以下對三種算法在不同場景下的性能進行對比：

圖5 特征點匹配正確個數對比Fig.5 Comparison of correct number of feature points matching

圖6 特征點匹配正確率對比Fig.6 Comparison of matching accuracy of feature points

（1）尺度變化

圖7（a）、（b）、（c）為三種算法在bark數據下經過RANSAC算法濾除誤匹配后的匹配結果，表2為三種算法的匹配數量、正確匹配數量、準確率的實驗數據。從表2中可以看出本文改進算法相比于其他兩種算法所提取到的特征點的數目更多，其準確率三種算法基本保持一致。圖7（a）中可以看出提取到的特征點集中在某一區域，因為尺度的變化從而導致兩幀之間的特征點無法進行匹配，導致所參與匹配的特征點較少。Qtree_ORB算法因為保留了部分質量較差的特征點，所以參與匹配的特征點的數目也比較少。本文Improved_Qtree_ORB算法有效地改進了其他兩種算法的缺陷。

圖7 bark數據集的特征點匹配結果Fig.7 Feature point matching results of bark data

表2 尺度變化下各算法性能對比（bark）Table 2 Performance comparison of various algorithms under scale changes（bark）

（2）亮度變化

圖8（a）、（b）、（c）為三種算法在leuven數據匹配結果，表3為三種算法在leuven數據下的實驗數據。

圖8 leuven數據的特征點匹配結果Fig.8 Feature point matching results of leuven data

在圖8中明顯可以看出ORB算法相比于其他兩種算法在下方光線暗的地方無法提取到特征點，其特征點主要集中在光線較亮的區域。由表3可以看出Improved_Qtree_ORB算法的匹配數量明顯多于其他兩種算法，同時也保證了其匹配的正確率。Qtree_ORB算法匹配正確率較低的原因是其在提取特征點的過程中保留了一些質量差的特征點，比如地面的特征點從而最后影響了匹配正確率。

表3 亮度變化下各算法性能對比（leuven）Table 3 Performance comparison of algorithms under brightness changes（leuven）

在算法運行效率上，圖9是三種算法的運行時間的比較，表4是具體的實驗數據。由圖9可以得出其ORB算法的效率最高，其次為Improved_Qtree_ORB算法，運行效率最慢的為Qtree_ORB算法。這是因為Qtree_ORB算法中添加了四叉樹來管理特征點，其次對每層圖像金字塔進行網絡劃分來提取FAST特征點。在進行四叉樹劃分時，每一次劃分都要確定父節點中特征點的歸屬，同時由于沒有限制其劃分深度所以運行時間較長，本文通過對四叉樹劃分在不同圖像金字塔層上限定劃分深度，對圖像金字塔進行自適應網格劃分來提取特征點提高了其效率。

圖9 運行時間比較Fig.9 Comparison of running time

表4 Mikolajczyk數據集特征點提取時間Table 4 Feature point extraction time of Mikolajczyk dataset ms

3.2 軌跡誤差

在本實驗中采用開源的SLAM系統ORB-SLAM2對本文改進算法進行測試，其評價標準為絕對軌跡誤差（ATE）和相對軌跡誤差（PRE）。ATE指的是相機位姿的真實值與SLAM系統估計值之間的差。該標準非常適合于評估視覺SLAM系統的性能。RPE用于計算相同兩個時間戳上的位姿變化量的差，該標準適合于估計系統的漂移。

在實驗中選擇TUM[16]公共數據集用來評估室內動態場景下SLAM系統的精度和穩定性。因為ORBSLAM2系統中采用的是Qtree_ORB算法，因此本實驗只用本文改進算法Improved_Qtree_ORB算法與其對比，不再與傳統ORB算法進行對比。

圖10中（a）、（c）為絕對軌跡誤差圖，其中紅色線段代表誤差，可以明顯看出本文算法相對于Qtree_ORB算法的整體誤差較小。圖10中（b）、（d）為相對軌跡誤差的波動情況，可以看出本文改進算法的相對軌跡誤差的波動情況較小，有效地改善了SLAM系統的漂移程度。表5～表7分別為絕對軌跡誤差和相對軌跡誤差在SLAM系統中的測試值。從表5可以看出，改進算法在SLAM系統中有了很大改善。在絕對軌跡誤差中均方根誤差相比ORB-SLAM2降低了24.58%，標準差降低了44.6。

表5 絕對軌跡誤差（ATE）Table 5 Absolute trajectory error（ATE）

表6 相對位移軌跡誤差（RPE）Talble 6 Relative displacement trajectory error（RPE）

表7 相對位旋轉軌跡誤差（RPE）Table 7 Relative position rotation track error（RPE）

圖10 軌跡誤差圖Fig.10 Trajectory error grap

4 結語

本文提出了一種限制四叉樹劃分深度的Improved_Qtree_ORB算法。通過實驗對本文改進算法的均勻度、匹配性能和在SLAM系統下的性能進行了測試。實驗結果表明Improved_Qtree_ORB算法在均勻度方面性能優于ORB算法和Qtree_ORB算法，在保證高匹配正確率的情況下依然能夠匹配足夠多的特征點。通過在Mikolajczyk數據集下的測試，本文改進算法相比于其他兩種算法其綜合性能有明顯提升。在SLAM系統測試中，其軌跡精度較優，在絕對軌跡誤差中其均方根誤差提高了24.58%；因此可看出本文改進算法的可靠性較高，可以運用在SLAM系統中。

盡管本文改進算法在SLAM系統中可靠性較高，但是也存在著一些不足之處。在未來可以通過提取亞像素特征點來提高ORB特征點精度，進一步提高SLAM系統的性能。