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

周書杰，吳功平，何文山

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

0 前言

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

通常，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)，顯著提升了算法的魯棒性和精度，在各個數(shù)據(jù)集上表現(xiàn)優(yōu)異。然而基于特征點的SLAM系統(tǒng)在弱紋理場景中，提取到的點特征較少，往往無法滿足跟蹤需求，導(dǎo)致算法性能下降。

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

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

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

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

1 系統(tǒng)架構(gòu)

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

圖1 系統(tǒng)框架設(shè)計

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

2 線特征提取與匹配

2.1 線特征提取

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

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

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

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

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

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

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

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

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

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

(1)

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

2.2 連續(xù)幀之間的快速線段匹配

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

圖2 線段匹配算法示意

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

λ2x2=λ1Rx1+t

(2)

從而有式(3)：

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

(3)

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

p2=KRK-1p1

(4)

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

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

表1 快速線段匹配算法

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

(5)

(6)

為防止誤匹配，設(shè)置匹配因子β：

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

(7)

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

3 直線重投影誤差及后端優(yōu)化

3.1 直線重投影誤差

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

(8)

圖3 線特征重投影誤差

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

3.2 后端優(yōu)化

(9)

由IMU預(yù)積分可以獲得i、i+1時刻之間的相對旋轉(zhuǎn)Δ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)

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

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

(12)

圖4 后端優(yōu)化因子圖

4 實驗及結(jié)果分析

采用地面機器人OpenLORIS室內(nèi)數(shù)據(jù)集對提出的線段檢測與匹配算法效果和SLAM系統(tǒng)運行軌跡精度進行評估。

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

圖5 地面機器人

4.1 線特征提取與匹配實驗

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

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

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

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

圖6 線段提取效果

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

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

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

圖7 線段匹配算法對比

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

4.2 定位精度實驗

根據(jù)絕對軌跡誤差(Absolute Trajectory Error，ATE)，即計算算法得到的相機位姿與真值之間的均方根誤差(Root Mean Square Error，RMSE)來評估算法的定位精度。將ORB-SLAM3和文中算法在OpenLORIS數(shù)據(jù)集上進行對比，結(jié)果如表4所示?？梢钥闯觯合啾扔贠RB-SLAM3，除了在cafe2序列上，文中算法定位精度略微下降，但在場景中紋理較少的走廊環(huán)境下，文中算法表現(xiàn)亮眼，尤其是在corridor3序列上，文中算法精度提高87%，在走廊場景下其他序列上，精度均有提高。

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

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

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

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

圖10 地圖構(gòu)建

4.3 系統(tǒng)耗時

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

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

5 結(jié)束語

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