一種自適應(yīng)特征地圖匹配的改進(jìn)VSLAM算法

張峻寧 蘇群星 劉鵬遠(yuǎn) 朱慶 張凱

隨著機(jī)器人技術(shù)的深入發(fā)展,基于視覺的同時(shí)定位與地圖構(gòu)建(Visual simultaneous localization and mapping,VSLAM)的技術(shù)具有越來越多的應(yīng)用[1?2].VSLAM 技術(shù)同時(shí)需要機(jī)器人的三維環(huán)境信息和對應(yīng)的軌跡信息,因此VSLAM技術(shù)對機(jī)器人的定位實(shí)時(shí)性和精度要求較高.

目前,機(jī)器人精確的位姿估計(jì)主要依靠輪式里程計(jì)(Wheel odometry)[3]、全球定位系統(tǒng)(GPS)[4]、慣性導(dǎo)航系統(tǒng)(Inertial measurement unit,IMU)[5]以及視覺里程計(jì) (Visual odometry,VO)[6?9]等.隨著稀疏矩陣和非線性優(yōu)化理論的完善,基于VO技術(shù)的SLAM 得到越來越廣泛的發(fā)展[10?12],出現(xiàn)了一些經(jīng)典的實(shí)現(xiàn)方案,如LSD-SLAM、ORBSLAM、RGBD-SLAM 等算法.VO主要是基于視覺原理,通過相鄰兩幀的圖像信息計(jì)算相機(jī)的相對位姿.一般而言,SLAM 的VO計(jì)算主要分為兩類.一類是基于稀疏特征點(diǎn)的VO,另一類是稠密的方法.基于稀疏特征的VO根據(jù)SIFT(Scale invariant feature transform)、SURF(Speeded up robust features)、ORB等特征點(diǎn)的匹配建立幀間聯(lián)系.比如,Henry等[12]利用RANSAC求解兩幀圖像間的變換矩陣,然后利用迭代最近點(diǎn)算法(Iterative closest point,ICP)求解兩幀深度圖像間的最優(yōu)變換;Endres等[13]等在RANSAC求解變換矩陣的基礎(chǔ)上,通過圖優(yōu)化器g2o優(yōu)化當(dāng)前幀的位姿(RGBD-SLAM-V2),得到了全局一致的地圖;Huang等[11]借鑒立體視覺里程計(jì),設(shè)計(jì)了一套基于Kinect的視覺里程計(jì)(Fast odometry from vision,FOVIS);為了提高位姿估計(jì)的精度,Dryanovski等[14]提出了一種幀到特征地圖的視覺里程計(jì)(Fast visual odometry,FVO),實(shí)現(xiàn)了單幀對特征地圖點(diǎn)的位姿估計(jì).高翔等[15]在FVO基礎(chǔ)上,提出了一種g2o(General graph optimization)優(yōu)化的幀到特征地圖的視覺里程計(jì),為后端和建圖提供了更準(zhǔn)確的位姿信息.Cao等[16]在提取的關(guān)鍵幀的重疊區(qū)域,通過塊匹配重建重疊區(qū)域,提取了少于5%的關(guān)鍵幀,有效地構(gòu)建了關(guān)鍵幀的地圖匹配.Martins等[17]提出了一種基于RGB和ICP點(diǎn)對平面誤差項(xiàng)調(diào)節(jié)的激活函數(shù),有效降低了前端視覺里程計(jì)的誤差影響,但是付出了更多運(yùn)算時(shí)間.Wadenbck等[18]利用相鄰兩幀的平面特征,簡化了參數(shù),提高了效率,但是不適合復(fù)雜的場景.稠密方法最早是由Steinbrcker等[19]提出的,在圖像一致性的假設(shè)下,相比于特征點(diǎn)法能夠充分利用像素信息,但缺點(diǎn)是需要耗費(fèi)大量計(jì)算資源,通常需要GPU加速.

綜上所述,目前已有方法的視覺里程計(jì)大多還是精度不夠,或者對硬件要求較高.為此,本文在幀到特征地圖配準(zhǔn)的基礎(chǔ)上,針對特征地圖數(shù)據(jù)量大、耗費(fèi)計(jì)算資源的缺陷,提出一種自適應(yīng)特征地圖的改進(jìn)SLAM算法.首先,以劃分的特征地圖的子區(qū)域?yàn)樽钚卧?根據(jù)特征角點(diǎn)響應(yīng)強(qiáng)度提取少量高效的特征點(diǎn),并進(jìn)行各幀配準(zhǔn);其次,為應(yīng)對小規(guī)模地圖出現(xiàn)的配準(zhǔn)失蹤的情況,提出一種子區(qū)域特征點(diǎn)補(bǔ)充和特征地圖擴(kuò)建的方法,快速實(shí)現(xiàn)當(dāng)前幀的再次匹配;最后,為了進(jìn)一步提高VO的位姿估計(jì)精度,增加特征地圖優(yōu)化環(huán)節(jié),且在該環(huán)節(jié)中提出一種幀到幀、幀到特征地圖的局部圖優(yōu)化模型,并通過g2o實(shí)現(xiàn)了相機(jī)位姿和地圖特征點(diǎn)的同時(shí)優(yōu)化.

1 VSLAM算法框架

本文通過圖優(yōu)化的方式實(shí)現(xiàn)VSLAM算法,主要包括三部分:視覺里程計(jì)、圖優(yōu)化以及地圖構(gòu)建,其中視覺里程計(jì)是利用本文提出的自適應(yīng)特征地圖匹配完成,框架如圖1所示.

1.1 視覺里程計(jì)

視覺里程計(jì)(VO)的主流方法是特征點(diǎn)法,但是該方法仍存在各幀特征點(diǎn)錯(cuò)誤匹配率高的問題.為了提高VO的計(jì)算精度、減少誤匹配,本文在VSLAM的框架中,加入了自適應(yīng)特征地圖,將各幀與自適應(yīng)規(guī)模的特征地圖配準(zhǔn),在減少計(jì)算量的同時(shí),提高了VO的位姿估計(jì)精度.

