融合全局描述子和半直接法的雙目SLAM

梁 鴻，陳靈娜+，何嘯峰，周揚帆，蔣良衛

(1.南華大學 計算機學院，湖南 衡陽 421001； 2. 湖南財經工業職業技術學院 電子信息系，湖南 衡陽 421000)

0 引 言

基于視覺的同時定位與地圖構建(visual simultaneous localization and mapping，VSLAM)在自動駕駛、移動機器人、增強現實、虛擬現實等新興技術中發揮著重要作用。回環閉合作為視覺SLAM必不可少的一部分，能確保系統在長期定位與跟蹤過程中更加準確；回環閉合最重要的一步是閉環檢測。目前，大部分視覺SLAM方案主要使用基于局部特征的視覺Bow模型進行閉環檢測。由于可以重用前端視覺里程計的描述子，常被基于間接法的SLAM系統所采用，如流行的ORB-SLAM2[1]、OpenVSLAM[2]都訓練了各自的視覺詞典用于回環閉合和重定位。Gao等[3]將DSO[4]作為前端視覺里程計，計算關鍵幀的ORB特征描述子并建立Bow模型，將其擴展為具有回環閉合功能的直接法SLAM。Lee等[5]提出了一種松耦合的半直接法SLAM，亦使用DSO作為前端，并行執行ORB-SLAM2部分功能實現閉環和重定位，并構建全局一致的地圖。此類方法不同于間接法SLAM可以重用前端的Bow模型，在使用直接法完成前端位姿跟蹤后，需對關鍵幀提取特征點并計算相應的描述子，增加了計算量。此外，基于局部特征的Bow模型會丟失更高層次的幾何和結構信息，不能應對較強的環境變化，且需預先訓練視覺詞典；相對于局部特征描述子，表示整張圖像的全局描述子對外觀變化的魯棒性更好[6]。

本文在借鑒文獻[7]的思想上，提出了一種融合全局描述子和半直接法雙目SLAM方案—GDSLAM(global descriptor SLAM)。在ORB-SLAM2框架上，融合半直接法進行位姿跟蹤，避免了計算特征詞向量和特征匹配；利用知識蒸餾的方法訓練得到全局描述子計算模型，結合主成分分析(PCA)和K近鄰檢索，實現快速準確的閉環檢測，為系統重定位和回環閉合模塊實現了高效準確的定位。

1 系統框架

融合全局描述子和半直接法的雙目SLAM框架如圖1所示，包括4個部分：雙目跟蹤模塊、局部建圖模塊、關鍵幀的全局描述子管理模塊、回環閉合模塊。位姿跟蹤模塊通過半直接法實現位姿跟蹤。局部建圖模塊包括構建地圖點和維護局部地圖，并進行共視圖優化。關鍵幀全局描述子管理模塊負責對關鍵幀計算全局描述子并進行存儲和檢索。回環閉合模塊負責閉環檢測和全局優化。其中，在模塊內部或者模塊之間的雙向箭頭表示數據的傳遞和反饋。

圖1 GDSLAM系統框架

2 全局描述子的計算與檢索

研究表明基于深度學習得到的場景描述在應對環境變化時表現更好[8]，并產生了一些優秀的端到端圖像描述模型用于解決大規模的視覺場景識別問題[9]。但是這些模型由于參數太多、計算量太大，不適合用于需要實時計算的SLAM。Sarlin等[7]借助知識蒸餾[10]的方法將大型圖像檢索模型壓縮為具有移動實時推理功能的小型網絡，在移動平臺上的檢索與定位。文獻[12]使用學習方式代替傳統手工設置的特征提取，通過知識蒸餾實現同時預測局部和全局描述子，實現了類似的分層定位功能，并取得最新的成績。

2.1 基于MobileNet的全局描述子生成網絡

