基于圖優(yōu)化的單目線特征SLAM算法

董蕊芳 柳長安 楊國田 程瑞營

(華北電力大學(xué)控制與計算機工程學(xué)院, 北京 102206)

基于圖優(yōu)化的單目線特征SLAM算法

董蕊芳 柳長安 楊國田 程瑞營

(華北電力大學(xué)控制與計算機工程學(xué)院, 北京 102206)

提出了基于圖優(yōu)化的單目線特征同時定位和地圖構(gòu)建(SLAM)的方法.首先,針對主流視覺SLAM算法因采用點作為特征而導(dǎo)致構(gòu)建的點云地圖稀疏、難以準確表達環(huán)境結(jié)構(gòu)信息等缺點,采用直線作為特征來構(gòu)建地圖．然后,根據(jù)現(xiàn)有線特征的SLAM算法都是基于濾波器的SLAM框架、存在線性化及更新效率的問題,采用基于圖優(yōu)化的SLAM解決方案以提高定位精度及地圖構(gòu)建的一致性和準確性．將線特征的Plücker坐標和Cayley參數(shù)化方式相結(jié)合,一方面采用Plücker坐標便于線性投影計算,另一方面采用Cayley參數(shù)化方式有利于線特征參數(shù)的非線性優(yōu)化．仿真實驗結(jié)果顯示：所提出算法的位姿估計誤差平方和與均方根誤差分別是里程計位姿估計的2.5%和10.5%,是基于EKF線特征SLAM算法估計位姿誤差的22.4%和33%,重投影誤差僅為45.5像素;實際圖像實驗中的位姿估計誤差平方和為958 cm2,均方根誤差為3.9 413 cm,從而證明了所提出算法的有效性和準確性.

單目SLAM;線特征;圖優(yōu)化;Plücker坐標;Cayley直線

機器人同時定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)是機器人實現(xiàn)完全自主移動的一項關(guān)鍵技術(shù).該技術(shù)使機器人在未知環(huán)境移動過程中借助所攜帶的傳感器實現(xiàn)對自身的定位,同時完成對周圍環(huán)境的地圖構(gòu)建[1-2].視覺傳感器具有體積小、成本低、易于安裝、且包含豐富的環(huán)境信息等優(yōu)點,因而被廣泛應(yīng)用于SLAM系統(tǒng)中[3].

傳統(tǒng)的視覺SLAM算法主要是提取圖像中的點作為特征進行制圖和定位,構(gòu)建的地圖為點云地圖,點特征易于提取，現(xiàn)有點特征描述算法及跟蹤算法成熟,但稀疏的點云難以準確地描述周圍環(huán)境的結(jié)構(gòu)信息;與點特征相比,線特征廣泛存在于各類環(huán)境場景中,由于直線比點高一個維度,其表示的地圖能更加準確地表達環(huán)境的信息,尤其是對于結(jié)構(gòu)化場景,如輸電桿塔、工業(yè)建筑、鐵路及室內(nèi)走廊等,線特征具有明顯的優(yōu)勢;此外,圖像中線特征的匹配較點特征具有較好的光照不變性,且不易受視點變化的影響,更有利于進行回環(huán)檢測[4-6].

線特征的參數(shù)化方法是研究基于線特征SLAM算法的基礎(chǔ),目前主要分為非線性最小維度4-參數(shù)表達方式以及線性冗余維度(over-parameters)參數(shù)化方式兩大類.對于第1類參數(shù)化方法,Roberts[7]采用2個方向余弦結(jié)合1個2D坐標來表示空間直線的方向和位置;Ohwovoriole等[8]采用2個平面相交得到直線的表達方式;Zhang等[9]采用Cayley參數(shù)化方式.空間直線具有4個自由度,不存在冗余維度,也無需一致性約束,因此這類參數(shù)化方式能夠更好地適用于非線性優(yōu)化算法.然而,由于該類參數(shù)化方法的非線性,不合適于對線特征進行線性投影計算.

