孫沁璇 苑 晶 張雪波 高遠兮

視覺里程計(Visual odometry,VO)在移動機器人定位和導航等任務中發(fā)揮著越來越重要的作用[1-4].近年來,高層幾何特征,如平面、直線、邊緣等,在視覺里程計以及同步定位與建圖(Simultaneous localization and mapping,SLAM)中得到了越來越多的關注[5-7].室內環(huán)境是移動機器人的重要作業(yè)環(huán)境之一,由于室內場景屬于典型的人造環(huán)境,其中存在大量平面、直線等幾何結構,能夠為機器人定位與建圖提供重要的高層環(huán)境特征[8-10].與目前最常用的點特征相比[11],高層幾何特征對環(huán)境紋理缺失以及光照變化有更強的魯棒性[12-13].另外,幾何特征是對原始傳感數(shù)據的高層參數(shù)化表示,能夠在盡可能減小數(shù)據存儲量的前提下保留環(huán)境的結構信息,為機器人的自主導航[14]及場景理解[15]提供導航地圖和語義描述.

在眾多高層幾何特征(平面、直線、線段、邊緣、輪廓等)中,平面特征具有魯棒性強、易穩(wěn)定提取和表示簡便等優(yōu)勢,因而在VO 和SLAM 領域得到了較好的應用[16-18].然而,在使用平面參數(shù)計算視覺里程計時,常常面臨因約束不足而導致的相機位姿求解退化問題[5,19-20].Taguchi 等[19]同時使用點特征和平面特征,給出了可以完全約束相機位姿的特征組合方式,但并沒有對只使用平面特征時的退化情況進行具體的分析.Cupec 等[20]在基于平面和線段特征的場景識別中,假設環(huán)境包含至少兩個非平行平面,此時平面特征對相機位姿估計提供至少5 自由度(Five degree of freedom,5 DoF)約束,使用擴展卡爾曼濾波(Extended Kalman filter,EKF)對該5 DoF 位姿進行估計,之后再利用平面與線段的投票機制對剩余的一個自由度進行計算.然而,由于RGB-D 相機視野范圍以及觀測距離的限制,會出現(xiàn)難以滿足環(huán)境包含至少兩個非平行平面的假設,如只能觀測到平行的桌面和地面的情況.Sun等[5]根據平面特征在三維空間的分布情況,檢測出用平面計算位姿時的退化情況,并提出基于STING(Statistical information grid)的掃描匹配方法,對無法被平面約束的位姿自由度進行計算.

除了平面特征外,直線特征也廣泛應用于移動機器人室內環(huán)境VO 和SLAM[21-24].與平面特征相比,直線特征在室內場景中的數(shù)量更多,也更不容易出現(xiàn)退化情況(只要存在兩條非平行直線即可完全約束位姿的計算).同時,相比于點特征,直線特征對低紋理環(huán)境具有更強的魯棒性[22-23].然而,通常情況下直線都分布于物體的邊緣區(qū)域,導致其深度值與RGB 值存在很大的觀測噪聲[25],因此,直線參數(shù)估計的準確性和穩(wěn)定性不及平面特征.

鑒于上述問題以及平面和直線特征在機器人位姿估計方面的互補性,本文提出基于平面和直線融合的RGB-D 視覺里程計(Plane-line-based RGBD visual odometry,PLVO).首先,針對多幾何特征的關聯(lián)問題,本文對Sun 等[5]提出的平面關聯(lián)圖(Plane association graph,PAG)進行擴展,提出平面-直線混合關聯(lián)圖(Plane-line hybrid association graph,PLHAG),在PAG 的基礎上,加入對平面與直線之間幾何關系的考慮,實現(xiàn)對兩類不同幾何特征的統(tǒng)一關聯(lián).其次,提出基于平面和直線融合的RGB-D 相機位姿估計方法,在用平面計算位姿出現(xiàn)退化情況時,引入直線特征作為平面特征的補充,通過二者的自適應融合,完成6 DoF 位姿的計算.值得說明的是,由于平面特征參數(shù)的準確性和穩(wěn)定性均優(yōu)于直線特征,故本文根據平面特征對位姿的約束情況來控制直線特征對位姿求解的貢獻,使得直線特征只對平面特征無法約束的自由度進行約束,因此,即使在用平面特征求解時出現(xiàn)退化的情況,也盡可能保證平面特征占主導作用,充分利用平面特征良好的準確性和穩(wěn)定性,提升位姿估計的效果.