常用的特征點(diǎn)有:SIFT、SURF、ORB等[15,20],考慮到實(shí)時(shí)性要求,本文選擇ORB算子進(jìn)行特征提取與匹配.ORB算子于2011年由Rublee等[21]提出,其保持了特征具有的旋轉(zhuǎn)和尺度不變性的同時(shí),速度效率提升明顯.在ORB的基礎(chǔ)上結(jié)合漢明距離,利用BruteForceMatcher匹配特征地圖與各幀的特征點(diǎn),并針對誤匹配、匹配點(diǎn)數(shù)不足的情形,分別利用區(qū)域特征點(diǎn)補(bǔ)充和地圖擴(kuò)建的方式再次匹配各幀特征點(diǎn);最后,利用匹配的特征點(diǎn)計(jì)算內(nèi)點(diǎn)和變換矩陣,并計(jì)算幀間相對運(yùn)動(dòng)距離.

圖1 VSLAM算法框架Fig.1 Framework of VSLAM algorithm

1.2 圖優(yōu)化

VO完成后,可通過位姿圖優(yōu)化相機(jī)位姿,常用的方法有:基于非線性的最小二乘優(yōu)化、基于松弛的優(yōu)化方法、隨機(jī)梯度下降法以及流行優(yōu)化方法.前三類方法都在歐氏空間下計(jì)算但實(shí)際的相機(jī)位姿表達(dá)是非歐氏空間的,利用此類方法容易產(chǎn)生歐氏空間的奇異值,因此,可用四元數(shù)表示相機(jī)位姿并通過流行空間優(yōu)化位姿.g2o是Kmmerle等[22]提出的一種用于優(yōu)化流行空間的開源工具包,具有更高的效率和開發(fā)精度.本文在VSLAM 算法中,為了進(jìn)一步提高各幀位姿的精度,不僅利用g2o進(jìn)行后端圖優(yōu)化[15],而且在前端也利用g2o優(yōu)化特征地圖的內(nèi)外點(diǎn)以及前端位姿估計(jì).

1.3 地圖構(gòu)建

三維空間表示有很多方法,主要為3D點(diǎn)云圖和3D octomap[23]地圖.3D點(diǎn)云圖在表示地圖環(huán)境時(shí)清楚、直觀,但是冗余信息較多且占據(jù)大量內(nèi)存,不適合復(fù)雜的環(huán)境和終生SLAM(Life-long SLAM)算法.相反3D octomap地圖則避免了上述缺點(diǎn),通過較小的內(nèi)存空間直觀的表示地圖中占據(jù)、空閑和未知區(qū)域,更有利于機(jī)器人的導(dǎo)航和路徑規(guī)劃.

2 自適應(yīng)特征地圖構(gòu)建和幀到幀、幀到模型的g2o的特征地圖優(yōu)化

視覺里程計(jì)是通過幀間的匹配信息估計(jì)相機(jī)的位姿變化,為了提高VO的位姿估計(jì)精度,一般將當(dāng)前幀與歷史幀集做特征點(diǎn)匹配,但是需要多次匹配且計(jì)算量大,耗費(fèi)了大量計(jì)算資源.本文在Dryanovski提出的模型[14]基礎(chǔ)上,以較小規(guī)模維護(hù)特征地圖.首先,針對小規(guī)模特征地圖配準(zhǔn)失蹤的情況,補(bǔ)充子區(qū)域特征點(diǎn)和特征地圖擴(kuò)建,在保證位姿精度的同時(shí)縮短了運(yùn)算時(shí)間;其次,提出一種幀到幀、幀到特征地圖的圖優(yōu)化模型更新當(dāng)前幀的位姿和地圖特征點(diǎn)位置.

2.1 自適應(yīng)特征地圖構(gòu)建

在不考慮光照、遮擋的特殊情況下,特征地圖的特征點(diǎn)是豐富的,除了特征地圖與各幀匹配的特征點(diǎn),大部分特征點(diǎn)是冗余的,例如地圖中不在當(dāng)前幀視野內(nèi)或者視野角度過大的特征點(diǎn)等.為此,本文從VSLAM 的實(shí)時(shí)性出發(fā),研究如何維護(hù)一個(gè)低規(guī)模的特征地圖,使得滿足各幀的正確匹配點(diǎn)集,以及提出一種未滿足配準(zhǔn)時(shí)當(dāng)前幀的快速再次配準(zhǔn)的方法.

2.1.1 特征地圖構(gòu)建

假設(shè)Kinect在第k時(shí)刻采集的數(shù)據(jù)集為Ik,設(shè)利用特征提取和相機(jī)投影模型得到的特征點(diǎn)集為Lk(li,i=1,···,n),其中每個(gè)特征點(diǎn)為為第i個(gè)特征點(diǎn)在相機(jī)坐標(biāo)系下的位置向量,具體計(jì)算見式(1);是從OpenCV的SIFT庫檢測到的該特征點(diǎn)的角點(diǎn)響應(yīng)強(qiáng)度,表征該特征點(diǎn)的角點(diǎn)強(qiáng)度大小.

式中,[u,v]為第i個(gè)特征點(diǎn)的像素位置,fx,fy,cx,cy為相機(jī)內(nèi)參,d為Kinect采集的對應(yīng)像素深度值.

