基于采樣點的直線匹配視覺慣性SLAM算法

林 林，黃宜慶*，李 宏

(1.安徽工程大學 高端裝備感知與智能控制教育部重點實驗室，安徽 蕪湖 2410002.安徽工程大學 安徽省電氣傳動與控制重點實驗室，安徽 蕪湖 241000)

在自動駕駛和智能機器人領域中，同時定位與建圖得到了廣泛的應用。視覺慣性系統亦稱VIO(Visual-inertial Odometry)系統，是融合相機和IMU(Inertial Measurement Unit)數據實現SLAM(Simultaneous Localization and Mapping)的算法，也是目前SLAM研究領域的研究熱點。相機在快速運動時會出現圖片模糊的問題，而IMU正好可以解決該類問題，且其價格便宜，IMU的缺點是它經過積分所得的加速度和角速度存在漂移問題，而相機沒有這個缺點，所以兩者可以很好地互補。

Tong等[1]提出并開源了VINS-Mono，這是一個具有魯棒性的初始化過程，采用了一種基于緊耦合、非線性優化的方法，其效果在當時的VIO系統中是非常優秀的。但是基于點特征的VIO系統，其視覺包含的信息總是有限的，在光照強度較弱和快速運動的場景中，點特征的提取就變得尤為困難，使得SLAM系統難以保持較強的魯棒性和較高的精度。線特征廣泛存在于人造環境中，它可以提供額外的視覺約束，并為自動導航建立一個信息更豐富的地圖，因此將線特征信息加入到視覺前端很有意義。文獻[2]在VINS-Mono的框架基礎上將線特征集成到VIO系統中。文獻[3]使用梯度密度濾波器剔除圖像中線特征的密集區域，使得線特征的誤匹配大大降低，但加入濾波器后,密集區域的內部將不進行線段檢測,而僅在其周圍輪廓區域提取部分線段，這會導致很多的線段沒有被檢測到，從而失去很多線段信息。文獻[4]則提出引入線段分組的方法，將可能是同一條線上的線段合并，生成匹配和提取效率高的長線段。但生成的長線段存在一些誤匹配，影響了系統的精度。

文獻[5]在ORB-SLAM2[6]的系統上進行創新，采用幾何約束進行線特征匹配，利用線特征的深度信息構造虛擬線段，提出不一樣的重投影誤差計算方法，但其重投影誤差的計算僅采用圖像的像素觀測，卻忽略了深度觀測。文獻[7]將直線分為結構化直線和非結構化直線，并對這兩類直線建立不同的數學模型，構建殘差，加入到目標函數中進行優化。

基于上述研究，在視覺前端加入線特征，提出一種新的直線匹配方法，將幀到幀和幀到平面的兩種直線匹配方法結合，并在線特征提取階段，設置動態的長度閾值過濾掉短線特征，用以提高系統在弱紋理和快速運動的場景中的魯棒性和精度；在視覺后端，將構建好的線特征重投影誤差模型加入到基于滑動窗口內的目標函數進行求解，最后在數據集上驗證所提算法。

1 算法框架

本文算法以VINS-Mono為系統框架，采用優化方法將點線特征的視覺觀測數據與IMU觀測數據緊密耦合。算法的系統框架如圖1所示。由圖1可知，系統框架主要包含3個線程：數據預處理、回環檢測與全局位姿圖優化、滑動窗口優化。其中，算法改進的地方已經用底紋標出。

數據預處理：分別對IMU和相機得到的點線特征進行提取、匹配并對齊。在點特征方面，采用Harris角點[8]，LK(Lucas-Kanade)光流以及雙向跟蹤[9]相結合的方法提高跟蹤的精度。在正向光流跟蹤中，如果上一幀有特征點，就直接進行LK跟蹤并進行預測，然后統計和上一幀成功匹配的數目；如果小于10，就擴大搜索，輸入的最大金字塔層數為3；如果沒有進行預測的話，就直接輸入最大金字塔層數為3。接下來進行反向跟蹤，如果正向反向都能找到特征點，則認為這些特征點是符合要求的。

在線特征方面，為更好地解決位姿估計問題，在LSD(Line Segment Detector)算法[10]上進行改進，設置提取線特征的長度閾值[11]來去除掉短線特征，提高提取效率。并在直線匹配中加入基于點特征跟蹤的平面到平面(2D-2D)和平面到立體(2D-3D)的直線匹配算法，從而提高匹配模塊的速度和精度。

滑動窗口優化，回環檢測和全局位姿圖優化與VINS-Mono的方法大致相同，不同之處將在下文介紹，這里不再贅述。

2 線特征提取及匹配

2.1 線特征提取

對于線特征的處理方法，現階段基本上都是采用LSD算法。LSD算法是將梯度方向相近的像素點合并，然而由于噪聲等一些因素，一條長的直線可能被分割成很多條短的線段，這些短的線段所包含的圖像信息不足，用于后面的匹配則效率過低，為此設置長度抑制策略。

長度抑制策略：使用最小長度閾值Lmin來去除短線特征：

Lmin=[γ×min(WL,HL)],

(1)

(2)

式中,min(WL,HL)為輸入圖像的寬度與高度二者之間較小的那個值;γ為長度比例因子;i為提取到線特征的數量。在文獻[11]中，作者是根據經驗將γ設為一個固定的值，但這樣設置的γ不能使提取數量始終維持在一個合理的值，因此根據提取線特征的數量來動態地改變其值。當線段數量較少時，增大長度閾值,反之減小。為減少計算資源，該算法依然會設置最小長度的下限。通過該方法，線特征提取精度得到提升。

2.2 2D-2D的直線匹配