1 PLVO 系統(tǒng)框架

PLVO 系統(tǒng)框架如圖1 所示.首先,平面特征提取模塊[5]從RGB-D 相機采集的深度圖像中提取平面特征,而直線特征提取模塊[26]從RGB 圖像中檢測二維直線,并利用深度信息將其投影到相機坐標系得到三維直線特征.其次,將平面和直線特征輸入到平面與直線特征匹配模塊中,其中包含兩個子模塊:PLHAG 構建和PLHAG 匹配.在PLHAG 構建子模塊中,對相鄰兩幀提取的特征分別構建PLHAG,并將其傳入PLHAG 匹配子模塊完成特征的關聯(lián).然后,將具有關聯(lián)關系的平面和直線特征輸入RGB-D 相機位姿估計模塊,優(yōu)先基于平面特征對相機位姿進行計算,當出現(xiàn)退化情況時,用直線特征對平面無法約束的位姿自由度進行計算,最終得到6 DoF 相機位姿的估計結果.

2 特征提取與關聯(lián)

2.1 平面與直線特征提取

使用基于STING 的平面提取算法[5]對深度圖像中的平面特征進行提取,并將提取出的平面的參數(shù)表示為π=[nT,d]T,其中,n∈S2表示平面的單位法向量(S2表示三維歐氏空間R3中的單位球面),d∈R表示相機坐標系原點到平面的垂直距離.平面π上的點對應的圖像RGB 值的均值和協(xié)方差分別表示為rπ和Sπ.

使用LSD (Line segment detector)算法[26]檢測RGB 圖像中的直線,并將其投影到三維相機坐標系下,得到的三維空間直線參數(shù)表示為L=[uT,vT]T,其中,v∈S2表示直線的單位方向向量,而u∈R3的方向為直線與相機坐標系原點構成平面的法向量方向,u的長度為原點到直線的垂直距離.

2.2 平面-直線混合關聯(lián)圖(PLHAG)構建

文獻[5]提出了基于PAG 的平面特征關聯(lián)方法.PAG 是一種全連接圖結構,其節(jié)點表示平面特征,節(jié)點之間的邊則代表平面之間的幾何關系.相鄰幀之間的平面特征匹配是通過比較每幀構建的PAG 之間的節(jié)點相似度完成的.本文對PAG 進行擴展,針對多幾何特征的關聯(lián)問題,提出基于PLHAG 的平面和直線混合關聯(lián)方法.在PLHAG 中,不僅考慮平面之間的幾何關系,另外加入了直線與平面之間的幾何關系,同時建立平面與平面、直線與直線之間的關聯(lián)關系.

對于某一幀中提取出的平面集合{πi},i∈{1,···,Nπ}與直線集合{Lj},j∈{1,···,NL},構建的PLHAG 表示為G=(V,E).V表示節(jié)點集合,其中包含兩類節(jié)點類型,即平面節(jié)點vπ,i=πi與直線節(jié)點vL,j=Lj.相應地,邊的集合E也包含兩種類型,即平面節(jié)點之間的邊(Plane-plane edge,PPE)eπ,ik,以及連接直線節(jié)點與平面節(jié)點的邊(Line-plane edge,LPE)eLπ,jk.與PAG 不同的是,PLHAG 不是全連接圖結構.考慮到兩類幾何特征之間的差異,直線節(jié)點與直線節(jié)點之間不進行連接,具體原因將在第2.3 節(jié)中詳細說明.

平面節(jié)點之間的邊eπ,ik定義為

類似地,直線節(jié)點與平面節(jié)點之間的邊eLπ,jk定義為