同理,構(gòu)造特征地圖M=[mi],特征點(diǎn)mi=.不僅包含第i個(gè)特征點(diǎn)的位置向量和特征角點(diǎn)響應(yīng)強(qiáng)度,還有該特征點(diǎn)的ORB描述子,以及歷史匹配次數(shù).另外,需要強(qiáng)調(diào)的是,特征地圖初始化是由最初的前k?1個(gè)時(shí)刻的特征點(diǎn)組成,并且全部都是以世界坐標(biāo)系為參考(本文以第一幀為世界坐標(biāo)系).隨著相機(jī)運(yùn)動(dòng),需要往地圖里實(shí)時(shí)添加新的特征點(diǎn).為了能夠讓VO做得更快、節(jié)約計(jì)算資源,利用VSLAM 中已有的關(guān)鍵幀線程,直接將距離當(dāng)前幀近的關(guān)鍵幀的特征點(diǎn)添加到特征地圖中,當(dāng)特征地圖達(dá)到一定規(guī)模之后,再把距離當(dāng)前幀遠(yuǎn)的特征點(diǎn)篩除,具體過程如下:

步驟1.設(shè)幀間的旋轉(zhuǎn)向量r和平移向量t,可求解出各幀間的相對運(yùn)動(dòng)距離:.利用D添加特征地圖:

1)若Dmin≤D≤Dmax,則Framecurr=Framekey;

2)若D

3)若D>Dmax,則Framecurrkey.其中,Framecurr是當(dāng)前幀,Framekey是關(guān)鍵幀,Dmin、Dmax分別是兩幀之間的最小和最大的運(yùn)動(dòng)距離,只有滿足條件(1)的當(dāng)前幀才被選作關(guān)鍵幀,該關(guān)鍵幀的特征點(diǎn)才能被添加至特征地圖中.

步驟2.設(shè)特征地圖中特征點(diǎn)個(gè)數(shù)為,特征地圖規(guī)模閾值為γ,當(dāng)時(shí),將特征地圖中最早添加的幀丟掉.

2.1.2 自適應(yīng)的特征地圖維護(hù)

特征地圖中冗余特征點(diǎn)的匹配的計(jì)算量是巨大的,一般CPU無法滿足SLAM的實(shí)時(shí)性要求.為此,本文提出一種自適應(yīng)規(guī)模的特征地圖用于幀間匹配,該特征地圖以較小的規(guī)?？焖俚赜?jì)算各幀配準(zhǔn),即使未能正常匹配,該特征地圖也能通過子區(qū)域特征點(diǎn)補(bǔ)充和軌跡預(yù)測的地圖擴(kuò)建方法快速再次匹配,具體地圖構(gòu)維護(hù)如下:

1)剔除特征地圖中不在視野內(nèi)的點(diǎn)

計(jì)算地圖特征點(diǎn)pM在位姿Ti+1下的像素位置:中特征信息冗余、匹配時(shí)間增加,不能滿足實(shí)時(shí)性的要求,若取值過小則不能充分利用特征地圖信息,造成當(dāng)前幀與特征地圖配準(zhǔn)個(gè)數(shù)偏少,計(jì)算的位姿偏差,甚至視覺里程計(jì)追蹤丟失的情況.因此θ的取值應(yīng)適宜,依據(jù)經(jīng)驗(yàn)常選取為[0.5～0.7](具體特征地圖的有效因子實(shí)驗(yàn)見第3.2節(jié)),但是實(shí)際工況復(fù)雜,很有可能相鄰兩幀之間的旋轉(zhuǎn)、平移距離較遠(yuǎn)或者一些相機(jī)旋轉(zhuǎn)抖動(dòng)的特殊情況,造成當(dāng)前幀與減小規(guī)模后的特征地圖匹配點(diǎn)數(shù)偏少,甚至VO追蹤丟失.為此,本文對此類情形進(jìn)一步討論,研究各幀配準(zhǔn)點(diǎn)較少時(shí)的特征地圖的擴(kuò)充方法.

上述算法中,length rgb,widthrgb分別為各幀圖像的長、寬.需要注意的是,由于當(dāng)前的位姿估計(jì)不準(zhǔn)確,一般將視野比例擴(kuò)大.因此,通過上述算法能夠初步從特征地圖中提取出在第i+1幀視野內(nèi)的特征點(diǎn).

2)基于視野區(qū)域分塊的特征地圖構(gòu)建

由于特征地圖是由各參考幀為地圖貢獻(xiàn)的一些信息組成,那么在相同的視野區(qū)域內(nèi),不同參考幀貢獻(xiàn)的信息就存在冗余.為了剔除冗余信息,本文對視野區(qū)域內(nèi)的特征地圖分塊,依據(jù)角點(diǎn)響應(yīng)強(qiáng)度(越大,該特征點(diǎn)被匹配的概率越高)對地圖子區(qū)域中的特征點(diǎn)排序,然后將部分角點(diǎn)強(qiáng)度大的特征點(diǎn)集作為特征地圖,具體步驟為:

步驟1.對每幀圖像區(qū)域分塊,即均勻地將圖像分成m×n塊,并對各子區(qū)域編號見圖2所示;再根據(jù)特征點(diǎn)的像素位置,將特征點(diǎn)標(biāo)記到對應(yīng)的子區(qū)域中.

步驟2.利用對子區(qū)域中的特征點(diǎn)排序,并從各子區(qū)域中提取θDj(0<θ<1)個(gè)特征點(diǎn)組成第i+1幀的特征地圖,其中Dj是j區(qū)域的特征點(diǎn)個(gè)數(shù),θ為區(qū)域特征有效因子.

按照上述算法構(gòu)建的特征地圖,有效地利用了各參考幀中易被匹配的特征點(diǎn),在保證位姿估計(jì)精度的同時(shí)縮減了運(yùn)算時(shí)間,但是在該算法中θ的取值受到大小兩端的限制,若θ取值過大將造成地圖

圖2 子區(qū)域劃分Fig.2 Division of sub regions

3)配準(zhǔn)點(diǎn)較少時(shí)的特征地圖擴(kuò)充

