張昊宇，柳祥樂，王思山

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

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

1 整體算法框架

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

圖1 改進算法整體方案

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

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

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

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

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

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

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

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

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

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

2.2 特征點匹配及剔除

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

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

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

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

分別對特征點提取、特征匹配及剔除和整體改進算法進行實驗測試。實驗平臺為Ubuntu18.04和ROS Melodic，測試所用電腦CPU 為Intel i7 4700HQ、GPU為GTX960M。

3.1 特征提取測試

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

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

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

3.2 GMS-NN測試

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

圖4 NN匹配

圖5 RANSAC誤匹配剔除

圖6 GMS誤匹配剔除

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

總體來說，GMS 比RANSAC 運行速度快，可以較好地剔除誤匹配的同時保留有效匹配信息。

3.3 改進算法測試

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

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

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

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

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

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

圖8 改進算法軌跡

4 結(jié)論

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