基于局內(nèi)點(diǎn)分布的圖優(yōu)化方法

翁秀玲,王云峰,吳　煒,郭東輝

(廈門大學(xué)信息科學(xué)與技術(shù)學(xué)院,福建 廈門 361005)

圖優(yōu)化是即時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)[1]的后端．它利用序列圖像的閉環(huán)對(duì)前端視覺里程計(jì)獲得的相機(jī)軌跡進(jìn)行優(yōu)化,對(duì)消除位姿估計(jì)的累積誤差和構(gòu)建一致性的地圖至關(guān)重要．

圖優(yōu)化問題通常被歸為非線性最小二乘問題,采用高斯-牛頓法(Gauss-Newton,GN)或列文伯格-馬夸爾特法(Levenberg-Marquardt,LM)進(jìn)行求解．其求解思路為在當(dāng)前解處對(duì)目標(biāo)函數(shù)進(jìn)行如一階泰勒展開近似的線性化操作,從而得到線性最小二乘方程．求解線性系統(tǒng)只需令導(dǎo)函數(shù)為零,通過迭代直至達(dá)到最大迭代次數(shù)或收斂．在迭代求解的問題上,研究者發(fā)現(xiàn)SLAM問題的稀疏性可以被充分利用,從而提高求解效率．Dellaert等[2]通過稀疏喬列斯基分解對(duì)線性系統(tǒng)進(jìn)行求解．Kaess等[3]對(duì)系統(tǒng)的信息矩陣作正交三角分解,并選擇性地對(duì)其進(jìn)行增量式更新,從而避免了每次重新計(jì)算系統(tǒng)的信息矩陣,提高了求解效率．Konolige等[4]提出了一種根據(jù)給定圖的約束關(guān)系快速構(gòu)造稀疏矩陣的方法．Kummerle等[5]公開了基于稀疏矩陣分解的通用圖優(yōu)化庫G2O,得到了廣泛的應(yīng)用．但迭代求解問題[2-5]對(duì)初始值較敏感,易落入局部極值;并且求解效率與稀疏矩陣的稀疏程度相關(guān),最佳情況下時(shí)間復(fù)雜度為O(n),最壞情況下時(shí)間復(fù)雜度為O(n3),無法保證實(shí)時(shí)優(yōu)化,不適合在線應(yīng)用．

針對(duì)非線性最小二乘問題迭代求解存在的效率和初值敏感問題,Dubbelman等[6-7]提出COP-SLAM方法(closed-form online pose-chain SLAM,COP-SLAM),可以實(shí)現(xiàn)實(shí)時(shí)優(yōu)化．該方法根據(jù)視覺里程計(jì)約束的信息矩陣設(shè)置位姿鏈中邊的權(quán)重,從而將單個(gè)閉環(huán)約束的誤差項(xiàng)分配到視覺里程計(jì)的總約束上，直接得到解析解．在保證位姿鏈優(yōu)化精度的情況下,大大地縮短了優(yōu)化所消耗的時(shí)間．該方法基于局內(nèi)點(diǎn)數(shù)量設(shè)置視覺里程計(jì)約束的信息矩陣,但是當(dāng)局內(nèi)點(diǎn)分布不均勻即集中位于圖像的特定區(qū)域時(shí),視覺里程計(jì)精度下降,故該信息矩陣不能很好地衡量視覺里程計(jì)的精度,進(jìn)而影響優(yōu)化精度．

為了進(jìn)一步提高COP-SLAM方法的優(yōu)化結(jié)果,本研究提出了基于局內(nèi)點(diǎn)分布的后端圖優(yōu)化方法．定義局內(nèi)點(diǎn)集的面積表示局內(nèi)點(diǎn)的分布,并通過局內(nèi)點(diǎn)分布結(jié)合局內(nèi)點(diǎn)數(shù)量設(shè)置位姿圖中約束邊的信息矩陣,提高了信息矩陣對(duì)視覺里程計(jì)精度的衡量準(zhǔn)確度．實(shí)驗(yàn)結(jié)果表明,該優(yōu)化方法有效地降低了絕對(duì)軌跡誤差．

1　COP-SLAM[7]

COP-SLAM[7]屬于輕量型的圖優(yōu)化算法,是根據(jù)單個(gè)閉環(huán)約束對(duì)環(huán)路內(nèi)的位姿節(jié)點(diǎn)進(jìn)行優(yōu)化．算法采用位姿鏈模型,設(shè)相機(jī)的位姿用4×4的矩陣Ai表示,連續(xù)位姿Ai-1和Ai之間的視覺里程計(jì)約束用4×4的變換矩陣Mi表示,則n時(shí)刻的位姿可由式(1)表示．式中相機(jī)的初始位姿A0一般設(shè)為4×4的單位矩陣I4×4．