對于線性冗余維度參數(shù)化方法,文獻[10-11]采用2個線段端點表示空間直線;Montiel等[12]采用線段的中點及方向向量表達空間直線.對于這2種參數(shù)化方式,線段的中點及2個端點在圖像中都不能保證被穩(wěn)定地檢測到.Sola等[13]和Lemaire等[14]均采用Plücker坐標來表示直線,可以很方便地對線特征使用線性投影模型,但不利于進行非線性優(yōu)化.

SLAM本質(zhì)上是一個狀態(tài)估計問題,其求解方法可大致分為基于濾波器的方法和基于圖優(yōu)化或基于平滑的方法.對于基于線特征的視覺SLAM,大部分都采用第一類方法,如文獻[6,11,13-14]都采用擴展卡爾曼濾波方法,這類方法存在線性化及更新效率等問題,難以應(yīng)用于大規(guī)模環(huán)境地圖的創(chuàng)建[15].與濾波方法中每一步只考慮機器人當(dāng)前位姿不同,基于平滑的方法通過所有的觀測信息估計機器人完整的運動軌跡及地圖,因而也被稱為完全SLAM方法,地圖的一致性和精度較好.近年來,該類方法在基于點特征的視覺SLAM中得到廣泛的應(yīng)用,但是基于線特征的視覺SLAM方法中則很少使用該類方法,文獻[9]將SBA(sparse bundle adjustment)[16]應(yīng)用到基于線特征的運動恢復(fù)結(jié)構(gòu)(structure from motion,SfM)問題中.

綜上,考慮到非線性最小維度4-參數(shù)表達方式以及線性冗余維度(over-parameters)的優(yōu)點與不足,本文采用Plücker坐標與Cayley結(jié)合的線特征表達方式,一方面采用Plücker坐標便于線特征的線性投影計算,另一方面采用Cayley參數(shù)化方式有利于對線特征的位置進行非線性優(yōu)化.由此提出了一種基于圖優(yōu)化的單目線特征SLAM算法,重點闡述線特征的參數(shù)化、初始化及圖優(yōu)化.

1 3D線特征的參數(shù)化方式

本文的線特征采用Plücker坐標結(jié)合Cayley參數(shù)化的方式,下面分別對這2種方式進行說明,并給出2種參數(shù)化方式間的相互轉(zhuǎn)換方法,同時闡述了Plücker直線的初始化方法.

1.1 直線的Plücker坐標

假定直線上存在任意2點{A,B}∈R3,則該直線的Plücker坐標為一個6維向量L=(n;v),如圖1所示,其中,世界坐標的原點為O,n為直線的矩向量,方向垂直于原點與該直線確定的平面η,幅值為OAB所組成三角形面積的2倍,v為直線的方向向量,由A指向B.該坐標可由下式進行計算:

(1)

圖1 Plücker直線

直線的Plücker坐標具有如下性質(zhì):

1) 滿足Plücker約束v⊥n,即v·n=0;

1.1.1Plücker直線的坐標變換

令Lc表示Plücker直線在攝像機坐標系下的坐標,Lw表示Plücker直線在世界坐標系下的坐標,則直線在2個坐標系下的相互轉(zhuǎn)換關(guān)系為

Lw=HLc

(2)

(3)

式中,t為相機中心在世界坐標系下的三維位置坐標;R為一個3×3的旋轉(zhuǎn)矩陣,表示相機坐標系的朝向;H為變換矩陣.

將空間中的Plücker直線Lc投影到成像平面,得到直線l, 即

l=KRT(nw-t×vw)=Knc

(4)

(5)

式中,K為攝像機內(nèi)參數(shù)矩陣；αv,αu為像素焦距；u0，v0為主點坐標．根據(jù)式(4)可知,經(jīng)過投影變換后,直線Plücker坐標中僅nc是可觀測的,而vc包含了直線的深度及方向信息,經(jīng)過投影變換后都不可觀測.

1.1.2 Plücker直線的位置初始化

單目SLAM問題是一個部分可觀測的SLAM問題,從一幅圖像中無法獲得特征的完整位置信息,如線特征的深度及方向都無法通過單幅圖像計算出來,因此存在特征位置的初始化問題.特征位置初始化過程是指通過圖像中的特征觀測信息來獲得其3D位置信息.Sola等[17]提出了基于非遲延(undelayed)的初始化方法,并結(jié)合逆深度參數(shù)化[18]的方式對線特征進行了初始化,該方法的優(yōu)點在于在低視差的情況下也依然有效,且與遲延(delayed)類的初始化方法相比,能夠利用更多的特征信息.其初始化過程如下:

