基于點線結合特征的單目視覺里程計

李鐵維，王牧陽，周 炎

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

0 引 言

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

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

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

1 點線結合的視覺里程計

算法包括四個部分：特征提取與匹配、線特征的表達和投影、關鍵幀判定和圖優化解算。

1.1 特征提取與匹配

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

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

圖1 線段提取和匹配結果

(數據集為TUM-Fr1-room手持相機數據，匹配線段用相同編號標注)

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

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

1.2 線特征三維表達和投影

(1)

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

(2)

圖2 參數θ1、θ2、θ3的幾何意義

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

(3)

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

(4)

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

(5)

直線普呂克坐標對應的相機內置矩陣如式(6)，其中fx、fy、cx、cy為相機標定參數。

(6)

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

(7)

1.3 關鍵幀判定

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

1.4 圖優化解算

1.4.1 算法詳解

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

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

(8)

其中，Ωk為信息矩陣，即協方差矩陣的逆，其含義為對誤差相關性的大致估計，通常設置為對角矩陣，對角線元素表示各個誤差值的權重。

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

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

(9)

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

epk,i=pk,i-KTk,cwPi

(10)

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

(11)

算法使用光束法平差對相機姿態和特征坐標進行估計和優化，平差的代價函數根據式(10)和式(11)可得，如式(12)所示。

(12)

其中，Covk,i和Covk,j分別表示點線特征在第k幀中的協方差矩陣，與特征尺度相關，hp和hl分別表示代價函數中點線特征對應的魯棒Huber核函數。

向系統輸入新幀k時，需要根據該幀中提取特征的匹配結果，以目前的運動模型對姿態進行粗略估計，再使用光束法平差對該幀的相機姿態和特征坐標進行計算。設k時刻相機的轉換矩陣為Tk,cw，Tk,cw的估計過程如式(13)所示：

(13)

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

1.4.2 雅克比矩陣

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

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

(14)

(15)

(16)

最后根據鏈式法則，結合式(14)～式(16)可得線特征重投影誤差對位姿擾動求導，如式(17)所示。

(17)

2 實驗結果

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

2.1 TUM數據集

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

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

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

表1 TUM數據集實驗結果

2.2 魚眼相機數據

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

圖4 自采集魚眼相機數據和預處理掩膜

圖5 進行篩選前后的線段提取結果對比

魚眼數據實驗結果與精度分析如表2所示。

表2 魚眼相機會議室序列影像數據實驗結果

使用魚眼影像數據生成的場景地圖如圖6所示，其中左圖為場景稀疏特征地圖，右圖為左圖中實線框內部分對應實際場景。可見點特征難以真正包含場景結構信息，而根據線特征地圖對照影像數據可以清晰地看出屬于場景的哪一部分。

圖6 場景稀疏特征地圖

3 結束語

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