張昊宇，柳祥樂(lè)，王思山

（湖北汽車工業(yè)學(xué)院 電氣與信息工程學(xué)院，湖北 十堰 442002）

同步定位與地圖構(gòu)建（simultaneous localization and mapping，SLAM）作為環(huán)境感知技術(shù)的代表，主要分為視覺SLAM和激光SLAM。近些年來(lái)，由于相機(jī)傳感器較激光傳感器價(jià)格低、易部署、數(shù)據(jù)易處理［1］，視覺SLAM 逐漸成為研究的焦點(diǎn)。傳統(tǒng)視覺前端可分為特征點(diǎn)法［2-4］與直接法［5-7］。Juan D.Tardos 基于具有方向性的快速特征點(diǎn)和旋轉(zhuǎn)不變的描述（oriented FAST and rotated BRIEF，ORB）特征提出了ORB-SLAM2［4］，隨后又增加IMU融合算法和多地圖系統(tǒng)，提出了ORB-SLAM3 算法。此算法具有較強(qiáng)的魯棒性，但在紋理單一的場(chǎng)景中，易出現(xiàn)特征點(diǎn)數(shù)量較少或堆積的情況，造成幀間位姿估計(jì)誤差較大，甚至特征點(diǎn)跟蹤丟失［8］。Jakob Engel 基于光度不變性提出了直接法SLAM算法DSO［5］，雖然計(jì)算量減少，運(yùn)行速度變快，但是對(duì)相機(jī)內(nèi)參以及曝光非常敏感，在光照變化大的場(chǎng)景或者快速運(yùn)動(dòng)時(shí)容易跟蹤失?。?］。近年來(lái)，隨著機(jī)器學(xué)習(xí)的迅猛發(fā)展，深度學(xué)習(xí)以其強(qiáng)大的學(xué)習(xí)能力在計(jì)算機(jī)視覺領(lǐng)域中展現(xiàn)出其優(yōu)勢(shì)。Daniel DeTone等人提出的特征點(diǎn)提取深度自監(jiān)督卷積神經(jīng)網(wǎng)絡(luò)SuperPoint［10］，通過(guò)構(gòu)建已知位姿關(guān)系的圖片進(jìn)行興趣點(diǎn)自標(biāo)注，在構(gòu)建損失函數(shù)時(shí)加入姿態(tài)誤差，使其在位姿解算方面更有優(yōu)勢(shì)。英國(guó)牛津大學(xué)提出VINet［11］，結(jié)合IMU 進(jìn)行絕對(duì)姿態(tài)估計(jì)，在面對(duì)時(shí)間不同步和外參標(biāo)定不準(zhǔn)確的多視覺慣導(dǎo)數(shù)據(jù)時(shí)表現(xiàn)出了一定的優(yōu)勢(shì)，并且無(wú)需建立復(fù)雜的運(yùn)動(dòng)模型，也不考慮相機(jī)參數(shù)以及尺度變化，但其準(zhǔn)確性與魯棒性依賴神經(jīng)網(wǎng)絡(luò)的設(shè)計(jì)，且需要大量不同場(chǎng)景數(shù)據(jù)進(jìn)行訓(xùn)練，泛用性較差［12］。針對(duì)上述傳統(tǒng)視覺前端魯棒性不足及基于學(xué)習(xí)的幀間位姿估計(jì)存在苛刻條件等問(wèn)題，文中基于ORB-SLAM3框架使用深度學(xué)習(xí)對(duì)視覺前端進(jìn)行改進(jìn)，并引入誤匹配剔除策略進(jìn)行優(yōu)化，提高位姿估計(jì)的精確度。

1 整體算法框架

文中改進(jìn)算法整體方案如圖1 所示，在ORBSLAM3框架的基礎(chǔ)上對(duì)前端進(jìn)行了優(yōu)化。系統(tǒng)分為視覺（慣性）前端和非線性優(yōu)化后端部分，運(yùn)行時(shí)有跟蹤、局部建圖和閉環(huán)檢測(cè)3個(gè)線程。

圖1 改進(jìn)算法整體方案

1）跟蹤 跟蹤線程對(duì)輸入圖像進(jìn)行特征點(diǎn)提取及匹配。根據(jù)相鄰幀的特征匹配結(jié)果進(jìn)行幀間運(yùn)動(dòng)估計(jì)，跟蹤已經(jīng)重建的局部地圖并優(yōu)化位姿，之后判斷當(dāng)前幀是否為關(guān)鍵幀。

