李鐵維，王牧陽，周 炎

(1.武漢大學(xué) 遙感信息工程學(xué)院，湖北 武漢 430079；2.香港理工大學(xué) 建設(shè)及環(huán)境學(xué)院，香港 999077)

0 引 言

視覺里程計是SLAM(即時定位與地圖構(gòu)建)系統(tǒng)的前端部分，其主要任務(wù)是增量式地估計相鄰圖像之間的運(yùn)動關(guān)系[1]。根據(jù)位姿估計時目標(biāo)函數(shù)的不同構(gòu)建方式，可分為直接法和特征點法。直接法假設(shè)影像灰度不變，通過計算最小光度誤差估計相機(jī)姿態(tài)，而特征點法主要通過特征匹配和跟蹤，計算最小重投影誤差來進(jìn)行估計的。直接法所需計算量更小，而特征法不易受到光照變化的影響，更為穩(wěn)定。目前最具有代表性的直接法SLAM是J.Engel等人提出的LSD-SLAM[2]。LSD-SLAM針對像素進(jìn)行，可以構(gòu)建半稠密的場景地圖。早期的特征法單目SLAM是以點特征為基礎(chǔ)，使用擴(kuò)展卡爾曼濾波器進(jìn)行位姿估計，如A.J.Davison提出的第一個單目SLAM系統(tǒng)MonoSLAM[3]。Klein等人提出的PTAM(parallel tracking and mapping)系統(tǒng)[4]第一個使用了非線性優(yōu)化方法，引入了關(guān)鍵幀機(jī)制，并實現(xiàn)了跟蹤和構(gòu)圖過程并行化，奠定了此后基于特征的單目視覺SLAM算法的雛形。PTAM系統(tǒng)的繼承者ORB-SLAM[5]則使用具有旋轉(zhuǎn)和縮放不變性的ORB點特征，提出了跟蹤、局部構(gòu)圖和閉環(huán)三線程并行的方案。ORB-SLAM的缺點是在僅旋轉(zhuǎn)的軌跡中容易跟丟，且生成的特征稀疏地圖無法直觀地表現(xiàn)場景特征。

點特征簡單易表達(dá)，但維度單一，缺少約束，形成的特征稀疏地圖難以真正表達(dá)地理信息，而加入線特征可有效地改善這些缺點。Albert Pumarola等人提出的PL-SLAM[6]中，使用線段的兩端點表達(dá)線特征，但實際在不同幀中，提取的三維線端點常常沿直線方向產(chǎn)生漂移，因此需要將其與在某幀投影中最匹配線段的端點綁定[7]。三維直線也可使用在兩平面上的投影進(jìn)行表達(dá)，如Spetsakis[8]和Rother[9]所使用的。Chandraker[10]提出的雙目系統(tǒng)利用了線段從四個投影平面出發(fā)(包括相鄰兩幀各從兩個視點采集影像)必然交于一線的原理。Joan Solà等人[11]總結(jié)了基于普呂克坐標(biāo)的線特征表達(dá)方式。普呂克坐標(biāo)可由直線上任意兩點齊次坐標(biāo)的轉(zhuǎn)換得到，且不隨所取點的位置而變化，因此避免了由端點漂移帶來的不確定因素，是一種更為穩(wěn)定的直線特征表達(dá)方式。在普呂克坐標(biāo)系的基礎(chǔ)上，Adrien Bartoli[12]提出了只需四個參數(shù)的正交法表達(dá)方法，并在Zhang Guoxuan等人提出的雙目系統(tǒng)slslam[13]中得到了應(yīng)用。

該文實現(xiàn)了一種基于點線結(jié)合特征的視覺里程計算法，在ORB-SAM[5]系統(tǒng)的基礎(chǔ)上加入了線特征，并使用了基于圖優(yōu)化(graph optimization)方法估算和優(yōu)化位姿圖，相較于僅包含點特征的系統(tǒng)，提高了相機(jī)跟蹤的精度和穩(wěn)定性。

1 點線結(jié)合的視覺里程計

算法包括四個部分：特征提取與匹配、線特征的表達(dá)和投影、關(guān)鍵幀判定和圖優(yōu)化解算。

1.1 特征提取與匹配

