基于逆深度濾波的雙目折反射全景相機(jī)動(dòng)態(tài)SLAM系統(tǒng)

1、基于逆深度濾波的雙目折反射全景相機(jī)動(dòng)態(tài)SLAM系統(tǒng)在同時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）理論不斷突破與發(fā)展的基礎(chǔ)上，移動(dòng)機(jī)器人等技術(shù)的研究水平顯著提高。視覺傳感器的引入拓寬了SLAM的應(yīng)用。視覺SLAM通過(guò)圖像序列來(lái)估計(jì)相機(jī)運(yùn)動(dòng)，根據(jù)圖像特征的提取方法主要分為基于特征點(diǎn)的視覺SLAM方法和直接SLAM法兩種1-2。雙目立體視覺可以很容易在各種場(chǎng)景中提取視差，從而得到場(chǎng)景中的深度信息。雙目SLAM既可以在運(yùn)動(dòng)時(shí)估計(jì)深度，亦可在靜止時(shí)估計(jì)，對(duì)周圍的物體和自身移動(dòng)的軌跡的感知更加精確。但是現(xiàn)實(shí)中不存在理想靜態(tài)環(huán)境，移動(dòng)對(duì)象的存在不

2、可避免3。動(dòng)態(tài)對(duì)象會(huì)給相機(jī)位姿估計(jì)帶來(lái)誤差4。因此，如何解決動(dòng)態(tài)環(huán)境中動(dòng)態(tài)點(diǎn)帶來(lái)誤差的問(wèn)題成為視覺SLAM的關(guān)鍵。全景相機(jī)具有大視場(chǎng)的特性，即使面對(duì)快速運(yùn)動(dòng)的情況，依然可以獲得幀之間的對(duì)應(yīng)關(guān)系5。近年來(lái)，諸多學(xué)者使用大視場(chǎng)相機(jī)進(jìn)行SLAM。Matsukih等將魚眼相機(jī)用于DSO6（Direct Sparse Odometry），證明了大視場(chǎng)相機(jī)用于DSO算法效果較好7。為了精確估計(jì)相機(jī)位置，文章8-10采用魯棒特征和傳統(tǒng)的擴(kuò)展卡爾曼濾波（EKF）-SLAM方法，但這些方法的處理速度太低，并且缺乏有效的反饋定位方法。Lukierski等人提出一種密集3D重建算法，不依賴關(guān)鍵點(diǎn)，能夠更好地采集強(qiáng)梯

3、度區(qū)域圖像的像素，用來(lái)估計(jì)相機(jī)位置。文獻(xiàn)11-12基于全景相機(jī)使用稀疏視覺上的方法進(jìn)行視覺里程計(jì)研究，更好地定位相機(jī)。針對(duì)動(dòng)態(tài)環(huán)境，Li等13提出了實(shí)時(shí)深度邊緣的RGB-D SLAM系統(tǒng)。該系統(tǒng)計(jì)算每個(gè)關(guān)鍵幀中點(diǎn)是靜態(tài)點(diǎn)的可能性，使用靜態(tài)加權(quán)方法降低動(dòng)態(tài)物體對(duì)相機(jī)位姿估計(jì)的影響。但是，該系統(tǒng)需要依賴RGB-D相機(jī)提供的深度信息，這對(duì)于普通相機(jī)是不適用的。文獻(xiàn)14-15利用深度學(xué)習(xí)對(duì)動(dòng)態(tài)目標(biāo)的檢測(cè)，達(dá)到了去除動(dòng)態(tài)特征點(diǎn)，提高位姿估計(jì)精度的目的。上述工作都只能利用相鄰兩幀之間的信息來(lái)判斷場(chǎng)景中對(duì)象是動(dòng)態(tài)還是靜態(tài)的。為了利用更多圖像幀的信息，更加準(zhǔn)確地判斷場(chǎng)景中對(duì)象的運(yùn)動(dòng)狀態(tài)，本文提出基于逆深度濾波