2）建圖 建圖線程主要完成局部地圖構(gòu)建，包括對(duì)新關(guān)鍵幀的處理、剔除并生成新的地圖點(diǎn)。在融合當(dāng)前幀與相鄰幀重復(fù)的地圖點(diǎn)后使用BA進(jìn)行局部?jī)?yōu)化，最后對(duì)插入的關(guān)鍵幀進(jìn)行篩選，剔除冗余的關(guān)鍵幀。

3）閉環(huán)檢測(cè) 閉環(huán)檢測(cè)主要分為閉環(huán)條件檢測(cè)和全局優(yōu)化。閉環(huán)條件檢測(cè)使用詞袋模型計(jì)算相似度，在檢測(cè)到閉環(huán)的時(shí)候計(jì)算Sim3 變換之后進(jìn)行閉環(huán)融合和圖優(yōu)化。

2 傳統(tǒng)視覺前端的改進(jìn)

2.1 基于GCNv2深度網(wǎng)絡(luò)的特征點(diǎn)提取

ORB-SLAM3 算法中雖然利用四叉樹對(duì)特征點(diǎn)進(jìn)行均勻分發(fā)，但并沒有從本質(zhì)上解決特征點(diǎn)堆疊的問(wèn)題。文中利用GCNv2網(wǎng)絡(luò)代替ORB算法進(jìn)行圖像的特征點(diǎn)提取，通過(guò)非極大值抑制，得到平穩(wěn)均勻的特征點(diǎn)輸出。

1）GCNv2 網(wǎng)絡(luò)結(jié)構(gòu) 受Superpoint 網(wǎng)絡(luò)結(jié)構(gòu)啟發(fā)，GCNv2 網(wǎng)絡(luò)在設(shè)計(jì)上比上一代更加輕量化，其網(wǎng)絡(luò)結(jié)構(gòu)如表1 所示。特征點(diǎn)檢測(cè)網(wǎng)絡(luò)和描述子生成網(wǎng)絡(luò)共用同一個(gè)編碼網(wǎng)絡(luò)，整體網(wǎng)絡(luò)簡(jiǎn)化模型如圖2 所示。整個(gè)網(wǎng)絡(luò)主要分為特征點(diǎn)提取和計(jì)算描述子部分。特征點(diǎn)檢測(cè)網(wǎng)絡(luò)和描述子提取網(wǎng)絡(luò)共用了卷積層1到卷積層4的卷積網(wǎng)絡(luò)進(jìn)行圖像特征編碼，之后分別使用卷積層F和卷積層D卷積網(wǎng)絡(luò)進(jìn)行解碼，再利用亞像素卷積和雙線性插值進(jìn)行處理，得到特征點(diǎn)和相應(yīng)的描述子。

圖2 GCNv2網(wǎng)絡(luò)簡(jiǎn)化模型

表1 GCNv2網(wǎng)絡(luò)結(jié)構(gòu)

2）特征提取流程 改進(jìn)算法增加了深度學(xué)習(xí)特征提取的相關(guān)函數(shù)，主要包括新增深度學(xué)習(xí)特征提取函數(shù)ExtractGCN（），并在Frame 類中新增基于深度學(xué)習(xí)特征提取的構(gòu)造幀函數(shù)GCNextractor（），該新增函數(shù)接口對(duì)傳入的圖像進(jìn)行深度學(xué)習(xí)特征點(diǎn)提取，同時(shí)保持輸出類型與原算法輸出的特征點(diǎn)及其描述子類型一致。與傳統(tǒng)ORB-SLAM3 算法不同，GCNv2 網(wǎng)絡(luò)特征提取算法流程要更加簡(jiǎn)潔。將圖像輸入到GCNv2 網(wǎng)絡(luò)后，先通過(guò)模型輸出可得到對(duì)應(yīng)的特征點(diǎn)和描述子，再利用非極大值抑制得到區(qū)域最佳特征點(diǎn)的位置，使提取到的特征點(diǎn)不會(huì)密集堆疊，從而得到良好的輸出結(jié)果。

2.2 特征點(diǎn)匹配及剔除

常見的匹配點(diǎn)對(duì)剔除策略是隨機(jī)抽樣一致性采樣［13］（random sample consensus, RANSAC），它能在一組包含錯(cuò)誤的數(shù)據(jù)樣本中得到有效樣本數(shù)據(jù)。由于ORB-SLAM3 中并沒有專門的誤匹配剔除算法，僅使用地圖點(diǎn)投影進(jìn)行內(nèi)外點(diǎn)區(qū)分，因此文中使用GMS-NN 算法進(jìn)行特征點(diǎn)匹配及剔除，并與RANSAC進(jìn)行對(duì)比測(cè)試。

