改進特征匹配的ORB-SLAM 稠密建圖算法

劉洋，陳俊，胡詩佳，賴佳華

（1.福州大學(xué) 先進制造學(xué)院，福建 晉江 362200；2.中國科學(xué)院海西研究院泉州裝備制造研究中心，福建 晉江 362200）

0 概述

同步定位與建圖（Simultaneous Localization and Mapping，SLAM）指在未知環(huán)境下，機器人在運動中估計自身軌跡并構(gòu)建周圍環(huán)境的模型。使用相機作為傳感器來解決定位與建圖問題的一般稱為視覺SLAM。MUR-ARTAL等［1］編寫1 套基于特征點法的視覺SLAM 系統(tǒng)。文獻［2］開源了支持單目、雙目、RGB-D 3 種接口的ORB-SLAM2。文獻［3］推出與混合地圖數(shù)據(jù)關(guān)聯(lián)的ORB-SLAM3，提出基于特征的緊耦合慣性視覺里程計，定位的精度和魯棒性得到顯著提升。然而，ORB-SLAM3 在跟蹤階段依舊使用暴力搜索描述子距離得到大量匹配點對，并結(jié)合比率測試篩選出置信度較高的匹配點對，或者將上一幀跟蹤的地圖點投影到當(dāng)前幀，建立旋轉(zhuǎn)直方圖用于檢測旋轉(zhuǎn)一致性。這2 種方法用于特征點匹配，對于相機變換角度較大的場景容易產(chǎn)生誤匹配，而且其建立的是稀疏地圖。稀疏地圖是指地圖只建模特征點部分而不是所有看到過的部分，僅用于對相機進行定位，難以推斷特征點之間的空間結(jié)構(gòu)，從而無法實現(xiàn)導(dǎo)航、避障、重建等高層次的應(yīng)用［4］。

針對特征匹配環(huán)節(jié)效率和準確度較低的問題，研究人員從不同的角度提出解決方案。從描述符入手，文獻［5］提出一種基于稀疏光流與描述符無關(guān)的兩階段關(guān)鍵點匹配方法，文獻［6］手工設(shè)計一種基于投票的環(huán)采樣模式的特征組合二進制描述符，同時編碼強度和梯度。文獻［7-9］基于深度學(xué)習(xí)方法生成緊湊的二進制描述符，文獻［10］比較了手工設(shè)計、經(jīng)過訓(xùn)練的卷積神經(jīng)網(wǎng)絡(luò)（Convolutional Neural Network，CNN）、來自預(yù)訓(xùn)練的CNN 這3 類描述符，研究發(fā)現(xiàn)基于CNN 的描述符優(yōu)于手工設(shè)計的描述符。研究人員引入其他特征來增強特征匹配的魯棒性，文獻［11-12］針對低紋理環(huán)境，增加線特征，結(jié)合點和線段2 種特征進行匹配。文獻［13-15］采用更優(yōu)的匹配策略以及外點剔除方法，提升匹配準確率。但以上方法會延長特征匹配時間。針對稀疏地圖表達能力弱的問題，文獻［16］將卷積神經(jīng)網(wǎng)絡(luò)與SLAM 相結(jié)合，生成帶有語義標簽的地圖，文獻［17］提出一種稠密映射框架來補充稀疏視覺SLAM 系統(tǒng)，將SLAM 系統(tǒng)生成的相機位姿、關(guān)鍵幀和稀疏點作為輸入，通過TSDF 融合進行全局一致的稠密3D 重建。不同的地圖類型適用不同的應(yīng)用場景。

本文從提升特征匹配環(huán)節(jié)效率和生成一種易進行2 次加工的地圖這2 個角度出發(fā)，在特征點匹配階段提出一種改進的ORB-GMS 匹配策略并在建圖方面加入稠密點云構(gòu)建線程，從而提升定位精度與建圖的效果。點云地圖提供一個可視化功能，通過點云地圖，構(gòu)建滿足導(dǎo)航需求的占據(jù)網(wǎng)格地圖［18-19］，也可以進行泊松重建［20］滿足三維重建的目的，從而擴大算法的使用場景。

1 相關(guān)原理

1.1 特征點法