4、的雙目折反射全景相機(jī)動(dòng)態(tài)SLAM系統(tǒng)，以去除動(dòng)態(tài)點(diǎn)為目標(biāo)，利用靜態(tài)點(diǎn)對(duì)相機(jī)進(jìn)行位姿估計(jì)。使用雙目全景相機(jī)通過(guò)立體視覺計(jì)算的距離信息對(duì)視覺里程計(jì)進(jìn)行補(bǔ)償推斷，采用一種基于貝葉斯濾波的逆深度濾波器，對(duì)動(dòng)態(tài)地圖點(diǎn)進(jìn)行剔除，在動(dòng)態(tài)環(huán)境下得到更加精確的相機(jī)位姿估計(jì)結(jié)果。此方法可以解決傳統(tǒng)SLAM系統(tǒng)面對(duì)動(dòng)態(tài)環(huán)境時(shí)計(jì)算軌跡出現(xiàn)漂移的現(xiàn)象，并且計(jì)算的位姿結(jié)果也更加精確。2 雙目折反射全景相機(jī)系統(tǒng)2.1折反射全景相機(jī)投影模型折反射全景相機(jī)通過(guò)頂端反射鏡擴(kuò)大相機(jī)的視場(chǎng)，由反射鏡和成像模組兩部分組成。其結(jié)構(gòu)如圖1所示。圖1折反射全景相機(jī)Fig.1Catadioptric panoramic camera為了表示世

5、界坐標(biāo)系下點(diǎn)到像素坐標(biāo)系下的投影過(guò)程，應(yīng)用球面投影模型對(duì)折反射全景相機(jī)的投影模型進(jìn)行分析16。球面投影模型的成像過(guò)程如圖2所示。圖2折反射全景相機(jī)投影過(guò)程Fig.2Catadioptric panoramic camera projection process首先，將世界坐標(biāo)系下的空間點(diǎn)Xc以球心為投影中心投影到單位球上于點(diǎn)Xs，坐標(biāo)為：Xs=xs,ys,zsT=XcXc（1）折反射全景相機(jī)反射鏡的鏡面參數(shù)由來(lái)表示。將投影中心由球心移動(dòng)到Cp=(0,0,)T，以Cp為投影中心將單位球上的三維點(diǎn)Xs投影到與Cp距離為1的二維平面上對(duì)應(yīng)點(diǎn)Xm，對(duì)應(yīng)關(guān)系如下：Xm=D(Xs)=xs+zs,ys+zs

6、,1T（2）最后，經(jīng)過(guò)廣義投影矩陣K，完成圖像坐標(biāo)系到像素坐標(biāo)系之間的變換，關(guān)系如下：p=Kxm=fx00fxsfy0u0v01xm（3）上述推導(dǎo)公式表示世界坐標(biāo)系下的點(diǎn)在全景相機(jī)下的成像過(guò)程，很容易得到像素點(diǎn)到空間點(diǎn)的反投影過(guò)程。2.2雙目全景相機(jī)三角測(cè)量雙目折反射全景相機(jī)系統(tǒng)可以利用雙目圖像計(jì)算特征點(diǎn)的深度信息。將深度信息輸入逆深度濾波器，對(duì)動(dòng)態(tài)地圖點(diǎn)進(jìn)行剔除。這里通過(guò)三角化來(lái)計(jì)算特征點(diǎn)深度，針對(duì)折反射全景相機(jī)的投影模型，將特征點(diǎn)反投影到球面，在球面上分析三角化過(guò)程。如圖3所示，空間點(diǎn)P同時(shí)被雙目折反射全景相機(jī)觀測(cè)到，pl和pr分別是左右兩個(gè)折反射相機(jī)上對(duì)應(yīng)的特征點(diǎn)反投影到球面上的向量。其

7、中利用兩個(gè)幾何關(guān)系相對(duì)固定的相機(jī)以不同視角獲取被測(cè)物體的影像信息，再利用同一個(gè)視覺信息在不同相機(jī)下的匹配特征不同進(jìn)行雙目相機(jī)標(biāo)定17-18。兩個(gè)相機(jī)間的位姿關(guān)系通過(guò)標(biāo)定是已知的，根據(jù)對(duì)極幾何的定義，pl和pr滿足以下關(guān)系：srpr=slRpl+t（4）圖3雙目折反射全景相機(jī)三角化Fig. 3Triangulation of binocular catadioptric panoramic camera在式（4）的兩側(cè)同時(shí)乘一個(gè)pr，得到：srprpr=0=slprRpl+prt（5）通過(guò)方程可以求得sl。于是本文計(jì)算了特征點(diǎn)的深度信息，可以確定其在空間中的坐標(biāo)。將深度信息輸入到逆深度濾波器模塊