設(shè)定各幀與特征地圖的配準(zhǔn)個(gè)數(shù)為q,配準(zhǔn)閾值為λ,則造成當(dāng)前幀與特征地圖配準(zhǔn)個(gè)數(shù)q小于λ的原因可能是:a)取值偏小,造成特征地圖中對當(dāng)前幀的有效特征數(shù)量偏少;b)當(dāng)前幀的幀間運(yùn)動(dòng)Ti,i+1與前一幀的幀間運(yùn)動(dòng)Ti?1,i相等的假設(shè)不成立,即幀間運(yùn)動(dòng)相差較大,造成特征地圖中保留的是假視野內(nèi)的特征點(diǎn).

對于a)情形,雖然特征地圖與當(dāng)前幀的配準(zhǔn)點(diǎn)數(shù)較少,但是匹配得到的配準(zhǔn)點(diǎn)是正確的,根據(jù)當(dāng)前幀與特征地圖的配準(zhǔn)點(diǎn)分布,從對應(yīng)的子區(qū)域和相鄰子區(qū)域中補(bǔ)充歷史匹配次數(shù)多的特征點(diǎn)(越大,該特征點(diǎn)被匹配的概率越高).由于各子區(qū)域的位置分布不同,其補(bǔ)充特征點(diǎn)的方式也不相同,如圖3所示.其中圖3(a)是當(dāng)配準(zhǔn)點(diǎn)分布在對應(yīng)的視野邊界角區(qū)域(對應(yīng)圖1中1,n,m,mn子區(qū)域)時(shí)的地圖特征點(diǎn)補(bǔ)充方式,基于子區(qū)域特征點(diǎn)補(bǔ)充的最大原則,考慮到邊界角區(qū)域相鄰邊較少,在該區(qū)域的特征點(diǎn)補(bǔ)充從邊相鄰1,3區(qū)域以及角相鄰的2區(qū)域補(bǔ)充;圖3(b)是配準(zhǔn)點(diǎn)分布在邊界區(qū)域時(shí)的特征點(diǎn)補(bǔ)充方式,直接從邊相鄰區(qū)域1,2,3補(bǔ)充;同理,圖3(c)是配準(zhǔn)點(diǎn)在中間區(qū)域時(shí)的特征點(diǎn)補(bǔ)充方式,直接從邊相鄰區(qū)域1,2,3,4補(bǔ)充.

針對a)情形,按照圖3的補(bǔ)充方式能夠有效地補(bǔ)充地圖特征點(diǎn),再將補(bǔ)充的特征點(diǎn)與當(dāng)前幀做匹配,加上首次的配準(zhǔn)點(diǎn),就能夠快速完成當(dāng)前幀的特征點(diǎn)匹配.

當(dāng)然a)的地圖特征補(bǔ)充方式是不適合b)情形的.當(dāng)?shù)趇+1幀的幀間運(yùn)動(dòng)Ti,i+1與前一幀的幀間運(yùn)動(dòng)Ti?1,i相差較大,則之前假設(shè)Ti,i+1=Ti?1,i而構(gòu)建的特征地圖是不合適的.為此,本文提出一種基于位姿預(yù)測的特征地圖擴(kuò)建方法.首先提出一種簡單有效的位姿預(yù)測,利用預(yù)測的位姿尋找對應(yīng)地圖中的視野區(qū)域,再將該區(qū)域的特征點(diǎn)與當(dāng)前幀匹配,具體過程如下:

圖3 各子區(qū)域的特征點(diǎn)補(bǔ)充Fig.3 Complement each region of feature points

i)增大地圖視野.設(shè)地圖視野緩沖長度β,將地圖視野分別從長寬兩端增加為length rgb+2β,width_rgb+2β,然后按照1)中步驟剔除特征地圖中不在視野內(nèi)的特征點(diǎn),則增大的地圖視野見圖4所示,其中p,e,b,w分別作為特征地圖的增大區(qū)域的標(biāo)記,其編號按照順時(shí)針方向編排.

圖4 擴(kuò)建特征地圖Fig.4 Extension feature map

圖5 各區(qū)域的特征地圖擴(kuò)建方式Fig.5 Characteristic map expansion methods in different regions

圖5是配準(zhǔn)點(diǎn)分布在不同區(qū)域的地圖擴(kuò)建方式,其中圖5(a)是配準(zhǔn)點(diǎn)分布在視野邊界角區(qū)域(對應(yīng)圖1中編號為1,n,mn?m+1,mn的子區(qū)域)時(shí)的地圖擴(kuò)建方式.基于地圖擴(kuò)建的最大原則,由于該配準(zhǔn)點(diǎn)在視野的左邊界角區(qū)域,只是基于該區(qū)域的配準(zhǔn)點(diǎn)估計(jì)該幀的最大位姿運(yùn)動(dòng)方向如圖5(a)中箭頭方向,那么包含該配準(zhǔn)點(diǎn)的最大視野應(yīng)包含邊相鄰和角相鄰1,2,3區(qū)域,因此該區(qū)域的擴(kuò)建地圖應(yīng)包圍相鄰的1,2,3區(qū)域.圖5(b)是配準(zhǔn)點(diǎn)在邊界區(qū)域時(shí)的特征地圖擴(kuò)建方式,當(dāng)配準(zhǔn)點(diǎn)位于邊界區(qū)域時(shí),那么該配準(zhǔn)點(diǎn)的最大視野應(yīng)包含圖中1,2,3區(qū)域,因此從邊相鄰區(qū)域1,2,3擴(kuò)建地圖.圖5(c)是配準(zhǔn)點(diǎn)對應(yīng)第二層邊界角區(qū)域時(shí)的特征地圖擴(kuò)建方式,從最大位姿運(yùn)動(dòng)考慮,該地圖特征點(diǎn)補(bǔ)充從1,2,3,4,5區(qū)域補(bǔ)充,但是因?yàn)樵撆錅?zhǔn)點(diǎn)是在第二層邊界角區(qū)域,相對于圖5(a)、圖5(b)的地圖擴(kuò)建的面積應(yīng)該線性減少,面積計(jì)算見式(2).同理,圖5(d)是配準(zhǔn)點(diǎn)在中間區(qū)域時(shí)的地圖擴(kuò)建方式,直接從邊相鄰區(qū)域1,2,3,4補(bǔ)充,地圖擴(kuò)建面積的計(jì)算見式(2).