其中,枚舉量ωπ,ik和ωLπ,jk分別表示平面與平面以及直線與平面之間的幾何關系類型,即平行關系(parallel)與非平行關系(non-parallel),如式(2)和式(6)所示,在本文實驗中閾值δrad取值為10°.由式(3)和式(7)可知,απ,ik為ni與nk之間的夾角,αLπ,jk為vj與nk之間的夾角.根據式(4)和式(8)可知,當兩個平面(或直線和平面)之間為平行關系時,dπ,ik(dLπ,jk)的值為兩個平行平面(或平行直線與平面)之間的垂直距離.至此,完成PLHAG 的構建,下面將分別使用相鄰兩幀特征構建的PLHAG 完成平面特征與直線特征的匹配.

2.3 基于PLHAG 的特征匹配

對于從相鄰兩幀提取的特征集合{πci,Lcj},i∈{1,···,Nπc},j∈{1,···,NLc}和{πrm,Lrp},m∈{1,···,Nπr},p∈{1,···,NLr},其中,下標c與r分別代表當前幀與參考幀,使用第2.2 節(jié)提出的方法分別構建PLHAGGc=(Vc,Ec)和Gr=(Vr,Er).在本節(jié)中,通過計算Gc中節(jié)點與Gr中節(jié)點之間的相似度,建立兩幀特征之間的匹配關系.

首先,在計算節(jié)點相似度之前,分別定義兩個PPE 之間和兩個LPE 之間的關系,如式(9)和式(10)所示,其中閾值δdist在實驗中取值為0.06 m.

滿足eπc,ik=eπr,ml關系的兩個PPE (滿足eLπc,jk=eLπr,pl關系的兩個LPE)稱為相似邊,相似邊代表其所連接的兩個平面節(jié)點(直線節(jié)點與平面節(jié)點)之間相似的幾何關系,用于后文中對節(jié)點幾何相似度的定義.

接著,對平面和直線節(jié)點分別定義節(jié)點之間的相似度.兩個平面節(jié)點vπc,i∈Vc和vπr,m∈Vr之間的相似度sπ(vπc,i,vπr,m)由兩部分組成,即顏色相似度sπ,col(vπc,i,vπr,m)與幾何相似度sπ,geo(vπc,i,vπr,m).

顏色相似度sπ,col(vπc,i,vπr,m)∈[0,1]為

其中,Sπ=(Sπc,i+Sπr,m)/2.因為具有關聯(lián)關系的平面特征與其所在掃描幀中其他關聯(lián)特征之間應具有相同的幾何關系.所以,通過以相似邊連接的平面節(jié)點的顏色相似度計算節(jié)點之間的幾何相似度sπ,geo(vπc,i,vπr,m),即

其中,Iim為索引對的集合,索引到Vc和Vr中分別與vπc,i和vπr,m以相似邊(式(9))連接的平面節(jié)點.

而|Iim|則代表集合Iim中元素的個數(shù).

與平面特征不同,直線特征多數(shù)分布在幾何物體的邊緣以及圖像中RGB 值變化劇烈的區(qū)域,故直線上的點對應像素的顏色信息受觀測噪聲影響較大,不適合用于對直線特征的描述.因此,本文不計算兩個直線節(jié)點之間的顏色相似度,只計算其幾何相似度,因而在第2.2 節(jié)構建的PLHAG 中,直線節(jié)點和直線節(jié)點之間也不進行連接.

對于兩個直線節(jié)點vLc,j∈Vc和vLr,p∈Vr,二者之間的相似度sL(vLc,j,vLr,p)定義為

其中,sL,geo(vLc,j,vLr,p)表示直線節(jié)點的幾何相似度,由以相似邊連接的平面節(jié)點的顏色相似度計算:

其中,|Ijp|為集合Ijp中元素的個數(shù).

在使用PLHAG 進行特征關聯(lián)時,根據式(11)和式(16)分別對來自當前幀與參考幀的每一對平面節(jié)點之間以及直線節(jié)點之間的節(jié)點相似度進行計算,通過比較節(jié)點相似度的大小來尋找最佳匹配關系,具體算法流程如算法1 所示.

算法 1.基于PLHAG 的特征關聯(lián)算法