1）GMS 算法 GMS 是基于運(yùn)動(dòng)統(tǒng)計(jì)的快速魯棒特征匹配過(guò)濾算法，其核心思想是：匹配應(yīng)該是平滑的，正確匹配周圍會(huì)有較多的匹配去支持它，而錯(cuò)誤的匹配周圍支持它的匹配很少。該算法可以動(dòng)態(tài)地計(jì)算出閾值P，當(dāng)1 個(gè)匹配點(diǎn)對(duì)鄰域內(nèi)的匹配點(diǎn)對(duì)數(shù)量小于P時(shí)，判定該匹配是錯(cuò)誤的。

2）特征匹配及剔除 改進(jìn)算法在特征匹配上主要進(jìn)行了2個(gè)方面的改進(jìn)。在雙目圖像匹配時(shí)，新增ComputeStereoGMSMatches（）函數(shù)，利用GMSNN算法代替立體稀疏匹配；在幀間特征點(diǎn)匹配時(shí)，使用最近鄰（nearest neighbor，NN）匹配進(jìn)行代替。GMS-NN 算法先使用NN 匹配算法對(duì)特征點(diǎn)進(jìn)行匹配，再將圖像劃分為20×20 的網(wǎng)格，以加速GMS算法進(jìn)行誤匹配剔除，減少運(yùn)算時(shí)間，最后得到正確的匹配結(jié)果。

3 測(cè)試結(jié)果及分析

分別對(duì)特征點(diǎn)提取、特征匹配及剔除和整體改進(jìn)算法進(jìn)行實(shí)驗(yàn)測(cè)試。實(shí)驗(yàn)平臺(tái)為Ubuntu18.04和ROS Melodic，測(cè)試所用電腦CPU 為Intel i7 4700HQ、GPU為GTX960M。

3.1 特征提取測(cè)試

測(cè)試采用ORB-SLAM3 與GCNv2 網(wǎng)絡(luò)分別對(duì)同一圖像進(jìn)行特征提取，結(jié)果如圖3所示。圖3a為ORB-SLAM3 算法均勻化提取特征點(diǎn)后得到的結(jié)果，可以看到在局部特征明顯的地方出現(xiàn)特征點(diǎn)集中、扎堆的現(xiàn)象，不利于視覺SLAM系統(tǒng)進(jìn)行跟蹤，也會(huì)降低運(yùn)動(dòng)位姿估計(jì)的精度。在低紋理或快速運(yùn)動(dòng)時(shí)，依舊會(huì)出現(xiàn)跟蹤丟失的情況。GCNv2 網(wǎng)絡(luò)的特征點(diǎn)提取結(jié)果如圖3b 所示，特征點(diǎn)分布均勻，沒有發(fā)生特征點(diǎn)堆疊的情況。

圖3 不同算法的特征點(diǎn)提取結(jié)果

通過(guò)對(duì)比測(cè)試結(jié)果可知，在原算法提取不到ORB 特征點(diǎn)的地方，GCNv2 網(wǎng)絡(luò)也能檢測(cè)出特征點(diǎn)，其檢測(cè)的特征種類更加多樣，且改進(jìn)之后的特征點(diǎn)提取更加均勻。

3.2 GMS-NN測(cè)試

實(shí)驗(yàn)在提取2000 個(gè)ORB 特征點(diǎn)的情況下，使用NN匹配算法進(jìn)行特征匹配，測(cè)試RANSAC 誤匹配剔除算法與GMS誤匹配剔除算法的運(yùn)行時(shí)間和剩余正確點(diǎn)對(duì)數(shù)量，特征點(diǎn)匹配結(jié)果如圖4 所示，誤匹配剔除結(jié)果如圖5～6 所示。通過(guò)圖5～6 對(duì)比可知，RANSAC在保留正確篩選的同時(shí)也剔除了有效的匹配，只保留了電腦桌面附近的密集匹配對(duì)。表2 為不同特征點(diǎn)提取數(shù)量下3 種算法的測(cè)試結(jié)果，由于RANSAC 需要不斷的進(jìn)行迭代，其結(jié)果模型的可信度與迭代次數(shù)成正比，高度可信的結(jié)果往往需要花費(fèi)百倍于其他算法的時(shí)間。在特征點(diǎn)提取數(shù)量較少的情況下，RANSAC過(guò)濾后的特征點(diǎn)對(duì)比GMS略多，但是保留下來(lái)的特征點(diǎn)對(duì)密集堆積，不利于視覺SLAM進(jìn)行跟蹤，且丟失其他有效匹配對(duì)，造成局部特征信息丟失。在特征點(diǎn)數(shù)量較多的情況下，RANSAC過(guò)濾后的匹配對(duì)數(shù)量小于GMS。

