改進點線特征的雙目視覺SLAM算法

(上海大學 機械制造及其自動化系,上海 200072)

0 引言

同步定位和地圖構建技術[1](SLAM)在過去二十年中一直是計算機視覺和機器人領域的熱門研究方向，被認為是實現移動機器人自主化的核心技術。視覺SLAM需要將觀測到的圖像信息同環境進行數據關聯，其中點特征經常被用于數據關聯。

基于點特征的典型算法有PTAM[2]，SIFT[3]和SURF[4]等。由于SIFT和SURF算法計算量太大，難以滿足實時計算要求，Mur-Artal等人在PTAM算法的基礎上提出ORB-SLAM算法[5]，通過快速連續地跟蹤ORB特征，以適應實時計算。ORB-SLAM 在紋理豐富的場景中，有令人印象深刻的表現；但在處理低紋理場景時，由于點特征不足或者分布不均而導致算法性能下降。

與點特征相比，線特征承載更多的結構信息。而且人造場景中具有豐富的邊緣和線性形狀的規則結構，線特征尤為明顯。近年來，基于線特征的算法受到越來越多的關注。Albert Pumarola等人提出了單目點線結合的視覺PL-SLAM算法，通過端點來表示線段，并計算端點到線的投影距離來統計誤差[6]。Xiaojia Xie等人改進了空間線的表示方法，首次采用正交表示作為最小化參數來模擬線特征以及視覺SLAM中的點特征，并分析得出了關于線參數的重新投影誤差的雅可比行列，改進了SLAM的解決方案[7]。Ruben Gomez-Ojeda等人又將點線融合的SLAM系統擴展到了雙目視覺[8]。

針對部分低紋理場景中點特征不足以及現有LSD線特征提取算法存在線段過分割[9]問題，本文提出一種改進點線特征的雙目視覺SLAM算法(ISSLAM- Improved Stereo SLAM)。通過梯度密度濾波，改進LSD線特征提取算法；線段合并機制，優化提取出的斷線；采用聯合點線特征的擴展詞袋模型，提高了SLAM算法的精度和魯棒性。

1 改進點線特征的雙目視覺SLAM算法

本文所提的改進點線特征的雙目視覺SLAM算法(ISSLAM- Improved Stereo SLAM) 如圖1所示，主要包括特征跟蹤、局部建圖和閉環檢測3個步驟。首先對輸入的圖像序列進行預處理，在此基礎上進行點線特征提取，并通過線段合并來優化點線特征。利用這些點線特征進行特征匹配及相機位姿估計，并從中篩選出有價值的幀作為關鍵幀插入到局部地圖庫中。最后調用場景識別庫來進行閉環檢測，優化全局地圖。

局部建圖調用地圖庫通過增刪等操作進行更新與優化。地圖庫中的信息主要包括關鍵幀、地圖點和地圖直線。場景識別庫由點線視覺詞典和圖像數據庫組成，可以計算圖像間的相似程度，用于判斷場景是否形成閉環。

圖1 改進點線特征的雙目視覺SLAM算法

2 特征跟蹤

當下流行的特征跟蹤方式通常是基于ORB的特征點進行相機運動軌跡的估計，然而在某些低紋理場景中，經常由于特征點采集不足而導致跟蹤失效。因此我們將線特征加入進來。在線檢測方法中，LSD[9](line segment detector)算法被認為是最好和最流行的方法之一。它是按照梯度方向的一致性直接將鄰近像素編組為線段區域，能在線性時間內得出亞像素級精度的檢測結果。

雖然LSD算法在速度和精度上都取得了不錯的效果，但是也存在自身缺陷。由于它在采集特征時沒有篩選與合并機制，會在圖像中的局部密集區域采集大量相似線特征，且易發生線段過度分割的現象[9]。

有鑒于此，本文首先通過梯度密度濾波器[10]進行圖像預處理，剔除局部密度過高的區域；然后采用LSD算法進行線特征提取，并且將LSD提取出的線段進行合并，以減少線特征檢測的數量，提高特征檢測的精度和效率。

2.1 基于梯度密度濾波器的圖像預處理