需要說明的是,算法第4 行和第10 行中閾值δs的作用是對不正確的匹配關系進行排除,如對于當前幀中新觀測到的特征,參考幀中不存在任何特征與其匹配,這種情況就需要通過對節(jié)點相似度數(shù)值的限制進行排除.由于關聯(lián)特征與非關聯(lián)特征所對應節(jié)點的相似度數(shù)值的差異比較大,因此,閾值δs的選擇對算法結果的影響非常小.本文實驗中δs取值為0.99.在圖2 中,針對場景中某種具體的特征分布情況,給出了PLHAG 結構的示意圖,其中,圖2(b)中實線和虛線分別標識了平面和直線節(jié)點.對于圖2(a)中平面,假設有

其中,η接近1,ε充分小.以節(jié)點vLc,1和vLr,1為例,從圖2(a)中特征之間位置關系可以看出

則vLc,1和vLr,1所對應的索引集合為I11={(1,1),(2,2),(3,3)},根據式(15)可知

同理可知

根據算法1 可知,{Lc1,Lr1}為一對匹配的直線特征.基于PLHAG 的多特征關聯(lián)方法利用圖結構充分考慮特征之間的幾何關系,提高了特征關聯(lián)的魯棒性.與PAG 相比,PLHAG 對原單一節(jié)點類型進行拓展,將平面和直線特征納入統(tǒng)一框架,同時考慮了不同特征之間的約束關系,提升了算法的魯棒性和適用性.

3 RGB-D 相機位姿估計

對于RGB-D 相機,平面特征比直線特征具有更強的穩(wěn)定性和魯棒性,因為平面特征多提取于規(guī)則幾何物體表面,如地面、墻面、桌面等,而直線特征多分布于物體邊緣,對應的點云觀測噪聲顯著高于來自規(guī)則物體表面的點云觀測噪聲[25,27].為此,本文主要使用平面特征計算相機位姿.然而,由于RGB-D 相機的觀測視野較小,且觀測距離有限,導致能夠提取出的平面特征數(shù)量不夠充足,因此,經常出現(xiàn)平面特征無法完全約束6 DoF 相機位姿計算的情況,產生位姿求解的退化問題.針對此問題,本文提出基于平面和直線自適應融合的RGB-D 相機6 DoF 位姿估計方法,在平面特征無法完全提供全部約束時,使用直線特征作為平面特征的補充,完成6 DoF 位姿的計算.

使用第2.2 節(jié)中的方法進行特征關聯(lián),得到具有關聯(lián)關系的平面特征集合Π={πci,πri},i∈{1,···,Nπ}以及直線特征集合L={Lcj,Lrj},j∈{1,···,NL}.首先,計算RGB-D 相機的三維空間旋轉變換R∈SO(3)和平移變換t∈R3,使得平面特征的匹配誤差最小,即目標函數(shù)(24)和(25)達到最小.

為了檢測是否出現(xiàn)退化解,構造矩陣H=,并計算其奇異值分解 (Singular value decomposition,SVD).

其中,Qr=[qr1,qr2,qr3]和Qc=[qc1,qc2,qc3]為正交單位矩陣,而Λ=diag{λ1,λ2,λ3},λ1≥λ2≥λ3為對角矩陣.根據矩陣H的奇異值,可以判斷平面特征是否能夠完全約束6 DoF 位姿的計算,具體分為3 種情況,即6 DoF 約束情況、5 DoF 約束情況和3 DoF 約束情況,其中后兩種為退化情況.下面分別對這3 種情況給出詳細的分析和具體的位姿計算方法.

3.1 6 DoF 約束情況

如果H為非奇異矩陣,即λ1≥λ2≥λ3＞0,則使式(24)和式(25)達到最小值的解分別由式(27)和式(28)得到.

3.2 5 DoF 約束情況

如果H為奇異矩陣,且λ1≥λ2＞λ3=0,此時平面特征的法向量{nci}({nri}),i∈{1,···,Nπ}在空間中是共面關系,且垂直于向量qc3(qr3).在這種情況下,旋轉變換為

由此可知,沿著向量qc3方向的平移變換無法被關聯(lián)的平面特征約束.因此,在計算平移變換的誤差函數(shù)中加入直線特征的匹配誤差,即