(1)

圖1　COP-SLAM的優(yōu)化示意圖Fig.1 Optimization diagram of COP-SLAM

(2)

(3)

(4)

(5)

(6)

(7)

(8)

(9)

Mi←Mi×Ui.

(10)

(11)

2　基于局內(nèi)點(diǎn)分布的圖優(yōu)化方法

2.1　基于特征點(diǎn)分布的信息矩陣

視覺里程計(jì)的精度不僅受局內(nèi)特征點(diǎn)數(shù)目影響,還受局內(nèi)特征點(diǎn)分布影響,因此本研究根據(jù)局內(nèi)特征點(diǎn)的分布和數(shù)目設(shè)置信息矩陣．通過局內(nèi)特征點(diǎn)之間的面積來表示局內(nèi)特征點(diǎn)的分布,在局內(nèi)點(diǎn)數(shù)量的基礎(chǔ)上引入局內(nèi)點(diǎn)集的面積,提高了對(duì)視覺里程計(jì)精度的衡量．

COP-SLAM一般根據(jù)圖像特征局內(nèi)點(diǎn)數(shù)量N按式(12)對(duì)信息矩陣的σ2進(jìn)行設(shè)置,當(dāng)局內(nèi)點(diǎn)對(duì)分布不均勻,即局內(nèi)點(diǎn)集中分布于圖像的特定區(qū)域時(shí),視覺里程計(jì)精度下降,該σ2構(gòu)成的信息矩陣則不能很好地衡量視覺里程計(jì)的精確程度,影響優(yōu)化效果．

σ2=1/N.

(12)

對(duì)于2幀圖像的視覺里程計(jì)結(jié)果的準(zhǔn)確度可由平移誤差ε[8]衡量,

(13)

圖2　KITTI00匹配特征點(diǎn)Fig.2 Matched feature points of KITTI00

以KITTI數(shù)據(jù)集[10]為例,采用開源的雙目視覺里程計(jì)libviso2[11]對(duì)相機(jī)位姿進(jìn)行估計(jì)．圖2為雙目數(shù)據(jù)集KITTI00中2對(duì)連續(xù)圖像,圖2(a)和(b)所示的圓點(diǎn)為連續(xù)對(duì)圖像根據(jù)斑點(diǎn)和角點(diǎn)算子檢測(cè)出來并經(jīng)過匹配的特征點(diǎn),匹配特征點(diǎn)集中分布在圖像中上方,通過隨機(jī)抽樣一致性算法 (random sample consensus,RANSAC)[12]進(jìn)行位姿求解得到局內(nèi)點(diǎn)數(shù)目為128,根據(jù)式(13)得到視覺里程計(jì)的平移誤差為0.037 m．圖2(c)和(d)為另一對(duì)分布更加均勻匹配特征點(diǎn)的連續(xù)圖像對(duì),其局內(nèi)點(diǎn)數(shù)目為125,根據(jù)式(13)得到視覺里程計(jì)的平移誤差為0.011 m．由此可見,盡管圖2(a)和(b)中圖像設(shè)置的σ2小,但里程計(jì)的準(zhǔn)確度更差．故基于局內(nèi)點(diǎn)數(shù)量得到的信息矩陣并不能很好地衡量視覺里程計(jì)的精確程度．

在局內(nèi)點(diǎn)數(shù)量的基礎(chǔ)上,本研究考慮局內(nèi)點(diǎn)占據(jù)的平均面積并對(duì)信息矩陣進(jìn)行設(shè)置．雙目視覺里程計(jì)libviso2[11]首先通過斑點(diǎn)和角點(diǎn)算子對(duì)連續(xù)2幅圖像提取特征點(diǎn)并進(jìn)行匹配,獲得當(dāng)前圖像匹配點(diǎn)PMatch,再通過RANSAC[12]對(duì)PMatch進(jìn)行位姿估計(jì),得到局內(nèi)點(diǎn)集合PIn,如下所示:

PIn={P1,P2,…，PN},

(14)

其中,Pi的坐標(biāo)為(xi,yi)．

(15)

最后按式(16)設(shè)置信息矩陣唯一的參數(shù)σ2．

(16)

2.2　基于局內(nèi)點(diǎn)分布的圖優(yōu)化