其中,β是地圖視野緩沖長度,n是特征地圖子區(qū)域的邊長,inlier(i)表示第i配準(zhǔn)點(diǎn)所在的子區(qū)域編號,layer(i)為第i配準(zhǔn)點(diǎn)最靠近視野邊界所在的層數(shù),layers是視野中心到視野邊界的層數(shù),S1,S2,S3,S4為邊界區(qū)域編號的集合,分別為:S1={2,3,···,n?1},S3={mn?m+1,mn?m+2,···,mn?1},S2={2n,3n,···,mn?n},S4={n+1,2n+1,···,(m?2)n+1},S5是第二層邊界角區(qū)域編號的集合,即:S5={n+2,2n?1,mn?n?m+2,mn?n?1}.

針對b)情況,按照圖5的特征地圖擴(kuò)建方法能夠有效地補(bǔ)充正確視野下的特征點(diǎn),然后再將這部分特征點(diǎn)與當(dāng)前幀做匹配,加上之前匹配的配準(zhǔn)點(diǎn),就能夠完成當(dāng)前幀的特征點(diǎn)匹配.圖6給出了各幀的視覺里程計(jì)的配準(zhǔn)流程,其中pnp表示匹配的特征點(diǎn)數(shù),λ是各幀配準(zhǔn)點(diǎn)個(gè)數(shù)的閾值,curr代表當(dāng)前幀,update是更新配準(zhǔn)點(diǎn)所在的子區(qū)域編號.

2.2 基于幀到幀、幀到模型的g2o的特征地圖的更新

本文構(gòu)建的自適應(yīng)特征地圖與當(dāng)前幀做視覺里程計(jì),極大地縮短了匹配時(shí)間,但付出了匹配精度下降的代價(jià),為了提高位姿估計(jì)精度,減少累積漂移的問題,本文引入Kummerle提出的g2o圖優(yōu)化算法,提出一種幀到幀、幀到模型的g2o優(yōu)化特征地圖和相機(jī)位姿的優(yōu)化算法.

圖6 視覺里程計(jì)過程Fig.6 The process of visual odometry

首先,通過改進(jìn)的特征地圖與當(dāng)前幀的視覺里程計(jì),計(jì)算出轉(zhuǎn)換矩陣,見式(3)所示.

根據(jù)該轉(zhuǎn)換矩陣,將當(dāng)前幀與特征地圖建立起對應(yīng)關(guān)系,并定義當(dāng)前幀與特征地圖匹配的特征點(diǎn)為內(nèi)點(diǎn),未建立對應(yīng)關(guān)系的特征點(diǎn)稱為外點(diǎn),因此特征地圖點(diǎn)的更新分為內(nèi)點(diǎn)更新和外點(diǎn)更新.

外點(diǎn)的更新則是將各幀中未建立關(guān)系的特征點(diǎn)利用式(4)和式(5)轉(zhuǎn)換為世界坐標(biāo)系下的三維空間點(diǎn),然后直接導(dǎo)入特征地圖中.

其中,[u,v]是當(dāng)前幀中外點(diǎn)的像素坐標(biāo),d是Kinect測定的深度,K為相機(jī)內(nèi)參,pc,pw是該外點(diǎn)對應(yīng)相機(jī)坐標(biāo)系和世界坐標(biāo)系的坐標(biāo).

分析式(4)和式(5),在不考慮Kinect外在干擾的情況下,K、d可認(rèn)為是常量,則該外點(diǎn)的世界坐標(biāo)值只與當(dāng)前幀的轉(zhuǎn)換矩陣相關(guān),即更新的外點(diǎn)的精度直接取決于當(dāng)前幀的位姿估計(jì).因此,本文通過圖優(yōu)化器g2o構(gòu)建最小二乘問題,然后尋找最優(yōu)的相機(jī)位姿,使它最小化,如式(6)所示.

其中,K為相機(jī)內(nèi)參,ξ是相機(jī)位姿(R,t)對應(yīng)的李代數(shù),也稱為g2o中的節(jié)點(diǎn),ui是第i個(gè)外點(diǎn)的像素坐標(biāo),pi是第i個(gè)外點(diǎn)的世界坐標(biāo),表示反對稱矩陣,g2o中每個(gè)3D點(diǎn)在相機(jī)中的投影,以觀測方程來描述:zj=h(ξ,pj),具體的g2o優(yōu)化示意圖如圖7所示.

圖7 外點(diǎn)的g2o圖優(yōu)化Fig.7 g2o graph optimization of exterior points

對于內(nèi)點(diǎn)的更新,通常的做法是與外點(diǎn)的更新一樣,但是這樣構(gòu)建的g2o約束條件較少,優(yōu)化的精度不明顯.為此,本文將地圖點(diǎn)作為優(yōu)化變量考慮進(jìn)來,為了提高g2o的優(yōu)化精度,建立了當(dāng)前幀與相鄰的關(guān)鍵幀(為了方便敘述,稱其為歷史關(guān)鍵幀)以及特征地圖點(diǎn)的約束關(guān)系,提出一種幀到幀、幀到模型的g2o優(yōu)化模型,其基本思想將當(dāng)前幀與歷史關(guān)鍵幀建立匹配關(guān)系,因?yàn)闅v史關(guān)鍵幀中除了特征點(diǎn)還有各自的位姿信息,這樣能夠與當(dāng)前幀建立更豐富的約束關(guān)系,有利于提高g2o的優(yōu)化精度.