8、可以區(qū)分出地圖點(diǎn)的類別。3 逆深度濾波關(guān)鍵算法深度濾波器采用概率的方式對(duì)空間點(diǎn)進(jìn)行深度估計(jì)，隨著新的圖像幀的進(jìn)入，對(duì)深度的后驗(yàn)概率分布進(jìn)行更新。Forster等19發(fā)現(xiàn)在大規(guī)模場(chǎng)景下逆深度的統(tǒng)計(jì)直方圖更接近高斯均勻混合分布，因此，提出了逆深度濾波器的方法，就是使用空間點(diǎn)深度值的倒數(shù)代替深度值在深度濾波器中對(duì)地圖點(diǎn)進(jìn)行處理。逆深度濾波器將最新時(shí)刻求得的逆深度觀測(cè)值，與上一時(shí)刻的逆深度估計(jì)值進(jìn)行融合，直到地圖點(diǎn)的逆深度值收斂。使用逆深度濾波器可以將地圖點(diǎn)分為有效的和無(wú)效的測(cè)量值兩種類別。雖然文獻(xiàn)20中模型最初是用來(lái)找到不同基線下最準(zhǔn)確的逆深度測(cè)量值的，但是該模型也適合檢測(cè)動(dòng)態(tài)環(huán)境下雙目折反射全景相

9、機(jī)SLAM系統(tǒng)中的動(dòng)態(tài)地圖點(diǎn)，因此將逆深度濾波器應(yīng)用到雙目折反射全景相機(jī)的動(dòng)態(tài)SLAM系統(tǒng)中，使用逆深度濾波器來(lái)區(qū)分地圖點(diǎn)中的動(dòng)態(tài)點(diǎn)和靜態(tài)點(diǎn)。式（6）給出了逆深度濾波器的概率模型：p(x|Z,)=N(x|Z,2)+(1)U(x|Zmin,Zmax)（6）針對(duì)雙目折反射全景相機(jī)的動(dòng)態(tài)SLAM系統(tǒng)，本文對(duì)式（6）中各符號(hào)進(jìn)行了重新定義。使用代表計(jì)算出的地圖點(diǎn)是靜態(tài)點(diǎn)的概率；對(duì)應(yīng)的1代表的是該點(diǎn)是動(dòng)態(tài)點(diǎn)的概率。N(x|Z,2)是指靜態(tài)點(diǎn)是服從Z為均值，2為方差的高斯分布，其中x代表由雙目全景相機(jī)計(jì)算得到的逆深度值，Z代表真實(shí)的逆深度值，2代表一個(gè)像素對(duì)逆深度的擾動(dòng)；U(x|Zmin,Zmax)是指動(dòng)

10、態(tài)點(diǎn)在(Zmin,Zmax)內(nèi)服從均勻分布，可以由場(chǎng)景中幾何的先驗(yàn)得到Zmin和Zmax。對(duì)于一個(gè)地圖點(diǎn)，可以隨著新的圖像幀的進(jìn)入，不斷更新對(duì)概率的估計(jì)，直到它收斂，以此來(lái)區(qū)分動(dòng)態(tài)地圖點(diǎn)和靜態(tài)地圖點(diǎn)。如圖4所示，基于ORB-SLAM2的框架，添加了逆深度濾波線程。使用雙目折反射全景相機(jī)對(duì)空間點(diǎn)的深度進(jìn)行測(cè)量，然后轉(zhuǎn)換成逆深度。將得到的地圖點(diǎn)輸入逆深度濾波器，隨著觀測(cè)次數(shù)的增加，地圖點(diǎn)的逆深度值逐漸收斂。對(duì)于收斂的地圖點(diǎn)，判斷它是靜態(tài)點(diǎn)的概率是否大于閾值，區(qū)分出動(dòng)態(tài)地圖點(diǎn)和靜態(tài)地圖點(diǎn)。然后剔除動(dòng)態(tài)地圖點(diǎn)，只有靜態(tài)地圖點(diǎn)被輸入到跟蹤模塊用于相機(jī)的位姿估計(jì)，以此達(dá)到更加魯棒和精確的相機(jī)位姿計(jì)算。圖4