其中

此時,使得式(32)最小化的平移最優(yōu)解為

其中

其中,In為n維單位矩陣.

由式(32)和式(33)可知,直線特征{Lcj,Lrj}對誤差函數(shù)(32)的貢獻量由權值wj進行控制.由式(34)可見,對于t∈/null(Aπ),即在平面特征能提供較強約束的平移方向上,直線特征的權值較小,則其對平移變換求解的貢獻也較小;而對于t∈null(Aπ),即在平面特征無法提供約束的平移方向上,直線特征的權值較大,則其對平移變換求解的貢獻也較大;當直線方向與qc3垂直時,wj=1,此時直線對平移變換求解的貢獻達到最大.圖3 給出了一些典型室內場景中平面與直線特征的空間分布,并通過灰度映射的方式反映出每條直線特征的權值大小.從圖中可以看出,對qc3方向的平移約束最強的直線,也就是垂直于該方向的直線,其權值最大(圖中黑色直線);而對于平行于該方向的直線,其權值最小(圖中淺灰色直線).

圖3 5 DoF 約束情況下直線權值Fig.3 The weights of lines in 5 DoF constraint cases

總體來說,在平面特征對平移求解約束較強的方向上,盡量使用準確性和穩(wěn)定性更強的平面參數(shù)進行求解,而當產生退化時,再使用直線特征對平面特征無法約束的位姿方向的求解進行補充,實現(xiàn)了兩類特征的自適應融合.這樣既確保了算法的準確性,同時也有效地解決了使用平面特征求解位姿時出現(xiàn)的退化問題.

3.3 3 DoF 約束情況

如果H為奇異矩陣,且λ1＞λ2=λ3=0,此時平面特征的法向量{nci}({nri}),i∈{1,···,Nπ}在空間中是共線關系,且都沿著向量qc1(qr1)的方向分布.在這種情況下,以qc1為旋轉軸的旋轉變換無法被平面特征所約束,即在最小化式(24)時存在退化情況.

假設存在旋轉變換R1使得兩幀平面特征的法向量對齊,即nci=R1nri,?i∈{1,···,Nπ},而R1與相機真正的旋轉變換R之間還相差一個1 DoF旋轉,即以qc1為旋轉軸的旋轉變換R2.在本文方法中,R2是通過直線特征計算的,下文分析直線特征的姿態(tài)對計算R2準確性的影響,并基于此給出求解相機旋轉變換時,平面與直線特征的自適應融合方法.

考慮任意一對具有匹配關系的直線特征vcj和vrj,假設vrj經過R1變換后為,則由vcj和可以計算R2,即

其中

令?=[0,min{2?,2(π-?)}](〈·,·〉表示向量之間的夾角),如圖4 所示,則式(40)可以化簡為

圖4 角度θ,β與?示意圖Fig.4 Illustration of anglesθ,βand?

計算旋轉角θ對β的偏導數(shù),可得

圖5 給出了在?取不同值的時候,偏導數(shù)的曲線.從圖5 和式(41)均可以看出,當?=0.5π,即與旋轉軸qc1垂直時,θ與β呈線性關系;而?接近0 或π,即與qc1之間接近共線時,β的微小擾動會使得θ劇烈變化,大幅降低旋轉角度估計的準確性和穩(wěn)定性.基于此,在直線特征匹配誤差中,通過自適應加權方法調整每個直線特征對求解旋轉變換的貢獻,即

圖5 ?取不同值時偏導數(shù)隨β變化曲線Fig.5 Shape of the function w.r.t.βas the value of?changes

其中,權值wRj的計算方法為

其中,QLr=[qLr1,qLr2,qLr3]和QLc=[qLc1,qLc2,qLc3]為正交單位矩陣,而 ΛL=diag{λL1,λL2,λL3},λL1≥λL2≥λL3為對角矩陣.最小化式(43)的最優(yōu)解可由式(47)進行計算,即

對于平移變換的求解,也無法直接由式(28)得到,而此時矩陣Aπ的零空間為