考慮到VSLAM的實(shí)時(shí)性要求,為了建立當(dāng)前幀與歷史關(guān)鍵幀的特征點(diǎn)對應(yīng)關(guān)系,本文在特征地圖點(diǎn)更新的過程中標(biāo)記特征點(diǎn)的來源幀信息,這樣在VO中,若當(dāng)前幀與特征地圖匹配成功,由于配準(zhǔn)點(diǎn)具有標(biāo)記的歷史關(guān)鍵幀信息,即可建立當(dāng)前幀與歷史關(guān)鍵幀的特征點(diǎn)匹配關(guān)系,再構(gòu)建當(dāng)前幀與歷史關(guān)鍵幀的優(yōu)化問題,并通過g2o求解,具體過程為:

步驟1.建立投影關(guān)系(假設(shè)參考幀的相機(jī)位姿為單位矩陣);

其中,L表示當(dāng)前幀,ref表示歷史關(guān)鍵幀,即:ref=L?1,L?1,···,L?c,c為歷史關(guān)鍵幀的數(shù)目,ξi,ξj分別表示第i和第j個(gè)相機(jī)位姿,?ξi,j表示ξi和ξj之間的運(yùn)動(dòng),dref,dL是對應(yīng)像素的深度,pi是世界坐標(biāo),uref,uL是兩個(gè)相機(jī)坐標(biāo)下的像素坐標(biāo),K是相機(jī)內(nèi)參.

步驟2.在圖優(yōu)化中,構(gòu)建一個(gè)優(yōu)化問題并利用圖去求解.這里優(yōu)化問題可以表示成對所有特征點(diǎn)的誤差求和,構(gòu)建最小二乘優(yōu)化如下:

步驟3.構(gòu)建內(nèi)點(diǎn)g2o圖優(yōu)化,再加上外點(diǎn)g2o的位姿優(yōu)化,本文的特征地圖g2o優(yōu)化如圖8所示.

在本文構(gòu)造的內(nèi)外點(diǎn)更新優(yōu)化模型中,由于考慮了當(dāng)前幀與歷史關(guān)鍵幀的匹配關(guān)系,增加了更多的約束,提高了g2o的優(yōu)化精度,但是特征點(diǎn)在坐標(biāo)轉(zhuǎn)換中也會存在誤差,因此當(dāng)前幀與歷史關(guān)鍵幀的匹配不宜過多,根據(jù)經(jīng)驗(yàn)選取歷史關(guān)鍵幀的數(shù)目c=1,2,3,優(yōu)化效果較好.

圖8 內(nèi)外點(diǎn)的g2o圖優(yōu)化Fig.8 g2o graph optimization of internal and external points

3 實(shí)驗(yàn)與分析

所有實(shí)驗(yàn)均是在ubuntu14.04系統(tǒng)下完成,電腦配置如下:CPU(2.0GHZ Intel i7),運(yùn)行內(nèi)存8GB.實(shí)驗(yàn)數(shù)據(jù)分別來自于TUM RGB-D Benchmark數(shù)據(jù)集[24],實(shí)驗(yàn)主要做了三方面驗(yàn)證:1)不同算法的特征地圖規(guī)模對比;2)驗(yàn)證了配準(zhǔn)不滿足時(shí)的特征地圖補(bǔ)充方法的有效性;3)比較了RGBDSLAM2、FVO、文獻(xiàn)[15]的算法、本文改進(jìn)FVO算法(1)(未使用幀到幀、幀到模型的g2o優(yōu)化)以及本文改進(jìn)FVO算法(2)(包含幀到幀、幀到模型的g2o優(yōu)化)對VSLAM 的實(shí)時(shí)性、特征地圖累計(jì)規(guī)模、軌跡精度、建圖的效果.另外,由于本文是對視覺里程計(jì)的改進(jìn),為了弱化后端對前端的優(yōu)化,在全部的VSLAM 算法中保留較少的關(guān)鍵幀,設(shè)定關(guān)鍵幀的參數(shù)為:min_translation=0.25,min_rotation=0.25,以對比前端視覺里程計(jì)的改進(jìn)效果.

3.1 改進(jìn)的特征地圖規(guī)模對比

特征地圖的難點(diǎn)是維護(hù)它的規(guī)模,一旦地圖規(guī)模太大,匹配會消耗大量時(shí)間.以fr1-room數(shù)據(jù)為例,圖9對比了RGBD-SLAM2、FVO、文獻(xiàn)[15]算法以及本文算法(區(qū)域特征有效因子θ=0.3)每幀匹配的特征地圖規(guī)模以及累計(jì)地圖規(guī)模.

由圖9可知,相比于RGBD-SLAM-V2、文獻(xiàn)[15]算法的地圖數(shù)據(jù)量大、信息冗余以及部分幀特征規(guī)模波動(dòng)較大的缺點(diǎn),本文算法利用區(qū)域特征提取、子區(qū)域特征補(bǔ)充、特征地圖擴(kuò)建的方法,保證了地圖規(guī)模穩(wěn)定在(200～400)的較低區(qū)間,有效地實(shí)現(xiàn)了低規(guī)模地圖的位姿估計(jì).此外,從累計(jì)地圖規(guī)模分析,本文算法的地圖規(guī)模約占RGBD-SLAM-V2算法的1/5(文獻(xiàn)[15]算法的1/3),減少了內(nèi)存、縮短了匹配時(shí)間,滿足了實(shí)時(shí)性要求.

3.2 配準(zhǔn)不滿足時(shí)特征地圖補(bǔ)充方法的有效性驗(yàn)證

本文以小規(guī)模的特征地圖配準(zhǔn),為了應(yīng)對過程中可能出現(xiàn)配準(zhǔn)點(diǎn)數(shù)較少的情形,本節(jié)驗(yàn)證此情形下特征地圖補(bǔ)充方法的有效性.區(qū)域特征有效因子是算法中控制地圖規(guī)模的參數(shù),本節(jié)通過設(shè)定不同的來控制特征地圖的規(guī)模,以fr1-room數(shù)據(jù)為例,圖10給出了一組不同值的VSLAM的軌跡誤差對比曲線.

