基于多特征場景描述的閉環(huán)檢測方法

王通典， 劉潔瑜， 吳宗收， 李文華， 沈 強

(火箭軍工程大學導彈工程學院,西安 710000)

0 引言

同時定位與地圖構(gòu)建(SLAM)是移動載體實現(xiàn)自主定位導航與路徑規(guī)劃的關(guān)鍵技術(shù)。視覺里程計在長時間、大范圍的位姿估計過程中，不可避免會導致累計誤差的產(chǎn)生，較大的累計誤差會嚴重影響定位精度，甚至導致算法收斂到完全錯誤的值。而閉環(huán)檢測是SLAM問題的重要組成部分[1]，閉環(huán)檢測系統(tǒng)可以準確高效地檢測出當前場景是否存在于歷史場景中，從而消除里程計產(chǎn)生的累計誤差。

閉環(huán)檢測是通過對比當前場景與歷史場景的相似性從而判斷移動載體是否經(jīng)過同一位置，該問題本質(zhì)上是一個場景識別問題[2]。基于外觀相似性的閉環(huán)檢測方法是詞袋模型(BoW)方法[3]。詞袋模型的方法是對圖像提取的局部特征采用K-means的方式實現(xiàn)聚類生成視覺詞典，隨后對查詢的場景特征設(shè)置相應(yīng)權(quán)重，將查詢場景的特征向量和歷史場景的特征向量利用單詞量化，進行相似性對比從而識別閉環(huán)。FAB-MAP[4]在BoW基礎(chǔ)上結(jié)合貝葉斯模型實現(xiàn)閉環(huán)檢測方法，該方法在構(gòu)建觀測模型時采用LIU樹擬合詞匯間的拓撲關(guān)系，從而實現(xiàn)觀測概率計算，該方法在計算觀測似然時計算時間較長，為此后續(xù)提出了FAB-MAP2.0[5]以縮短算法用時。ANGELI等[6]將詞袋模型擴展到增量條件下，提出了一種增量式閉環(huán)檢測方法，無需離線訓練詞典，并結(jié)合SIFT特征和顏色直方圖實現(xiàn)特征的視角不變性，該方法利用貝葉斯濾波估計閉環(huán)概率實現(xiàn)了較高的準確率；針對大視角變化的閉環(huán)檢測問題，李維鵬等[7]選取場景中密集特征聚集成為一個顯著區(qū)域從而實現(xiàn)特征描述，結(jié)合貝葉斯濾波概率模型和BoW方法進行閉環(huán)判斷，有效提升大視角變化的閉環(huán)召回率；MURARTAL等[8]采用ORB特征構(gòu)建詞袋模型實現(xiàn)閉環(huán)檢測算法，該算法通過不斷地提高篩選條件，多層篩選閉環(huán)候選幀，最終得到最優(yōu)匹配的閉環(huán)結(jié)果。

上述方法采用局部特征構(gòu)建詞袋模型實現(xiàn)閉環(huán)判斷。除此之外，還可以采用全局特征對圖像進行描述。MILFORD等[9]采用全局特征描述圖像基于序列匹配對閉環(huán)進行判斷，該方法具有一定的環(huán)境不變性，但是計算效率低；SIAM等[10]針對序列匹配的耗時問題進行改進；劉國忠等[11]采用ORB特征結(jié)合SURF特征對圖像進行全局描述，并采用改進的混合K最近鄰方法實現(xiàn)閉環(huán)查詢。因只對圖像進行全局描述，該方法具有較高的運行速率。

相較于局部描述特征，全局描述具有較好的外觀不變性，對環(huán)境變化具有較強的魯棒性，而局部描述特征具有較好的旋轉(zhuǎn)和尺度不變性，對視角變化和位姿變化較為魯棒?；谠~袋模型的方法雖具有廣泛的應(yīng)用性，但仍存在一定問題。由于特征量化為單詞是無序的，從而會導致感知偏差問題，產(chǎn)生大量的誤匹配。

本文考慮單詞之間的拓撲關(guān)系，基于閉環(huán)概率模型，結(jié)合全局特征和局部特征，提出了一種多特征場景描述的閉環(huán)檢測方法，以提高場景描述的魯棒性，排除大部分的感知偏差問題。

1 算法流程

本文算法首先對采集的圖像進行場景描述，采用兩種不同的特征空間進行場景描述：基于SURF局部特征的場景描述和基于ORB特征的全局描述。對于局部SURF特征進行特征單詞化，對于全局特征進行歸一化，融合局部特征和全局特征實現(xiàn)場景描述。隨后對單詞化的局部特征更新逆向索引，根據(jù)索引檢測可能閉環(huán)場景；對全局ORB特征檢索歷史幀中可能閉環(huán)場景。構(gòu)建閉環(huán)概率模型，根據(jù)以上結(jié)果更新閉環(huán)候選幀的后驗概率。最后采用極限約束進一步篩選閉環(huán)幀。閉環(huán)檢測算法流程如圖1所示。

