李 卓， 劉潔瑜， 周 偉

(火箭軍工程大學導彈學院，西安 710025)

0 引言

視覺里程計(Visual Odometer,VO)僅利用攝像機的輸入圖像對載體的位姿進行估計[1]，近十幾年來已廣泛應用于各類機器人自主導航[2-3]。VO一般將局部運動估計級聯(lián)得出攝像機的全局導航信息，從而使得誤差不斷積累繁殖，在長時間大范圍情況下導航精度不高。

為實現(xiàn)大規(guī)模復雜環(huán)境下的準確導航，消除載體位姿估計的累計誤差，閉環(huán)檢測成為了視覺導航領域的基礎問題和研究熱點。所謂閉環(huán)檢測就是為了使相機識別出曾經到達過的場景，進而校正累積誤差。

由于閉環(huán)檢測應用場景越來越復雜，環(huán)境規(guī)模越來越龐大，其對圖像信息的壓縮要求也越來越高，因此滿足此要求的基于圖像特征相似性的閉環(huán)檢測在實際應用中占據了主導地位[4]，且大多采用視覺詞袋(BoVW)[5-9]技術。其中：ANGELI等[5]研究了增量式視覺詞典構建問題；CUMMINS等[6-7]采用Chow-Liu樹繼承關系的閉環(huán)概率方法；LABBE等[8-9]設計了內存管理機制,提高了實時性。由于采用不同的特征提取和描述算子，基于SURF算子的FAB-MAP算法[10]無法保證實時性；GALVEZ-LOPEZ提出了基于FAST檢測算子和BRIEF特征算子的二進制視覺詞袋技術，計算效率得到了顯著提升[11]；MUR-ARTAL等[12]采用具有旋轉不變性的ORB特征構建詞袋，保證實時性的同時提高了穩(wěn)定性；2016年LEVI[13]提出了LATCH二進制描述子，將以往二進制描述子的像素點對的比對轉變?yōu)槿M圖像塊的F范數比對，在保證二進制描述子實時性的同時提高了魯棒性。另外在閉環(huán)驗證方面，基于文獻[11]的DBoW2庫只考慮圖像中的特征描述，缺乏特征的幾何約束。

基于以上問題與分析，本文建立基于圖像關鍵幀數據庫，并融合特征跟蹤率與RGB直方圖匹配的關鍵幀選取方法，降低信息冗余。采用視覺詞袋查找當前幀的候選閉環(huán)關鍵幀，利用RANSAC-HORN運動估計方法來驗證閉環(huán)，最終進行當前幀位姿優(yōu)化。在KITTI數據集進行仿真實驗，驗證了該閉環(huán)方法的有效性，進一步提高了VO精度。

1 LATCH二進制描述子與視覺詞袋

1.1 LATCH二進制描述子

通常二進制描述子由抽樣模式、方向補償以及抽樣比對3部分組成，目前的幾種二進制描述子在抽樣的策略和辨別能力上均有所不同。

先前的二進制描述子以檢測到的特征點為中心選取一定大小的圖像塊W，一個二進制描述子bW由T對抽樣坐標序列S={st}t=1,…,n={[pt,1,pt,2]}t=1,…,n組成，其中，pt,1=(xt,1,yt,1)和pt,2=(xt,2,yt,2)定義在W坐標系。索引t既與W中的一對坐標關聯(lián)，又與高斯光滑核σt=(σt,1,σt,2)t=1,…,n關聯(lián)。對于每一抽樣對st，比較pt,1和pt,2經過光滑后的灰度，從而設置二進制中的相應位的值，即

(1)

