基于ORB-SLAM2系統的快速誤匹配剔除算法與地圖構建

席志紅，王洪旭，韓雙全

（哈爾濱工程大學信息與通信工程學院，哈爾濱 150001）

（?通信作者電子郵箱504715799@qq.com）

0 引言

同步定位與地圖構建（Simultaneous Localization And Mapping，SLAM）技術隨著智能家居機器人和特殊用途工業機器人的應用成為當前機器人技術領域的熱點研究之一。目前SLAM 技術已在家庭服務機器人、無人機及無人車等領域大顯身手，如：波士頓動力公司研發的家庭服務機器人已經可以實現簡單自主導航、開門、扔垃圾等日常任務；大疆公司生產的消費級無人機已經完全實現了視覺定位，視覺避障以及目標跟蹤等核心技術。視覺SLAM（Visual SLAM，VSLAM）因主要采用視覺傳感器作為數據源而相比激光雷達等其他SLAM技術具有一定價格優勢，近年來成為SLAM 研究領域的熱點方向之一。

VSLAM 技術根據對圖像信息的利用方式可以分為直接法和特征法。直接法是VSLAM研究的一重要方法，它在2011年才被提出，但是經過幾年的迅速發展，它已經達到了和特征點法同樣重要的地位。Engel 等［1］通過添加回環檢測和關鍵幀管理模塊來對半稠密視覺里程計進行改進，并在2014 年提出了LSD-SLAM（Large-Scale Direct monocular SLAM）系統，將直接法應用到了半稠密的單目SLAM 中，并在2015 年提出了分別用于雙目相機和全景相機的LSD-SLAM［2］系統。該系統能夠構建大尺度的，全局一致性的環境地圖，并能夠基于直接圖像配準得到高度準確的相機位姿。直接法中是耦合的，而在特征點法中則是解耦的。耦合能夠更整體性地處理數據關聯；而解耦能夠在位姿不確定的情況下，僅利用圖像信息去解數據關聯問題，所以直接法更擅長求解連續圖像的定位，而特征點法則更適合回環檢測與重定位［3］?；谔卣鼽c法的VSLAM 方法是VSLAM 研究另一個重要方向。2007 年，Klein等［4］提出了PTAM（Parallel Tracking And Mapping）系統，并實現了跟蹤與建圖過程的并行化。2015 年Mur-Artal 等［5］在完全繼承PTAM 系統基礎上提出ORB-SLAM2（Oriented FAST and Rotated BRIEF SLAM2）系統，該方法圍繞ORB（Oriented FAST and Rotated BRIEF）特征計算具有良好的旋轉和縮放不變性，提出了包括視覺里程計與回環檢測的詞袋字典模型，實現實時的回環檢測與高魯棒性的重定位，較好解決直接法的定位丟失問題。

圖1 本文SLAM系統框架Fig.1 SLAM system framework in this paper

ORB-SLAM2 系統在特征匹配時采用隨機抽樣一致（RANdom SAmple Consensus，RANSAC）算法［6］，該算法在圖像匹配上應用廣泛。在二值化互信息篩選ORB 特征點得到的集合中選擇內點，內點比例高，采用RANSAC算法能快速找到最優估計［7］?；贠RB特征進行的RANSAC改進算法來進行圖像拼接，在已有特征點基礎上根據內點計算變換矩陣H，有效節省匹配時間［8］。在RANSAC 算法基礎上改進的漸進一致采樣法（PROgressive SAmple Consensus，PROSAC）算法也得到廣泛應用。將PROSAC 算法用于剔除圖形匹配異常值，在保證數據精度基礎上有效提高了采樣效率，節省計算時間［9］。但由于RANSAC 算法的隨機性特點不得不通過提高迭代次數來提高得到正確模型參數的概率，進而導致了該算法的低效。本文在ORB-SLAM2 系統基礎上在特征匹配時采用PROSAC 算法替代RANSAC 算法［10］，PROSAC 算法是對經典的RANSAC 算法的一種優化［11］。相比經典的RANSAC 方法均勻地從整個集合中采樣，PROSAC 方法是從不斷增大的最佳對應點集合中進行采樣的，所以這種方法可以節省計算量，提高運行速度。另一方面，ORB-SLAM2系統只利用稀疏點云地圖，而稀疏點云地圖并不能在機器人導航中使用［12］，所以本文在原有方法基礎上添加稠密點云地圖［13］與八叉樹地圖［14-15］的建立線程，無論稠密點云地圖還是八叉樹地圖都可以用于機器人導航與路徑規劃。