在我們所檢測的環境中，某些區域中過高的線段檢測密度(如圖2中所示的網格圖案)通常會導致線匹配的結果不正確且耗費了很多時間。而且在同一密集區域中，大量相似的線條在匹配過程中也很容易出錯。為此，我們設計了一個濾波器，利用局部像素梯度的密度值作為篩選準則，對于特征過于稠密的區域，以輪廓檢測替代完整的線特征檢測，以避免出現大量的無效特征，從而提高特征提取與匹配的效率，降低算法的計算量與誤匹配率。

圖2 局部線特征密集區

圖像梯度反映了圖像中灰度變化的劇烈程度，線特征通常出現在梯度較高的區域。若某區域內梯度較高的像素數量過多時，意味著該區域高梯度的像素過于密集，預判為線特征密集區域，將其剔除以減小后期特征匹配的計算復雜度，提高匹配精度。

將單位區域內梯度較高的像素點數所占的百分比定義為像素梯度密度，以衡量該區域是否為特征密集區域，作為濾波過程中剔除圖像中特征密集區域的依據。

設像素點(i,j)的梯度值為[11]，其強度的定義如式(1)所示，當梯度值大于強度閾值時標記為1，否則為0。

(1)

區域的梯度密度ρij定義為：以像素點(i,j)為中心的k×k區域內，梯度大于閾值Gs的像素點數所占的百分比。

(2)

為了判別梯度密度過高的像素區域，以進行濾波處理，減少圖像中特征稠密區域對于特征匹配的影響。以像素區域梯度密度閾值ρDENSE為衡量標準，將梯度密度ρij大于ρDENSE的像素區域視為特征稠密區域，線段檢測時視為無效區域，不予處理。由此，線段檢測有效區域標識定義為:

(3)

在濾波過程中，梯度密度閾值ρDENSE對濾波效果影響很大。閾值設置過大，無法濾除特征密集區域，濾波效果不顯著；閾值設置過小，則會濾除許多有效的檢測區域。在此設定ρDENSE=0.75。

實際濾波過程分檢測和標記兩個步驟。首先將k值設為5，找到小范圍的高梯度密度區域；然后在此基礎上進行區域擴展，將k值設置為21，最終將的區域標記為無效區域，并對該區域禁用線段檢測。

圖3(a)和3(b)分別為LSD算法檢測的結果和加入梯度密度濾波器后的效果對比，由此可知，加入濾波器后，密集區域內部不再進行線段檢測，而只是在其周圍輪廓區域提取部分線段，避免在后期匹配過程中過多細碎分段帶來的時間和精度上的影響，減小計算代價，提高系統精度。

利用梯度密度濾波器，剔除了局部的梯度較高的密集區域，最終得到圖像中有效的檢測區域，為后續點線特征的提取打下基礎。

2.2 基于LSD線段合并的線特征提取

在線特征提取階段，利用LSD算法提取線特征[9]，在此基礎上利用各線段的角度特性、空間位置特性和長度特性，進行斷線的分組和合并，解決LSD算法過分割的弊端，減少線特征的數量。

LSD線段合并算法由線段分組和線段合并來完成。在線段分組階段，利用角度貼近度和空間接近度來判斷同一直線上的斷線。考慮計算的復雜程度，依次利用角度貼近度、水平坐標接近度和垂直坐標接近度粗略篩選出可合并的同一直線上的候選斷線，分為一組。在線段合并階段，引入線段長度因素，以線段角度、歸一化后的長度和空間距離作為可合并性標準，精確判斷并完成斷線合并。

2.2.1 線段分組

首先，按線段長度的降序對LSD算法提取出的線段進行排序，得到{L1,L2,L3,…,Ln}。線段分組從最長的線段L1開始進行，因為較長的線段往往來自具有連續強梯度的圖像區域，所以更可靠。

線段Li的兩個端點為(Ai,Bi)，其坐標分別為(xAi,yAi)和(xBi,yBi)。Li的長度為li，與水平方向的夾角為θi。

不失一般性，設當前最長線段為L1，則線段集合L={L2,L3,…,Ln}中滿足角度篩選條件式(4)、水平距離篩選條件式(5)、垂直距離篩選條件式(6)的線段分為一組，構成線段組。

線段分組流程如圖4所示。

圖4 線段分組流程

經過角度篩選，得到候選線段組L1：

L1=[Li∈L:(|θi-θ1|<θs) ]

(4)

其中:θS為衡量線段特征角度相近程度的角度篩選閾值。