① 根據(jù)圖像中檢測到的線段z=(p1,p2),其中p1(p1x,p1y),p2(p2x,p2y)為線段在圖像中的2個二維端點坐標,計算該線段在圖像中的Plücker坐標l={l1,l2,l3}T,即

(6)

② 對l進行反投影變換,計算其所對應(yīng)的3D Plücker直線坐標Lc,即

nc=K-1l

(7)

l的反投影過程如圖2所示,但如前所述,Lc中只有nc子向量可測.

圖2 圖像中直線l的反投影示意圖[13]

由圖2可看出,l所對應(yīng)的3D空間直線必定位于l與攝像機中心確定的平面η上,因此,只要構(gòu)造一組平面η上的正交基ε={ξ1,ξ2},則向量vc便可由下式表示:

vc=β1ξ1+β2ξ2β1,β2∈R

(8)

(9)

式中,β={β1,β2}為對應(yīng)ε的2個基方向系數(shù),給定任意一組β都能得到一個向量vc?η,并且無論β取何值,都能滿足Plücker約束v⊥n.

1.2 直線的Cayley表征方式

由于直線的Plücker坐標表示方式存在Plücker約束v⊥n,不利于后端的非線性優(yōu)化過程．文獻[9]指出該約束只能在線特征的初始化時滿足,若直接使用Plücker坐標進行優(yōu)化,則在優(yōu)化過后無法保證該約束條件.因此,為了方便實現(xiàn)非線性優(yōu)化過程,本文結(jié)合了直線的Cayley表征方式.

Cayley表達式定義為一個4維向量W=(d,s),其中,d表示直線到坐標系原點的距離,s={sx,sy,sz}T表示該直線在空間中的旋轉(zhuǎn)信息,如圖3所示.

以下為將Plücker直線L轉(zhuǎn)換為Cayley直線

圖3 3D直線的Cayley表示[9]

的過程.

① 根據(jù)下式計算出Plücker直線L上距離坐標原點最近點P0:

P0=(v×n)/‖v‖2

(10)

m=P0×vn

(11)

③ 按照下式構(gòu)建正交矩陣Q:

(12)

根據(jù)下式計算出一個由向量s構(gòu)成的反對稱矩陣:

[s]×=(Q-I)(Q+I)-1

(13)

至此,Cayley直線的坐標表達式W=(d,s)已經(jīng)求出,但當(dāng)‖m‖=0時,該參數(shù)化方式容易存在奇異解,因此,需要進行修正以解決該問題,即

(14)

式中,i1,i2,vn為空間坐標系中的一組正交基,由約束(vn)T·i=0構(gòu)建;τ為接近于0的一個無限小值.當(dāng)‖m‖=0時,Q=[vn,i1,i2],從而解決了上述問題.

而將Cayley直線轉(zhuǎn)換為Plücker直線的過程亦需要構(gòu)造矩陣Q,可通過下式進行:

Q={q1,q2,q3}=(I-[s]×)-1(I+[s]×)=

(15)

根據(jù)Q的定義(12)，可得Plücker直線坐標為

vn=q1m=dq2

(16)

2 基于線特征的單目SLAM后端優(yōu)化

首先,對本文中的狀態(tài)量做如下定義:

1)X={xi(Ri,Ti),i∈0,1,…,M}表示攝像機的一系列位姿;

2)Γ={Lj=(nj;vj),j∈1,2,…,N}表示一系列線特征的Plücker坐標;

3)W={Wj=(dj,sj),j∈1,2,…,N}表示一系列線特征的Cayley參數(shù).

基于圖優(yōu)化的SLAM框架通常將求解過程分為前端和后端2個部分:前端完成圖的構(gòu)建，得到初始狀態(tài)估計;后端對初始狀態(tài)估計做進一步優(yōu)化,得到位姿的最大似然估計．在基于圖優(yōu)化的視覺SLAM中,相機的位姿和路標的位置表示為圖中的節(jié)點,觀測信息在經(jīng)過處理后轉(zhuǎn)變?yōu)檫B接節(jié)點之間的約束,也稱圖中的邊,本文采用因子圖模型來描述SLAM問題.