圖1 閉環(huán)檢測算法流程

2 場景描述

2.1 SURF局部特征的場景描述

詞袋模型的方法是對提取的特征進行聚類從而構(gòu)建視覺字典?？紤]SURF特征[12]較為穩(wěn)定，對旋轉(zhuǎn)和尺度具有不變性，同時對仿射變換以及光照變化較為魯棒，本文的局部特征空間描述采用SURF特征。

SURF算法基于SIFT進行了改進，采用一種更高效的方式實現(xiàn)特征點的定位與特征描述計算。SURF算法在尺度空間通過計算Hessian矩陣行列式的局部極大值定位圖像的特征點，對于圖像特征點p=f(u,v)經(jīng)過高斯濾波后Hessian矩陣構(gòu)造如下

(1)

式中：Lxx(p,σ)為二階偏導與圖像的卷積；σ為尺度。則Hessian矩陣的行列式為

det(H)=DxxDyy-(0.9Dxy)2

(2)

式中：Dxx，Dxy，Dyy為特征點經(jīng)過濾波的結(jié)果；在得到矩陣行列式后經(jīng)過濾波得到響應(yīng)點，構(gòu)建尺度空間，通過將響應(yīng)點與圖像空間鄰域8個點以及上下尺度空間鄰域中的18個點進行比較篩選出穩(wěn)定的特征點。SURF特征點定位示意圖見圖2。

圖2 SURF特征點定位示意圖

在提取特征點后進行主方向分配，從而保證SURF特征的旋轉(zhuǎn)不變性。在提取的特征點圓形鄰域范圍內(nèi)相對水平x方向以及豎直y方向計算Harr小波響應(yīng)，隨后對x和y方向的小波響應(yīng)進行高斯加權(quán)，加權(quán)后的值分別表示在水平和豎直方向的方向分量。以特征點為中心，在張角為π/3的扇形滑窗內(nèi)計算Harr小波響應(yīng)dx，dy的累加，即

(3)

滑窗以弧度間隔在圓形范圍內(nèi)滑動，統(tǒng)計范圍內(nèi)Harr小波特征響應(yīng)，模長為

(4)

將模長最大響應(yīng)值lmax對應(yīng)的扇形方向θmax作為該特征的主方向，即

(5)

在得到特征點主方向后，生成該特征點對應(yīng)的描述子。取特征點圖像鄰域的4×4區(qū)域塊生成矩形區(qū)域，將矩形區(qū)域旋轉(zhuǎn)至主方向，并統(tǒng)計各子矩形塊內(nèi)的8個梯度方向的Harr小波特征，分別求和構(gòu)成該子矩形塊的特征描述，將16個子矩形塊的特征描述合并從而形成該特征的描述符，該描述符由64維構(gòu)成，即

A=(a1a2a3…a64)。

(6)

當進行場景描述時，對查詢的圖像首先提取SURF特征，并計算特征描述符。一幅圖像有ns(設(shè)定的提取數(shù)量)個特征，根據(jù)視覺詞典對特征描述符向量單詞化，以單詞對應(yīng)索引存儲特征，由此得到局部特征場景描述。

2.2 基于ORB全局特征的場景描述

全局特征描述采用ORB算法，該算法具有較高的運行速率，由FAST角點檢測和計算BRIEF描述子組成。FAST角點通過比較選定像素和周圍像素灰度差值進行檢測。以一定半徑圓上的像素和中心點像素的差值求和進行比較，大于設(shè)定閾值的中心點作為角點。ORB算法在尺度金字塔上提取FAST角點，增加ORB特征的尺度不變性，結(jié)合灰度質(zhì)心法增加特征點的旋轉(zhuǎn)不變性。

BRIEF[13]描述子是一種二進制描述子，在提取的FAST角點的附近像素區(qū)域內(nèi)選取256個點對，將點對的灰度值比較結(jié)果進行二進制編碼，從而得到一個256維的由0和1組成的二進制特征向量。選取的256個點對的矩陣為

(7)

為增加BRIEF描述子的旋轉(zhuǎn)不變性，采用灰度質(zhì)心法進行補償。以特征點為中心，根據(jù)鄰域圖像灰度值I(x,y)計算圖像矩,即

(8)

根據(jù)鄰域圖像的圖像矩，計算鄰域圖像質(zhì)心

(9)

根據(jù)特征點到質(zhì)心z的矢量計算特征點的方向為

θ=arctan 2(m01,m10)。

(10)

以特征點方向θ作為旋轉(zhuǎn)矩陣Rθ,將Rθ與矩陣S相乘，由此得到具有旋轉(zhuǎn)不變性的Rotation BRIEF描述子。對于兩個描述子H1和H2相似性，本文采用漢明距離進行計算，即