經(jīng)典視覺SLAM 框架由傳感器信息讀取、視覺里程計（Visual Odometry，VO）、優(yōu)化、回環(huán)檢測和建圖5 個模塊組成。其中，VO 的主要任務(wù)是通過相鄰兩幀圖像確定相機的運動，計算相機位姿。根據(jù)實現(xiàn)原理的不同，VO 主要分為特征點法和直接法2類。特征點法具有較優(yōu)的穩(wěn)定性且對光照和動態(tài)物體不敏感，成為較主流的方式。特征點法的主要思想是對輸入的圖像進行特征點檢測并提取描述子，然后進行特征匹配，計算相機位姿和建圖。特征點指圖像中一些辨識度高的像素點，一般圖像中的角點、邊緣和區(qū)塊都可以當(dāng)成圖像中具有代表性的地方，其中在不同圖像之間角點相比邊緣或者區(qū)塊的辨識度更強。常用的特征點法有FAST［21］、SIFT［22］、SURF［23］、ORB［24］等，ORB 特征點法能實現(xiàn)速度和質(zhì)量的平衡［24］。ORB 特征由關(guān)鍵點和描述子2 部分組成。關(guān)鍵點稱為Oriented FAST，是一種加入尺度不變性和旋轉(zhuǎn)不變性的FAST 角點［21］，描述子稱為BRIEF［25］，用于在不同圖像中匹配的特征點。由于BRIEF 描述子是二進制的，因此比較2 點的特征非?？欤ㄟ^暴力匹配另一幀上的所有特征點，計算最小的漢明距離得到最佳匹配。但是該方式在重復(fù)紋理或者缺乏紋理信息的區(qū)域內(nèi)會產(chǎn)生大量的誤匹配，因此后續(xù)還會采用一些方法剔除誤匹配。

1.2 ORB-SLAM3 算法框架及原理

2015 年西班牙薩拉戈薩大學(xué)開源ORB-SLAM。2021 年ORB-SLAM3 被提出。基于ORB-SLAM 和ORB-SLAM2 的代碼框架，ORB-SLAM3 依舊采用3 個線程，分別是跟蹤、局部建圖和閉環(huán)&地圖融合。ORB-SLAM3 在之前工作的基礎(chǔ)上實現(xiàn)了基于視覺與IMU 的緊耦合SLAM 系統(tǒng)，完全依賴最大似然估計，使得穩(wěn)定性大幅提升，并且與以前的方法相比精度提高了2～10倍［3］。

圖1 所示為OpenCV 和ORB-SLAM3 在同一幅圖像上提取ORB 特征點的結(jié)果（彩色效果見《計算機工程》官網(wǎng)HTML 版）。ORB-SLAM3 采用的ORB特征點是一種改進的FAST 角點，并添加了尺度和旋轉(zhuǎn)描述。通過比較中心像素與周圍像素的亮度來判斷該點是否為特征點。由此產(chǎn)生的角點經(jīng)常出現(xiàn)“扎堆現(xiàn)象”，OpenCV 提取的特征點如圖1（a）所示。ORB-SLAM3 采用多種策略提升特征點質(zhì)量，利用非極大值抑制和四叉樹均勻分布策略解決角點集中的問題，通過高斯模糊原圖像提取更顯著的特征點，構(gòu)建圖像金字塔以實現(xiàn)尺度不變性，通過灰度質(zhì)心法增加旋轉(zhuǎn)不變性。ORB-SLAM3 在提取關(guān)鍵點之后，還需計算相對應(yīng)的二進制描述子。從圖1 可以看出，ORB-SLAM3 提取的特征點數(shù)更多且更均勻。

圖1 OpenCV 與ORB-SLAM3 提取ORB 特征點對比Fig.1 Comparison between OpenCV and ORB-SLAM3 in extracting ORB feature points

