基于ORB-SLAM2系統(tǒng)的快速誤匹配剔除算法與地圖構(gòu)建

席志紅，王洪旭，韓雙全

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

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

0 引言

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

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

圖1 本文SLAM系統(tǒng)框架Fig.1 SLAM system framework in this paper

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

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

1 本文SLAM系統(tǒng)

ORB-SLAM系統(tǒng)基于ORB特征實現(xiàn)，包含跟蹤定位、局部地圖構(gòu)建、回環(huán)檢測三個主線程和全局BA（Bundle Adjustment）優(yōu)化線程，在該系統(tǒng)基礎(chǔ)上，本文用PROSAC 算法替代原RANSAC算法，并添加建立稠密點云地圖和八叉樹地圖線程到ORB-SLAM2框架內(nèi)，本文系統(tǒng)的技術(shù)框架圖如圖1所示。

1.1 系統(tǒng)線程

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

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

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

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

1.2 PROSAC算法原理

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

PROCAS 方法的成立假設(shè)：具有更高相似性的數(shù)據(jù)點更可能是類內(nèi)點。

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

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

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

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

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

1）確定數(shù)據(jù)集大小。

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

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

最終它們之間的遞歸關(guān)系為：

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

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

所以生長函數(shù)可以被定義為：

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

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

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

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

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

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

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

在un*中發(fā)現(xiàn)一個非隨機的結(jié)果必須要滿足

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

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

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

算法流程如下所示。

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

輸出 單應矩陣H。

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

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

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

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

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

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

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

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

8）用更新后t重新計算單應矩陣H及新內(nèi)點。

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

1.3 稠密點云地圖與八叉樹地圖的構(gòu)建

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

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

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

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

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

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

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

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

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

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

八叉樹地圖構(gòu)建具體原理如下所示：

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

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

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

通過上述公式，不斷對當前的觀測數(shù)據(jù)進行相關(guān)融合，實現(xiàn)節(jié)點的不斷更新，從而實現(xiàn)八叉樹地圖的構(gòu)建。

2 實驗結(jié)果及分析

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

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

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

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

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

2.2 選取RGB-D數(shù)據(jù)集與誤差分析分析標準

本文選用公開TUM 數(shù)據(jù)集中的RGB-D 相關(guān)數(shù)據(jù)在前文提到過平臺進行實驗。ORB-SLAM2 系統(tǒng)在相機快速運動時容易出現(xiàn)跟蹤丟失情況，即無法正確對相機位姿進行估計與定位，所以引入回環(huán)檢測來減小累計誤差。基于此問題，本文選取有回環(huán)和無回環(huán)的兩組RGB-D 數(shù)據(jù)進行對比實驗，分別是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 數(shù)據(jù)集進行相關(guān)實驗時，會采用以下兩個指標作為系統(tǒng)誤差評判的標準。

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

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

2.3 實驗結(jié)果及誤差分析

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

圖4 fr2_np與fr2_wp序列中ORB-SLAM2與本文系統(tǒng)的相對位姿誤差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與本文系統(tǒng)的絕對軌跡誤差Fig.5 Absolute trajectory error of ORB-SLAM2 and the proposed system for fr2_np and fr2_wp sequences

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

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

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

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

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

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

3 結(jié)語

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