基于點線特征的快速視覺慣性SLAM算法

周書杰，吳功平，何文山

(武漢大學動力與機械學院，湖北武漢 430072)

0 前言

同時定位與建圖(SLAM)可以解決移動機器人在GPS信號較弱的環境中的定位問題，同時通過其自身的傳感器獲得周圍環境的三維結構及自身的姿態，進而完成相關任務。視覺SLAM相比于激光SLAM，其相機成本低廉、功耗低、體積小，可以獲取豐富的環境信息，受到了工業界與學術界的廣泛關注。

通常，V-SLAM/VIO算法利用圖像特征點來跟蹤定位。例如，QIN等[1]提出的VINS-Mono以FAST角點[2]為特征點，用KLT光流法[3]追蹤完成特征匹配。MUR-ARTAL等[4]提出的ORB-SLAM，基于改進的ORB算法[5]來提取特征點，使得特征點在圖像中分布均勻。CAMPOS等[6]提出的ORB_SLAM3，在ORB-SLAM中融入慣性測量單元(Inertial Measurement Unit，IMU)，顯著提升了算法的魯棒性和精度，在各個數據集上表現優異。然而基于特征點的SLAM系統在弱紋理場景中，提取到的點特征較少，往往無法滿足跟蹤需求，導致算法性能下降。

為解決以上問題，研究人員將圖像中的線特征引入到V-SLAM/VIO中，相比于點特征，線特征可以提供更多的結構信息，尤其是在人造場景中，如辦公樓和城市建筑。因此，線特征可以彌補點特征的不足，提高基于特征點SLAM系統的魯棒性和穩定性。PUMAROLA等[7]在ORB-SLAM的基礎上融入線特征，構建一個單目點線SLAM系統，用兩個端點來表示空間直線。HE等[8]提出的PL-VIO，在VINS-Mono框架中加入線特征，用普呂克坐標和正交表示法[9]來表示空間直線，減少待優化變量個數，提高線特征優化精度。ZUO等[10]提出的雙目點線SLAM系統PL-SLAM，將線段誤差函數定義為線段投影端點到線段距離。

王丹[11]改進了SVO中提取特征點的方式并加入線特征，使輪式機器人在室內環境獲得了更精確的定位。魏琛琛[12]利用點線特征的共線性，增加了SLAM系統中的測量約束，使得移動機器人在低紋理的走廊環境中定位精度遠高于其他算法。曾繼超[13]用梯度過濾機制改進傳統LSD算法，提升了線段提取的穩定性。

以上研究在線特征檢測時均采用LSD算法[14]，線段匹配用LBD描述子[15]。然而，LSD線段檢測和提取LBD描述子速度較慢，對算力弱的計算平臺不友好，限制了點線特征SLAM的跟蹤速度。且LSD算法容易受圖像質量影響，檢測到的線段被分割為多個短線，易產生線段誤匹配，降低SLAM系統定位精度。許多基于線特征的SLAM算法都對LSD進行改進，提升了系統精度，但同時也增加了SLAM前端耗時[10，13，16]。

針對以上點線特征SLAM系統存在的問題，本文作者提出一種融合點線特征的快速單目視覺慣性SLAM算法，使用ELSED(Enhanced Line Segment Drawing)線段檢測算法[17]，快速提取場景中的長線段，剔除較短的線段，減少短線誤匹配對系統的干擾。在追蹤普通幀時，基于連續幀間微小運動假設，并利用幾何約束實現線段快速匹配，顯著提升了線特征匹配速度與匹配準確率。并在地面機器人OpenLORIS數據集上對此算法進行測試，結果表明：此算法在定位精度和實時性上都表現良好。

1 系統架構

文中SLAM系統包括跟蹤、局部建圖和回環檢測3個線程，系統框架如圖1所示，該系統在ORB-SLAM3系統的基礎上加入線特征，其中涉及點特征的操作與ORB-SLAM3基本一致。

圖1 系統框架設計