兩幀間的特征匹配是為了確定不同幀之間特征點的對應(yīng)關(guān)系。通過準確匹配圖像與圖像或者圖像與地圖之間的描述子，為后續(xù)的姿態(tài)估計和后端優(yōu)化降低計算量。暴力匹配是ORB-SLAM3 最常用的特征匹配方法，通過計算當(dāng)前圖像上每個特征點與另一幅圖像上所有特征點的BRIEF 描述子的漢明距離，并對其進行排序，取最近的1 個特征點作為匹配點。ORB-SLAM3 先采用ratio test（若最近的特征點距離除以次近距離小于某個閾值，則接收這對特征點）篩選出一部分比較顯著的匹配特征點，然后在特征點的一定半徑內(nèi)快速搜索候選匹配特征點，計算旋轉(zhuǎn)角度，并放入將360°分為30 個小格子的直方圖中，利用角度一致性檢測過濾掉角度差超過12°的外點，從而快速得到相對“正確”的匹配特征點對。然而，對于角度差在12°以內(nèi)的錯誤匹配或者是相機視角變化較大的場景，ORB-SLAM3 就無法得到正確的匹配。

2 改進方案

2.1 改進的ORB-GMS 方法

基于網(wǎng)格的運動統(tǒng)計（Grid-based Motion Statistics，GMS）算法［26］是一種基于網(wǎng)格運動統(tǒng)計的快速魯棒的特征匹配方法，主要用于多視圖立體（Multi-View Stereo，MVS）三維重建領(lǐng)域，以解決因相機角度變化較大而存在相鄰兩幀對應(yīng)特征不顯著的問題。本文對該方法進行改進，融合ORB-SLAM3的特征匹配階段，在提升特征匹配準確率的同時縮短所用時間。本文基本思想是正確匹配滿足于運動平滑性約束，即1 個正確匹配點對的旁邊肯定還會有幾個正確的匹配點對來支撐，這些匹配點具有運動一致性，而1 個錯誤匹配點對（假設(shè)錯誤是隨機發(fā)生的）旁邊的支撐匹配點對幾乎很少，其原因為很難找到出現(xiàn)同樣錯誤的匹配點對。因此，通過統(tǒng)計特征點鄰域內(nèi)的匹配點總數(shù)來判斷當(dāng)前匹配是否正確。針對SLAM 算法實時性要求高和相機拍攝幀率較快的特點，本文放棄使用GMS 原本擴展的運動核，舍棄旋轉(zhuǎn)不變性和尺度不變性，追求更高效的特征匹配。網(wǎng)格化圖片可以加快算法效率，使得原本中心點鄰域由圓變成了3×3 的網(wǎng)格，而GMS 忽視了不同網(wǎng)格到中心點的距離和置信度不同。因此，本文根據(jù)與中心網(wǎng)格的距離，對各網(wǎng)格賦予不同的權(quán)重，以達到更加公平計數(shù)的目的。

假設(shè)圖像Image1 和Image2 分別有p、q個特征點，xi={pi，qi}表示第i個點鄰域內(nèi)的特征點匹配對，|xi|表示數(shù)量，定義匹配點鄰域支持度Si=|xi|-1。為加快算法效率，將圖像分成20×20 的網(wǎng)格，通過網(wǎng)格化來加速運算。把原本鄰域內(nèi)匹配點的問題簡化為網(wǎng)格的對應(yīng)匹配問題，選擇匹配點對附近的3×3網(wǎng)格作為鄰域，如圖2 所示。

圖2 特征點對的運動核Fig.2 Motion kernel of feature point pairs

本文定義網(wǎng)格的鄰域為它周圍的8 個網(wǎng)格，其中a1、a3、a7、a9與中心網(wǎng)格a5的距離是a2、a4、a6、a8到a5距離的倍，因此支持度Sab的計算式更新為：

其中：|xakbk|表示ak到bk匹配點對數(shù)量；Sab表示匹配點支持度的得分。

假設(shè)每個特征點匹配都是相互獨立的，那么支持度的概率分布近似于二項分布：

其中：|xi|代表第i個點鄰域內(nèi)特征點的匹配數(shù)量；pt和pf分別表示正確匹配和錯誤匹配在其對應(yīng)鄰域內(nèi)有支持度支持的概率。pt通常取決于特征質(zhì)量，與匹配的正確率接近，而pf通常很小，誤匹配的分布幾乎是隨機的，其對應(yīng)的期望與方差如下：

正確匹配和錯誤匹配支持度對應(yīng)的概率分布如圖3 所示。

圖3 正確匹配和錯誤匹配支持度對應(yīng)的概率分布Fig.3 Probability distribution corresponding to support for correct matching and wrong matching support