在大型場景識別問題中，基于卷積神經網絡(CNN)的研究工作通常分兩步生成圖像的全局描述子：首先，通過網絡編碼層計算圖像的高級特征圖，然后映射到一個緊湊的全局描述子中；最初使用最大池化作為映射層。Arandjelovic等[9]使用局部聚合特征(VLAD)描述方法為 CNN 設計了新的池化層，稱為NetVLAD層，更加有效地利用卷積特征，提高了對同類別圖像的表達能力和圖像區分能力。原始NetVLAD網絡由于參數太多，計算量太大，不適合資源受限的平臺使用。Sandler等[12]發布了MobileNet，這是一種針對資源受限的設備而設計的編碼器體系結構。由于NetVLAD的訓練過程很復雜，耗時長，不能滿足實時性；本文采取與文獻[7]類似的方法，通過知識蒸餾訓練一個較小的網絡(MobileGDNet)來學習NetVLAD生成的圖像全局描述子，這大大減少了新模型的訓練時間。通過最小化MobileGDNet與預訓練的NetVLAD網絡生成的全局描述子之間的均方誤差損失(MSE loss)來訓練MobileGDNet,訓練過程如圖2所示。

圖2 通過知識蒸餾的方法訓練MobileGDNet

2.2 訓練與測試

由于原始的NetVLAD網絡使用matlab實現，不方便擴展訓練；Cieslewski等[13]基于Tensorflow重新實現了NetVLAD(NetVLAD-Tensorflow)，在本實驗中也使用基于Tensorflow的NetVLAD作為訓練網絡模型。MobileGDNet的編碼器是MobileNetV2模型，并使用在ImageNet上預訓練的權重進行初始化。為了減少計算，從包含較少的通道數(320)的倒數第二個圖像編碼層計算全局描述子。每個VLAD層具有32個聚類簇，共生成10 240維的圖像描述子，該描述子進一步向下映射到4096維，保持與NetVLAD網絡相同的輸出維數。考慮到許多數據集不是RGB數據而是灰度圖，所以網絡的輸入為640*480的灰度圖像，并選擇包含各種環境和尺寸的Google Landmarks數據集[14]，從中隨機選擇10萬張圖片作為訓練數據。實驗環境為：操作系統環境為Ubuntu16.04，主頻為3.6 GHz的CPU，顯存為11 Gb的GTX1080Ti，16 G內存。

為了驗證MobileGDNet的圖像描述能力，選擇包含多個閉環的KITTI數據集00序列作為評估數據。使用主成分分析(PCA)減少描述子的維數，提高檢索速度。首先，計算NetVLAD-Tensorflow、MobileGDNet在KITTI-00上的全局描述子，以及通過PCA降維之后的描述子(64維、128維、512維)。然后，計算距離協方差矩陣并獲取最相似位置向量。最后，分別與ground truth計算的最相似位置向量進行比較，計算準確率、召回率和AUC值。相應的PR(準確率-召回率)曲線和AUC值，如圖3所示。

圖3 KITTI-00序列上的PR曲線和相應的AUC值

從圖3可見，MobileGDNet生成的描述子與NetVLAD-Tensorflow生成的描述子表現出了相似圖像描述能力，而且通過PCA對描述子降維，沒有明顯削弱其描述能力。從表1可見，通過知識蒸餾，MobileGDNet的模型大小相當于NetVLAD-Tensorflow的1/20，而計算速度提升了6倍以上；此外，在GPU顯存為2 G的GTX1050上的計算速度達到了28幀/秒，對于計算資源有限的平臺較有利。

表1 模型大小與CPU計算耗時情況

3 融合半直接法的雙目SLAM

GDSLAM在ORB-SLAM2框架上，選擇具有固定基線的雙目相機模型，能夠恢復圖像點深度信息，解決了單目相機尺度漂移的問題；使用基于光度最小化的半直接法估計位姿，具有更快的位姿跟蹤速度。在文中，假設以左相機作為相機坐標系；令π表示相機坐標系下的3D點p到特征點u的投影模型；相應的π-1表示特征點u到相機坐標系3D點p的反投影模型；令Ik-i表示k-i時刻的幀信息，Ik表示當前幀；令Tk,w表示當前幀相機在世界坐標系下的位姿，Tk,k-i表示當前幀與第k-i幀的相對位姿變換。