算法中，點特征的提取與匹配參考ORB-SAM[5]中介紹的方法。對于線特征，算法使用了LSD(line segment detector)[14]方法提取圖像線段特征，LSD算法能夠以較高的效率提取圖像中帶有明顯地理信息的主要線特征。在LSD方法的基礎(chǔ)上，使用基于LBD描述子(line band descriptor)[15]的方法對不同圖像中提取的線段進(jìn)行匹配。在處理線段匹配的過程中，加入以下兩個篩選步驟，以優(yōu)化匹配的有效性：(1)對于彩色圖像，進(jìn)行RGB多通道匹配，僅有在復(fù)數(shù)個通道中被匹配的線段可被標(biāo)記為有效匹配；(2)對每次匹配，使用K最近鄰(K nearest-neighbor)方法尋找距離僅次于該次匹配的線段，僅當(dāng)兩次匹配的距離差大于設(shè)定的閾值時，才將該次匹配標(biāo)記為有效匹配。

圖像中線段提取和匹配的部分結(jié)果如圖1所示。

圖1 線段提取和匹配結(jié)果

(數(shù)據(jù)集為TUM-Fr1-room手持相機(jī)數(shù)據(jù)，匹配線段用相同編號標(biāo)注)

每新加入一幀影像，算法首先使用LBD方法提取線段特征，然后與已加入地圖的有效線特征進(jìn)行描述子差異計算，為匹配完成的線特征加入該幀的信息。在位姿估計完成后，根據(jù)有效匹配點線特征和新加入特征的數(shù)量，判定該幀是否作為關(guān)鍵幀加入地圖，并把在關(guān)鍵幀新檢測到的線段加入地圖。

隨著相機(jī)移動，大量線段會在視野中消失，算法為每個線特征Li設(shè)置了一個計數(shù)器參數(shù)cti，在每次對全圖線特征進(jìn)行匹配之前，將所有線特征對應(yīng)的cti加1，匹配結(jié)束之后得到匹配的參數(shù)減1；當(dāng)cti參數(shù)累積達(dá)到閾值τc，即直線Li已在τc幀影像中不可見時，將直線Li標(biāo)注為不可見，不再參與之后新加入幀的匹配。

1.2 線特征三維表達(dá)和投影

(1)

其中，v指向直線方向，n指向直線與坐標(biāo)原點構(gòu)成平面的垂直方向。則線段的正交坐標(biāo)[12]可表示為式(2)：

(2)

圖2 參數(shù)θ1、θ2、θ3的幾何意義

設(shè)從世界坐標(biāo)系到相機(jī)坐標(biāo)系的轉(zhuǎn)換矩陣為Tcw∈SE(3)，由旋轉(zhuǎn)矩陣Rcw∈SO(3)和位移向量tcw∈3組成，如式(3)所示：

(3)

直線的普呂克坐標(biāo)在世界坐標(biāo)系中表示為Lw=[n,v]T，在相機(jī)坐標(biāo)系中表示為Lc=[nc,vc]T，根據(jù)Bartoli和Sturm等人的研究[12]，從Lw到Lc的轉(zhuǎn)換關(guān)系可表示為式(4)。

(4)

其中，[·]×表示向量對應(yīng)的斜對稱矩陣，可用于簡便地計算向量叉乘(a×b=[a]×b)。若向量a=[a1,a2,a3]T，則斜對稱矩陣可表示為式(5)。

(5)

直線普呂克坐標(biāo)對應(yīng)的相機(jī)內(nèi)置矩陣如式(6)，其中fx、fy、cx、cy為相機(jī)標(biāo)定參數(shù)。

(6)

直線投影到影像后，坐標(biāo)l如式(7)所示。

(7)

1.3 關(guān)鍵幀判定

結(jié)合ORB-SLAM2[5]的關(guān)鍵幀判定方式，加入關(guān)鍵幀的條件為：(1)距離上次提取關(guān)鍵幀已經(jīng)經(jīng)過至少n個普通幀；(2)該幀內(nèi)追蹤到的特征數(shù)小于閾值τt，且至少檢測到了τc個新特征。根據(jù)實際實驗測試，取n=9，τt=100，τc=70時，可得到較好的結(jié)果。

1.4 圖優(yōu)化解算

1.4.1 算法詳解

圖優(yōu)化(graph optimization)是目前最前沿的SLAM位姿估計方法。圖優(yōu)化使用最大后驗估計進(jìn)行非凸優(yōu)化，從而估算相機(jī)位姿。待優(yōu)化變量組成圖的頂點(vertex)，其間轉(zhuǎn)換關(guān)系(在SLAM中通常為觀測方程)為圖的邊(edge)，調(diào)整頂點的值使得位姿和頂點坐標(biāo)盡量滿足邊的約束關(guān)系，從而達(dá)到優(yōu)化的目的。在此過程中，使用高斯-牛頓法等非線性優(yōu)化技術(shù)迭代解算。

對于一個帶有n條邊的圖，其目標(biāo)函數(shù)的形式通常如式(8)所示：

(8)