由兩者的分布可看出正確匹配點和錯誤匹配點具有可分性，并且根據(jù)GMS［26］的推導(dǎo)，可分性正比于，說明隨著特征匹配點數(shù)量的增多，兩者的期望相差會越來越大，可分性也越來越明顯。閾值設(shè)置為：

閾值越大說明劃分標準越嚴格，匹配點對更可能是正確的匹配，由GMS［26］得知，a在4～6范圍 內(nèi)時，結(jié)果較優(yōu)。因此，本文取α=6。當(dāng)支持度Si大于閾值時，說明pi和qi是1 對正確的匹配，否則是1 對錯誤的匹配。

2.2 稠密點云地圖與濾波

ORB-SLAM3 只能生成稀疏點云地圖，因此，本文在原算法框架上加入稠密點云構(gòu)建線程，利用RGB-D 相機進行稠密重建。RGB-D 相機可以通過傳感器中的硬件測量得到深度，無須消耗大量的計算資源來估計。在ORB-SLAM3 3 個線程的基礎(chǔ)上，增加1 個稠密點云構(gòu)建線程，首先匹配RGB 圖像與深度圖，然后根據(jù)前端估算相機位姿，在新增關(guān)鍵幀時將位姿信息和深度信息傳入稠密點云構(gòu)建線程，計算圖像上所有像素的空間坐標，生成點云圖。利用RGB 圖像信息給點云上色，隨著關(guān)鍵幀不斷新增，與生成的地圖不斷進行點云拼接和全局優(yōu)化，最后得到1 個由離散點組成的彩色點云地圖，實現(xiàn)稀疏到稠密的轉(zhuǎn)變。本文算法架構(gòu)如圖4所示，中間虛線方框部分為新增的稠密點云構(gòu)建線程。

圖4 本文算法架構(gòu)Fig.4 Architecture of the proposed algorithm

經(jīng)過遍歷匹配好的RGB 圖像和深度圖，提取像素坐標和深度值，根據(jù)針孔相機成像模型可得：

其中：Z為深度值和實際空間距離的比例關(guān)系因子；K為相機內(nèi)參矩陣；R和t為相機的旋轉(zhuǎn)矩陣和平移矩陣；PW為像素點在世界坐標系下的三維坐標。由于相機已進行標定，因此內(nèi)參矩陣K為已知量，通過跟蹤線程來計算相機位姿，以確定R和t，進一步確定RGB 圖像中每個像素的空間坐標。本文調(diào)用PCL（Point Cloud Library）庫處理計算得到的點云數(shù)據(jù)。點云pi的坐標可由式（6）計算得出：

利用RGB 圖像三通道的色彩信息給點云上色，然后讀取下一個關(guān)鍵幀并進行點云拼接。

在實際構(gòu)建稠密點云地圖時存在如下問題：1）產(chǎn)生的點云非常冗余，由于多個視角存在視野重疊，因此在重疊區(qū)域內(nèi)會存在大量位置十分相近的點；2）受RGB-D 相機天然技術(shù)缺陷的影響，導(dǎo)致一些部位的深度測量出現(xiàn)錯誤，如深黑色物體、表面光滑物體、透明物體，產(chǎn)生的點云雜亂。為獲得更好的視覺效果，本文加入外點去除濾波器和體素網(wǎng)格的降采樣濾波器，把平均距離在標準范圍之外的點視為離群點并從數(shù)據(jù)中去除。假設(shè)所有的點云在1 個大立方體的空間內(nèi)，根據(jù)體素濾波器中設(shè)定的分辨率0.01 m×0.01 m×0.01 m，將該大立方體分割成1 個1 cm3的立方體，求解每個小立方體的質(zhì)心坐標，用該坐標近似表達整個小立方體，相當(dāng)于對三維空間進行降采樣，從而節(jié)省存儲空間。

3 實驗與結(jié)果分析

3.1 實驗環(huán)境