分析圖10可知,隨著區(qū)域特征有效因子的降低、特征地圖規(guī)模逐漸減小,造成位姿軌跡誤差越大.分析改進(jìn)前的軌跡誤差曲線,可以發(fā)現(xiàn)θ從0.72開始,軌跡誤差逐漸增大,至θ為0.32時(shí),軌跡誤差大于1m,而通過改進(jìn)后的特征地圖在θ為[1?0.23]范圍內(nèi),軌跡誤差都小于0.2m,避免了因θ取值較小造成的軌跡誤差,有效地降低了特征地圖的規(guī)模,在保證VO位姿精度的前提下減小了匹配時(shí)間.此外,不同復(fù)雜程度的場景中,最佳的θ值是不相等的,如果每次都通過大量實(shí)驗(yàn)尋找最佳θ值,不僅費(fèi)時(shí)費(fèi)力,而且也不滿足實(shí)際的需要,而通過本文的特征地圖補(bǔ)充方法,延長了θ值的有效區(qū)間,弱化了θ的取值.

3.3 VSLAM的實(shí)時(shí)性分析

實(shí)時(shí)性是VSLAM 中十分重要的性能指標(biāo),本文利用TUM RGB-D Benchmark[19]中的數(shù)據(jù)集fr1-xyz,fr1-360,fr1-room,fr2-large-noloop( flnp),fr1-desk,fr1-desk2,fr1-long-officehousehold驗(yàn)證本文算法的實(shí)時(shí)性優(yōu)勢,表1給出了RGBD-SLAM2、FVO、文獻(xiàn)[15]算法、本文改進(jìn)算法(1)(未使用特征地圖優(yōu)化)以及本文改進(jìn)算法(2)(使用特征地圖優(yōu)化)的實(shí)時(shí)性和特征地圖規(guī)模比較.其中,k表示特征地圖的累計(jì)特征點(diǎn)個(gè)數(shù).

圖9 特征地圖規(guī)模比較Fig.9 Cumulative scale of feature map points

表1 不同算法的實(shí)時(shí)性、特征地圖累計(jì)規(guī)模比較Table 1 Comparison of real time and feature map cumulative size of each algorithm

圖10 地圖改進(jìn)前后的軌跡誤差曲線Fig.10 Trajectory error curve before and after improvement

經(jīng)分析,RGBD-SLAM2是幀與多個(gè)參考幀的視覺里程計(jì)模型,由于需要多次匹配,計(jì)算復(fù)雜、耗費(fèi)資源;FVO是幀與模型的位姿估計(jì),只需要一次匹配,縮短了時(shí)間,但是易累積漂移;文獻(xiàn)[15]算法是以FVO為基礎(chǔ),通過g2o優(yōu)化得到了與實(shí)際一致的位姿估計(jì),但是構(gòu)建的特征地圖仍然存在冗余特征點(diǎn),而本文算法自適應(yīng)維護(hù)特征地圖規(guī)模,可以發(fā)現(xiàn),無論是否對特征地圖g2o優(yōu)化,本文算法在幀到特征地圖的配準(zhǔn)中,相比于其他算法都是運(yùn)算時(shí)間最快、需要的地圖特征點(diǎn)的規(guī)模是最小的,尤其在數(shù)據(jù)量大的 flnp場景中,本文算法的實(shí)時(shí)性更加突出.

3.4 VSLAM的建圖和定位精度分析

以fr1-room數(shù)據(jù)為例,圖11是上述算法構(gòu)建的3D點(diǎn)云地圖,對比發(fā)現(xiàn):利用RGBD-SLAM2、文獻(xiàn)[15]算法構(gòu)建的3D點(diǎn)云地圖存在冗余點(diǎn)和漂移點(diǎn),圖中桌子、電腦、椅子以及門輪廓都不是很清楚,特別是在圖右邊部分,由于關(guān)鍵幀的參數(shù)設(shè)置較高,此處的關(guān)鍵幀數(shù)量較少,圖11(a)、圖11(b)中右邊白色邊框的電腦無法構(gòu)建,相比于圖11(a)、圖11(b),本文算法在關(guān)鍵幀參數(shù)設(shè)置相同條件下,構(gòu)建的點(diǎn)云圖的冗余點(diǎn)和漂移點(diǎn)則明顯要少一些,且圖中電腦、桌子、椅子等輪廓更加清楚,右邊部分白色邊框的電腦也能正常顯現(xiàn).圖11(d)是由幀到幀、幀到模型的g2o優(yōu)化后的本文改進(jìn)算法,相比于圖11(c),圖11(d)中漂移點(diǎn)和冗余點(diǎn)更少,特別是在墻壁窗臺處的圍欄能夠更好地重合,沒有重影.因此,由本文改進(jìn)的算法相比于其他算法,在3D點(diǎn)云地圖的構(gòu)建具有更好的精度.

