張超,曹雄,徐春鳳,韓成,白燁
(1.長(zhǎng)春理工大學(xué) 計(jì)算機(jī)科學(xué)技術(shù)學(xué)院,長(zhǎng)春 130022;2.長(zhǎng)春理工大學(xué)光電信息學(xué)院,長(zhǎng)春 130022)
SLAM(Simultaneous Localization and Mapping,同時(shí)定位與建圖)的主要功能是指搭載特定傳感器的主體,在沒(méi)有環(huán)境先驗(yàn)信息的情況下,在運(yùn)動(dòng)過(guò)程中估計(jì)自己的位置和狀態(tài)信息,同時(shí)建立環(huán)境模型[1]。2007年,Andrew Davison首次提出了基于視覺(jué)的MonoSLAM[2],但是MonoSLAM只適合小場(chǎng)景。Klein和Murray在MonoSLAM基礎(chǔ)上提出PTAM[3],將視覺(jué)SLAM分為跟蹤和建圖線(xiàn)程,但PTAM具有跟蹤易丟失,適合小場(chǎng)景等缺點(diǎn)。2014年Jakob Engel等人提出基于直接法的LSD-SLAM方法[4]。LSD-SLAM使用直接法進(jìn)行半稠密地圖的構(gòu)建,并且算法的魯棒性也會(huì)增加不少,而缺點(diǎn)是計(jì)算量大,精度相對(duì)較低。2016年Mur-Artal提出了ORB-SLAM2[5],這種方法具有優(yōu)秀的特征點(diǎn)和回環(huán)算法,但同時(shí)也存在著依賴(lài)特征點(diǎn)、計(jì)算量大等缺點(diǎn)。為此文獻(xiàn)[6]使用平方根濾波方法進(jìn)行SLAM的改進(jìn)。
為了提高視覺(jué)SLAM算法構(gòu)建地圖的精度,通過(guò)分析對(duì)比目前的直接法和特征點(diǎn)法SLAM算法中的優(yōu)缺點(diǎn),提出了基于稀疏直接法的SLAM算法(簡(jiǎn)稱(chēng)SD-SLAM,sparse direct SLAM),最后通過(guò)實(shí)驗(yàn)驗(yàn)證了算法能夠有效提高相機(jī)軌跡的精度。
SD-SLAM流程如圖1所示,在前端中使用改進(jìn)的Shi-Tomasi特征點(diǎn)檢測(cè)算法進(jìn)行稀疏特征點(diǎn)的提取,然后基于稀疏特征點(diǎn)采用直接法進(jìn)行相機(jī)位姿估計(jì),同時(shí)利用構(gòu)建的地圖進(jìn)行位姿優(yōu)化,以提高相機(jī)軌跡精度。在后端中加入文獻(xiàn)[5]中的回環(huán)檢測(cè)模塊以保證算法的完整性。
圖1 算法流程圖
文獻(xiàn)[7,8]使用SURF和SIFT特征提取算法進(jìn)行目標(biāo)跟蹤,但SURF和SIFT特征提取算法并不能滿(mǎn)足SLAM的實(shí)時(shí)性。Shi-Tomasi角點(diǎn)檢測(cè)法[9]具有計(jì)算量大和不具備尺度不變性的缺點(diǎn),而FAST角點(diǎn)檢測(cè)算法[10]僅僅通過(guò)檢測(cè)亮度差來(lái)確定角點(diǎn),具有較快的速度。因此結(jié)合Shi-Tomasi和FAST角點(diǎn)檢測(cè)算法,并對(duì)融合后的算法進(jìn)行改進(jìn)。使用改進(jìn)后的算法提取特征點(diǎn),然后基于稀疏特征點(diǎn)進(jìn)行后續(xù)的相機(jī)位姿估計(jì)。特征點(diǎn)提取的步驟如下:
(1)對(duì)圖像構(gòu)建高斯金字塔;
(2)在圖像金字塔每層上檢測(cè)FAST特征點(diǎn)作為候選特征點(diǎn);
(3)對(duì)每個(gè)候選特征點(diǎn)進(jìn)行Shi-Tomasi角點(diǎn)檢測(cè)并排序,選取前N個(gè)作為特征點(diǎn);
(4)使用灰度質(zhì)心法[11]為選擇出來(lái)的特征點(diǎn)添加方向信息。
如圖2所示,在追蹤線(xiàn)程中,相機(jī)運(yùn)動(dòng)到當(dāng)前幀Im時(shí),已知上一幀Im-1的位姿Tm-1、幀上提取的特征點(diǎn)u以及u對(duì)應(yīng)深度du。需要求得當(dāng)前幀Im和上一幀Im-1的位姿變換Tm,m-1。
圖2 幀間灰度誤差
因?yàn)閱蝹€(gè)特征點(diǎn)的灰度沒(méi)有區(qū)分性,所以使用特征塊(特征點(diǎn)的4*4領(lǐng)域)的灰度值。將Im-1幀上特征塊u根據(jù)其深度du逆投影(π為投影方程)到Im-1幀相機(jī)坐標(biāo)系中的三維空間點(diǎn)P,P經(jīng)過(guò)幀間位姿Tm,m-1變換到Im幀相機(jī)坐標(biāo)系中,再投影到Im幀上形成u′。
由于噪聲導(dǎo)致位姿估計(jì)存在誤差,所以u(píng)′與P的真實(shí)投影(圖2中虛線(xiàn)框所示)存在誤差e。
采用非線(xiàn)性?xún)?yōu)化方法,將誤差e定義為誤差項(xiàng),將相機(jī)位姿Tm,m-1定義為優(yōu)化變量,不斷調(diào)整位姿使誤差最小,構(gòu)建最小二乘問(wèn)題求解位姿Tm,m-1。
公式(3)為幀與幀之間的相機(jī)位姿估計(jì),但不可避免的累計(jì)誤差會(huì)導(dǎo)致相機(jī)軌跡精度下降,所以需要對(duì)相機(jī)位姿Tm,m-1進(jìn)行優(yōu)化。
文獻(xiàn)[12]中提出,當(dāng)前幀上的部分特征點(diǎn)對(duì)應(yīng)的空間點(diǎn)可以被局部地圖中的某些關(guān)鍵幀觀測(cè)到,利用這個(gè)數(shù)據(jù)關(guān)聯(lián)可以?xún)?yōu)化當(dāng)前幀特征u′i的位置。如公式(4)所示,其中Ai為仿射變換函數(shù)[13],Im為當(dāng)前幀,In為ui所在關(guān)鍵幀。
由于關(guān)鍵幀和當(dāng)前幀的環(huán)境光源和相機(jī)曝光可能發(fā)生變化,造成P點(diǎn)投影在幀Im和In中的點(diǎn)的灰度發(fā)生變化,不符合直接法的灰度不變性假設(shè)。所以對(duì)公式(4)進(jìn)行改進(jìn)以修正環(huán)境造成的偏差,得到精確的特征位置u′i。
其中,b為灰度補(bǔ)償參數(shù)(下標(biāo)m,n分別代表當(dāng)前幀和關(guān)鍵幀)。在求得優(yōu)化后的特征位置u′i后,以當(dāng)前幀相對(duì)世界坐標(biāo)系的位姿Tm,w為優(yōu)化變量,將世界坐標(biāo)系下空間點(diǎn)Pi的投影誤差作為誤差項(xiàng),通過(guò)最小化誤差項(xiàng),優(yōu)化相機(jī)位姿Tm,w。
幀序列相對(duì)于世界坐標(biāo)系的相機(jī)位姿Tm,w構(gòu)成相機(jī)軌跡。因此在前端中使用稀疏特征點(diǎn)和直接法進(jìn)行相機(jī)位姿估計(jì),同時(shí)使用構(gòu)建的地圖優(yōu)化位姿,從而得到精度更高的相機(jī)位姿估計(jì)。
前端的作用為追蹤相機(jī)運(yùn)動(dòng),后端需要對(duì)數(shù)據(jù)進(jìn)行優(yōu)化。首先在圖像幀序列中生成關(guān)鍵幀,關(guān)鍵幀的選取關(guān)系到算法的運(yùn)行效率。算法中關(guān)鍵幀的判斷標(biāo)準(zhǔn)為:
(1)與上一關(guān)鍵幀間隔最少20幀;
(2)跟蹤到的地圖點(diǎn)的數(shù)量不少于50;
(3)與上一關(guān)鍵幀觀測(cè)到地圖點(diǎn)的重合率小于10%。
同時(shí)滿(mǎn)足以上三個(gè)條件則創(chuàng)建關(guān)鍵幀,并對(duì)關(guān)鍵幀上的特征點(diǎn)進(jìn)行Brief描述子[13]的計(jì)算,以便進(jìn)行回環(huán)檢測(cè)。關(guān)鍵幀中存儲(chǔ)著幀位姿、特征點(diǎn)以及對(duì)應(yīng)的地圖點(diǎn)信息。
在局部?jī)?yōu)化中刪除冗余的地圖點(diǎn)。如果觀測(cè)到該地圖點(diǎn)的關(guān)鍵幀數(shù)量小于3,則認(rèn)為該地圖點(diǎn)冗余并刪除該點(diǎn)。
最后添加了文獻(xiàn)[5]的回環(huán)檢測(cè)模塊,使算法擁有回環(huán)的能力,提高算法的魯棒性。
算法運(yùn)行平臺(tái)采用Intel-E3處理器,NVIDIA Quadra k620顯卡,內(nèi)存8Gb,操作系統(tǒng)為Ubuntu14.04。
采用TUM標(biāo)準(zhǔn)數(shù)據(jù)集作為實(shí)驗(yàn)使用的圖像序列。TUM數(shù)據(jù)集是一個(gè)用于估計(jì)攝像頭定位精度的優(yōu)秀數(shù)據(jù)庫(kù),提供了許多圖像序列,包括對(duì)應(yīng)的軌跡真值。為了比較ORB-SLAM2和SD-SLAM算法的軌跡精度,使用TUM提供的檢測(cè)工具檢測(cè)軌跡的絕對(duì)誤差。表1記錄了ORB-SLAM2算法和SD-SLAM算法運(yùn)行不同圖像序列的估計(jì)軌跡和真實(shí)軌跡的均方根誤差對(duì)比。圖3展示了SD-SLAM算法運(yùn)行不同圖像序列的估計(jì)軌跡和真實(shí)軌跡的對(duì)比。
表1中可以看出在fr1/room、fr2/desk和fr2/360h圖像序列中,SD-SLAM算法的軌跡精度相對(duì)ORB-SLAM2算法有所提高。圖3展示了SD-SLAM算法運(yùn)行4個(gè)圖像序列的相機(jī)估計(jì)軌跡在xy軸上與真實(shí)軌跡的對(duì)比圖,圖中實(shí)線(xiàn)為真實(shí)軌跡,虛線(xiàn)軌跡為估計(jì)軌跡。fr2/360h圖像序列相比其它圖像序列場(chǎng)景中的紋理較少,而SD-SLAM算法需要提取特征進(jìn)行位姿估計(jì),所以SD-SLAM算法在運(yùn)行fr2/360h圖像序列時(shí)誤差相對(duì)較大。
總體來(lái)說(shuō)SD-SLAM能完整運(yùn)行4個(gè)圖像序列,在部分圖像序列中相對(duì)ORB-SLAM2算法提高了軌跡精度。
表1 4個(gè)圖像序列下兩種算法的位姿軌跡均方根誤差(m)對(duì)比
圖3 SD-SLAM在4個(gè)圖像序列中的估計(jì)軌跡與真實(shí)軌跡對(duì)比
通過(guò)在前端中使用改進(jìn)Shi-Tomasi特征點(diǎn)檢測(cè)算法進(jìn)行特征點(diǎn)提取,然后基于提取的稀疏特征點(diǎn)采用直接法進(jìn)行相機(jī)位姿估計(jì),同時(shí)利用構(gòu)建的地圖進(jìn)行位姿優(yōu)化。在后端中加入回環(huán)檢測(cè)模塊,保證算法的完整性。實(shí)驗(yàn)驗(yàn)證了SD-SLAM算法在適合特征提取的場(chǎng)景下相對(duì)ORB-SLAM2算法能有效地提高相機(jī)軌跡精度。
SD-SLAM算法的改進(jìn)之處還有很多,例如算法的效率還需要進(jìn)一步提高;在環(huán)境缺少紋理、相機(jī)強(qiáng)旋轉(zhuǎn)運(yùn)動(dòng)時(shí),跟蹤容易丟失的問(wèn)題依舊存在。