3.1 融合半直接法的雙目位姿跟蹤

首先，多線程對矯正之后的雙目灰度圖提取ORB特征并進行匹配，計算雙目特征對的特征點深度值和地圖點的3維坐標，完成初始化，此過程與ORB-SLAM2一致。當前幀的相機位姿Tk,w采用勻速運動模式與匹配關鍵幀兩種初始化策略，其中勻速運動模式與ORB-SLAM2一致。直接匹配關鍵幀初始化相機位姿：基于光度不變假設(即兩幀觀察到同一個3D點的像素塊之間的光度應該相同)，采取從粗到精的迭代順序進行圖像對齊(首先在svo[15]中提出，能夠有效地執行兩幀間的對齊)；待優化的殘差為關鍵幀Ik-i的特征點u對應的圖塊與投影到當前幀Ik上圖塊的光度差。在已知特征點深度du， 相應的最小二乘公式為

(1)

解決該最小二乘問題，一般有前向組合和逆向組合兩種算法；其中，逆向組合算法可以提前計算雅可比矩陣J和海塞矩陣H，避免反復計算H而減少計算量；因此，使用逆向組合算法優化該最小二乘模型能夠加速運算。通過此方法，得到當前幀與參考關鍵幀之間的相對變換位姿Tk,k-i， 進而可以得到當前幀的相機位姿Tk,w，Tk,w與幀間相對位姿Tk,k-i的變換公式為

Tk,w=Tk,k-i·Tk-i,w

(2)

通過勻速運動模型或者跟蹤關鍵幀獲得初始位姿之后，再與局部地圖中的地圖點進行匹配，進一步優化當前幀的位姿，待優化的重投影誤差如式(3)。實現中使用了開源的圖優化算法g2o；由于此處僅優化相機位姿，故也稱為Motion-only BA(bundle adjustment)

(3)

3.2 關鍵幀的全局描述子管理

采取與ORB-SLAM2一致的關鍵幀判斷策略，對于新插入的關鍵幀，由MobileGDNet計算相應的全局描述子，并通過PCA降維之后加入關鍵幀的全局描述子存儲庫中，即建立KD樹并添加新的結點；在重定位和回環閉合模塊中，結合K近鄰檢索與半直接法匹配完成閉環檢測。

3.3 閉環檢測

閉環檢測作為視覺SLAM非常重要的一個部分，它的目的是識別以往到過的地方。對于視覺SLAM系統，有效的閉環檢測可以幫助系統準確及時地完成回環閉合，極大地減少系統運行過程中的累積誤差，從而構建連續的環境地圖。此外，閉環檢測也可以用來幫助系統在跟蹤失敗時進行重定位。

回環閉合與重定位相同的地方在于都可以通過閉環檢測確定當前幀的大概位置；但重定位在跟蹤丟失的情況下，一般在上一幀附近尋找匹配關鍵幀，確定相機目前的大概位置；而回環閉合為了避免錯誤的閉環給系統帶來嚴重的影響，在閉環檢測過程中，則會排除前不久的關鍵幀。此外，回環閉合的閉環驗證還可以加入更多的邏輯判斷，比如相機姿態的空間一致性。不同于ORB-SLAM2通過視覺詞袋方法檢測候選幀，本文通過關鍵幀全局描述子管理模塊選擇重定位和回環檢測的候選幀，然后通過位姿匹配驗證空間一致性。

3.3.1 重定位

(1)對當前關鍵幀的灰度圖計算全局描述子，并通過K近鄰搜索從地圖關鍵幀的描述子存儲庫中檢索得到前幾個最相似的關鍵幀索引，根據關鍵幀id倒序排列(優先選擇當前幀附近的關鍵幀)。

(2)當前幀使用類似位姿跟蹤的方法與檢索到的關鍵幀圖像進行位姿對齊，當平均光度誤差較小時執行下一步。