式中，(W(pt,1,σt,1),(W(pt,2,σt,2))是圖像塊W中坐標pt,1(pt,2)經標準差σt,1(σt,2)高斯濾波后的值。

最終的二進制串bW定義為

(2)

(3)

在運行時間方面，LATCH二進制描述子保持了二進制描述子的優(yōu)勢，比基于直方圖描述子快一個數量級；在魯棒性方面，LATCH二進制描述子在大多數數據集上的效果優(yōu)于其他二進制描述子，縮小了與基于直方圖描述子的差距[13]。

1.2 建立視覺詞袋

在計算機視覺領域，圖像通常以特征點及其描述來表達，以特征描述看作單詞構建出相應的詞袋模型，圖像便可以轉化為低維稀疏的數字向量，便于大量圖像信息的處理，圖像間的相似度轉化為向量間的相似度，其本質是一個信息壓縮的過程。

圖1 K叉樹視覺字典示意圖Fig.1 Sketch map of the K-ary tree visual dictionary

根據文獻[14]所采用的頻率-逆文檔頻率(TF-IDF)方法為每個葉節(jié)點賦予權重。頻率-逆文檔頻率為

(4)

式中:niIt為單詞wi在圖像中It出現(xiàn)的次數;nIt為It中的單詞總數;N為訓練數據庫的圖像總量;ni為單詞i在整個訓練圖像庫中出現(xiàn)的次數。

TF部分的思想是某單詞在一幅圖像中的出現(xiàn)次數越高，其區(qū)分度就越高。IDF思想是某單詞在字典中出現(xiàn)的頻率越低，則分類圖像時區(qū)分度越高。TF-IDF越大，表明該單詞在某圖像中出現(xiàn)的頻率高，并且在其他圖像中較少出現(xiàn)，因此該單詞具有很好的區(qū)別分類性。

2 關鍵幀閉環(huán)檢測與驗證

2.1 關鍵幀的選取

與傳統(tǒng)的基于運動變化的關鍵幀選取方法不同，本文選取方法采用當前幀與上一關鍵幀之間的相似度。為提高相似度的可靠性，融合特征跟蹤率與RGB直方圖匹配評判相似度，相似度函數定義為

γ=αcolorhk-hc+αkeypoint·

(5)

在式(5)中可根據αcolor和αkeypoint來調整特征匹配和色彩直方圖匹配的權重，鑒于特征數目較少時隨機因素容易對關鍵幀的選取產生影響，因此本文根據特征數目適當調整αkeypoint，即

(6)

式中,me為期望的特征提取數量。

2.2 關鍵幀閉環(huán)檢測

根據式(4)可計算圖像It中特征點對應的單詞及其權重，從而獲得該圖像的BoW，即

A={(w1,η1),(w2,η2),…,(wN,ηN)}vA

(7)

通過詞袋可用向量vA描述圖像A，而向量vA是稀疏向量，非零部分表達了圖像中含有的單詞種類，且這部分的值為TF-IDF的值。對于給定vA和vB,采用1范數形式來表示向量間的差異(圖像間的距離),即

(8)

從式(8)可以得出：兩圖像越相似，s(vA,vB)得分越高(距離越小)，而當兩圖像無差別時，距離s(vA,vB)=1。為進一步提高評分的可靠性，在先驗相似度s(vt,vt-Δt)基礎上進行歸一化，即

s(vt,vtj)′=s(vt,vtj)/s(vt,vt-Δt)

(9)

先驗相似度表示某時刻關鍵幀圖像與前一時刻關鍵幀的相似性，從而s(vt,vtj)′反映了當前幀與之前的關鍵幀間在先驗相似度基礎上歸一化的相似度。當歸一化相似度達到一定閾值條件時，對應的關鍵幀構成了閉環(huán)候選幀。

基于詞袋的閉環(huán)檢測依賴于外觀而未利用任何幾何信息，從而易造成外觀相似的圖像被檢測為閉環(huán)，為了保證閉環(huán)檢測的可靠性，需要對閉環(huán)進行驗證，以及進行時間和空間一致性的檢驗后,閉環(huán)候選幀才會被認為是正確的。時間一致性需要設立閉環(huán)緩存機制，在一段時間內持續(xù)檢測到閉環(huán)；空間一致性是對檢測到的閉環(huán)兩幀進行特征匹配，估計攝像機幀間運動，最后將該運動級聯(lián)檢查是否滿足閉環(huán)要求。

2.3 基于RANSAC-HORN運動估計的閉環(huán)驗證

與此同時也看到,詞袋只考慮了圖像中的特征描述，丟失了特征的幾何約束。文獻[11]采用隨機抽樣一致性(RANSAC)方法計算兩幅圖像之間的基本矩陣，判斷其內點數是否足夠，從而對當前幀和閉環(huán)候選幀進行幾何一致性校驗。由于雙目相機不存在尺度問題，本文采用基于RANSAC-HORN運動估計的閉環(huán)驗證方法。

HORN方法最少可以使用3對匹配的三維點求取圖像間的運動信息，具體實現(xiàn)為：

1) 設當前關鍵幀的3個三維點質心坐標為Poc，則將三維點減去質心坐標得到質心坐標系下的新坐標Pc{Pc1,Pc2,Pc3}，同法可得到閉環(huán)候選幀3個三維點在其質心Pol坐標系下的新坐標Pl{Pl1,Pl2,Pl3}；

(10)

則此夾角對應的四元數向量為

(11)

(12)

5) 由qa和ql得到旋轉矩陣Ra和Rl，則總旋轉矩陣為Rz=Rl×Ra，平移T=Pol-RPoc。