(11)

式中：⊕為異或運算；bitsum(·)為對位進行計數(shù)；R為ORB特征描述符的全局空間。

本文的全局特征計算過程：首先對圖像進行預(yù)處理，即灰度化和均衡化；隨后對圖像進行下采樣，下采樣至60 像素×60 像素大小的圖像塊；對下采樣的圖像中心提取FAST角點并計算方向信息；以圖像中心作為中點，整個圖像作為鄰域計算BRIEF描述符，結(jié)合旋轉(zhuǎn)信息，以此作為整個圖像的全局場景描述。

3 閉環(huán)概率模型

(12)

(13)

(14)

(15)

式中，Zm為不同的特征空間場景描述。本文有兩個特征空間，即基于詞袋模型的SURF局部特征和ORB的全局描述，為此分別構(gòu)建局部特征的觀測概率和全局特征的觀測概率。

3.1 局部特征觀測似然概率

不同尺度空間相互獨立，共同表示同一個圖像。本文基于詞袋模型的方法表示局部特征，為了避免與歷史圖像幀逐幀比較從而導致計算過于耗時，本文采用BoW方法的逆向索引結(jié)構(gòu)[13]從大量的歷史圖像幀中匹配與當前圖像幀最相似的場景。對于當前圖像中的每一個單詞，逆向索引檢索該詞出現(xiàn)過的歷史圖像幀列表，當需要查詢可能的閉環(huán)歷史幀時，只需根據(jù)當前圖像幀的單詞索引歷史幀列表，從而找出單詞個數(shù)滿足要求的歷史幀場景，以實現(xiàn)快速檢索歷史圖像幀。

逆向索引結(jié)構(gòu)算法如圖3所示。

圖3 逆向索引結(jié)構(gòu)算法示意圖

考慮視覺字典樹中不同尺度空間特征的表征能力，不同于文獻[6]的方法，本文采用分層金字塔熵(Term Frequency-Inverse Document Frequency，TF-IDF[14])得分匹配計算似然概率，TF-IDF的熵為

(16)

式中：nw為單詞w在圖像It中出現(xiàn)的次數(shù)；n為圖像It出現(xiàn)的單詞總次數(shù)；Nw為包含單詞w的總圖像數(shù)，N為圖像總數(shù)；為整合不同層次的得分信息，設(shè)在第l層圖像It和Ii的匹配得分為

(17)

整合全部字典層的圖像匹配得分為

(18)

式中，參數(shù)k1表示底層單詞的權(quán)重系數(shù)，本文設(shè)置k1=2。對當前圖像的所有單詞矢量化并且根據(jù)式(16)計算得分后，觀測似然概率計算為

(19)

式中，σ1,μ1分別為當前幀與歷史幀匹配得分的均值和方差。當匹配得分高于均值和方差之和時，基于當前似然概率更新后驗概率。

3.2 全局特征觀測似然概率

(20)

dw對當前幀It和歷史圖像幀Ii的漢明距離進行加權(quán)，即

(21)

由此可得ORB全局特征的閉環(huán)觀測概率為

(22)

式中，σ2,μ2分別為當前幀與k個歷史幀匹配得分的均值和方差。當匹配得分高于均值和方差之和時，基于當前似然概率更新后驗概率。

3.3 閉環(huán)先驗概率

在得到不同尺度空間的觀測似然概率，后驗概率的更新仍需要閉環(huán)先驗概率。先驗概率表征了當前圖像的閉環(huán)概率與歷史圖像閉環(huán)概率之間的拓撲關(guān)系，根據(jù)全概率公式，閉環(huán)先驗概率為

(23)

先驗概率分兩部分：t-1時刻后驗閉環(huán)概率以及轉(zhuǎn)移概率。轉(zhuǎn)移概率表征從t-1時刻到t時刻狀態(tài)轉(zhuǎn)移的可能性，本文分以下4種情況進行討論。

4 多步閉環(huán)候選幀篩選方法

在視覺SLAM中，與圖像檢索問題不同，圖像幀是連續(xù)采集的，因此具有時間上的一致性。當計算出發(fā)生閉環(huán)的后驗概率，為盡可能減少詞袋模型方法在多歧義場景帶來的感知偏差問題，本文考慮時間一致性提出一種多步的閉環(huán)候選幀篩選方法。

3) 在得到閉環(huán)候選幀后，基于極線約束對閉環(huán)候選圖像幀進一步篩選排除錯誤閉環(huán)結(jié)果；對提取SURF特征進行特征匹配，RANSAC篩選內(nèi)點，根據(jù)內(nèi)點數(shù)量確定閉環(huán)圖像幀。

5 實驗與分析