(3)在當前幀中搜索來自關鍵幀的三維點。如果正確匹配的點超過20個，則最小化匹配之間的重新投影誤差，并跳過其余關鍵幀。

只要找到一個有效的關鍵幀，跟蹤就繼續以新的估計相機位姿進行。如果重定位無效，則將在接收到下一幀時再次執行。

3.3.2 回環閉合

回環閉合一般分兩步執行，首先檢測與驗證閉環，然后執行位姿圖優化與全局BA完成回環閉合。

(1)針對新插入的關鍵幀，通過K近鄰檢索前10幀最相似的關鍵幀；由于回環閉合對候選幀準確性要求更高，需要檢測候選幀的連續性，故檢索的候選幀數大于重定位的設置值。過濾掉相似性較大的局部共視幀之后，為了加速閉環候選幀檢測，只在圖像金字塔最高層將當前關鍵幀與候選幀進行幀間對齊，當光度誤差較小時，進行接下來的候選幀連續性檢驗。

(2)利用位姿圖優化減小系統的運動漂移，并執行全局BA，將閉環中所有關鍵幀的位姿和3D點作為優化變量，通過最小化重投影誤差獲得全局一致性更好的運動軌跡和環境地圖。

4 實驗與分析

實驗所使用的環境配置:主頻為2.4 GHz的四核i5處理器，16 G內存，顯存為2 G的GTX1050，系統環境為Ubuntu16.04。此外，選擇公開的KITTI數據集和EuRoc數據集[16]驗證算法的有效性和準確性，并與當前流行的開源算法ORB-SLAM2、OpenVSLAM進行對比。評估指標使用絕對軌跡誤差的均方根誤差(absolute trajectory error’s RMSE，ATErmse)表示，絕對軌跡誤差是算法的估計位姿和真實位姿的直接差值,反應了算法精度和軌跡全局一致性。為了方便與SLAM框架整合，實驗過程中使用tensorflow的C++編譯版本部署MobileGDNet；在對已經建好的KD-tree檢索時使用了libnabo，主要用于低維空間的快速K最近鄰檢索。

在回環閉合模塊的閉環檢測部分，使用輪詢匹配所有關鍵幀的方法代替使用全局描述子檢測，實驗中以SDSLAM標識，作為對比算法，驗證基于全局描述子進行閉環檢測在SLAM方案中的及時性和準確性。

4.1 算法加載時間與跟蹤幀率

ORB-SLAM2、OpenVSLAM、SDSLAM、GDSLAM這4種算法啟動后加載配置參數和詞典/模型所用時間的均值和中值見表2。算法的加載時間在實際應用中會一定程度上影響用戶的感受。ORB-SLAM2和OpenVSLAM加載了原作者發布的詞典，其中ORB-SLAM2加載的詞典為文本型，而OpenVSLAM所使用的詞典為二進制。從表2中可見，二進制詞典相對文本型詞典加載時間更短，且占用存儲更少。GDSLAM加載的模型較小，時間較短。采用最小化光度誤差作為位姿跟蹤的SDSLAM加載時間最短。

表2 4種算法加載時間的均值和中值/s

位姿跟蹤模塊是實時執行的，ORB-SLAM2、OpenVSLAM、GDSLAM這3種算法在KITTI數據集03序列上的跟蹤頻率如圖4所示，該序列共有800幀雙目圖像；在00-10序列上的平均跟蹤頻率如圖5所示。從圖4和圖5中可見，GDSLAM平均跟蹤耗時較ORB-SLAM2和OpenVSLAM更少，主要原因是：GDSLAM不需要計算圖像幀的詞向量，并且使用最小化光度誤差的半直接法匹配代替特征點匹配。

圖4 3種算法在KITTI數據集03序列上的跟蹤頻率

圖5 3種算法在KITTI數據集上的跟蹤頻率(FPS)

4.2 SDSLAM與GDSLAM的比較分析