RANSAC算法可實現(xiàn)對外點的濾除，對輸入數據進行多次隨機采樣并確定模型，根據確定的模型對所有數據進行劃分。經過多次隨機抽樣試驗后會找到符合誤差范圍內最多點的集合，利用該內點集合確定最終的模型?？紤]RANSAC算法的抽樣點數盡量小的原則，根據HORN方法確定抽樣點數的最小集為3對三維點。當將RZ和T確定后，可引導當前關鍵幀特征點在閉環(huán)候選幀中的大致區(qū)域，以及閉環(huán)候選幀中特征點在當前關鍵幀中的大致區(qū)域，查找更多匹配以彌補在詞袋匹配環(huán)節(jié)的漏匹配，當匹配特征數目達到足夠點數時，該閉環(huán)候選幀才被接收為成功閉環(huán)，從而實現(xiàn)了閉環(huán)驗證。

3 閉環(huán)位姿優(yōu)化

經過2.3節(jié)基于RANSAC-HORN運動估計的閉環(huán)驗證后，已知當前關鍵幀與閉環(huán)關鍵幀之間的旋轉矩陣RZ和平移T以及由此引導查找到滿足一定數目的匹配點，則此閉環(huán)候選幀被接收為閉環(huán)關鍵幀，實現(xiàn)對閉環(huán)驗證。為對當前關鍵幀位姿進一步優(yōu)化，將閉環(huán)關鍵幀的局部地圖點投影到當前幀并最小化重投影誤差。

(13)

式中：ρ為具備魯棒性的Huber代價函數;Σ為與特征點尺度相關的協(xié)方差矩陣，目標函數為

(14)

投射函數π定義為

(15)

式中：(fx,fy)為攝像機焦距；(cx,cy)為投影中心；a為基線長度。以上參數均能通過相機的標定來獲取。

HΔx=-b

(16)

式中：b為系數向量；H為系數矩陣(信息矩陣)。通過求解式(16)線性方程組可得最小值Δx*，新的狀態(tài)向量估計值可依據

(17)

進行更新，進而迭代直到滿足預定要求。

常用的非線性最小平方算法有高斯-牛頓算法(GN)，列文伯格-馬夸爾特算法(LM)等，均可以直接用來求最后的最優(yōu)解。由于系數向量和系數矩陣在形式上都是稀疏的，一些開源的稀疏矩陣運算算法結合非線性最小二乘算法可提高計算效率。本文利用在圖優(yōu)化通用框架(g2o)[16]中的LM算法來實現(xiàn)非線性優(yōu)化。

4 實驗與分析

本文實驗數據采用KITTI數據集[17-18]，該數據集是具備組合測量系統(tǒng)的車輛進行跑車實驗而采集得到的，包括搭載有Flea2 Firewire攝像機采集的高質量雙目圖像序列(已預先矯正)和OXTS RT 3000 IMU/GPS測量系統(tǒng)采集的地理數據(作為真值)。運行計算機為DELL OPTIPLEX 7010臺式電腦(Intel Core i5-3470 CPU，主頻3.20 GHz，4.00 GB內存，64位的Windows7操作系統(tǒng))。

在KITTI數據集中,00-10號圖像序列是測試序列，包括了城市道路、小鎮(zhèn)、鄉(xiāng)村、高速多種環(huán)境，00,02,05,06,07,09號圖像序列含有閉環(huán)，包含較多的相似場景。

圖2所示為本文算法在KITTI 數據集上的重建軌跡，圖中，紅色線段為閉環(huán)檢測到的路段，可以看出，在含有閉環(huán)的00,02,05,06,07,09號圖像序列中全部正確識別閉環(huán)。

圖2 本文算法在KITTI數據集的重建軌跡Fig.2 Reconstructed trajectory of the proposed algorithm on KITTI data set

經本文算法閉環(huán)優(yōu)化后的重建軌跡,平移和旋轉誤差均顯著減小。在含閉環(huán)的00,02,05,06,07,09序列下，雙目VO平均平移誤差1.071 4%，平均旋轉誤差為0.003 050 (°)/m；而本文提出基于閉環(huán)檢測的VO平均平移誤差0.835 8%，平均旋轉誤差為0.002 712 (°)/m。

在實時性方面，由于閉環(huán)檢測采用關鍵幀和詞袋技術，同時優(yōu)化只是當前幀位姿優(yōu)化而非全局優(yōu)化，因此在實時性上略有下降,但并不影響VO的實時導航。

綜上實驗結果對比分析，本文提出的閉環(huán)檢測和位姿優(yōu)化算法能夠有效地對閉環(huán)進行準確檢測和驗證，并對VO導航過程的誤差積累進行閉環(huán)位姿優(yōu)化，提高了視覺導航精度。