針對直線匹配的精度和穩定性，采用2D-2D和2D-3D的直線匹配方法。一般情況下，使用2D-2D的跟蹤方法來匹配新的檢測到的線，如果匹配到的線數量太少，將啟用2D-3D的方法來提升匹配到的線的數量。

此方法是對前一幀中的所有線進行采樣，得到采樣點的集合P1{Ω1}，使用極線搜索和塊匹配[12]的方法在新圖像中檢測到對應的候選匹配點。使用零均值差平方和(Zero-mean Sum of Squared Difference，ZMSSD)[13]計算兩點的匹配分數，選取匹配分數最高的點作為跟蹤點，最后可以獲取跟蹤點的集合P2{Ω2}。如果被跟蹤的點到當前幀的直線距離小于我們設定的距離閾值，則認為該被跟蹤的點是有效的。如果這些有效的點的數量大于前一幀線上采樣點的0.8倍，則這條線被認為是匹配最佳的線。

(3)

它可以被近似為

(4)

式中,N是修補的寬度乘以修補的高度。

(5)

計算兩張圖像之間的匹配點的ZMSSD值，從而判斷匹配點的相似程度。

2.3 2D-3D的直線匹配

在2D-3D的直線匹配中，采用零均值歸一化互相關(Zero-normailized Cross-correlation,ZNCC)[14]的匹配方法，相比于歸一化互相關(Normailized Cross-correlation，NCC)[15]，它在公式中去除了窗口內的均值，對于光照的變化能表現出更強的魯棒性。兩張圖像的亮度可能由于照明和曝光條件而發生變化，第一張圖像可以首先被歸一化。這通常是通過減去平均值和除以標準差的方式在每一步完成。也就是說，兩張圖像之間的相似程度可以為

(6)

式中,n是f1和f2的像素數；μ是f的平均值；σ是f的標準差。

當相機快速運動和有場景遮擋時，需要匹配的線段長度會相差很大，從而影響匹配的精度。所以采用極線幾何約束來限制線的采樣范圍，配合ZNCC進行匹配。即計算了基本矩陣和當前幀之間的本質矩陣之后，在基本矩陣中線的端點根據極線幾何投影到當前幀，并且通過兩條極線與當前幀的匹配線來確定對應點的采樣范圍，這樣成功地提高了ZNCC匹配方法。

3 線重投影誤差模型及其優化

3.1 線重投影誤差模型

線重投影殘差以點-線距離為模型。首先，定義線幾何變換，給定一個從世界坐標系平面πw到相機坐標系平面πc的變換矩陣Tcw=[Rcw,tcw]，其中Rcw∈SO(3)和tcw∈R3定義為旋轉和平移。利用這個矩陣，將Lw從πw變換到πc為

(7)

然后將Lc進行投影得到投影直線：

I=[l1,l2,l3]T=KLnc,

(8)

式中，KL表示直線投影矩陣。

最后假設Lw為第i個相機幀觀測到的第j條空間線，線重投影誤差可以定義為

(9)

3.2 基于滑動窗口的優化

在滑動窗口中定義與IMU、點和線測量信息相關的所有狀態向量：

(10)

oT=(ΦT,φ),

(11)

式中,Φ為一個三維向量;φ定義為一個標量。

通過最小化所有測量殘差項之和來優化滑動窗口內的所有狀態變量：

(12)

4 實驗驗證與結果分析

本文采用公共數據集EuRoc對此算法進行評估，并將評估的結果與經典SLAM算法VINS-Mono的結果進行比較。本實驗在臺式電腦上展開，電腦的基本配置為：Intel(R)Core(TM)i7-9700 CPU@3.00GHz 8G內存，Ubuntu 16.04。

本文算法在MH_05_difficult序列上的可視化如圖2、3所示。由圖2、3可以看出，在光照較暗的場景中線特征的提取效果依然良好。在V2_03_difficult序列上的兩種算法軌跡與真實軌跡的對比如圖4所示。由圖4可以明顯看出，改進后的算法軌跡更接近真實的軌跡。

為驗證本文改進系統相比于VINS-Mono具有更高的精確度，采用場景復雜程度較高的3個不同序列進行實驗，系統所得結果與實際結果的誤差如圖5～7所示。本文算法在EuRoc數據集運行的絕對位姿誤差的結果如表1所示，絕對位姿誤差與VINS-Mono的對比結果如表2所示。

表2 絕對位姿誤差與VINS-Mono的對比 (單位：m)

根據表1、2可得，在公共數據集EuRoc的7個不同難度、不同場景的序列中，本文算法相比于VINS-Mono都有提升，在MH_01_easy和MH_02_easy兩個場景比較簡單的序列中，提升效果不明顯，究其原因是在簡單場景中，點特征所包含的視覺信息足以使系統達到了一定的精度，即使加入線特征并改進了算法，也不能將精度提高很多，相反線特征的加入會增加計算量，降低系統的實時性。但在復雜的場景中，該算法的優越性就得到了體現，其中在快速運動的場景中，精度提高得更明顯。這得益于在線特征提取階段使用長度抑制策略，匹配階段使用了兩種直線匹配的方法。

表1 本文算法在EuRoc數據集運行的絕對位姿誤差的結果(單位：m)

5 結束語

該算法在VINS-Mono的基礎上，通過將線特征融合到系統中，解決點特征包含視覺信息不足的問題，并在原有的直線特征提取算法上通過長度抑制過濾掉短線特征，用來改善提取效果。并且將2D-2D與2D-3D的直線匹配方法結合使用，用于提高匹配的精度。而后建立線重投影誤差模型，整合各部分殘差，構建目標優化函數。通過在不同難度場景下的實驗仿真，得到本系統的軌跡和誤差，并與VINS-Mono運行結果進行對比，通過分析可知該算法在快速運動和弱紋理的情況下表現出更強的魯棒性和更高的精度。