2.1 優(yōu)化中的誤差模型

在SLAM優(yōu)化問題中,主要用到相機的位姿預(yù)測誤差模型和線特征的投影誤差模型.

令ui(i∈1,2,…,M)表示相鄰2幀間的控制輸入量或測量得到的相對位姿,則位姿預(yù)測誤差模型為

e(xi,xi-1)=xi-f(xi-1,ui)

(17)

式中,xi和xi-1表示相機在第i時刻和i-1時刻的位姿;f(xi-1,ui)表示相機的運動模型,通常情況下,該模型是非線性函數(shù).

令zk(p1,p2),k∈1,2,…,K表示在圖像中線特征測量值.與點特征不同的是,線特征的投影誤差模型不再是簡單的特征投影預(yù)測值與測量值之間的差,在本文中,線特征的誤差計算模型為

(18)

式中,l={l1,l2,l3}T表示圖像中線特征的預(yù)測值,由式(4)計算得到.式(18)為一個2維的向量e=(e1,e2),直觀理解如圖4所示．

圖4 線特征的投影誤差計算示意圖

2.2 后端優(yōu)化

SLAM可以看作是一個非線性最小二乘問題,在前端完成整個因子圖的構(gòu)建后,后端對目標函數(shù)進行優(yōu)化,本文基于線特征的目標優(yōu)化函數(shù)為

(19)

式中,Θ(X,Γ)為需要優(yōu)化的狀態(tài)量,X{xi},Γ{Lj};ei(xi-1,xi)為第i時刻的相機運動估計誤差,通過式(17)計算;ek(xik,Ljk)為線特征的投影預(yù)測誤差,通過式(18)計算;(xik,Ljk)為第k個測量相機在位姿xi處對Lj的觀測;Ωi和Λk分別表示2個誤差的協(xié)方差矩陣.

通過求解優(yōu)化目標函數(shù)(19)，可得到一組最優(yōu)的相機位姿X和特征位置Γ,使得所有Ljk在xik中的投影位置與zk盡可能靠近.求解目標函數(shù)(19)的過程也稱為集束調(diào)整(bundle adjustment, BA)[19].Levenberg-Marquardt(LM)算法[20]是目前使用最廣泛的非線性最小二乘問題求解方法,其獨特的快速迭代性能和抑制局部最優(yōu)解的能力在視覺SLAM模型中具有更快的效率和更準確的精度.因此本文采用該算法進行優(yōu)化.

首先對式(19)中的誤差項進行線性近似,近似方法與基于點特征的SLAM相同.但如前所述,線特征的投影誤差模型較點特征復(fù)雜,因此近似過程中與線特征相關(guān)的Jacobian矩陣求解公式有所不同,線特征的參數(shù)更新過程亦不同,本文僅給出優(yōu)化過程中與線特征相關(guān)的Jacobian矩陣求解方法.

線特征在投影過程中采用Plücker坐標,而優(yōu)化更新過程采用Cayley參數(shù)化方式,此時投影誤差對線特征的Jacobian矩陣J的求解過程為

(20)

(21)

(22)

(23)

線特征的狀態(tài)更新式為

Wj+1=Wj+δW

3 實驗結(jié)果與分析

本文算法的實現(xiàn)中使用了GTSAM和MRPT兩個庫,系統(tǒng)配置為Windows 764位,12 GB內(nèi)存,2.5 GHz主頻,開發(fā)環(huán)境為Visual Studio 2013,用GTSAM庫搭建SLAM框架,MRPT庫實現(xiàn)可視化.

3.1 仿真實驗

進行仿真實驗方便將計算結(jié)果與真值進行對比,從而更好地評估實驗結(jié)果．本文首先采用如圖5(a)所示的仿真場景[21],一個由23根線段組成的簡易房屋,實驗中沒有考慮遮擋,所有線條可見．所模擬的相機視場FOV為90°,圖像尺寸為640×480像素,機器人以半徑為5 m繞模型移動一圈．相機方向始終朝向房屋,共拍攝180幀圖像．實驗中對每一幀仿真圖像中的線段端點坐標都添加了高斯噪聲,還模擬了機器人的碼盤定位,并為其添加高斯噪聲,以此作為運動預(yù)測.