這種情況下,與5 DoF 約束情況類似,誤差函數(shù)中加入直線特征的匹配誤差,構造為式(32),而平移變換的求解方法也如式(35)所示.與5 DoF約束情況不同的是,權值wj的計算方法由式(34)調整為式(49),即

圖6 給出了3 DoF 約束情況的兩個場景下平面與直線特征的空間分布,并通過灰度映射的方式反映出每條直線特征的旋轉權值wRj以及平移權值wj的大小.從圖6(b)可以看出,垂直于qc1方向(即平行于平面)的直線(圖中黑色直線)的旋轉權值wRj較大,即這些直線對于求解旋轉變換的貢獻較大;而平行于qc1方向的直線(圖中淺灰色直線)的權值wRj較小,即對求解旋轉變換的貢獻較小.而從圖6(c)可以看出,垂直于平面方向的直線(圖中黑色直線)的平移權值wj最大,因為這樣的直線對于平面特征無法約束的平移自由度,即平行于平面方向的平移的約束是最大的.

圖6 3 DoF 約束情況下直線權值Fig.6 The weights of lines in the 3 DoF constraint case

4 實驗與分析

本文實驗主要分為3 個部分:1)基于PLHAG 的平面-直線特征關聯(lián)算法驗證(第4.1 節(jié));2)不同VO 算法在數(shù)據集上的對比實驗(第4.2節(jié));3)移動機器人真實環(huán)境VO 實驗(第4.3 節(jié)).實驗所用處理器為Intel i7 CPU (1.8 GHz),運行內存8 GB.第4.1 節(jié)和第4.2 節(jié)實驗中所用數(shù)據集為TUM RGB-D 公開數(shù)據集[28];第4.3 節(jié)真實環(huán)境實驗中所用移動機器人平臺為Pioneer 3-DX,傳感器為Microsoft Kinect 2.0.

4.1 特征關聯(lián)實驗

為了驗證本文提出的基于PLHAG 的平面-直線混合關聯(lián)算法的有效性,對TUM 數(shù)據集中5 個圖像序列中的平面與直線特征進行人工標注,并將本文算法與Proen?a 等[25]提出的視覺里程計(簡稱為Prob-RGBD-VO)中使用的平面與直線的匹配方法進行比較.本文使用準確率(Precision rate)和召回率(Recall rate)作為特征關聯(lián)實驗的評測指標.其中,準確率定義為被關聯(lián)算法成功關聯(lián)并且被人工標注為關聯(lián)特征的特征數(shù)量除以被關聯(lián)算法成功關聯(lián)的特征數(shù)量.召回率定義為被關聯(lián)算法成功關聯(lián)并且被人工標注為關聯(lián)特征的特征數(shù)量除以人工標注為關聯(lián)特征的特征數(shù)量.在實驗中,對圖像序列中每一幀特征關聯(lián)結果的準確率和召回率進行計算,并將每個序列上的統(tǒng)計結果以箱線圖的形式進行展示,如圖7 所示,以此驗證特征關聯(lián)算法的性能.

圖7 特征關聯(lián)算法實驗結果Fig.7 Comparison of feature association algorithms

在對比算法Prob-RGBD-VO 中,對平面和直線特征分別進行關聯(lián).對于平面特征,先利用平面在圖像上的重疊區(qū)域和幾何關系(法向量夾角與截距差)篩選出可能具有匹配關系的候選,接著,對于兩個平面π和π′,定義平面到平面的距離為‖dnd′n′‖,然后在匹配候選中選出平面到平面距離最小的匹配對.對于直線特征,首先對圖像中提取出的直線特征計算描述子LBD (Line band descriptor)[29],并使用k近鄰搜索建立匹配關系,然后保留滿足圖像2D 直線幾何約束(直線方向和截距相似)的匹配對[25].

實驗分別使用兩種特征關聯(lián)方法進行相鄰幀之間的平面與直線關聯(lián),并對其準確率與召回率進行統(tǒng)計.基于PLHAG 的關聯(lián)方法和Prob-RGBD-VO中所使用的關聯(lián)方法的平均準確率分別為91.6%和88.9%,平均召回率分別為95.0%和91.7%.由于Prob-RGBD-VO 中關聯(lián)方法將兩類特征分別進行關聯(lián),且并未考慮同一掃描幀的特征之間的幾何關系,而在PLHAG 中,充分考慮了平面與平面、平面與直線之間的幾何關系,進而提高了方法的準確性和魯棒性.