本文選用TUM 中RGB-D 數(shù)據(jù)集［27］的Handheld SLAM 類別進行實驗，該數(shù)據(jù)集主要是在室內(nèi)場景下進行統(tǒng)計，紋理比較豐富且場景深度較小，有利于特征點法發(fā)揮優(yōu)勢。大部分圖像因相機的移動而模糊，有利于驗證本文改進方案的有效性。實驗平臺是搭載Ubuntu18.04 系統(tǒng)的筆記本電腦，處理器是Intel i7-9750H，主頻2.60 GHz，運行內(nèi)存16 GB。

3.2 特征匹配實驗

特征匹配優(yōu)劣的評價通常從速度和匹配數(shù)量2 個方面進行分析。因此，本文從每個數(shù)據(jù)集中隨機抽取相近的兩幀進行特征匹配，每幀提取1 000 個特征點，模擬實際運行時關(guān)鍵幀間的特征匹配。不同方法在特征匹配所花費的時間以及匹配的特征點數(shù)量如表1 所示，加粗表示最優(yōu)數(shù)據(jù)。從表1 可以看出，本文算法縮短了特征匹配所需的時間，并且在特征點匹配數(shù)量上也有大幅度提升。

表1 不同方法的特征匹配實驗結(jié)果Table 1 Experimental results of features matching among different methods

圖5所示為ORB-SLAM3、ORB-GMS 和本文算法篩除誤匹配的結(jié)果。

圖5 ORB-SLAM3、ORB-GMS與本文算法篩除誤匹配的結(jié)果Fig.5 The results of ORB-SLAM3，ORB-GMS and the propoesd algorithm for screening out mismatches

按平均表現(xiàn)來看，本文算法在匹配耗時上約為ORB-SLAM3 時長的1/2，匹配的特征點數(shù)比ORB-GMS 略高，相較于原ORB-SLAM3 匹配的特征點數(shù)提高了60%，說明本文算法克服ORB-SLAM3 效率低且誤匹配率高的缺點。本文算法對于視角變化較大的兩幀具有魯棒性，以fr3/lo 為例，兩幀的相機視角變化較大，在圖5 的左圖中相機位于辦公椅的側(cè)后方，無法看見辦公桌左側(cè)的垃圾桶，在圖5 的右圖中相機移到了辦公椅的正后方，能看見垃圾桶。左右圖的特征點差別較大，導(dǎo)致ORB-SLAM3 只有75 對匹配點，而本文算法是其3.5倍，說明在相機快速移動的情況下，本文算法具有更優(yōu)的魯棒性。

3.3 SLAM 算法精度實驗

本文從時耗、復(fù)雜度、精度等多個角度評估SLAM 算法的表現(xiàn)。其中，對精度的評價主要使用絕對軌跡誤差（Absolute Trajectory Error，ATE）和相對軌跡誤差（Relative Pose Error，RPE）2 個精度指標。絕對軌跡誤差是相機位姿的真實值與SLAM 系統(tǒng)估計值之間的差值，適合評估算法精度和相機軌跡的準確性。而相對軌跡誤差計算在相同2 個時間戳上估計位姿變化量與真實位姿變化量的差，適合估計系統(tǒng)的漂移。本文算法針對特征匹配環(huán)節(jié)，旨在獲得更加精確的位姿，并不會對系統(tǒng)的漂移產(chǎn)生影響，也通過實驗得到驗證，在此并不贅述。因此，本文選擇絕對軌跡誤差來判斷定位精度是否提升，并且選用平均誤差（MEAN）、均方根誤差（RMSE）、誤差平方和（SSE）這3 個指標來反映絕對軌跡誤差的變化。

在各個數(shù)據(jù)集上ORB-SLAM3 和本文算法的絕對軌跡誤差對比如表2所示。本文把RMSE 變化量小于0.01 m 或變化幅度在10%以內(nèi)的視為無明顯變化，用“—”表示，變化量大于0.01 m 且變化幅度小于-10%視為有下降趨勢，用“↓”表示，變化量大于0.01 m 且變化幅度大于10%視為有上升趨勢，用“↑”表示。

表2 ORB-SLAM3 與本文算法在TUM 數(shù)據(jù)集上的絕對軌跡誤差對比Table 2 Comparison of absolute trajectory error between ORB-SLAM3 and the proposed algorithm on the TUM datasets 單位：m