通過利用TUM（Technische Universit?t München）數據集的實驗結果顯示，本文方法與ORB-SLAM2系統對比，本文系統在均方根誤差（Root Mean Square Error，RMSE）僅相差0.003 2 m，在進行誤匹配對比實驗時，在CPU 為Intel i7，內存為16 GB 平臺上，RANSAC 算法處理圖片速度14 fps（frames per second）左右，PROSAC算法處理速度26 fps左右，圖片處理速度是原來的185%。

1 本文SLAM系統

ORB-SLAM系統基于ORB特征實現，包含跟蹤定位、局部地圖構建、回環檢測三個主線程和全局BA（Bundle Adjustment）優化線程，在該系統基礎上，本文用PROSAC 算法替代原RANSAC算法，并添加建立稠密點云地圖和八叉樹地圖線程到ORB-SLAM2框架內，本文系統的技術框架圖如圖1所示。

1.1 系統線程

跟蹤線程 這一部分主要工作是從RGB-D 圖像中提取ORB 特征，根據提取ORB 特征與上一幀進行匹配或者通過全局重定位初始化位姿，并利用PROSAC 算法進行誤匹配的剔除，然后跟蹤已經重建的局部地圖優化位姿，再確定當前幀是否為新的關鍵幀。

建圖線程 這一部分主要完成局部地圖構建。包括對關鍵幀的插入，驗證最近生成的稀疏點云地圖點并進行篩選，然后生成新的地圖點，使用局部BA（Local BA）優化，最后再對插入的關鍵幀進行篩選，去除多余的關鍵幀。

稠密點云地圖與八叉樹地圖構建線程 對稀疏點云地圖的稀疏點進行提取，根據稀疏點云進行稠密點地圖的構建，并在此基礎上構建八叉樹地圖。在對關鍵幀進行插入和刪除時也對建立的稠密點云地圖與八叉樹地圖進行地圖占有概率更新。

閉環檢測線程 這一部分主要分為兩個過程，分別是閉環探測和閉環校正：閉環檢測先使用詞袋模型進行探測，然后通過Sim3 算法計算相似變換；閉環校正主要是閉環融合和必要回環的圖優化。

1.2 PROSAC算法原理

PROSAC 算法是對經典的RANSAC 算法中采樣的一種優化。相比經典的RANSAC 方法均勻地從整個集合中采樣，PROSAC 方法是從不斷增大的最佳對應點集合中進行采樣，根據相似性進行排序的半隨機采樣不會比完全隨機采樣的效果更差，所以這種方法可以在保持原有方法的魯棒性基礎上，獲得與采用RANSAC 方法的內點基本相同的結果，并使計算速度更快。

PROCAS 方法的成立假設：具有更高相似性的數據點更可能是類內點。

引入評價函數q(uj)值：N個數據點的集合記為uN，根據評價函數，uN內數據點（如ui，uj）被降序滿足條件：

根據特征匹配點的歐氏距離的比值β，對評價函數q(uj)定量表示，表達式如下：

式中：dmin表示最小歐氏距離，dmin2表示次小歐氏距離。比值越小，表示特征點匹配質量越好，即評價函數值越大；反之亦然。

將評價函數值最大的前n 個數據點集合記為un，并記集合M，M中元素個數為m。

由于PROSAC 算法不是從所有數據點中進行隨機采樣，而是需要先對數據點進行排序，然后在評價函數值較高的數據點子集中進行隨機采樣，這個子集被稱為假設生成集，并且這個假設生成集的元素數量是不斷增大的，所以PROSAC 算法需要確定生成數據集大小和采樣過程的停止策略。

1）確定數據集大小。

生長函數用來確定假設生成集的大小，它既不能過分依賴評價函數的預排序作用，也不能地對所有數據點做相同處理。所以用P(uj)表示數據點uj是正確匹配點的概率，對這個概率和評價函數之間的聯系做單調性假設：

用Tn表示中數據點僅來源于評價函數最好的前n個數據點集合un的平均數，則有：

最終它們之間的遞歸關系為：