另外,為了驗證算法的時間性能,本文對基于PLHAG 以及PAG 的特征關聯(lián)算法的對數(shù)關聯(lián)時間分別進行統(tǒng)計,并繪制統(tǒng)計結果的箱線圖,如圖8(a)所示.需要說明的是,基于PLHAG 的特征關聯(lián)算法的平均關聯(lián)時間是0.33 ms,完全滿足算法實時性的要求.圖8(b)給出的節(jié)點數(shù)量統(tǒng)計直方圖顯示了PLHAG 與PAG 中節(jié)點數(shù)量的差異.可以看到,與基于PAG 的算法相比,PLHAG 由于直線特征的加入使得圖結構規(guī)模變大,節(jié)點數(shù)量增加,而在進行特征關聯(lián)時需要對圖結構進行搜索,因此,特征關聯(lián)過程的運行時間有所增加.盡管如此,從圖8(a)可以看出,增加的時間小于1 ms,對VO 系統(tǒng)實時性產生的影響可以忽略不計.

圖8 基于PLHAG 與PAG 特征關聯(lián)方法時間性能對比Fig.8 Comparison of real-time performance for PLHAG and PAG based feature association,respectively

4.2 公開數(shù)據集VO 對比實驗

在本節(jié)實驗中,在TUM 數(shù)據集5 個場景中運行本文提出的PLVO 算法,并將其結果與4 種VO算法plane-seg-VO[30],Prob-RGBD-VO[25],Canny-VO[31]以及STING-VO[5]進行對比.plane-seg-VO[30]是基于平面與線段特征的RGB-D 視覺里程計,在計算相機位姿時,先利用平面法向量和線段方向向量計算旋轉變換,接著通過對齊平面上的點以及線段端點確定平移變換,最后利用迭代最近點(Iterative closest point,ICP)算法對點云進行精確匹配.Prob-RGBD-VO[25]是基于點、直線和平面的RGB-D 視覺里程計,通過3 種特征的聯(lián)合優(yōu)化實現(xiàn)對位姿的估計.Canny-VO[31]通過對RGB 圖像中提取的Canny 邊緣進行掃描匹配,實現(xiàn)視覺里程計的計算.

實驗采用的對比性能指標為相對位姿誤差(Relative pose error,RPE)和絕對軌跡誤差(Absolute trajectory error,ATE),其評測工具分別為TUM 數(shù)據集網站提供的python 評測腳本evaluate_ate.py 和evaluate_rpe.py.本文分別對各個算法統(tǒng)計RPE 和ATE 的均方根誤差(Root mean square error,RMSE),結果如表1 和表2 所示.為了盡量避免調參不當對4 種對比算法性能的影響,planeseg-VO[30],Prob-RGBD-VO[25]與Canny-VO[31]的結果以及STING-VO[5]在fr2/desk 和fr3/cabinet 上的結果均取自于各自論文公開的實驗結果.從表1 和表2 中可以看出,本文提出的PLVO 在絕大多數(shù)場景中都能獲得最優(yōu)的準確性,原因在于PLVO利用平面和直線進行位姿估計時,充分利用了兩種特征的互補性,即平面特征的準確性和穩(wěn)定性,以及直線特征克服退化的能力.并且,在PLVO 利用直線特征對平面特征無法約束的位姿自由度進行補充的同時,盡可能地保留平面特征的主導作用,充分發(fā)揮平面特征參數(shù)提取的準確性和穩(wěn)定性優(yōu)勢,實現(xiàn)了兩類特征的自適應融合.圖9 展示了其中三個圖像序列的ATE 與RPE 結果評測圖,從中可以看出,使用PLVO 計算得到的傳感器軌跡與運動捕捉系統(tǒng)獲得的真實軌跡有著極高的重合度,RPE 曲線也穩(wěn)定在較小的范圍內,充分證明了本文算法的準確性和魯棒性.