SDSLAM與GDSLAM在KITTI數據集各序列上的絕對軌跡誤差ATErmse，如圖6所示。在00序列上的軌跡如圖7所示，該序列共有4541幀雙目圖像，并包含多個閉環，很適合用來做回環閉合模塊的檢測。從圖6可見，GDSLAM相對于SDSLAM，在大部分序列上都得到更低的誤差值，尤其在包含回環的序列(如：00、02、05、06、07、08、09)，GDSLAM的絕對軌跡誤差ATErmse都比SDSLAM低許多。從圖7可見，GDSLAM能夠很好檢測到各處的回環閉合點，而SDSLAM在最后的回環閉合處不能準確識別并減少累計誤差。可以驗證，在回環閉合模塊中，基于全局描述子的檢測方法相對于使用直接法匹配更加及時且準確。

圖6 SDSLAM和GDSLAM在KITTI數據集下的 絕對軌跡誤差ATErmse

圖7 SDSLAM和GDSLAM在KITTI數據集的 00序列上的軌跡

4.3 KITTI和Euroc數據集

ORB-SLAM2、OpenVSLAM、SDSLAM、GDSLAM這4種算法在公開的雙目數據集KITTI和Euroc上的實驗結果見表3。從表3中可見，ORBSLAM2和OpenVSLAM作為當前流行且開源的間接法SLAM在兩個數據集上的表現相差不大。本文的GDSLAM在KITTI數據集的03、05、06、09序列上誤差最小，其中05、06、09序列只有一個閉環，且都處于圖像序列的結尾部分，對閉環檢測的及時性要求較高(在圖像序列處理完之前，若不能及時檢測到閉環，就不能減少累積誤差，實驗誤差較大)；GDSLAM在09序列上的誤差相對ORB-SLAM2和OpenVSLAM分別減少了57%和32%；由此也驗證了GDSLAM閉環檢測的及時性和準確性。

從表3中也可見，GDSLAM在Euroc數據集的V101、V201、MH05序列上誤差最小。SDSLAM和GDSLAM在Euroc數據集上表現相當，而在KITTI數據集上，SDSLAM表現較差。由于KITTI數據集為室外環境且幀間的運動較大，要求算法能夠及時且準確地檢測到閉環并減少累積誤差。Euroc數據集為室內環境，算法中回環閉合模塊對實驗結果的影響較小。

從以上實驗可見，本文提出的GDSLAM相對于兩種流行的間接法SLAM(ORBSLAM2、OpenVSLAM)在跟蹤耗時方面有一定的優勢；相對于在回環閉合模塊中采用遍歷所有關鍵幀的方式進行閉環檢測(如本文所設計的SDSLAM)，GDSLAM能夠及時準確地檢測閉環候選幀，完成回環閉合并減少累計誤差。

5 結束語

為了有效地解決基于半直接法視覺SLAM方案的閉環檢測問題，提出了融合全局描述子和半直接法的雙目SLAM(GDSLAM)。在ORB-SLAM2框架上，使用直接法代替基于ORB特征匹配，避免計算特征詞向量，一定程度上提高了跟蹤幀率；通過知識蒸餾，訓練得到圖像的全局描述子生成模型，減少了訓練時間且提高了已有網絡的通用性。構建關鍵幀的全局描述子管理模塊，在GDSLAM的重定位和回環閉合模塊中能夠快速準確地完成閉環檢測。在兩個公開數據集KITTI和Euroc上進行實驗，并且與流行的開源算法ORB-SLAM2、OpenVSLAM進行對比；實驗結果表明，相對于SDSLAM(遍歷所有關鍵幀進行閉環檢測)，GDSLAM能夠更及時地檢測到閉環，得到全局一致性更好的軌跡和地圖；在KITTI數據集中包含閉環的序列上，GDSLAM在其中大部分序列上取得了更小的絕對軌跡誤差；同時，在取得相似跟蹤精度的情況下，能夠得到更高的跟蹤幀率。在接下來的工作中，進一步提高前端跟蹤幀率，融入更高層圖像信息(如圖像語義信息等)，設計魯棒性更好的全局描述子生成網絡。

表3 4種算法在KITTI和Euroc數據集下的絕對軌跡誤差ATErmse