主要針對傳統線特征提取和匹配計算耗時久的問題，引入新的線段提取算法ELSED，提高線段提取的質量，并利用連續幀之間的微小運動假設，提出一種相鄰圖像之間的無線段描述子的線特征匹配算法，避免了不必要的線特征描述子的計算。在關鍵幀上提取線段描述子，完成關鍵幀之間線段匹配，創造新的地圖線。

2 線特征提取與匹配

2.1 線特征提取

點線特征SLAM算法領域普遍使用LSD算法提取線特征，但LSD算法消耗大量計算資源，實時性差，并且其對圖像遮擋、局部模糊等情況魯棒性差，容易將一條長線檢測為多條短線，加大匹配難度。故本文作者使用ELSED算法，其線段檢測大致為以下6步：

(1)高斯濾波去除噪聲；

(2)利用Sobel算子計算像素梯度與方向；

(3)選取局部梯度最大的像素作為錨點像素；

(4)依托錨點同時進行邊緣檢測與直線擬合；

(5)長線段斷口檢測合并；

(6)剔除錯誤檢測線段。

ELSED有普通檢測和跳躍檢測2種模式，二者區別在于跳躍檢測模式執行其算法的第5步，而普通檢測跳過該步。由于圖像模糊，圖像像素梯度方向變化或者梯度值轉為零，進而出現線段邊緣不連續，導致一條長線段被檢測為多條短線。為盡可能檢測出完整長線段，算法一旦檢測到不連續邊緣，會跳過不連續邊緣并沿線段方向繼續檢測線段，在一定像素區間內，若仍可檢測出線段，則擬合該方向所有線段形成長線段。

ELSED中大多數參數已根據經驗設置，無需進行更改。其檢測的最短線段長度設置為15，較短的線段容易導致誤匹配，連續跟蹤效果差，過多的短線往往會降低點線融合的SLAM系統精度，故對線段提取的最短長度Lmin做一些調整：

Lmin=[α·min(W，H)]

(1)

其中：W和H分別為圖像寬度和高度，取其中較小值為閾值基準，以適應不同尺寸圖像；α是尺度因子，根據經驗設置為0.083；[·]定義為向下取整函數，如848像素×480像素的圖片，min(W，H)=480像素，Lmin=40像素。

2.2 連續幀之間的快速線段匹配

現在大多數基于線特征的視覺SLAM系統采用LBD線段描述子進行線段匹配跟蹤，這種方式在大多數場景下表現良好，但計算量相對較大。通常，相機在相鄰幀之間運動量較小，線特征匹配可以不需要描述子完成，本文作者提出一種連續幀間快速線段匹配算法，算法示意如圖2所示。

圖2 線段匹配算法示意

對于連續兩幀圖像I1和I2，將I1中提取的n條線特征存入集合L1={l1，l2，…，li，…，ln}，li表示圖像I1中第i條線段；將I2中提取的m條線特征存入集合L2={l1，l2，…，lj，…，lm}，lj表示圖像I2中第j條線段。R和t分別代表兩圖像之間的相對旋轉與位移。

λ2x2=λ1Rx1+t

(2)

從而有式(3)：

λ2K-1p2=λ1RK-1p1+t

(3)

一般情況下，相機若不是極其劇烈的抖動，其連續兩幀的相對位移t基本趨于0，匹配像素對應的深度λ1≈λ2，因此，可將公式(3)簡化為

p2=KRK-1p1

(4)

其中，文中算法中旋轉矩陣R是由ORB-SLAM3跟蹤線程中恒速運動模型得到的當前幀初始位姿，在IMU未初始化時，通過上一幀的位姿以及上一幀的速度獲得當前幀的位姿初始值，在IMU完成初始化后，當前幀的位姿初始值為IMU預積分得到。

本文作者提出的快速線段匹配算法基于以上假設完成，具體步驟如表1所示。

表1 快速線段匹配算法

其中，相比于傳統的KLT算法，式(4)給予KLT迭代優化一個更可靠的初值，從而提高關鍵點跟蹤精度。線段間的匹配距離γij為

(5)

(6)

為防止誤匹配，設置匹配因子β：