Tn個采樣點集合只包含un的數據點，Tn+1個采樣點集合包含un+1的數據點。由于un+1=un∪{un+1}（un+1是一個數據點），所以Tn+1個采樣點集合包括1 個數據點un+1和m -1個來源于un的數據點。因此Tn+1采樣點集合是在un中隨機抽取m -1個采樣點，在此基礎上從m 開始到N -1隨機取值獲得數據點un+1，因此可以快速得到采樣點排序后集合

由于Tn值一般是非整數，所以令且有：

所以生長函數可以被定義為：

式中：n表示采樣次數大于等于t次時最小的n值。

2）采樣過程的停止策略。

如果集合un*中的類內點數In*滿足下面條件，PROSAC 算法就停止運算：

非隨機性 n*個數據點中的類內點恰好也是任意一個錯誤模型的類內點的概率小于Ψ（一般為5%）；

極大性 存在一個解在un*中有比In*更多的內點情況，并且在這種情況下k 次采樣后還未被發現的概率小于η0（一般小于5%）。

這兩個條件中非隨機性可以防止由類外點產生的模型也與算法相同。隨機類內點集合的元素個數的分布是二項分布：

其中β 為與一個錯誤模型相對應的點不在采樣集合中的概率，并且該采樣點包含一個類外點。

在un*中發現一個非隨機的結果必須要滿足

極大性定義了需要多少次采樣才能保證解的置信度，這同樣也是RANSAC 算法唯一的停止策略。從un中采樣得到的m個點都來自類內點的概率為：

其中：In是un中類內點的數量，εn=In/n 表示類內點數的比例。那么k次采樣也都是非類內點的概率為(g(k) ≤n)：

給定概率閾值η0后，采樣次數需要滿足下列的條件：

算法流程如下所示。

輸入 最大迭代次數Im，判斷是否為內點的誤差極限δ，設定內點數目閾值T，判斷是否為內點的誤差閾值ε；

輸出 單應矩陣H。

1）計算特征點的最小歐氏距離dmin，求解歐氏距離比值β。

1）根據假設生成集g(t)確定n的值。

2）根據β衡量匹配質量，根據匹配質量選取前n個特征點進行降序排列。

3）選取排序后4 個點為一組，計算每組匹配質量之和，再根據質量之和進行排序。

4）在排序匹配對中選取匹配質量最高的4 組匹配點，計算單應矩陣H。

5）將上述4 組匹配點剔除，對其他特征點根據單應矩陣計算相應投影點。

6）計算其他點與投影點的誤差e，并與ε 進行比較：如果e ＜ε，表示該點為內點；反之為外點。

7）計算內點數目t與內點數目閾值T進行比較：t ＞T更新內點數目t；否則重復上述步驟，并對迭代次數加1。

8）用更新后t重新計算單應矩陣H及新內點。

9）若迭代次數不大于Im時滿足n ＜n*，返回H 及內點集合；否則，找不到模型。

1.3 稠密點云地圖與八叉樹地圖的構建

ORB-SLAM2系統只利用了稀疏點云地圖，所以本文在此基礎上，利用稀疏點云建立稠密點云地圖，并建立八叉樹地圖。

1）利用RGB-D圖像的信息來進行稠密點云地圖的構建。

通過定義點序列：X={x1，x2，…，xn}表示空間點的集合，其中xi=[x，y，z]表示點的三維坐標，根據相機模型成像原理：

其中：C 是相機內參，R 和t 是相機的旋轉矩陣和平移矩陣，s是深度值和實際空間距離的比例關系因子。

本文利用PCL（Point Cloud Library）軟件來處理由RGB-D圖像計算來的點云數據。點云數據點pi的pi.x，pi.y，pi.z可以根據深度信息計算：

其中：d 為深度圖像m 行n 列像素點的深度值，cx、cy、fx、fy為相機內參。

根據ORB-SLAM2 系統選取的關鍵幀對應的相機位姿與根據式（16）獲取的點云數據創建局部稠密點云地圖，對局部稠密點云地圖進行點云拼接，得到全局稠密點云地圖。但點云地圖存在細節冗余導致其體積過大，所以在保留關鍵地圖的基礎上建立八叉樹地圖，可有效縮減地圖大小。

2）八叉樹地圖（octomap）是通過八叉樹結構進行信息存儲的地圖，常用于機器人導航等方向。