為驗證本文閉環(huán)檢測算法的有效性，采用牛津大學移動機器人研究小組公開的數(shù)據(jù)集New College和City Center進行驗證。數(shù)據(jù)集軌跡如圖4所示，黃色軌跡線表示相近運行軌跡，紅色表示發(fā)生閉環(huán)情況，分別由2146和2474張采集圖像組成，采集間隔為1.5 m。其中包含了較多的歧義場景，可以較全面地評估本文算法針對感知偏差的情況。對比算法采用FAB-MAP以及BoW方法，實驗硬件平臺參數(shù)如表1所示。

圖4 數(shù)據(jù)集軌跡圖

表1 實驗平臺參數(shù)表

數(shù)據(jù)集分左、右相機采集，本文將兩個數(shù)據(jù)集分左、右相機歸類分別進行閉環(huán)實驗。數(shù)據(jù)集提供的真實的標注矩陣也采用同樣的方法分類，共分4組進行閉環(huán)實驗。

5.1 閉環(huán)誤匹配

為驗證本文算法在多歧義場景下，可以減少閉環(huán)誤匹配，選取New College和City Center 數(shù)據(jù)集中奇數(shù)圖像序列進行閉環(huán)檢測，對比算法采用BoW方法，閉環(huán)檢測結(jié)果的可視化矩陣如圖5和圖6所示。其中,藍色的為真實閉環(huán)結(jié)果，紅色的為誤正閉環(huán)結(jié)果(圖中TTP表示閉環(huán)檢測為閉環(huán)的數(shù)量，F(xiàn)FP表示非閉環(huán)檢測為閉環(huán)的數(shù)量)。從圖中可以看出，本文算法可以剔除絕大多數(shù)的誤正閉環(huán)，且擁有較高的匹配率，即圖中藍色線條更多，更接近真實閉環(huán)結(jié)果，紅色線條更少。這是由于本文算法融合兩種特征進行場景描述對相似的場景具有較強的魯棒性，且本文通過多步篩選確定閉環(huán)候選幀，解決了大部分的感知偏差問題。

圖5 New College閉環(huán)檢測對比

圖6 City Center閉環(huán)檢測對比

5.2 閉環(huán)檢測性能分析

為進一步驗證算法的有效性，采用準確率-召回率曲線(Precision-Recall，PR)進行評估。準確率P表示所檢測到的閉環(huán)結(jié)果中的正確閉環(huán)匹配比例，召回率R表示所檢測到的正確閉環(huán)占路徑真實閉環(huán)個數(shù)的比例。P和R的算式如下

(24)

(25)

式中：TTP表示是閉環(huán)檢測為閉環(huán)的數(shù)量；FFP表示不是閉環(huán)檢測為閉環(huán)的數(shù)量；FFN為閉環(huán)檢測為不是閉環(huán)的數(shù)量。本文對2個數(shù)據(jù)集奇數(shù)和偶數(shù)圖像序列分4組進行閉環(huán)檢測實驗，與FAB-MAP2.0，BoW算法對比結(jié)果如圖7所示。從圖7中可以看出，在召回率接近零時，3組算法的準確率都為1；在保證100%準確率的情況下，在4個閉環(huán)實驗中，本文算法的最大召回率均高于FAB-MAP2.0以及BoW算法；隨著召回率的提高，準確率逐漸下降，本文算法整體趨勢下降較慢，閉環(huán)檢測效果優(yōu)于BoW和FAB-MAP2.0的方法；其中，BoW的方法沒有考慮單詞之間的排序，在多歧義場景易產(chǎn)生感知偏差問題，使得整體趨勢下降最快；本文算法由于采用全局特征和局部特征相結(jié)合進行場景描述構(gòu)建閉環(huán)概率模型，因此對場景變化較為魯棒，且考慮場景的時間一致性和空間一致性并采用極線約束檢驗對閉環(huán)候選幀進行篩選，因此可以解決大部分的感知偏差問題，減少閉環(huán)誤匹配。圖8和圖9分別為本文閉環(huán)檢測算法在New College和City Center數(shù)據(jù)集中的閉環(huán)檢測結(jié)果展示，可知本文算法可以準確地查詢出閉環(huán)情況。

圖7 準確率-召回率曲線

圖8 New College閉環(huán)檢測圖像

6 結(jié)束語

閉環(huán)檢測是視覺SLAM中的一個關(guān)鍵問題，針對多歧義場景,為提高閉環(huán)檢測的準確率，提出了一種多特征空間描述的閉環(huán)檢測方法，以提高閉環(huán)檢測的魯棒性和閉環(huán)準確性;基于閉環(huán)概率模型的框架下，融合局部SURF特征和全局ORB特征，提出基于極線約束的閉環(huán)候選幀篩選方法，進一步提高閉環(huán)準確率。