11、雙目折反射全景相機(jī)動(dòng)態(tài)SLAM算法框圖Fig. 4Block diagram of dynamic SLAM algorithm for binocular panoramic camera現(xiàn)假設(shè)有一組通過(guò)雙目折反射全景相機(jī)測(cè)量得到的含有噪聲的地圖點(diǎn)的逆深度值x1,x2,xN，它們之間是相互獨(dú)立的，現(xiàn)在要估計(jì)真實(shí)的逆深度值Z和概率，可以通過(guò)求最大后驗(yàn)概率得到。根據(jù)貝葉斯公式，其后驗(yàn)具有以下形式：p(Z,|x1,x2,xN)=p(Z,)p(x1,x2,xN|Z,)p(x1,x2,xN)p(Z,)p(x1,x2,xN|Z,).（7）由于深度測(cè)量值相互獨(dú)立，因此式（7）右邊可寫成：p(Z,)p(x1

12、,x2,xN|Z,)p(Z,)np(xn|Z,).（8）真實(shí)后驗(yàn)概率可以用貝塔-高斯分布來(lái)近似18，如下：q(Z,|aN,bN,N,N)=Beta(|aN,bN)N(Z|N,N),（9）其中aN和bN是Beta分布的參數(shù)。為了減少計(jì)算量，繼續(xù)推導(dǎo)公式的迭代形式是必要的。通過(guò)推導(dǎo)可以得到迭代形式：q(Z,|aN,bN,N,N)q(Z,|aN1,bN1,N1,N1)p(xN|Z,).（10）通過(guò)匹配Z和的第一階和第二階矩，用BetaGaussian分布近似式（10）的后驗(yàn)，導(dǎo)出了aN,bN,N和2N的更新公式。推導(dǎo)過(guò)程參考文獻(xiàn)21。計(jì)算q(Z,|aN,bN,N,N)的一階矩，用來(lái)近似估計(jì)和Z：Z=

13、N（11）=aNaN+bN（12）因此，每當(dāng)有新的一幀圖片傳入系統(tǒng)，本文使用式（11）對(duì)逆深度Z進(jìn)行更新，使用式（12）對(duì)該地圖點(diǎn)是靜態(tài)點(diǎn)的概率進(jìn)行更新。對(duì)設(shè)定一個(gè)閾值，若地圖點(diǎn)收斂后大于該閾值，則認(rèn)為該地圖點(diǎn)為靜態(tài)點(diǎn)；否則剔除該地圖點(diǎn)，不參與折反射全景相機(jī)的位姿解算。圖5是逆深度濾波的具體實(shí)現(xiàn)示意圖。首先，通過(guò)雙目折反射全景相機(jī)初始化出一組地圖點(diǎn)，隨著新的雙目圖像輸入系統(tǒng)，生成新地圖點(diǎn)的觀測(cè)并對(duì)已存在的地圖點(diǎn)生成重復(fù)觀測(cè)，不斷對(duì)地圖點(diǎn)的深度和概率更新。地圖點(diǎn)被分為已收斂的地圖點(diǎn)和未收斂的地圖點(diǎn)，未收斂的地圖點(diǎn)繼續(xù)更新直到收斂。已經(jīng)收斂的地圖點(diǎn)是靜態(tài)點(diǎn)的概率大于等于閾值時(shí)被判斷是靜態(tài)點(diǎn)，參與全

14、景相機(jī)的位姿估計(jì)；概率小于閾值的是動(dòng)態(tài)點(diǎn)，被剔除。圖5逆深度濾波算法示意圖Fig.5Schematic diagram of inverse depth filtering algorithm4 實(shí)驗(yàn)與分析為了驗(yàn)證基于逆深度濾波的雙目折反射全景相機(jī)動(dòng)態(tài)SLAM系統(tǒng)的性能，在公開數(shù)據(jù)集22上進(jìn)行實(shí)驗(yàn)。該數(shù)據(jù)集由裝載2個(gè)折反射全景相機(jī)的汽車在城市街道中采集，共包含12 607幀圖像。圖像中有大量的動(dòng)態(tài)物體和靜態(tài)物體，非常適合對(duì)動(dòng)態(tài)環(huán)境下折反射全景相機(jī)的SLAM系統(tǒng)性能進(jìn)行測(cè)試。在數(shù)據(jù)集中選取多段包含動(dòng)態(tài)對(duì)象的圖像序列，并分為低動(dòng)態(tài)序列和高動(dòng)態(tài)序列。選取了一段低動(dòng)態(tài)和兩段高動(dòng)態(tài)的圖像序列進(jìn)行實(shí)驗(yàn)，對(duì)