在fr1/360、fr1/fl、fr2/de、fr2/wl 數(shù)據(jù)集上，本文算法的MEAN、RMSE、SSE 均得到有效降低，除fr2/he數(shù)據(jù)集以外，在其他大部分數(shù)據(jù)集上都保持與ORBSLAM3 一樣的精度。經(jīng)過觀察分析數(shù)據(jù)集的視頻序列，誤差無變化的數(shù)據(jù)集相機移動緩慢且場景紋理比較豐富，這也意味著相鄰兩幀間的變化非常小，ORB-SLAM3 方法也能得到較正確的匹配，本文算法沒有發(fā)揮其優(yōu)勢，但是可以減少匹配的耗時。在fr2/he 數(shù)據(jù)集上本文算法表現(xiàn)不佳的原因是相機在地面與天花板之間來回移動，而天花板是一大片重復(fù)紋理，反而增多了誤匹配點的數(shù)量，從而導(dǎo)致誤差增大，說明本文算法在處理重復(fù)紋理場景時存在局限性。但本文算法在發(fā)揮作用時，明顯展現(xiàn)出它的優(yōu)勢，絕對軌跡誤差的各個指標全面下降，平均降低約32%。

Fast ORB-SLAM［5］以及PL-SLAM［12］算法同樣是通過提高匹配的準確度來提高定位精度，本文在TUM 數(shù)據(jù)集不同的序列下比較3 種算法的定位誤差，將絕對軌跡誤差的均方根誤差（RMSE）作為評價指標，結(jié)果如表3 所示，表中數(shù)據(jù)來自文獻［5，12］，“—”表示無法從文獻中得到相應(yīng)的值，加粗表示最優(yōu)數(shù)據(jù)。本文算法總體具有更優(yōu)的準確度。

表3 不同算法在TUM 數(shù)據(jù)集下的均方根誤差對比Table 3 Root mean squared error comparison among different algorithms on TUM dataset 單位:m

3.4 稠密建圖實驗

稀疏地圖以及濾波前后的稠密點云地圖效果對比如圖6 所示，圖6（a）所示為ORB-SLAM3 的可視化運行界面，生成的稀疏地圖對人眼不友好。由于稀疏地圖中點與點之間沒有結(jié)構(gòu)，無法展現(xiàn)立體感，因此人們無法理解地圖保存的路標點。通過稠密點云構(gòu)建線程，利用RGB-D 相機獲取的深度信息生成三維彩色空間點云，利用統(tǒng)計濾波器去除外點并采用體素濾波對生成的點云進行降采樣。表4 所示為在fr1/fl 數(shù)據(jù)集上，對比濾波前后的點云數(shù)量、生成模型大小。從表4 可以看出，濾波后冗余和雜亂的點云數(shù)量急劇減少，存儲空間也大幅縮減，均只有濾波前的1/4，而建圖效果下降不明顯，有利于大場景下的稠密建圖。

表4 濾波前后點云數(shù)量與模型大小對比Table 4 Comparison of point cloud number and model size without filtering and after filtering

圖6 稀疏地圖以及濾波前后的稠密點云地圖效果Fig.6 Sparse map and dense point cloud map without filtering and after filtering

4 結(jié)束語

針對ORB-SLAM3 中特征匹配環(huán)節(jié)費時且不準確的問題，本文提出一種改進的ORB-GMS 匹配策略，將運動平滑性約束作為特征點運動統(tǒng)計的方法，把圖像網(wǎng)格化，對不同的網(wǎng)格按距離賦予不同的權(quán)重，通過比較與特征點相鄰的8 個網(wǎng)格內(nèi)匹配點對數(shù)量和預(yù)設(shè)閾值的大小來快速判斷當(dāng)前匹配是否正確。針對ORB-SLAM3 未能生成稠密地圖的問題，本文基于PCL 庫添加1 個點云構(gòu)建線程，實現(xiàn)稠密建圖并進行濾波處理，得到更符合人們可視化習(xí)慣且能進一步加工處理的稠密點云地圖。實驗結(jié)果表明，本文算法能有效提高特征匹配的效率和定位精度，為后續(xù)更高層次的應(yīng)用奠定基礎(chǔ)。下一步將研究如何利用圖像的全局信息來解決特征點法在重復(fù)紋理或無紋理場景下產(chǎn)生的退化問題。