經過水平距離篩選，得到候選線段組L2：

L2=

(5)

經過垂直距離篩選,得到線段組PL1：

PL1=

(6)

其中:ds為衡量線段特征水平和垂直距離接近程度的距離篩選閾值。

依據3個篩選條件，將角度和空間上都與L1很接近的線段分為一組，最終得到候選線段集PL1={L2,L3,…，Ln}為后續線段合并打下基礎。值得注意的是3個篩選條件的順序很重要，因為角度篩選的計算最簡便，所以應該最先使用。此外，以水平距離篩選和垂直距離篩選取代歐幾里德距離篩選，由于僅涉及絕對值的加減運算，極大地提高篩選效率，減小計算代價。

2.2.2 線段合并

經過線段分組得到的候選線段集PL1，還需要經過端點距離、角度修正值和夾角偏差的進一步精確篩選，以完成線段合并，流程如圖5所示。

圖5 線段合并流程圖

如圖6所示，兩條線段Li(端點Ai,Bi)和Lj(端點Aj,Bj)的間距，用其最近端點的距離dij表示：

dij=min(|Ai-Aj|,|Ai-Bj|,

|Bi-Aj|,|Bi-Bj|)

(7)

圖6 兩線段最近端點距離表示

利用最近端點距離，篩選出兩條線段空間接近程度小于閾值ds的候選線段組L3：

L3=[?Li∈PL1:(dij

(8)

考慮到線段長度和間距的影響因素，需對候選線段組L3中的角度閾值做出修正。當前較長線段為L1，與其相匹配的線段li∈{L2,L3,…，Ls}的閾值修正系數為λi為:

(9)

綜合考慮線段長度和兩條線段間距對角度篩選的影響，線段Li的自適應角度閾值為：

(10)

(11)

其中:θM是預合并后的線段M的傾角。

線段分組及合并這兩個步驟，重復迭代地進行，直到無法再合并為止。

圖7(a)為LSD算法檢測出的線段，白色框中表明線段存在斷線的情況；圖7(b)為改進點線特征的雙目視覺SLAM算法(ISSLAM)的檢測結果。可以看出，ISSLAM算法將LSD檢測出的斷線很好的合并在一起(如白線框中區域所示)，提升了系統的精度。

圖7 線段合并效果對比

根據濾波處理、線段檢測和合并后得到的線段特征，計算通用的LBD[12]描述子，用于線段間的特征匹配和運動估計。

2.3 運動估計

利用圖像中提取出的特征點、線的描述子進行特征匹配，可以得到3D特征與2D特征的匹配關系。將當前幀觀測到的3D特征點、線投影到下一幀的2D圖像平面上，然后根據相鄰幀間投影點線與匹配的點線之間的誤差關系，可以進行相機姿態的初步估計[13]。具體方法不再贅述。

2.4 關鍵幀選擇

相機運動過程中會采集數據，連續運動一段時間后這些數據便組成了場景的地圖。但由于內存資源有限，我們不可能將所有幀都插入地圖中，而是會從中選擇出一些有代表性的關鍵幀。

本文沿用ORB-SLAM中關鍵幀選擇的策略[5]，對于當前幀，首先找到與之具有最多共同觀測的關鍵幀作為參考關鍵幀，滿足以下所有條件即可視為關鍵幀：

1)距離上一次關鍵幀的插入已經過了20幀或者局部地圖構建線程空閑

2)當前幀至少跟蹤到了50個特征點和15條空間直線

3)當前幀包含參考關鍵幀中的特征要少于75%

條件2保證了位姿估計的質量，條件3保證了兩幀圖像之間具有一定的視覺變化。

3 局部建圖

當篩選出的新關鍵幀插入地圖后，要建立當前關鍵幀與其他關鍵幀之間的連接關系。在跟蹤線程中已經獲得了相機位姿和環境地圖。在局部建圖線程中，要對位姿和路標進一步優化。局部建圖線程主要包括兩部分：局部地圖管理及局部地圖優化。

局部地圖管理的主要任務包括：(1)把當前處理的關鍵幀中提取的 2D 點、線特征3D 參數化到地圖數據庫中；(2)對地圖中重復的點、線特征進行融合；(3)對地圖中跟蹤質量不佳的點、線特征進行剔除；(4)對地圖中的冗余關鍵幀進行剔除。