八叉樹的基本結構如圖2 所示。八叉樹（Octree）是一種用于描述三維空間的樹狀數據結構，八叉樹的每個節點表示一個正方體的體積元素，每個節點有8 個子節點，這8 個子節點所表示的體積元素加在一起就等于父節點的體積。一般將物體的中心點作為節點的分叉中心。八叉樹地圖基于八叉樹結構對三維空間不斷切割，直到無法切割為止。八叉樹地圖的每個節點都保存了占有率，當某個方塊的子節點都沒被占或完全被占時，則不用對該方塊繼續展開，方便數據的壓縮。

圖2 八叉樹原理Fig.2 Principle of octree

八叉樹地圖構建具體原理如下所示：

假設1 表示節點被完全占據，0 表示節點是空白的，則節點的占據狀態可以用0～1 的浮點數x 來表示。為了防止x 超過0～1的范圍，可對x進行對數變換，其公式如下所示：

這樣將y ∈(-∞，+∞)轉換到x ∈[0，1]。用轉換概率數值y來表示節點是否被占據，繼而通過逆變換即可得到概率x的值，0.5 表示未確定，越接近1 表示該節點被占據的可能性越高。占有概率更新公式如下：

其中：P(n)為節點的先驗概率，P(n|zT)為第n 個節點基于T 時刻的后驗概率為1 到T 時間段的觀測集合，P(n|z1：T)為第n 個節點在1 到T 時間段觀測集合的后驗概率。通過式（17）對P(n)進行對數變換，則式（19）可以轉化為：

通過上述公式，不斷對當前的觀測數據進行相關融合，實現節點的不斷更新，從而實現八叉樹地圖的構建。

2 實驗結果及分析

2.1 RANSAC算法與PROSAC算法的匹配對比實驗

為驗證基于ORB特征的圖片在采用RANSAC與PROSAC算法情況下誤匹配剔除的效果，本文設置兩組單張圖片對比實驗。實驗平臺：系統Ubuntu16.04；CPU Intel-i7；內存16 GB，用Kinect2 相機采集實驗室真實環境的RGD-B 圖片，將匹配結果用Opencv 軟件進行效果顯示。實驗共做6 組，結果相似，只選取其中一組實驗結果展示如圖3。

圖3 兩種匹配算法效果對比Fig.3 Comparison of matching effects of two algorithms

從圖3中可以看出，基于ORB特征的直接匹配，匹配對較多，結果存在很多誤匹配，而基于ORB 特征的RANSAC 與PROSAC 算法進行剔除誤匹配后的匹配對明顯減少。用圖3圖片的匹配時間與誤匹配剔除時間作為指標對RANSAC算法與PROSAC 算法進行比較，結果如表1 所示，兩種算法獲得內點數基本相同，但采用PROSAC 算法無論匹配總時間與誤匹配剔除時間都大幅度減少，這在實時VSLAM 系統中有很大作用。

表1 RANSAC與PROSAC算法對相同圖片的匹配時間和誤匹配剔除時間Tab.1 Matching time and mismatch elimination time of RANSAC and PROSAC algorithms on the same image

2.2 選取RGB-D數據集與誤差分析分析標準

本文選用公開TUM 數據集中的RGB-D 相關數據在前文提到過平臺進行實驗。ORB-SLAM2 系統在相機快速運動時容易出現跟蹤丟失情況，即無法正確對相機位姿進行估計與定位，所以引入回環檢測來減小累計誤差?；诖藛栴}，本文選取有回環和無回環的兩組RGB-D 數據進行對比實驗，分別是freiburg2_large_no_loop（下文簡稱fr2_np）和freiburg2_large_with_loop（下文簡稱fr2_wp）。fr2_np 是一組時長112.37 s，軌跡長度26.086 m序列；fr2_wp是一組時長173.19 s，軌跡長度39.111 m 序列。通常采用TUM 數據集進行相關實驗時，會采用以下兩個指標作為系統誤差評判的標準。

1）絕對軌跡誤差（Absolute Trajectory Error，ATE）。直接計算相機位姿的真實值與SLAM 系統的估計值之間的差，代表軌跡的全局一致性，該標準非常適合于評估視覺SLAM 系統的性能。

2）相對位姿誤差（Relative Pose Error，RPE）。用于計算相同兩個時間戳上的位姿變化量的差。在用時間戳對齊之后，真實位姿和估計位姿均每隔一段相同時間計算位姿的變化量，然后對該變化量做差，以獲得相對位姿誤差，該標準適合于估計系統的漂移。