隨著教學改革的步伐不斷邁進，老師在課堂中不但要傳授學生知識，還需要把學生的探索精神與創造能力培養起來，但是由于受到根深蒂固的傳統教育觀念所影響，很多的老師在教學過程中并沒有把提升學生的綜合素質重視起來。在教學的過程中老師必須改變這種傳統的教學觀念，充分應用新課改背景下的現代化教學理念，尊重學生學習過程中的主觀能動性，引導每個學生都能夠主動深入思考，并能夠展現自己的個性。

(7)

其中：β為最小匹配距離γmin與次小匹配距離γmin2的比值。匹配因子β越小，匹配質量越好，若β大于某一閾值，則認為該匹配是誤匹配。

3 直線重投影誤差及后端優化

3.1 直線重投影誤差

文中線特征的重投影誤差為3D-2D的方式，為非線性誤差，如圖3所示。其中，誤差表示為空間直線投影到圖像平面的直線與匹配線段兩端點s=[us，vs，1]和e=[ue，ve，1]的距離誤差ds和de：

(8)

圖3 線特征重投影誤差

其中：rl為線段重投影誤差；l=[l1，l2，l3]為投影直線方程。

3.2 后端優化

(9)

由IMU預積分可以獲得i、i+1時刻之間的相對旋轉ΔRi，i+1、相對速度Δvi，i+1、相對位置Δpi，i+1和信息矩陣∑Ii，i+1，故慣性殘差可定義為

rIi，i+1=[ΔRi，i+1，Δvi，i+1，Δpi，i+1]

(10)

(11)

其中：Π(?)為相機重投影函數；uik表示地圖點在像素平面的觀測值；TCB∈SE(3)是從body坐標系(慣性坐標系)的轉移矩陣，一般通過傳感器標定獲得；運算符⊕表示李群SE(3)在R3上轉化操作。

ORB-SLAM3在局部或者全局優化中，將視覺和慣性的聯合優化視作基于關鍵幀的最小化問題，本文作者在其中加入線特征，故視覺慣性聯合優化問題可以寫為

(12)

圖4 后端優化因子圖

4 實驗及結果分析

采用地面機器人OpenLORIS室內數據集對提出的線段檢測與匹配算法效果和SLAM系統運行軌跡精度進行評估。

地面機器人的移動軌跡真值由其頭頂攜帶的激光雷達與動態捕捉系統提供，前置Intel的RealSense D435i攝像頭拍攝單目及深度圖像，并提供IMU信息，如圖5所示。實驗使用CPU為Intel Core i5-11400H@2.7 GHz，內存為16 GB，系統版本為Ubuntu 18.04LTS64位。

圖5 地面機器人

4.1 線特征提取與匹配實驗

為驗證ELSED算法的性能，在OpenLORIS數據集走廊場景和咖啡館場景上分別與LSD算法進行對比測試，其中走廊場景(corridor)包括5個子序列(corridor1-5)，咖啡館場景(cafe)包括2個子序列(cafe1-2)。統計線特征提取算法ELSED和LSD平均每幀耗時、線特征提取數量及線特征平均長度，其中ELSED采用跳躍檢測模式，兩者均剔除了長度小于Lmin的線段，結果如表2所示。

表2 線特征提取算法性能對比

由表2可以看出：ELSED和LSD兩種算法提取的線段數量基本一樣，在corridor序列上提取的線段數量均在140條左右，在cafe系列上提取的線段數量在130條左右；而ELSED算法耗時為2.4 ms，僅為LSD算法的13%左右，大大減少SLAM前端提取線特征時間；同時ELSED提取的平均線段長度為80像素，是LSD提取線段長度的1.3倍。

線段提取效果如圖6所示，其中圖6(a)原始圖像來自corridor4系列，文中算法提取效果如圖6(b)所示，LSD算法提取線段效果如圖6(c)所示。可以較為明顯看出，LSD提取某些長線段時，易受圖片質量的干擾，使得本來的長線段被分割為多條短線，而ELSED提取出高質量的長線段，提取時間短，更有利于系統定位精度提升，使系統更具魯棒性。

圖6 線段提取效果