局部地圖優化就是指從環境中抽取一部分位姿和路標進行優化。因為每一幀都是通過幀之間的相對運動得到的，所以不可避免的會有誤差積累，故需要優化，減少誤差。

其具體計算方式如下，首先定義變量，用ψ表示總的投影誤差，由點的投影誤差eij和線的投影誤差eik組成:

(12)

投影誤差eij為3D點Xωj投影到第i幀圖像上的2D點π(ξiω,Xωj)與實際像素位置xij之差：

eij=xij-π(ξiω,Xωj)

(13)

其中:π為投影函數，ξiω為關鍵幀i的位姿。

線的投影誤差eik由線段k的端點的3D位置(Pωk,Qωk)在第i幀圖像上的投影與實際線段所在直線lik計算而得：

(14)

采用Levenberg-Marquardt方法[14]優化投影誤差ψ，誤差的增量為:

Δψ=[H+λdiag(H) ]-1JTWe

(15)

其中的J、H代表了雅可比矩陣和海瑟矩陣，e是包含了點和線的總誤差，λ是變量因子，λ較小時，接近于高斯牛頓法，當λ較大時，接近于最速下降法。

ψ(k+1)=ψ(k)·Δψ

(16)

通過迭代計算式(16)直到收斂，求得最小化投影誤差ψ。

有別于運動估計階段僅考慮相鄰幀間的投影誤差，在局部地圖優化階段，需要綜合考慮局部地圖內一系列關鍵幀間的投影誤差和，通過求解多幀間誤差和的最小值，以調整局部地圖中的相機位姿和路標點位置，從而得到更精確的結果。

4 閉環檢測

視覺SLAM系統在缺少外部控制信息時，系統的累積誤差會不斷增大，長期積累后會造成定位失效。在實際應用中，相機會運動至先前經過的地方從而形成閉環。閉環的引入可有效的修正累積誤差，對長時間遠距離運動的視覺SLAM有重要意義。閉環檢測實質是判斷當前圖像與歷史較早圖像之間是否存在關聯，若兩圖像相似度達到一定程度，則認為形成了閉環。基于BoW的詞袋模型[15]方法是應用最廣泛的閉環檢測方法之一，它從圖像中提取特征點，矢量化成視覺單詞，然后建立詞匯樹用于圖像間相似度的判別。

本文對原有的基于特征點的詞袋模型方法進行擴展，將線特征也考慮進來，提出了一種新的詞匯樹構造方式，提升了回環檢測的精度和魯棒性。

4.1 擴展詞匯樹的構造方法

傳統的詞匯樹構建方法是基于點特征進行創建的，需使用大量的點特征描述子進行訓練，以構建視覺詞典，然后不斷用k均值聚類算法[16]建立詞匯樹。

圖8 融合點線特征的詞袋模型

盡管已經證明這種方法具有可擴展性和高效性，但它并不適用于同時具有點線特征類型的詞匯樹。因此，本文提出圖8所示的改進的詞匯樹。首先將訓練的ORB點特征描述子和LBD線特征描述子放在同一個視覺詞典中，然后將訓練的描述符劃分為點、線2個組，再分別對點、線特征描述符進行分層k均值聚類過程，最終構造擴展的樹結構。

為區分詞匯樹中不同的特征類型，在每個特征描述符的末尾附加一個額外字節(點描述子添加標志位0，線描述子添加標志位1)，以區分其所屬的特征類型。在此示例中，實心填充的是點特征詞匯，線條填充的是線特征詞匯，虛線包圍的葉子節點為視覺詞典中的視覺詞匯。

4.2 融合點線的詞包向量

改進后的詞包VPLi：

VPLi={α1(VP1,VP2,…,VPm),α2(VL1,VL2,…,VLm)}

(17)

其中，既包含了點特征詞包VPi=(VP1,VP2,…，VPm)，又包含了線特征詞包VLi=(VL1,VL2,…,VLm)。α1、α2分別代表點特征和線特征詞包所占的權重。針對不同場景的實際特點，合理設置權值，調整二者比重。例如紋理豐富的場景中點特征權重值大一些，在低紋理的環境中讓線特征的權重大一些。通常，權值默認為0.5。

4.3 詞包向量的相似度計算