2.3 實驗結果及誤差分析

對于絕對軌跡誤差與相對位姿誤差進行分析，采用評價指標是均方根誤差、平均誤差（Mean）、中值誤差（Median）和標準偏差（Standard Deviation，Std Dev），其中RMSE 反映估計值與真實值之間的偏差，平均誤差反映所有估計誤差的平均水平，中值誤差代表所有誤差的中等水平，標準偏差（Std Dev）反映系統軌跡估計的離散程度。通過以上幾種誤差分析可以綜合地反映系統估計的軌跡與真實軌跡值的誤差，并可以借此分析系統的穩定性和可靠性。表2 是ORB-SLAM2 系統與本文系統在ATE 與RPE 分析上的對比。為進一步分析在相對位姿誤差上ORB-SLAM2系統與本文系統的差別，本文將兩種方法的估計軌跡與實際軌跡進行對比，并繪制相對位姿誤差圖如圖4和絕對軌跡誤差圖如圖5。

圖4 fr2_np與fr2_wp序列中ORB-SLAM2與本文系統的相對位姿誤差Fig.4 Relative pose error of ORB-SLAM2 and the proposed system for fr2_np and fr2_wp sequences

圖5 fr2_np與fr2_wp序列中ORB-SLAM2與本文系統的絕對軌跡誤差Fig.5 Absolute trajectory error of ORB-SLAM2 and the proposed system for fr2_np and fr2_wp sequences

可以看出本文系統在剔除誤匹配時將時間縮短為原ORB-SLAM2 系統的一半的基礎上，保持與ORB-SLAM2 基本一致的絕對軌跡誤差和相對位姿誤差。從表2 也可以得到，在RMSE 的比較上，本文系統與ORB-SLAM2系統的ATE 差值在0.01 m 以內，平移漂移RPE 差值在0.02 m 以內，旋轉漂移RPE差值也在0.5°內，基本相同。

表2 ORB-SLAM2系統與本文系統絕對軌跡誤差與相對位姿誤差對比Tab.2 Comparison of absolute trajectory error and relative pose error between ORB-SLAM2 system and the proposed system

2.4 稠密點云地圖與八叉樹地圖

在ORB-SLAM2系統中構建的是稀疏點云地圖，可以用于機器人定位，卻無法進一步應用于機器人導航和機器人路徑規劃。本文在ORB-SLAM2 系統基礎上基于RGB-D 圖像信息進行稠密點云地圖構建和八叉樹地圖構建。在ORB-SLAM2系統中已將構建了稀疏點云地圖并提取了關鍵幀，所以本文在此基礎上對稀疏點云進行拼接，并基于關鍵幀的加入和剔除進行了稠密點云地圖的局部加入與剔除，在不影響機器人定位的情況下成功建立了稠密點云地圖與八叉樹地圖，為進一步在機器人領域應用做出了一定貢獻。因為fr2_np 與fr2_wp數據集是同一場景下的數據，所以建圖效果相同，下面只列出fr2_np的建圖結果。

圖6 fr2_np數據集稠密點云地圖和八叉樹地圖Fig.6 Dense point cloud map and octree map of fr2_np dataset

從圖6（a）可以看出，整個數據集的信息保留完整，其中桌子、椅子和墻面等細節可以清楚地看到，說明稠密點云地圖的構圖效果表現良好，可以應用于機器人的進一步導航等途徑。但與圖6（b）比較發現，稠密點云地圖存在細節過多，和部分冗余的情況，而八叉樹地圖在保留基本信息的基礎上減少了冗余，縮小地圖體積，同樣表現良好。

3 結語

基于ORB 特征進行軌跡估計與位姿估計時，快速有效的誤匹配剔除過程十分重要，為此本文在ORB-SLAM2系統的基礎上，采用PROSAC 算法替換原ORB-SALM2 框架中的RANSAC 算法進行誤匹配的剔除，在保證與ORB-SLAM2系統絕對軌跡誤差和相對位姿誤差基本一致的情況下有效提高了誤匹配剔除速率。另外，本文針對ORB-SLAM2系統只有稀疏點云地圖的不足，添加了稠密點云的構建與八叉樹地圖的構建環節，可以直接用于機器人的導航與路徑規劃，這是稀疏點云地圖無法提供的。在下一步的工作中，將研究如何構建語義地圖與語義地圖的定位。