其中，Ωk為信息矩陣，即協(xié)方差矩陣的逆，其含義為對誤差相關(guān)性的大致估計，通常設(shè)置為對角矩陣，對角線元素表示各個誤差值的權(quán)重。

為避免陷入局部最優(yōu)解，實驗時通常引入魯棒核函數(shù)，并選擇具有更大收斂池的迭代方法，如Levenberg-Marquardt法、隨機(jī)梯度下降法等。G2O(general graph optimization)開源庫[17]提供了實現(xiàn)圖優(yōu)化算法的C++工具。

根據(jù)直線從世界坐標(biāo)系到影像的投影l(fā)與實際量測線段端點p1和p2(用齊次坐標(biāo)表示)之間的距離，可得出重投影誤差，如式(9)所示，其中d(·)表示距離函數(shù)。

(9)

而點特征的重投影誤差為點從世界坐標(biāo)系到影像的投影與實際量測點的距離。即在第k幀中，特征點i的世界坐標(biāo)為Pi，影像坐標(biāo)為pk,i，重投影誤差如式(10)所示(其中包含一次從齊次坐標(biāo)到非齊次坐標(biāo)的轉(zhuǎn)換)；綜合式(4)～式(7)，設(shè)特征線j的世界普呂克坐標(biāo)為Lj，在該幀中對應(yīng)的量測線段pk,j用其端點的齊次坐標(biāo)表示，則特征線j的重投影誤差如式(11)所示，[]1-3表示向量的前三維。

epk,i=pk,i-KTk,cwPi

(10)

elk,j=d(lk,j,K[Hk,cwLj]1-3)

(11)

算法使用光束法平差對相機(jī)姿態(tài)和特征坐標(biāo)進(jìn)行估計和優(yōu)化，平差的代價函數(shù)根據(jù)式(10)和式(11)可得，如式(12)所示。

(12)

其中，Covk,i和Covk,j分別表示點線特征在第k幀中的協(xié)方差矩陣，與特征尺度相關(guān)，hp和hl分別表示代價函數(shù)中點線特征對應(yīng)的魯棒Huber核函數(shù)。

向系統(tǒng)輸入新幀k時，需要根據(jù)該幀中提取特征的匹配結(jié)果，以目前的運(yùn)動模型對姿態(tài)進(jìn)行粗略估計，再使用光束法平差對該幀的相機(jī)姿態(tài)和特征坐標(biāo)進(jìn)行計算。設(shè)k時刻相機(jī)的轉(zhuǎn)換矩陣為Tk,cw，Tk,cw的估計過程如式(13)所示：

(13)

使用G2O[17]提供的多邊圖優(yōu)化工具進(jìn)行光束法平差時，將使用正交表達(dá)式表示的線特征、世界坐標(biāo)表示的點特征和轉(zhuǎn)換矩陣表示的相機(jī)位姿分別設(shè)為頂點，同時需要設(shè)置空頂點，則構(gòu)成圖的可能有兩種邊：點-空線-相機(jī)(點特征邊)，或空點-線-相機(jī)(線特征邊)。對于每一幀，需要進(jìn)行多次重復(fù)平差計算，每次計算時將誤差大于閾值th的邊的對應(yīng)特征設(shè)置為離群值，并從平差計算中剔除后再進(jìn)行下一次迭代平差，以此來獲得較穩(wěn)定的位姿估值。在實驗中，取thp=5.991，thl=7.815。如果初步位姿計算后非離群特征數(shù)量足夠，則需要在局部地圖中搜索匹配特征，進(jìn)一步優(yōu)化位姿關(guān)系。

1.4.2 雅克比矩陣

算法使用LevenBerg方法估計使代價函數(shù)達(dá)到最小的相機(jī)位姿值，設(shè)相機(jī)位姿R,t分別表示相機(jī)的旋轉(zhuǎn)矩陣和平移向量，它的李代數(shù)表示為ξ，ξ的擾動用δξ表示，迭代優(yōu)化過程中需要計算重投影誤差對δξ求導(dǎo)的雅克比矩陣。

首先，根據(jù)el=[e1,e2]T=d(l0,l)，量測線段端點坐標(biāo)為p1=[xs,ys,1]T，p2=[xe,ye,1]T，誤差對重投影直線l=[l1,l2,l3]T求偏導(dǎo)得到雅克比矩陣(Jacobian matrix)結(jié)果如式(14)所示。

(14)

(15)

(16)

最后根據(jù)鏈?zhǔn)椒▌t，結(jié)合式(14)～式(16)可得線特征重投影誤差對位姿擾動求導(dǎo)，如式(17)所示。

(17)

2 實驗結(jié)果