表1 不同VO 算法相對位姿均方根誤差對比Table 1 Comparison of RMSE of RPE for different VO methods

表2 不同VO 算法絕對軌跡均方根誤差對比(m)Table 2 Comparison of RMSE of ATE for different VO methods (m)

圖9 PLVO 算法ATE 和RPE 結果評測圖Fig.9 Visualization of ATE and RPE for PLVO

為了更加充分地驗證多特征融合在VO 系統(tǒng)中的作用,分別計算基于平面特征的VO (P-VO)、基于直線特征的VO (L-VO)以及基于平面-直線(無自適應加權,即式(33)中wj=1,式(44)中wRj=1 )的VO,統(tǒng)計RPE 指標的RMSE 如表3所示.從表3 中可以明顯看出多特征融合以及自適應加權算法對系統(tǒng)準確性的提升作用.需要說明的是,在P-VO 中,會頻繁出現(xiàn)位姿求解退化情況,表4 統(tǒng)計了每個圖像序列上的求解退化出現(xiàn)次數(shù)所占的比例.為了便于對VO 定位精度進行更加客觀的比較,在表3 所示的P-VO 的實驗結果中,沒有統(tǒng)計出現(xiàn)求解退化時的位姿估計結果.而PLVO 中沒有出現(xiàn)過退化情況,由此可以看出多特征融合不僅提高了算法的準確性,而且避免了使用單特征的位姿求解算法可能出現(xiàn)的退化問題.

表3 相對位姿均方根誤差消融實驗結果Table 3 Results of ablation experiment in term of the RMSE of RPE

表4 P-VO 中位姿求解退化情況所占比例Table 4 Ratio of the degenerate cases in P-VO

此外,本文分別對PLVO、P-VO 以及L-VO算法的運行時間進行統(tǒng)計,其箱線圖如圖10 所示.相比于P-VO 與L-VO 算法,PLVO 算法分別增加了直線提取和平面提取的運行時間,因此實時性略遜于P-VO 與L-VO.盡管如此,PLVO 的幀率仍可達到6～10 Hz,滿足系統(tǒng)實時性的要求.

圖10 各個圖像序列上PLVO、P-VO 以及L-VO 每幀運行時間統(tǒng)計箱線圖Fig.10 Boxplot of statistics of the runtime per frame for PLVO,P-VO and L-VO

4.3 室內環(huán)境移動機器人VO 實驗

為了驗證本文算法的有效性,使用PLVO 進行真實環(huán)境的移動機器人視覺里程計實驗.將Microsoft Kinect 2.0 傳感器固定在Pioneer 3-DX 機器人上離地面大約0.34 m 的位置.在實驗中,用手柄控制機器人通過一個實驗室,環(huán)境規(guī)模為8.68 m×7.86 m.用PLVO 算法進行機器人的定位與增量式建圖,所構建的三維點云地圖如圖11 所示.圖11(a)為俯視全景圖,圖11(b)和圖11(c)給出了在全景圖中標注的I,II,III 三處對應的RGB 圖像和局部放大的點云地圖.從圖11 中可以看出,PLVO 在真實室內環(huán)境中能夠較準確地完成移動機器人的位姿估計.

圖11 實驗室場景下移動機器人定位與增量式建圖結果Fig.11 Real-world experiment in a laboratory using a mobile robot

5 結束語

本文提出了一種基于平面和直線自適應融合的視覺里程計算法.通過對相鄰兩幀的平面和直線特征統(tǒng)一構建PLHAG,充分考慮了平面與平面、平面與直線之間的幾何關系,實現(xiàn)了兩類特征的混合關聯(lián),提高了多特征關聯(lián)結果的準確率和召回率.而在估計RGB-D 相機位姿時,充分發(fā)揮了平面特征準確、穩(wěn)定的優(yōu)點及其在機器人位姿估計中的主導作用.針對平面特征在位姿估計中存在的退化問題,引入直線特征對平面無法約束的自由度進行約束,并設計能夠充分體現(xiàn)約束強度的權重,實現(xiàn)了兩類特征的主輔相濟與自適應融合.多角度的實驗驗證充分證明了本文提出算法的準確性和有效性.