5 結束語

本文提出了一種基于幾何約束的視覺閉環(huán)檢測和位姿優(yōu)化方法，旨在提高視覺導航精度。首先建立基于LATCH二進制描述子的視覺詞袋，并采用視覺單詞向量描述圖像進而歸一化相似度的閉環(huán)檢測方法，然后對閉環(huán)候選關鍵幀和當前關鍵幀進行基于RANSAC-HORN運動估計的閉環(huán)驗證，最后將閉環(huán)關鍵幀的局部地圖點投影到當前幀并最小化重投影誤差以使得位姿得到優(yōu)化。數據集實驗表明，本文提出的閉環(huán)檢測和位姿優(yōu)化算法能夠有效、準確地對閉環(huán)檢測和驗證，并對VO導航過程的誤差積累進行閉環(huán)位姿優(yōu)化，提高了視覺導航精度。下一步將在本文基礎上探索閉環(huán)驗證后位姿的全局優(yōu)化，以實現(xiàn)對閉環(huán)前路徑的優(yōu)化。

參 考 文 獻

[1] YOUSIF K,BAB-HADIASHAR A,HOSEINNEZHAD R.An overview to visual odometry and visual SLAM:applications to mobile robotics[J].Intelligent Industrial Systems,2015(1):289-311.

[2] KONOLIGE K,AGRAWAL M,SOLJ.Large-scale visual odometry for rough terrain[J].International Journal of Robotics Research,2011(66):201-212.

[3] ANNAIYAN A,YADAV M,OLIVARES-MENDEZ M A,et al.Visual odometry based absolute target geo-location from micro aerial vehicle[C]//IEEE International Conference on Robotics,Automation,Control and Embedded Systems，2015:1-7.

[4] WILLIAMS B,CUMMINS M,NEIRA J,et al.A comparison of loop closing techniques in monocular SLAM[J].Robotics and Autonomous Systems,2009,57(12):1188-1197.

[5] ANGELI A,FILLIAT D,DONCIEUX S,et al.A fast and incremental method for loop-closure detection using bags of visualwords[J].IEEETransactionsonRobotics, 2008,24 (5):1027-1037.

[6] CUMMINS M,NEWMAN P.Probabilistic appearance based navigation and loop closing[C]//IEEE International Conference on Robotics and Automation，2007:2042-2048.

[7] CUMMINS M,NEWMAN P.Highly scalable appearance-only SLAM-FAB-MAP 2.0[C]//Robotics:Science and Systems,2009.doi:10.15607/RSS.2009.V.039.

[8] LABBE M,MICHAUD F.Memory management for real-time appearance-based loop closure detection[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems,2011:1271-1276.

[9] LABBE M,MICHAUD F.Appearance-based loop closure detection for online large-scale and long-term operation[J].IEEE Transactions on Robotics,2013,29(3):734-745.

[10] CUMMINS M,NEWMAN P.Appearance-only SLAM at large scale with FAB-MAP 2.0[J].International Journal of Robotics Research,2011,30(9):1100-1123.

[11] GALVEZ-LOPEZ D,TRADOS J D.Bags of binary words for fast place recognition in image sequences [J].IEEE Transactions on Robotics,2012,28(5):1188-1197.

[12] MUR-ARTAL R,TARDOS J D.Fast relocalisation and loop closing in keyframe-based SLAM[C]//IEEE International Conference on Robotics and Automation(ICRA),2014:846-853.

[13] LEVI G,HASSNER T.LATCH:learned arrangements of three patch codes [C]//IEEE Winter Conference on Applications of Computer Vision (WACV),2016:202-210.

[14] CUMMINS M,NEWMAN P.Accelerating FAB-MAP with concentration inequalities[J].IEEE Transactions on Robotics,2010,26(6):1042-1050.

[15] MUR-ARTAL R,MONTIEL J M M,TARDóS J D.ORB-SLAM:a versatile and accurate monocular slam system[J].IEEE Transactions on Robotics,2015,31(5):1147-1163.

[16] KUMMERLE R,GRISETTI G,STRASDAT H,et al.G2o:a general framework for graph optimization [C]//IEEE International Conference on Robotics and Automation (ICRA),2011:3607-3613.

[17] PAZ L M,PINIES P,TARDOS J D,et al.Large-scale 6-DOF SLAM with stereo-in-hand[J].IEEE Transactions on Robotics,2008,24(5):946-957.

[18] SCARAMUZZA D,FRAUNDORFER F.Visual odometry part Ⅰ:the first 30 years and fundamentals[J].IEEE Robotics & Automation Magazine,2011,18(4):80-92.