基于局內(nèi)點(diǎn)分布的圖優(yōu)化算法通過閉環(huán)檢測(cè)獲得構(gòu)成閉環(huán)的兩個(gè)位姿節(jié)點(diǎn),采用視覺里程計(jì)得到2個(gè)位姿節(jié)點(diǎn)的變換矩陣,通過式(16)基于局內(nèi)點(diǎn)的平均分布面積計(jì)算該變換矩陣對(duì)應(yīng)的信息矩陣中的σ2．如果2個(gè)位姿節(jié)點(diǎn)為連續(xù)節(jié)點(diǎn),則通過式(1)根據(jù)Mi計(jì)算最后一個(gè)位姿節(jié)點(diǎn)的絕對(duì)位姿;如果2個(gè)位姿節(jié)點(diǎn)為非連續(xù)節(jié)點(diǎn),則根據(jù)Mm,n優(yōu)化環(huán)路內(nèi)位姿節(jié)點(diǎn)．

(17)

其中,R為3×3階的旋轉(zhuǎn)矩陣,t為3×1階的平移向量．

由式(7)得到的更新量Ui也可以表示成如式(18)所示的分塊矩陣．

(18)

其中,UR為3×3階的旋轉(zhuǎn)矩陣,Ut為3×1階的平移向量．

3　實(shí)驗(yàn)結(jié)果與分析

圖像數(shù)據(jù)集KITTI[10]是雙目SLAM系統(tǒng)研究應(yīng)用最多的圖像集,包含4組圖像數(shù)據(jù)集的詳細(xì)信息,如表1所示．在此圖像集合上通過雙目視覺里程計(jì)libviso2[11]和閉環(huán)檢測(cè)算法[14]實(shí)現(xiàn)了基于局內(nèi)點(diǎn)分布的圖優(yōu)化,最終輸出相機(jī)運(yùn)動(dòng)軌跡,如圖3所示．

表1　數(shù)據(jù)集的詳細(xì)信息

軌跡的度量標(biāo)準(zhǔn)是絕對(duì)軌跡誤差(absolute trajectory error,ATE)[15],其計(jì)算式如式(19)所示,是對(duì)估計(jì)軌跡與真實(shí)軌跡之間的絕對(duì)整體偏差進(jìn)行度量．

(19)

其中,tOi表示真實(shí)位姿的平移向量,tei表示估計(jì)位姿的平移向量．

以圖2為例,以局內(nèi)點(diǎn)數(shù)目作為信息矩陣的設(shè)置方法,優(yōu)化后圖2(a)和(b)的絕對(duì)位姿的平移誤差分別為3.425和3.426 m,圖2(c)和(d)的絕對(duì)位姿的平移誤差分別為2.231和2.281 m;而以局內(nèi)點(diǎn)數(shù)目結(jié)合分布設(shè)置信息矩陣,則得到圖2(a)和(b)的絕對(duì)位姿的平移誤差分別為3.268和3.272 m,圖2(c)和(d)的絕對(duì)位姿的平移誤差分別為1.977和2.026 m．因此,基于局內(nèi)點(diǎn)分布的圖優(yōu)化方法優(yōu)化效果好于COP-SLAM．

視覺里程計(jì)得到的相機(jī)運(yùn)動(dòng)軌跡、相機(jī)的真實(shí)軌跡以及本研究優(yōu)化后得到的相機(jī)運(yùn)動(dòng)軌跡如圖4所示．可以看出,基于局內(nèi)點(diǎn)分布的圖優(yōu)化得到的運(yùn)動(dòng)軌跡更加接近于真實(shí)軌跡,取得了優(yōu)化效果．

基于局內(nèi)點(diǎn)分布的SLAM后端圖優(yōu)化算法所需時(shí)間如表2所示,在KITTI數(shù)據(jù)集上的平均優(yōu)化時(shí)間約為G2O的1/10,與COP-SLAM相當(dāng),故仍屬于輕量型后端圖優(yōu)化算法,可實(shí)現(xiàn)實(shí)時(shí)優(yōu)化．

本研究所提出的圖優(yōu)化算法與其他一些優(yōu)化算法性能比較如表3所示,優(yōu)化結(jié)果不如LSD-SLAM[16],原因在于非線性迭代優(yōu)化具有更好的優(yōu)化效果,但該優(yōu)化運(yùn)行速度僅為本研究的1/50[7]．在所有數(shù)據(jù)集上絕對(duì)軌跡誤差均比Frost[17]的小,因?yàn)镕rost采用單目視覺里程計(jì)估計(jì)相機(jī)運(yùn)動(dòng),具有尺度不確定性,而本研究采用雙目視覺里程計(jì),無此問題．與同樣屬于輕量型優(yōu)化算法的COP-SLAM[7]相比,由于提高了對(duì)視覺里程計(jì)精度的衡量,因此4組圖像的優(yōu)化效果均有提升．