由于不同詞匯的重要程度不同，需對每個單詞賦予不同權重值，在本文中采用TF-IDF[17]進行加權判斷。

VPLi和VPLj分別代表第i幀和第j詞包向量，其相似度得分計算如下：

(18)

在此，采用L1范數進行距離計算。

在進行圖像相似度判斷時，可直接比較圖像詞包向量間的相似度得分，降低了計算復雜度提升了檢測效率。同時為了避免將相似度較高的鄰近幀誤判為閉環，設置了閾值k，只有當時j>i+k，才進行閉環檢測。

5 實驗與分析

實驗數據集來源于EuRoc微型飛行器(MAV)采集的兩個室內場景，其中包含來自20FPS的全局快門相機的立體圖像。每個數據集提供由VICON運動捕捉系統給出的地面實況軌跡。所有外在和內在參數也由數據平臺獲取[18]。

測試平臺為華碩K550D@2.10 Ghz ，12 GB內存，64位Linux操作系統。

圖9(a)～(d)展示由MAV采集的室內實景以及提取出的特征點線。

圖9 V1_02_medium數據集實景

圖10是由V1_02_medium圖像序列重建出的地圖。圖10(a)為基于點特征的ORB-SLAM2算法創建的地圖，圖10(b)為聯合點線特征的文本算法創建的地圖。

圖10 由V1_02_medium重建出的地圖

由10(a)和10(b)的對比可見，單純基于點特征所創建出來的地圖缺少結構信息，因此比較雜亂；而將線特征引入進來后，所創建的地圖結構化信息更多，從圖中的空間直線可以辨別出墻面，地面和墻的邊角等信息。更好地還原了場景結構。表明ISSLAM算法在結構化邊緣線特征多的人造場景中適用性更高。

由V1_02_medium圖像序列計算得到的相機運行軌跡分別顯示在圖11(a)和11(b)中。

圖11 ORB-SLAM2算法和ISSLAM算法軌跡圖對比

圖中，軌跡點上的顏色信息反映了誤差大小，通過顏色與誤差數值的對應關系(圖形右側誤差色帶)可以看出，ORB-SLAM2算法(圖11(a))的誤差區間為(0.017～0.106)，而本文所提出算法(圖11(b))的誤差區間僅為(0.003～0.081)。由此可知，本文所提出的算法運行結果的誤差更小，準確率更高。

為了說明ISSLAM算法在不同場景下的有效性與適應性，分別以EuRoc中的多組圖像序列(V1_01_easy、V1_02_medium、V2_03_difficult、MH_02_easy、MH_03_medium、MH_04_difficult)為實驗對象。這些序列分成，簡單、中等、和困難，這取決于MAV(微型飛行器)的速度，照明和場景的紋理。表1為ORB-SLAM2算法、點線聯合的PL-SLAM算法、ISSLAM算法得到的RMSE值[19](均方根誤差)。

表1 基于EuRoc MAV數據集的運動估計誤差

從表1中可以看出，對于結構化線條較為豐富的室內場景，聯合點線特征的SLAM算法魯棒性更好，即使在快速運動場景(V2_03_difficult 測試集)，不會出現ORB算法所面臨的特征跟蹤失敗的情況(表中X所示)。而且所提的ISSLAM算法比傳統的聯合點線特征的PL-SLAM算法測試的誤差更小，精確度更高。

綜上所述，本文提出的ISSLAM算法在精度和魯棒性上都表現出良好的優勢。

6 結語

本文提出一種改進點線特征的雙目視覺SLAM算法(ISSLAM)來進行位姿估計和地圖創建。在特征提取過程中，利用梯度密度濾波器進行圖像預處理，以剔除局部梯度密度過高的區域；然后采用LSD算法進行線特征提取，并且將LSD提取出的線段進行合并，以減少線特征檢測的數量，提高檢測精度。在閉環檢測線程中，對原有基于點特征的詞袋模型進行了擴展，將線特征也引入進來，提高了算法的精度及魯棒性。最后，以EuRoc公共數據集為實驗對象，通過與基于點特征的ORB-SLAM2算法和基于點線特征的PL-SLAM算法比較，表明了ISSLAM算法的有效性。

線特征的引入，雖然提高了算法精度，但增加了特征提取與匹配階段的計算代價，耗時更長，不利于算法的實時性，需要做進一步改進以提高算法效率。