圖4 NN匹配

圖5 RANSAC誤匹配剔除

圖6 GMS誤匹配剔除

表2 特征點(diǎn)提取數(shù)量不同時(shí)算法結(jié)果對(duì)比

總體來(lái)說(shuō)，GMS 比RANSAC 運(yùn)行速度快，可以較好地剔除誤匹配的同時(shí)保留有效匹配信息。

3.3 改進(jìn)算法測(cè)試

改進(jìn)SLAM 算法的測(cè)試評(píng)價(jià)指標(biāo)為絕對(duì)軌跡誤差（absolute trajectory error，ATE），其原理是計(jì)算不同時(shí)間戳下真實(shí)位姿和估計(jì)位姿之間的均方根誤差，計(jì)算公式為

文中主要采用雙目飛行器EUROC 數(shù)據(jù)集的v1_01_easy 與v2_01_easy 序列和TUM 數(shù)據(jù)集中 單目RGBD 的紋理與結(jié)構(gòu)分組、機(jī)器人SLAM 分組以及3D 重建分組。利用EVO 工具對(duì)改進(jìn)后的算法進(jìn)行評(píng)估，評(píng)價(jià)該算法在不同特征紋理環(huán)境下系統(tǒng)的精確性以及魯棒性。表3 為不同測(cè)試序列下的測(cè)試結(jié)果。其中none為由于提取特征點(diǎn)數(shù)量過(guò)少而導(dǎo)致初始化失敗，系統(tǒng)view 窗口沒有任何輸出；lost 為view 窗口有特征點(diǎn)輸出但是由于運(yùn)動(dòng)過(guò)快或紋理稀疏導(dǎo)致跟蹤失敗。從表中結(jié)果可以看出，改進(jìn)后的算法得益于特征點(diǎn)提取的多樣性與提取均勻化，使系統(tǒng)對(duì)不同紋理結(jié)構(gòu)場(chǎng)景下跟蹤的魯棒性大大提高。

表3 平均絕對(duì)軌跡誤差測(cè)試結(jié)果 m

使用TUM數(shù)據(jù)集fr3_nostr_text_near_loop序列分別對(duì)改進(jìn)前后算法進(jìn)行測(cè)試，改進(jìn)算法的平均誤差為0.0161 m，原算法的平均誤差為0.0192 m。實(shí)時(shí)絕對(duì)軌跡誤差結(jié)果如圖7 所示。對(duì)比可知改進(jìn)算法的絕對(duì)軌跡誤差比原算法小，表明改進(jìn)算法在位姿估計(jì)上誤差更小，估計(jì)結(jié)果更優(yōu)。

圖7 不同算法測(cè)試結(jié)果

使用TUM數(shù)據(jù)集序列fr3_large_cabinet分別對(duì)改進(jìn)前后算法進(jìn)行測(cè)試，算法估計(jì)軌跡與真實(shí)軌跡對(duì)比結(jié)果如圖8 所示。對(duì)比軌跡圖像可知原算法由于圖像特征點(diǎn)數(shù)量較少而跟蹤失敗，改進(jìn)算法在稀疏特征環(huán)境中也可以很好地提取特征點(diǎn)，從而跟蹤成功，其魯棒性更強(qiáng)。

圖8 改進(jìn)算法軌跡

4 結(jié)論

文中提出了基于ORB-SLAM3 框架的改進(jìn)算法，并對(duì)其魯棒性、位姿估計(jì)精度進(jìn)行了測(cè)試。實(shí)驗(yàn)結(jié)果表明，該算法在對(duì)結(jié)構(gòu)與紋理的檢測(cè)更加敏感，能有效去除誤匹配對(duì)的同時(shí)保留了整體匹配信息，且避免了特征點(diǎn)堆積的問(wèn)題。改進(jìn)算法雖然引入深度學(xué)習(xí)，增加了圖形運(yùn)算量，但在低紋理環(huán)境中，整體跟蹤性能更強(qiáng)，泛用性更廣。