(a) 模擬簡易房屋 (b) 重建后的房屋

圖6 重建模型的重投影誤差

圖7為在相同噪聲參數(shù)下,相機的真實運動路徑、碼盤估計路徑、EKF線特征SLAM算法估計路徑與本文算法估計的路徑對比圖.可以看出,使用本文SLAM方法的定位精度明顯比基于EKF算法所估計結(jié)果的精度高,而且隨著時間的推移，提高的程度越明顯,而碼盤估計有著嚴重的誤差累積.表1量化了各算法估計的位姿誤差平方和(SSE)和均方根誤差(RMSE),從表中也可以看出本文算法較EKF與碼盤估計的估計精度更高,且SSE和RMSE分別是里程計位姿估計的2.5%和10.5%,是基于EKF的線特征SLAM算法所估計位姿的22.4%和33%.

圖7 運動估計結(jié)果對比

參數(shù)里程計EKF算法本文算法SSE/m24863.3540.8120.87RMSE/m5.22701.67320.5513

3.2 實際圖像序列實驗

在實際圖像序列實驗中,采用目前流行的LSD (line segment detector)算法[22]作為圖像線特征的檢測算法,同時采用LBD (line band descriptor)[23]算法進行線特征的匹配.LBD算法在弱紋理、部分遮擋、圖像模糊、旋轉(zhuǎn)和尺度變化等條件下都能夠?qū)€特征進行較好的匹配.

本文的視頻序列圖像采用Logitech C270進行采集,圖像尺寸為640×480像素,實驗場景如圖8(a)所示,尺寸為120 cm×60 cm×60 cm,整個因子圖中共有位姿節(jié)點60個,路標節(jié)點400個,約束1 860個,整個過程運行時間45 s.圖8(b)為本文算法的重建結(jié)果,從圖中可以看出,場景中的小電塔模型沒有很好地構(gòu)建出來,是因為該模型中的線條太短,算法中對線段的長度設(shè)置了閾值40像素,當(dāng)圖像中長度小于40像素的線段則不做處理;另外,場景中的筆筒及筆也沒有很好地重建出來,是因為圖像中該部分的線特征沒有成功提取,因此建圖的質(zhì)量也依賴于圖像中線特征提取和匹配算法的性能.綜上,本文重建的線特征與場景基本一致,證明了本文算法的有效性.為了量化算法的精度,本文計算了所估計軌跡的誤差平方和為958 cm2,均方根誤差為3.941 3 cm,驗證了本文算法的高定位精度.

(a) 真實場景

(b) 重建結(jié)果

4 結(jié)論

1) 傳統(tǒng)的基于線特征的SLAM都采用基于濾波器的框架求解,本文采用基于圖優(yōu)化的SLAM解決方案來提高定位的精度及地圖構(gòu)建的一致性和準確性.

2) 本文結(jié)合了Plücker坐標和Cayley參數(shù)化方式來表征線特征,一方面采用Plücker坐標便于線性投影計算,另一方面采用Cayley方式有利于線特征位置的非線性優(yōu)化,解決了線特征在圖優(yōu)化方案中的表征難題.

3) 通過仿真實驗,將本文算法與碼盤估計及EKF線特征SLAM算法進行了對比,結(jié)果顯示本文算法的位姿估計誤差平方和及均方根誤差均小于碼盤估計及EKF線特征SLAM算法結(jié)果,同時,計算了本文算法構(gòu)建的線特征重投影誤差,從結(jié)果看出伴隨相機的運動,觀測量越來越多,重投影誤越來越小,從而說明本文算法具有較高的定位精度及建圖精度.

4) 通過實際圖像實驗分析了位姿估計的誤差平方和及均方根誤差,結(jié)果證明了本文算法的有效性、準確性及地圖構(gòu)建的一致性.

)