圖12是不同算法計(jì)算的軌跡對比圖,其中Ground truth是TUM計(jì)算機(jī)視覺組利用路徑捕捉系統(tǒng)得到的fr1-room場景的攝像機(jī)運(yùn)動(dòng)軌跡,可以認(rèn)為是真實(shí)路徑,Estimated是利用RGBDSLAM-V2、文獻(xiàn)[15]算法、本文改進(jìn)算法(未使用特征地圖優(yōu)化)以及本文改進(jìn)算法(使用特征地圖優(yōu)化)計(jì)算得到的運(yùn)動(dòng)軌跡(設(shè)定關(guān)鍵幀的參數(shù)為:min translation=0.05,minrotation=0.05,Difference是Ground_truth與Estimate軌跡的誤差.對比可知:相對于圖12(a)、圖12(b)點(diǎn)云圖,由本文算法構(gòu)建的圖12(c)和圖12(d)的軌跡誤差Difference區(qū)域更小,而相對于圖12(c),圖12(d)使用了幀到幀、幀到模型的g2o特征地圖優(yōu)化,該模型在求解較大旋轉(zhuǎn)和平移的位姿中更加準(zhǔn)確,如圖中黑色標(biāo)記處.此外,表2給出了不同算法在不同數(shù)據(jù)集下的位姿估計(jì)精度,其中E表示與真實(shí)軌跡的絕對誤差(Absolute trajectory error,ATE).

表2 不同算法的軌跡誤差對比Table 2 Comparison of trajectory errors of different algorithms

圖11 各算法構(gòu)建的3D點(diǎn)云地圖對比Fig.11 Comparison of 3D point cloud maps based on different algorithms

圖12 不同算法的軌跡對比Fig.12 Trajectory comparison of different algorithms

由表2知,RGBD-SLAM-V2的幀到幀模型配準(zhǔn),位姿精度較高,但消耗了大量的計(jì)算資源;FVO的幀與地圖的配準(zhǔn)雖然提高了位姿估計(jì)效率,但是位姿估計(jì)精度較低、容易長期累積漂移;文獻(xiàn)[15]在FVO的基礎(chǔ)上,通過g2o優(yōu)化得到了精度更高的位姿,但是構(gòu)建的特征地圖仍然存在特征點(diǎn)冗余的問題,本文算法一方面通過子區(qū)域分塊、特征點(diǎn)補(bǔ)充與地圖擴(kuò)建的方式自適應(yīng)維護(hù)地圖規(guī)模,使得幀到特征地圖的位姿估計(jì)兼顧了實(shí)時(shí)性和精度,另一方面提出了幀到幀、幀到模型的g2o特征地圖更新方式,進(jìn)一步提高了位姿估計(jì)的精度,消除了累計(jì)誤差.經(jīng)表2對比可知,本文算法在有無特征地圖優(yōu)化的情況下,都具有較好的精度和穩(wěn)定性,而且在配準(zhǔn)速度上,本文算法要明顯好于RGBD-SLAM2、文獻(xiàn)[15]的算法,更有利于用戶的實(shí)時(shí)體驗(yàn).

3.5 失效分析

雖然所提算法在上述測試集中表現(xiàn)較好,但也存在測試集失效的情況.例如比較困難的TUM中freiburg2-slam數(shù)據(jù)集,該視頻總長40.07m.圖13是利用本文算法測試該數(shù)據(jù)集的軌跡誤差圖和3D點(diǎn)云地圖.

可以發(fā)現(xiàn),測試結(jié)果與實(shí)際軌跡相比誤差較大,平均誤差值為0.4m,特別是在場景中急轉(zhuǎn)彎的部分,特征點(diǎn)提取、匹配十分困難,造成VO中出現(xiàn)多數(shù)誤匹配.本文雖然針對此類場景出現(xiàn)特征匹配不足的情形,會依據(jù)僅有的匹配成功的特征點(diǎn),通過地圖子區(qū)域以及地圖擴(kuò)建的方式補(bǔ)償特征點(diǎn)匹配,但是在freiburg2-slam數(shù)據(jù)集中補(bǔ)償效果不明顯.分析原因是初次匹配的特征點(diǎn)存在誤匹配,由于地圖子區(qū)域特征點(diǎn)補(bǔ)充和特征地圖擴(kuò)建都是依賴于初次匹配的特征點(diǎn)對,如果初次匹配點(diǎn)存在誤差,則本文改進(jìn)方法亦效果不佳.由此可見,首次的區(qū)域地圖特征點(diǎn)提取尤為重要.在算法中,首次地圖構(gòu)建提取的是ORB特征中角點(diǎn)響應(yīng)強(qiáng)度大的部分,這些特征點(diǎn)用于VO匹配效果較好,但是對于特征缺失的環(huán)境或有快速運(yùn)動(dòng)和轉(zhuǎn)彎出現(xiàn)的情形應(yīng)用性不強(qiáng),TUM中freiburg2-slam的數(shù)據(jù)集就是這樣的環(huán)境.針對此類場景,為了進(jìn)一步提升所提算法的魯棒性,下一步將嘗試?yán)枚喾N評估特征方法融合更高效的特征點(diǎn).

4 結(jié)論

VSLAM算法要求實(shí)時(shí)精確估計(jì)機(jī)器人的位姿,本文針對現(xiàn)有視覺里程計(jì)方法位姿估計(jì)精度不夠,或者對硬件要求較高的缺陷,提出了一種自適應(yīng)特征地圖配準(zhǔn)的VSLAM算法.首先對特征地圖劃分子區(qū)域,利用角點(diǎn)響應(yīng)強(qiáng)度提取不同子區(qū)域中高效的特征點(diǎn),解決特征地圖計(jì)算量大,耗費(fèi)計(jì)算資源的問題.其次,對于出現(xiàn)的配準(zhǔn)失敗的情形,提出一種區(qū)域特征點(diǎn)補(bǔ)充和特征地圖擴(kuò)建的算法,實(shí)現(xiàn)該情形下對應(yīng)幀的快速再次匹配.最后,在VO中引入g2o圖優(yōu)化以更新特征地圖點(diǎn),并提出一種幀到幀、幀到模型的g2o特征地圖優(yōu)化模型,實(shí)現(xiàn)了對內(nèi)點(diǎn)和外點(diǎn)的同時(shí)更新,提高了位姿估計(jì)的精度.通用數(shù)據(jù)集實(shí)驗(yàn)表明,本文提出的VSLAM 算法,在保證實(shí)時(shí)性前提下,具有較好的定位精度和建圖能力.

圖13 freiburg2-slam數(shù)據(jù)集的軌跡誤差圖和3D點(diǎn)云地圖Fig.13 Trajectory error map and 3D point cloud map for freiburg2-slam data sets