15、本文提出的SLAM算法進(jìn)行驗(yàn)證，對(duì)SLAM系統(tǒng)分別評(píng)估了絕對(duì)軌跡誤差（Absolute trajectory Error， ATE）和相對(duì)軌跡誤差（Relative Pose Error， RPE）兩個(gè)指標(biāo)。ATE是估計(jì)軌跡與ground truth之間的誤差，它可以直接反映出位姿估計(jì)算法的精度和全局一致性，通常用于評(píng)估整個(gè)SLAM系統(tǒng)的性能。RPE是評(píng)價(jià)系統(tǒng)漂移的指標(biāo)，它是固定時(shí)間段內(nèi)兩個(gè)位姿變化量的差?？紤]ATE和RPE在平移上的誤差，其均方根誤差（Root Mean Square Error，RMSE）的計(jì)算如下：Eatrans=1Ni=1Ntrans(T1gt,iTesti,i)22（

16、13）Ertrans=1Nti=1Nttrans(T1gt,iTgt,i+t)1(T1esti,iTesti,i+t)22（14）其中：N表示軌跡中包含的位姿個(gè)數(shù)，i=1,2,N；Tgt,i和Testi,i分別表示真實(shí)軌跡和SLAM系統(tǒng)估計(jì)出的軌跡；trans是取括號(hào)內(nèi)變量的平移部分。本文使用RMSE、平均誤差、中值誤差和標(biāo)準(zhǔn)差（Standard Deviation， SD）評(píng)價(jià)ATE指標(biāo)。將使用本文方法計(jì)算出的軌跡分別與單目全景視覺里程計(jì)方法及真值進(jìn)行對(duì)比。其中，單目全視覺里程計(jì)（Visual Odometry， VO）是基于ORB-SLAM2改進(jìn)的適合折反射全景相機(jī)的算法，把計(jì)算出的RMS

17、E、平均誤差、中值誤差和SD進(jìn)行統(tǒng)計(jì)，結(jié)果如表1所示。使用相對(duì)位移評(píng)價(jià)RPE指標(biāo)，圖6是本文計(jì)算出的軌跡與真值之間的誤差，其中6（a）是低動(dòng)態(tài)序列，6（b）和6（c）為高動(dòng)態(tài)序列。分析圖6可知，本文計(jì)算出的軌跡與真值的誤差一直在較小的范圍內(nèi)變化，沒(méi)有因?yàn)閯?dòng)態(tài)對(duì)象而出現(xiàn)軌跡的突變和漂移，證明了本文算法的魯棒性。表1不同方法在公共數(shù)據(jù)集測(cè)試的ATE結(jié)果Tab.1Comparison of absolute trajectory error （ATE） on public dataset(m)SequenceCatadioptric Pano VOOursRMSEMedianMeanStdRMSEM

18、edianMeanStdLow dynamic sequence（a）0.350.330.310.120.290.280.250.09High dynamic sequence（b）12.639.6910.674.110.520.510.470.16High dynamic sequence（c）15.3512.1412.135.290.560.520.530.19圖6不同序列位姿估計(jì)結(jié)果與真值間的RPEFig.6RPE between pose estimation results and ground truth for different sequences圖7中是不同方法得到的軌跡與真值的對(duì)比。本文計(jì)算出的軌跡與真值更加接近，而基于ORB-SLAM2改進(jìn)的折反射全景VO23在低動(dòng)態(tài)環(huán)境下與本文計(jì)算的軌跡最接近，因?yàn)樵诘蛣?dòng)態(tài)環(huán)境下RANSAC算法將動(dòng)態(tài)點(diǎn)作為外點(diǎn)，受到動(dòng)態(tài)點(diǎn)的影響小。而在另外兩個(gè)序列中存在高動(dòng)態(tài)目標(biāo)，此時(shí)RA