[1] Cadena C, Carlone L, Carrillo H, et al. Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age [J].IEEETransactionsonRobotics, 2016 ,32(6) :1309-1332.DOI:10.1109/TRO.2016.2624754.

[2] 段華旭,閆飛,莊嚴,等.無人車基于雙目視覺的同時定位與地圖構(gòu)建[J].華中科技大學(xué)學(xué)報(自然科學(xué)版),2015,43(z1):319-323. DOI:10.13245/j.hust.15S1076.

Duan Huaxu, Yan Fei, Zhuang Yan, et al. Simultaneous localization and mapping for ugvs with binocular camera[J].JournalofHuazhongUniversityofScienceandTechnology(NatureScienceEdition), 2015,43(z1): 319-323. DOI:10.13245/j.hust.15S1076.(in Chinese)

[3] 劉浩敏,章國鋒,鮑虎軍.基于單目視覺的同時定位與地圖構(gòu)建方法綜述 [J].計算機輔助設(shè)計與圖形學(xué)學(xué)報,2016,28(6):855-868. DOI:10.3969/j.issn.1003-9775.2016.06.001.

Liu Haomin, Zhang Guofeng, Bao Hujun. A survey of monocular simultaneous localization and mapping [J].JournalofComputer-AidedDesign&ComputerGraphics, 2016,28(6): 855-868. DOI:10.3969/j.issn.1003-9775.2016.06.001. (in Chinese)

[4] 周慧中.基于結(jié)構(gòu)線條的視覺 SLAM 方法[D].上海:上海交通大學(xué)電子信息與電氣工程學(xué)院,2015.

[5] Zhao L, Huang S, Yan L, et al. A new feature parametrization for monocular SLAM using line features [J].Robotica, 2015,33(3):513-536.DOI:10.1017/S026357471400040X.

[6] Esparza-Jiménez O, Devy M, Gordillo J L. Visual EKF-SLAM from heterogeneous landmarks [J].Sensors, 2016,16(4): 489.

[7] Roberts K S. A new representation for a line [C]//ComputerSocietyConferenceonproceedingsoftheComputerVisionandPatternRecognition. Ann Arbor, MI, USA, 1988:635-640.

[8] Ohwovoriole M S, Roth B. An extension of screw theory [J].JournalofMechanicalDesign, 1981,103(4):725-735. DOI:10.1115/1.3254979.

[9] Zhang L, Koch R. Structure and motion from line correspondences: Representation, projection, initialization and sparse bundle adjustment [J].JournalofVisualCommunicationandImageRepresentation, 2014,25(5): 904-915. DOI:10.1016/j.jvcir.2014.02.013.

[10] Smith P, Reid D, Davison A. Real-time monocular SLAM with straight lines [C]//ProceedingsoftheBMVC. Edinburgh, UK, 2006:17-26. DOI:10.5244/c.20.3.

[11] Gee A P, Mayol-Cuevas W. Real-time model-based SLAM using line segments [C]//ProceedingsoftheInternationalSymposiumonVisualComputing. Berlin, Heidelberg: Springer, 2006:354-363. DOI:10.1007/11919629_37.

[12] Montiel J M M,Tardós D, Montano L. Structure and motion from straight line segments [J].PatternRecognition, 2000,33(8):1295-1307. DOI:10.1016/s0031-3203(99)00117-x.

[13] Sola J, Vidal-Calleja T, Civera J, et al. Impact of landmark parametrization on monocular EKF-SLAM with points and lines [J].InternationalJournalofComputerVision, 2012,97(3):339-368.

[14] Lemaire T, Lacroix S. Monocular-vision based SLAM using line segments [C]//ProceedingsoftheProceedings2007IEEEInternationalConferenceonRoboticsandAutomation. Roma,Italy, 2007:2791-2796. DOI:10.1109/robot.2007.363894.

[15] 梁明杰,閔華清,羅榮華.基于圖優(yōu)化的同時定位與地圖創(chuàng)建綜述[J].機器人,2013,35(4):500-512. DOI:10.3724/SP.J.1218.2013.00500.

Liang Mingjie, Min Huaqing, Luo Ronghua. Graph-based SLAM: A survey [J].Robot, 2013,35(4): 500-512. DOI:10.3724/SP.J.1218.2013.00500. (in Chinese)