該文使用了多組影像數(shù)據(jù)進(jìn)行了實驗。由于單目視覺里程計無法獲取準(zhǔn)確的深度信息，實驗后通過計算相對位姿誤差(relative pose error，RPE)分析位姿精度。RPE精度計算方法通過選定某個窗口大小，比較待估計軌跡和真值在窗口內(nèi)的位姿變化一致性評價軌跡估算精度。精度估計過程中，TUM數(shù)據(jù)集的參照真值為數(shù)據(jù)集提供的軌跡數(shù)據(jù)，魚眼相機(jī)數(shù)據(jù)以Lidar SLAM算法和IMU綜合計算的軌跡數(shù)據(jù)作為真值。實驗表明，該文所述點線結(jié)合的跟蹤算法可以有效避免跟丟，有效提高軌跡長度和估計精度。

2.1 TUM數(shù)據(jù)集

TUM數(shù)據(jù)集[18]是一個常用的公開數(shù)據(jù)集，提供了一系列辦公室、工廠廠房、標(biāo)定棋盤格等RGB-D影像數(shù)據(jù)，并提供標(biāo)定相機(jī)內(nèi)置參數(shù)和帶有時間戳的軌跡真值信息。該文使用辦公室數(shù)據(jù)(fr3-long office household)和工廠數(shù)據(jù)(fr2-large with loop)進(jìn)行了實驗。影像每幀大小為640*480，數(shù)據(jù)場景如圖3所示。

只使用點數(shù)據(jù)跟蹤時，由于部分場景紋理缺失，常常出現(xiàn)跟丟的情況。加入線特征后，跟丟的情況有所改善。以上兩個數(shù)據(jù)集的實驗結(jié)果如表1所示。

圖3 fr3(左)和fr2(右)中的場景,

表1 TUM數(shù)據(jù)集實驗結(jié)果

2.2 魚眼相機(jī)數(shù)據(jù)

自采集數(shù)據(jù)為使用魚眼相機(jī)采集的會議室數(shù)據(jù)，數(shù)據(jù)大小為1 232*1 024。采集時，魚眼相機(jī)放置于裝有LiDAR等多種傳感器的背包上，在數(shù)據(jù)采集人員背著背包行走時越過采集者的頭頂采集前方數(shù)據(jù)。魚眼相機(jī)的焦距短，視野寬闊，同樣的采集頻率下相鄰幀之間可以達(dá)到更高的重疊度，但拍攝出的影像變形較大，處理之前需要先標(biāo)定相機(jī)，矯正影像畸變。矯正后的影像周圍會存在黑框和鏡頭被遮擋產(chǎn)生的陰影，需進(jìn)行掩膜處理以避免在陰影處和邊框邊緣錯誤地提取出特征。在提取點特征時，通過坐標(biāo)篩選剔除外點。而提取線特征時，考慮到處理效率，用一個簡單的判斷確定線是否位于無效區(qū)域：若線段li的兩端點psi、pei和中點pmi有兩個或以上位于無效區(qū)域時，標(biāo)記線端li為無效特征，不參與后續(xù)特征匹配。相機(jī)軌跡的真值由LiDAR點云數(shù)據(jù)和IMU傳感器計算得到。矯正后的影像實例和使用的掩膜如圖4所示，篩選前后的線特征如圖5所示。

圖4 自采集魚眼相機(jī)數(shù)據(jù)和預(yù)處理掩膜

圖5 進(jìn)行篩選前后的線段提取結(jié)果對比

魚眼數(shù)據(jù)實驗結(jié)果與精度分析如表2所示。

表2 魚眼相機(jī)會議室序列影像數(shù)據(jù)實驗結(jié)果

使用魚眼影像數(shù)據(jù)生成的場景地圖如圖6所示，其中左圖為場景稀疏特征地圖，右圖為左圖中實線框內(nèi)部分對應(yīng)實際場景?？梢婞c特征難以真正包含場景結(jié)構(gòu)信息，而根據(jù)線特征地圖對照影像數(shù)據(jù)可以清晰地看出屬于場景的哪一部分。

圖6 場景稀疏特征地圖

3 結(jié)束語

從特征提取、匹配、位姿估算和場景地圖生成等方面詳細(xì)介紹了一種基于點線結(jié)合特征的視覺里程計方法，并展示了使用多種數(shù)據(jù)對該方法進(jìn)行實驗的結(jié)果。這些結(jié)果證實了在基于點特征的SLAM系統(tǒng)中加入線特征的可行性和有效性，在增加可跟蹤軌跡長度的同時，提升了跟蹤的精度。在后續(xù)研究中，將會在閉環(huán)檢測等后端部分加入線特征運(yùn)算，將線的輔助效果最大化，以構(gòu)建完整的SLAM系統(tǒng)。