圖4　本研究優(yōu)化結(jié)果Fig.4 Optimization result of this research

數(shù)據(jù)集時(shí)間/msG2OCOP-SLAM本研究KITTI0025030.431KITTI0229614.715KITTI0512514.715KITTI066214.715

表3　KITTI的絕對(duì)軌跡誤差

4　結(jié)　論

圖優(yōu)化作為SLAM的重要環(huán)節(jié),對(duì)減小移動(dòng)相機(jī)的累積誤差,實(shí)現(xiàn)地圖的一致性至關(guān)重要．為了提高優(yōu)化效果,本研究提出了基于局內(nèi)點(diǎn)分布的后端圖優(yōu)化方法．在計(jì)算信息矩陣時(shí),通過局內(nèi)特征點(diǎn)之間的面積來表示局內(nèi)特征點(diǎn)的分布,在局內(nèi)點(diǎn)數(shù)量的基礎(chǔ)上引入局內(nèi)點(diǎn)集的面積,提高了信息矩陣對(duì)視覺里程計(jì)精確度的衡量度．在標(biāo)準(zhǔn)雙目圖像數(shù)據(jù)集上進(jìn)行了實(shí)驗(yàn),結(jié)果表明,提出的優(yōu)化方法有效地降低了絕對(duì)軌跡誤差．

參考文獻(xiàn)：

[1] CADENA C,CARLONE L,CARRILLO H,et al.Past,present,and future of simultaneous localization and mapping:towards the robust-perception age[J].IEEE Transactions on Robotics,2016,32(6):1309-1332.

[2]DELLAERT F,KAESS M.Square root SAM:simultaneous localization and mapping via square root information smoothing[J].International Journal of Robotics Research,2006,25(12):1181-1203.

[3]KAESS M,RANGANATHAN A,DELLAERT F.ISAM:incremental smoothing and mapping[J].IEEE Transactions on Robotics,2008,24(6):1365-1378.

[4]KONOLIGE K,GRISETTI G,KUMMERLE R,et al.Efficient sparse pose adjustment for 2D mapping[C]∥IEEE International Conference on Intelligent Robots and Systems.Taiwan:IEEE,2010:22-29.

[5]KUMMERLE R,GRISETTI G,STRASDAT H,et al.G2O:a general framework for graph optimization[C]∥IEEE International Conference on Robotics and Automation.Piscataway:IEEE,2011:3607-3613.

[6]DUBBELMAN G,BROWNING B.Closed-form online pose-chain SLAM[C]∥IEEE International Conference on Robotics and Automation.Karlsruhe:IEEE,2013:5190-5197.

[7]DUBBELMAN G,BROWNING B.COP-SLAM:closed-form online pose-chain optimization for visual SLAM[J].IEEE Transactions on Robotics,2015,31(5):1194-1213.

[8]SUNDERHAUF N.Robust optimization for simultaneous localization and mapping[D].Chemnitz:Chemnitz University of Technology,2012:13-193.

[9]THRUN S,LIU Y,KOLLER D,et al.Simultaneous localization and mapping with sparse extended information filters[J].The International Journal of Robotics Research,2004,23(7/8):693-716.

[10]GEIGER A,LENZ P,STILLER C,et al.Vision meets robotics:the KITTI dataset[J].International Journal of Robotics Research,2013,32(11):1231-1237.

[11]GEIGER A,ZIEGLER J,STILLER C.StereoScan:dense 3d reconstruction in real-time[C]∥IEEE Intelligent Vehicles Symposium.Baden-Baden:IEEE,2011:963-968.

[12]HAST A,NYSJ? J,MARCHETTI A.Optimal RANSAC：towards a repeatable algorithm for finding the optimal set[J].Journal of Wscg,2013,21(1):21-30.

[13]BARBER C B,DOBKIN D P,HUHDANPAA H.The quickhull algorithm for convex hulls[J].Acm Transactions on Mathematical Software,1998,22(4):469-483.

[14]LABBE M,MICHAUD F.Appearance-based loop closure detection for online large-scale and long-term operation[J].IEEE Transactions on Robotics,2013,29(3):734-745.

[15]STURM J,ENGELHARD N,ENDRES F,et al.A benchmark for the evaluation of RGB-D SLAM systems[C]∥IEEE International Conference on Intelligent Robots and Systems.Algarve:IEEE,2012:573-580.

[16]ENGEL J,STüCKLER J,CREMERS D.Large-scale direct SLAM with stereo cameras[C]∥IEEE International Conference on Intelligent Robots and Systems.Hamburg:IEEE,2015:1935-1942.