[16] Lourakis M I A, Argyros A A. SBA: A software package for generic sparse bundle adjustment [J].ACMTransactionsonMathematicalSoftware(TOMS), 2009,36(1):2-1-2-30.

[17] Sola J, Vidal-Calleja T, Devy M. Undelayed initialization of line segments in monocular SLAM [C]//Proceedingsofthe2009IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems. Louis, USA,2009: 1553-1558. DOI:10.1109/iros.2009.5354754.

[18] Civera J, Davison A J, Montiel J M. Inverse depth parametrization for monocular SLAM [J].IEEETransactionsonRobotics, 2008,24(5): 932-945. DOI:10.1109/tro.2008.2003276.

[19] Triggs B, Mclauchlan P F, Hartley R I, et al. Bundle adjustment—A modern synthesis [C]//ProceedingsoftheInternationalWorkshoponVisionAlgorithms. London, UK: Springer Springer-Verlag, 2000:298-372.

[20] Ranganathan A. The levenberg-marquardt algorithm [J].TutoralonLmAlgorithm, 2013,11(1):101-110.

[21] Solà J. Slam toolbox for matlab[EB/OL]. (2015-12)[2017-08].http://www.iri.upc.edu/people/jsola/JoanSola/eng/toolbox.html.

[22] von Gioi R G, Jakubowicz J, Morel J-M, et al. LSD: A line segment detector [J].ImageProcessingonLine, 2012,2:35-55. DOI:10.5201/ipol.2012.gjmr-lsd.

[23] Zhang L, Koch R. An efficient and robust line segment matching approach based on LBD descriptor and pairwise geometric consistency [J].JournalofVisualCommunicationandImageRepresentation,2013,24(7):794-805.DOI:10.1016/j.jvcir.2013.05.006.

Line-basedmonoculargraphSLAMalgorithm

Dong Ruifang Liu Changan Yang Guotian Cheng Ruiying

(School of Control and Computer Engineering, North China Electric Power University, Beijing 102206, China)

A new line based 6-DOF monocular algorithm for using graph simultaneous localization and mapping(SLAM) algoritm was proposed. First, the straight line were applied as a feature instead of points, due to a map consisting of a sparse set of 3D points is unable to describe the structure of the surrounding world. Secondly,most of previous line-based SLAM algorithms were focused on filtering-based solutions suffering from the inconsistent when applied to the inherently non-linear SLAM problem, in contrast, the graph-based solution was used to improve the accuracy of the localization and the consistency of mapping. Thirdly, a special line representation was exploited for combining the Plücker coordinates with the Cayley representation. The Plücker coordinates were used for the 3D line projection function, and the Cayley representation helps to update the line parameters during the non-linear optimization process. Finally, the simulation experiment shows that the proposed algorithm outperforms odometry and EKF-based SLAM in terms of the pose estimation, while the sum of the squared errors (SSE) and root-mean-square error (RMSE) of proposed method are 2.5% and 10.5% of odometry, and 22.4% and 33% of EKF-based SLAM. The reprojection error is only 45.5 pixels. The real image experiment shows that the proposed algorithm obtains only 958 cm2and 3.941 3 cm the SSE and RMSE of pose estimation. Therefore, it can be concluded that the proposed algorithm is effective and accuracy.

monocular slam; line feature; graph optimization; Plücker coordinates; Cayley representation

10.3969/j.issn.1001-0505.2017.06.003

TP242

A

1001-0505(2017)06-1094-07

2017-07-23.

董蕊芳(1989—),女,博士生;柳長安(聯(lián)系人),男,博士,教授,博士生導(dǎo)師,liuchangan@ncepu.edu.cn.

國家自然科學(xué)基金資助項目(61105083)、中央高校基本科研業(yè)務(wù)費專項資金資助項目(2015XS63).

董蕊芳,柳長安,楊國田,等. 基于圖優(yōu)化的單目線特征SLAM算法[J].東南大學(xué)學(xué)報(自然科學(xué)版),2017,47(6):1094-1100.

10.3969/j.issn.1001-0505.2017.06.003.