為定量評估文中提出的無描述符的線段匹配算法，使用ELSED檢測線段，用正確匹配對、正確匹配率和匹配時間作為評價指標。其中正確匹配率定義為正確匹配對占總匹配對的比值，將線段重投影誤差小于5像素的定義為正確匹配[18]，其余的定義為誤匹配，分別在OpenLORIS數據集的走廊場景和咖啡館場景做測試，結果如表3所示。

表3 文中線段匹配算法和LBD線段匹配方法對比

由表3可以看出：相比于文中算法，使用LBD獲得的正確匹配對更多，但正確匹配率相對較低，僅為80%左右，而文中算法的線段正確匹配率均在96%以上。這是因為是文中的匹配算法含有較為嚴格的幾何約束，對于匹配線段外形差別較大的容忍度低，減少線段無效匹配，提高SLAM系統的精度。其匹配效果如圖7所示。

圖7 線段匹配算法對比

同時文中算法的復雜度為O(n2)，與檢測線段數量密切相關，在相關數據集上完成130條線段的匹配耗時大致在2.6 ms左右，相比于LBD描述子，時間效率提高了83%，大大縮短了連續幀之間的線段匹配時間，提高SLAM系統的跟蹤速度。

4.2 定位精度實驗

根據絕對軌跡誤差(Absolute Trajectory Error，ATE)，即計算算法得到的相機位姿與真值之間的均方根誤差(Root Mean Square Error，RMSE)來評估算法的定位精度。將ORB-SLAM3和文中算法在OpenLORIS數據集上進行對比，結果如表4所示。可以看出：相比于ORB-SLAM3，除了在cafe2序列上，文中算法定位精度略微下降，但在場景中紋理較少的走廊環境下，文中算法表現亮眼，尤其是在corridor3序列上，文中算法精度提高87%，在走廊場景下其他序列上，精度均有提高。

表4 多種算法的絕對軌跡誤差對比 單位：m Tab.4 Comparison of APE of various algorithms Unit：m

圖8為在corridor3和cafe1序列上，各算法軌跡在xy平面上與軌跡真值的對比，也可以發現文中算法軌跡與真值最為貼近，精度最高。圖9(a)為corridor3序列上，各算法實時絕對位姿誤差(Absolute Pose Error，APE)，可以看出文中算法APE波動較小，誤差值在較低水平，算法效果穩定。圖9(b)為誤差箱形圖，從中位數和平均數來看，文中算法優于ORB-SLAM3。圖10分別表示了ORB-SLAM3算法和文中點線特征融合算法的在corridor3場景下地圖構建效果。

圖8 算法軌跡與軌跡真值對比

圖9 實時軌跡誤差(a)及誤差箱形圖(b)

圖10 地圖構建

4.3 系統耗時

ORB-SLAM3和文中算法在OpenLORIS數據集下各模塊的平均耗時如表5所示。文中算法在ORB-SLAM3的框架中加入線特征，相比于ORB-SLAM3在特征提取階段，用改進的ORB算法和ELSED算法并行提取點特征和線特征，故文中算法在特征提取時并未增加系統耗時。在連續幀跟蹤時，利用本文作者提出的無描述符的線段匹配算法跟蹤線特征，耗時增加2.8 ms。在創作關鍵幀時，需要提取LBD線段描述符，為局部建圖線程三角化新地圖線提供關鍵幀之間的線段匹配，使得耗時增加。文中算法的跟蹤線程平均耗時29.82 ms，幀率為33 幀/s，滿足系統的實時性要求。

表5 PC平臺算法耗時對比 單位：ms

5 結束語

提出一種在ORB-SLAM3基礎上加入線特征的點線特征融合的視覺慣性SLAM算法。為解決傳統線特征提取算法耗時久和提取線段質量低的問題，引入ELSED線段提取算法，并進行參數調整，在短時間內可提取出高質量的長線段。傳統線段匹配方式采用LBD線段描述子，對大多數場景魯棒，但同樣存在耗時過長的問題。本文作者提出的連續幀間的快速線段匹配算法，減少線段匹配時間和線段的誤匹配。在公共的數據集上實驗結果表明：與其他開源算法相比，文中算法具有更高的定位精度，且保證了系統的實時性。