季順平，秦梓杰

武漢大學(xué)遙感信息工程學(xué)院，湖北 武漢 430079

隨著傳感器、自動(dòng)化和平臺(tái)技術(shù)的發(fā)展，利用平臺(tái)上安置的光學(xué)或距離傳感器，同時(shí)實(shí)現(xiàn)自我定位與環(huán)境感知的智能系統(tǒng)成為攝影測(cè)量、計(jì)算機(jī)視覺和機(jī)器人學(xué)的新型研究方向[1]；并在移動(dòng)測(cè)圖系統(tǒng)、無人駕駛汽車、火星和月球的深空探測(cè)、無人機(jī)偵察、室內(nèi)導(dǎo)航等領(lǐng)域發(fā)揮著關(guān)鍵的作用[2-3]。若以光學(xué)傳感器為主要信息獲取源，這種系統(tǒng)通常稱為基于視覺的自動(dòng)定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)[4]。主流的視覺SLAM系統(tǒng)有兩種分類模式。一類按照所采用的傳感器劃分。包括單目SLAM(mono-SLAM)、雙目SLAM(stereo-SLAM)和4D相機(jī)SLAM(RGBD-SLAM)。另一類按照所采用的方法劃分，主流是基于點(diǎn)特征的SLAM(feature-based SLAM)和基于圖像自身的SLAM(direct SLAM)。成熟的、具代表性的SLAM系統(tǒng)如：基于特征點(diǎn)的ORB-SLAM[5]，直接法的LSD-SLAM[6]，將特征點(diǎn)法與直接法混用的SVO[7]、SVO2.0[8]，以及RBG-D SLAM的代表作RTAB-MAP[9]，等。

這些主流SLAM架構(gòu)采用傳統(tǒng)框幅式的相機(jī)或攝像機(jī)作為視覺信息獲取裝備。單目SLAM視場(chǎng)狹窄，尺度估計(jì)受累積誤差的影響，較依賴于閉環(huán)條件[10]。雙目SLAM雖然克服了尺度漂移，但是視差狹窄依然沒有改變。若局部成像區(qū)域信息較少(即提取的ORB[11]或其他特征較少)，或者出現(xiàn)較大的視角變化，則會(huì)引起跟蹤頻繁丟失。這也是SLAM技術(shù)尚未廣泛應(yīng)用于測(cè)繪行業(yè)的地面移動(dòng)測(cè)圖系統(tǒng)(mobile mapping system,MMS)的關(guān)鍵因素。

大視場(chǎng)成像設(shè)備，如魚眼鏡頭和全景鏡頭，理論上能夠克服視場(chǎng)狹窄的問題。全景視覺成像具有360°全視角成像的優(yōu)勢(shì)，已經(jīng)在測(cè)繪、機(jī)器人、計(jì)算機(jī)視覺等相關(guān)領(lǐng)域中逐步得到應(yīng)用[12-13]，如用于城市測(cè)圖、視頻監(jiān)控、交通監(jiān)督、虛擬現(xiàn)實(shí)、機(jī)器人導(dǎo)航、場(chǎng)景重建等。裝載于移動(dòng)測(cè)圖系統(tǒng)或普通汽車上的全景成像裝置通常只被用于街景收集，如谷歌和百度的街景圖像，無法實(shí)現(xiàn)量測(cè)功能；基于全景視覺的檢校，幾何定位或完整的SLAM系統(tǒng)也有較多的研究[12-22]。文獻(xiàn)[18—19]采用超廣角單目魚眼相機(jī)，使用卡爾曼濾波作為優(yōu)化方法；文獻(xiàn)[17]與SVO2.0[8]研究了多鏡頭組合系統(tǒng)的SLAM算法，前者采用了球面模型的粒子濾波作為優(yōu)化方法，而SVO2.0采用了多個(gè)平面相機(jī)模型，以光束法平差進(jìn)行優(yōu)化；文獻(xiàn)[16]使用直接法光流跟蹤作為前端的視覺里程計(jì)，光束法平差作為后端優(yōu)化方法；因此也繼承了直接法的優(yōu)點(diǎn)與缺點(diǎn)。本文嘗試將多鏡頭組合式全景相機(jī)與基于特征的SLAM方法(ORB-SLAM2[23])相結(jié)合，發(fā)展一套基于特征的三線程單目全景SLAM系統(tǒng)。

生產(chǎn)完美的球形鏡頭受到當(dāng)前制造工藝的限制。移動(dòng)全景視覺成像主要采用3種替代模式：多鏡頭組合式、旋轉(zhuǎn)式、折反射式[19]。其中，多鏡頭組合式是目前最流行的移動(dòng)全景成像技術(shù)，它借助一系列的魚眼相機(jī)分別成像，再拼接為無縫全景圖像。目前已經(jīng)實(shí)現(xiàn)了商業(yè)化生產(chǎn)，如PointGrey公司的Ladybug系列，中國測(cè)繪科學(xué)研究院也有相關(guān)產(chǎn)品。旋轉(zhuǎn)式成像借助一根CCD的高速旋轉(zhuǎn)，實(shí)現(xiàn)360°的連續(xù)成像。文獻(xiàn)[14]曾研究過旋轉(zhuǎn)式全景相機(jī)的傳感器成像模型和自檢校方法。然而，只有2000年前后德國的Noblex公司生產(chǎn)過少量的相關(guān)產(chǎn)品。折反射全景相機(jī)包括兩個(gè)組分：鏡子和透鏡。鏡子首先折射周圍的光至透鏡，透鏡再實(shí)現(xiàn)成像。由于復(fù)雜的成像機(jī)理和制造工藝，折反射全景相機(jī)應(yīng)用相對(duì)較少，也沒有商業(yè)上的推廣[24]。本文的全景SLAM以多鏡頭組合式全景相機(jī)作為視覺信息的獲取裝備。

目前，視覺SLAM已經(jīng)得到了蓬勃的發(fā)展，基于單目、雙目和RGB-D相機(jī)的SLAM技術(shù)日臻成熟，能應(yīng)用于全景相機(jī)的SLAM系統(tǒng)也有較多的研究。多鏡頭組合式全景相機(jī)的原理已經(jīng)由[25—27]等文獻(xiàn)建立，鑒于全景視覺的獨(dú)特優(yōu)勢(shì)，發(fā)展一套高度自動(dòng)化的、基于特征的多鏡頭組合式全景相機(jī)的SLAM系統(tǒng)并實(shí)現(xiàn)高精度的量測(cè)功能，無論在測(cè)繪行業(yè)還是機(jī)器人與計(jì)算機(jī)視覺都具有積極的意義。本文從全景構(gòu)像方程、魚眼相機(jī)檢校、初始化、局部地圖構(gòu)建、關(guān)鍵幀選擇、全圖優(yōu)化等各個(gè)步驟探討基于全景相機(jī)的完整的SLAM方法，并在每個(gè)步驟中都做出了大量創(chuàng)新性的工作。

1 全景相機(jī)的成像、檢校與優(yōu)化模型

1.1 多鏡頭組合式全景相機(jī)模型

多鏡頭組合式全景相機(jī)由一系列獨(dú)立、固定的魚眼鏡頭組成，多個(gè)鏡頭獨(dú)立成像，再拼接為全景圖。如圖1(b)所示，每個(gè)鏡頭具有各自的投影中心C，在實(shí)際制造過程中尚難以保證與球心S完全重合?？梢?，物理上的三點(diǎn)共線是C、uc和P′。這是獨(dú)立鏡頭的成像方程。為了實(shí)現(xiàn)統(tǒng)一的全景坐標(biāo)系統(tǒng)，需要將實(shí)際像素坐標(biāo)uc投影到某個(gè)指定半徑的球面上，得到u。此時(shí)，所有的魚眼圖像坐標(biāo)就可投影至統(tǒng)一的球面坐標(biāo)。

圖1 全景成像模型Fig.1 Panoramic imaging model

因此，第1步需要將獨(dú)立相機(jī)的圖像坐標(biāo)轉(zhuǎn)換為統(tǒng)一的全景球面坐標(biāo)。描述單相機(jī)像點(diǎn)uc(其坐標(biāo)用x表示)計(jì)算全景球面像點(diǎn)u(其坐標(biāo)用x′表示)的過程如式(1)所示

x′=mRiKi(x)+Ti

(1)

x′2+y′2+z′2=r2

(2)

式中，Ki是第i個(gè)魚眼鏡頭的內(nèi)參函數(shù)，參量包含鏡頭畸變差、CCD畸變差、焦距、主點(diǎn)等；Ri和Ti分別表示第i個(gè)魚眼鏡頭投影中心在全景坐標(biāo)系中的旋轉(zhuǎn)矩陣和偏移矢量；Ki、Ri、Ti通過嚴(yán)格標(biāo)定之后即為固定值；m為球面半徑r所確定的比例系數(shù)，聯(lián)合如式(2)所示的球面方程可同時(shí)解得m和x′。

從圖1可見，用于表述共線條件方程的光束是CuP′而非SuP。首先表達(dá)真實(shí)的射線(x′-Ti)，并將其平移至統(tǒng)一的球面坐標(biāo)(加上平移量Ti)，得到共線條件方程

Ti+λ(x′-Ti)=[R|T]Xw

(3)

當(dāng)Ti取值為0時(shí)，式(3)就表達(dá)一個(gè)完美的球面成像模型(圖1(a))，許多文獻(xiàn)都采用近似的理想成像模型[15，22]。本文將成像模型用于SLAM系統(tǒng)，需要像點(diǎn)、世界點(diǎn)之間的精確的正反算，因此保留參數(shù)Ti。

1.2 魚眼相機(jī)的檢校

內(nèi)參函數(shù)Ki可采用不同的相機(jī)檢校模型。魚眼鏡頭的內(nèi)參數(shù)同樣需要精確的檢校才能保證在一系列坐標(biāo)轉(zhuǎn)換中保持高精度。文獻(xiàn)[28]提出了一種通用魚眼相機(jī)檢校方法并得到廣泛的應(yīng)用。如式(4)所示

Xd=(k1θ+k2θ3)ur(φ)+Δr(θ,φ)ur(φ)+

Δt(θ,φ)μφ(φ)

(4)

式中

Δr(θ,φ)=(l1θ+l2θ3+l3θ5)(i1cosφ+i2sinφ+

i3cos 2φ+i4sin 2φ)

Δt(θ,φ)=(m1θ+m2θ3+m3θ5)(j1cosφ+

j2sinφ+j3cos 2φ+j4sin 2φ)

Xd=(xdyd)T是魚眼相機(jī)坐標(biāo)；θ和φ分別是天頂角和水平角。lk、mk、ik、jk為多項(xiàng)式系數(shù)。試驗(yàn)表明，基于該方法檢校魚眼相機(jī)并生成核線立體像對(duì)，其核線誤差約1～1.5像素。為使得核線誤差降低到1像素以下，本文對(duì)改模型加以改進(jìn)，提出一種更為精確的魚眼相機(jī)檢校方法。如式(5)所示

Δr(θ,φ)+Δt(θ,φ)+(A1r2+

(5)

式中，θ與φ可以由像素坐標(biāo)計(jì)算得出。此模型共23個(gè)參數(shù)，因此需要12個(gè)或以上原始圖像與糾正后圖像的同名點(diǎn)坐標(biāo)，以求解模型參數(shù)。本文顯式地加入了魚眼相機(jī)的成像過程(等式右邊的第1項(xiàng))，并將式(4)作為其系統(tǒng)誤差的附加參數(shù)項(xiàng)。這樣就減輕了通用多項(xiàng)式模型的擬合負(fù)擔(dān)(即擬合微小的系統(tǒng)誤差而不是將魚眼投影模型都看作系統(tǒng)誤差)。本文選取全圖像均勻分布的72個(gè)點(diǎn)進(jìn)行最小二乘求解，相機(jī)檢校精度均在1個(gè)像素以內(nèi)。

1.3 全景相機(jī)單像解析和前方交會(huì)

全景相機(jī)的定位全部在統(tǒng)一的全景球面坐標(biāo)系下，即在原始魚眼相機(jī)提取的特征點(diǎn)先根據(jù)式(3)轉(zhuǎn)換到球面坐標(biāo)系下。兩個(gè)全景球上的同名特征點(diǎn)同樣滿足對(duì)極幾何關(guān)系[18]，也即共面條件方程，如式(6)所示

(6)

從魚眼相機(jī)坐標(biāo)計(jì)算得到的球面坐標(biāo)點(diǎn)已經(jīng)包含了相機(jī)內(nèi)參信息。直接解算式(6)可得到兩個(gè)全景球之間的本質(zhì)矩陣E。使用SVD分解獲取旋轉(zhuǎn)矩陣R與平移分量t，分解得到4個(gè)解。通過局部坐標(biāo)系下球面特征點(diǎn)及其地圖點(diǎn)在同一個(gè)方向作為判別依據(jù)，可選出正確的R與t。平移量t具有尺度不確定性，本文將當(dāng)前場(chǎng)景的平均深度歸一化從而確定t的尺度，并使用李代數(shù)SE(3)的形式表示相機(jī)位姿。

獲取相機(jī)位姿的同時(shí)，需要根據(jù)匹配的特征點(diǎn)計(jì)算其地圖點(diǎn)。球面坐標(biāo)的前方交會(huì)公式如式(7)所示

(7)

將同名點(diǎn)代入式(7)可解算得到地圖點(diǎn)坐標(biāo)。式(7)中，T=[R|t]為變換矩陣，x1與x2為同名球面特征點(diǎn)。

當(dāng)已知相機(jī)位姿的初始值以及全景球面上的特征點(diǎn)所對(duì)應(yīng)的地圖點(diǎn)時(shí)，采用最小二乘法構(gòu)建目標(biāo)函數(shù)使球面重投影誤差最小

(8)

式中，ξ∈SE(3)為當(dāng)前位姿的李代數(shù)表示；Xw為地圖點(diǎn)的世界坐標(biāo)，兩者為未知參數(shù)；Xs為特征點(diǎn)的球面坐標(biāo)觀測(cè)值；r為全景球半徑。

獲取有效初值是最小二乘法的關(guān)鍵步驟。若已知圖像特征點(diǎn)與地圖點(diǎn)的對(duì)應(yīng)關(guān)系，文獻(xiàn)[29]所提出Efficient Perspective-n-Point(EPnP)算法可用于解算位姿的初始值。EPnP在跟蹤失敗后的重定位過程發(fā)揮著重要的作用，本文提出并建立了球面模型上的EPnP方法，用以求解位姿初始值。根據(jù)球面投影關(guān)系

(9)

式中，t為比例因子；(xj,yj,zj)為控制點(diǎn)坐標(biāo)；αj為坐標(biāo)權(quán)重；(u,v,w)為球面點(diǎn)坐標(biāo)。消元可得到球面坐標(biāo)系下控制點(diǎn)坐標(biāo)的齊次方程

(10)

式中，向量X具有12維，設(shè)有n個(gè)地圖點(diǎn)參與計(jì)算，則M矩陣具有3n維，M矩陣的核空間變?yōu)橄蛄縓的解。至此，4個(gè)控制點(diǎn)在全景球坐標(biāo)系下的坐標(biāo)及其在世界坐標(biāo)系下的坐標(biāo)已求出。再根據(jù)式(9)可得出所有正確的3D-2D匹配點(diǎn)在全景球坐標(biāo)系及對(duì)應(yīng)世界坐標(biāo)系下的坐標(biāo)。根據(jù)文獻(xiàn)[29]所述的坐標(biāo)對(duì)齊方法即可解除位姿估值[R|t]。

1.4 全景相機(jī)的非線性優(yōu)化方法

本文的非線性優(yōu)化方法基于g2o庫[30]。而g2o中的所有優(yōu)化方法只針對(duì)普通平面相機(jī)模型。本文對(duì)相關(guān)的優(yōu)化算法進(jìn)行改進(jìn)，使其能夠處理全景成像模型。借助于圖論，待優(yōu)化變量，即位姿、地圖點(diǎn)設(shè)定為圖的頂點(diǎn)，重投影誤差(如式(8))設(shè)定為邊，得到優(yōu)化圖(圖2)。結(jié)合g2o并給出誤差函數(shù)式(8)的解析導(dǎo)數(shù)形式，即誤差函數(shù)對(duì)于位姿及地圖點(diǎn)的雅克比矩陣，即可進(jìn)行優(yōu)化求解。假定球面位姿表達(dá)為ξ∈SE(3)，PC=exp(ξ^)，地圖點(diǎn)為PW=(XYZ)T且L=X2+Y2+Z2，e是代價(jià)函數(shù)。則e對(duì)于位姿的雅克比矩陣為

(11)

式中

(12)

(13)

e對(duì)于地圖點(diǎn)的雅克比矩陣為

(14)

式中，[R|t]=exp(ξ^)。

本文實(shí)現(xiàn)的優(yōu)化算法包含4類：單幀位姿優(yōu)化(僅根據(jù)匹配的地圖點(diǎn)計(jì)算當(dāng)前幀精確的位姿，圖2(a))、局部地圖優(yōu)化和平差(根據(jù)局部共視關(guān)鍵幀優(yōu)化位姿與局部地圖點(diǎn)，圖2(b))、圖優(yōu)化(即essential graph優(yōu)化，用于檢測(cè)到閉環(huán)后，對(duì)全局關(guān)鍵幀的位姿進(jìn)行調(diào)整，圖2(c))、全局光束法平差(優(yōu)化所有位姿和地圖點(diǎn)，圖2(d))。

魚眼圖像上不同位置投射到球面上的變形不同，例如，在本文試驗(yàn)中相機(jī)中心位置x0處一個(gè)像素的寬度，投影到球面上為0.037 6 m，圖像邊緣處一個(gè)像素寬度投射到球面上表現(xiàn)為0.014 45 m，因此，不同的點(diǎn)x采用不同的誤差域值，閾值s設(shè)定為

(15)

圖2 構(gòu)建的優(yōu)化圖(三角形節(jié)點(diǎn)代表位姿，圓形節(jié)點(diǎn)代表地圖點(diǎn)，邊代表誤差項(xiàng))Fig.2 Constructed optimization graph (in which the triangle node represents the pose, the circular node represents the map point, and the edge represents the error term)

2 全景SLAM流程

系統(tǒng)將分為3個(gè)線程并行工作，分別為跟蹤(tracking)，局部地圖構(gòu)建(local mapping)與閉環(huán)(loop closing)。跟蹤線程主要負(fù)責(zé)系統(tǒng)初始化，地圖點(diǎn)跟蹤，關(guān)鍵幀選取與位姿優(yōu)化等功能，局部地圖構(gòu)建線程主要負(fù)責(zé)特征點(diǎn)三角化，誤匹配點(diǎn)與冗余關(guān)鍵幀剔除，局部地圖優(yōu)化等功能，閉環(huán)線程主要負(fù)責(zé)閉環(huán)探測(cè)，閉環(huán)關(guān)鍵幀位姿改正與全局地圖優(yōu)化等功能。以下將逐一介紹各模塊的算法策略。

2.1 初始化

本文將ORB(oriented FAST and rotated BRIEF)特征[11]用于SLAM系統(tǒng)的特征提取。本文針對(duì)全景相機(jī)做了系統(tǒng)初始化的優(yōu)化工作。首先，魚眼相機(jī)變形較大，匹配難度高，誤匹配數(shù)量大。因此采用三幀而非兩幀進(jìn)行初始化。確定第1幀為參考幀，提取ORB特征點(diǎn)，等待系統(tǒng)連續(xù)傳入兩幀，進(jìn)行第1幀與第2幀，第2幀與第3幀的特征匹配。然后結(jié)合RANSAC剔除誤匹配點(diǎn)，保留3幀共視點(diǎn)，計(jì)算1-2幀和2-3幀的基礎(chǔ)矩陣F。1-2、2-3、1-3組合分別進(jìn)行三角化，得到3組3D地圖點(diǎn)坐標(biāo)值，設(shè)定3個(gè)地圖點(diǎn)之間的空間距離閾值為0.08 m若有任意兩個(gè)地圖點(diǎn)坐標(biāo)值的空間距離大于0.08 m，則認(rèn)為此地圖點(diǎn)不穩(wěn)定，將其剔除。若成功三角化的點(diǎn)數(shù)量大于正確匹配點(diǎn)數(shù)量的75%，則調(diào)用優(yōu)化模塊進(jìn)行平差并剔除誤匹配點(diǎn)。若平差后正確匹配點(diǎn)數(shù)量大于30個(gè)則視為初始化成功，否則判定為失敗，將第2幀作為參考幀，等待系統(tǒng)傳入第4幀，使用2、3、4三幀進(jìn)行初始化，依次迭代，直到初始化成功。

2.2 地圖點(diǎn)跟蹤

地圖點(diǎn)的跟蹤是SLAM自動(dòng)化的關(guān)鍵環(huán)節(jié)。在算法1中，列出了全景相機(jī)地圖跟蹤的算法。

算法1 跟蹤地圖點(diǎn)

(1) 采用恒速模型實(shí)現(xiàn)3D地圖點(diǎn)到當(dāng)前2D圖像的匹配定位。若成功，則結(jié)束，否則進(jìn)入(2)。

(2) 采用2D圖像到2D圖像的立體匹配定位。若成功，則結(jié)束，否則進(jìn)入(3)。

(3) 采用多視圖像匹配，作為初值，再進(jìn)行3D地圖點(diǎn)到2D當(dāng)前圖像的匹配。若成功，則結(jié)束，否則進(jìn)入(4)。

(4) 重定位。采用視覺詞袋和PnP解法，實(shí)現(xiàn)匹配并定位。

(5) 只要(1)—(4)有一步成功，調(diào)用位姿優(yōu)化模塊進(jìn)行位姿優(yōu)化。

步驟1：采用恒速模型(速度和角度不變)跟蹤下一幀地圖點(diǎn)。首先，利用恒速模型估計(jì)當(dāng)前相機(jī)位姿的初始值，將上一幀觀察到的地圖點(diǎn)投影到當(dāng)前幀的全景球面上。然后，根據(jù)球面特征點(diǎn)坐標(biāo)與魚眼相機(jī)的外方位元素，判斷該點(diǎn)所在的魚眼相機(jī)編號(hào)。最后根據(jù)模型(3)反投影到魚眼圖像上設(shè)定搜索窗口進(jìn)行特征匹配。同時(shí)計(jì)算地圖點(diǎn)到全景球面的距離。本文假定當(dāng)前恒速模型的水平旋轉(zhuǎn)角度絕對(duì)值為θ，地圖點(diǎn)到相機(jī)中心距離為d，則窗口大小W自適應(yīng)地設(shè)為

(16)

確定搜索窗口后，在窗口內(nèi)搜索同名特征點(diǎn)。這里的匹配閾值設(shè)置為：最鄰近次臨近之比大于0.7且描述子距離小于70。對(duì)所有地圖點(diǎn)進(jìn)行逐個(gè)匹配，若匹配數(shù)大于20則認(rèn)為匹配成功并結(jié)束跟蹤步驟。否則進(jìn)入第2步。

步驟2：在步驟1，即3D地圖點(diǎn)到2D圖像的匹配失敗時(shí)執(zhí)行。采用圖像到圖像匹配策略，方法是視覺詞袋(bag-of-words,BoW)技術(shù)。在當(dāng)前幀與上一幀的特征點(diǎn)間進(jìn)行匹配，若匹配點(diǎn)數(shù)小于10則進(jìn)入第3步。若匹配點(diǎn)數(shù)在10～25之間，則擴(kuò)大搜索范圍，匹配從當(dāng)前幀直到上一個(gè)關(guān)鍵幀所觀察到的所有地圖特征點(diǎn)。若匹配點(diǎn)數(shù)大于等于25，設(shè)上一幀的位姿為當(dāng)前幀的位姿，執(zhí)行位姿優(yōu)化同時(shí)剔除誤匹配粗差點(diǎn)(圖2(a))，統(tǒng)計(jì)優(yōu)化后的正確匹配點(diǎn)個(gè)數(shù)。若小于15，進(jìn)入第3步；否則判定為匹配成功，返回位姿并結(jié)束。

步驟3：采用多視匹配獲取當(dāng)前幀位姿初值并用于匹配。使用當(dāng)前幀與前兩幀的所有特征點(diǎn)進(jìn)行匹配，并提取三度重疊點(diǎn)。計(jì)算當(dāng)前幀與前一幀的本質(zhì)矩陣E，SVD分解后得到位姿初值，使用該初值將前一幀地圖點(diǎn)投影到當(dāng)前幀，并進(jìn)行匹配。搜索窗口大小設(shè)置方式同式(16)。若匹配點(diǎn)數(shù)小于30則判定失敗，進(jìn)入第4步。若匹配點(diǎn)數(shù)大于30，則執(zhí)行位姿優(yōu)化模塊。剔除粗差后若匹配點(diǎn)數(shù)再次小于30，則采用第2步BoW的匹配點(diǎn)執(zhí)行位姿優(yōu)化模塊同時(shí)剔除誤匹配點(diǎn)。若正確匹配點(diǎn)數(shù)大于15則判定成功，返回位姿；否則進(jìn)入第4步。

步驟4：在以上匹配都失敗時(shí)，進(jìn)行重定位(re-localization)。使用BoW搜索上一幀到上一個(gè)關(guān)鍵幀的所有匹配點(diǎn)，調(diào)用PnP配合RANSAC進(jìn)行重定位。

步驟5：以上步驟任意一步成功即可得到當(dāng)前幀的位姿的初始值。利用該值，將局部地圖點(diǎn)全部投影至當(dāng)前幀進(jìn)行匹配，匹配完成后進(jìn)行局部優(yōu)化同時(shí)剔除誤匹配點(diǎn)(圖2(b))。統(tǒng)計(jì)正確匹配點(diǎn)數(shù)，若大于20則判定為成功，若小于20判定為失敗，傳入下一幀，執(zhí)行步驟4。

2.3 關(guān)鍵幀選取

正確跟蹤當(dāng)前幀之后，判斷當(dāng)前幀是否作為關(guān)鍵幀。本文的全景影像框幅高達(dá)8000×4000像素，由于車輛高速行駛，存在內(nèi)存讀寫的限制，因此采用較大的采樣間隔(1～3 m)。判別當(dāng)前全景影像為關(guān)鍵幀的條件為滿足算法2列表中的任意一點(diǎn)。

算法2 關(guān)鍵幀選取的判別規(guī)則

(1) 超過8幀未插入關(guān)鍵幀，則確定當(dāng)前幀為關(guān)鍵幀。

(2) 當(dāng)前幀新插入的地圖點(diǎn)大于其跟蹤到的地圖點(diǎn)的1.7倍。

(3) 當(dāng)前幀跟蹤到的地圖點(diǎn)數(shù)小于上一幀跟蹤到的點(diǎn)數(shù)的85%且局部地圖構(gòu)建線程空閑。

(4) 當(dāng)前幀跟蹤到的地圖點(diǎn)數(shù)小于上一幀跟蹤到的50%，中斷局部地圖構(gòu)建并插入關(guān)鍵幀。

(5) 局部地圖構(gòu)建(local mapping)線程空閑。

2.4 局部地圖構(gòu)建

當(dāng)局部地圖線程檢測(cè)到新插入的關(guān)鍵幀后，首先應(yīng)更新當(dāng)前關(guān)鍵幀的連接關(guān)系，找出與其具有一定共視點(diǎn)數(shù)的關(guān)鍵幀作為共視關(guān)鍵幀。根據(jù)全景相機(jī)的特殊情形，本文設(shè)定共視關(guān)鍵幀必須同時(shí)滿足：① 與當(dāng)前關(guān)鍵幀的共視點(diǎn)數(shù)≥50個(gè)；② 與當(dāng)前關(guān)鍵幀在圖像序列中的間隔關(guān)鍵幀不超過40個(gè)；③ 其與當(dāng)前關(guān)鍵幀的共視點(diǎn)在兩幀所在金字塔層數(shù)差異>2的特征點(diǎn)數(shù)不超過總共視點(diǎn)數(shù)的85%。

2.5 閉環(huán)探測(cè)與全局優(yōu)化

使用詞袋模型進(jìn)行閉環(huán)探測(cè)的技術(shù)較為成熟，本文使用文獻(xiàn)[28，31]提出的閉環(huán)探測(cè)方法，利用ORB特征點(diǎn)結(jié)合DBoW2庫進(jìn)行閉環(huán)探測(cè)，流程如算法3所示。

算法3 閉環(huán)探測(cè)流程

檢測(cè)到新關(guān)鍵幀插入：

(1) 計(jì)算當(dāng)前關(guān)鍵幀的BoW向量，用于表示當(dāng)前幀的場(chǎng)景信息。

(2) 通過相鄰關(guān)鍵幀計(jì)算相似性閾值。

(3) 通過關(guān)鍵幀相連關(guān)系計(jì)算相似性累計(jì)得分，結(jié)合2的閾值選取閉環(huán)候選幀。

(4) 連續(xù)性檢驗(yàn)：認(rèn)為孤立的候選組具有很強(qiáng)的不確定性。

(5) 通過連續(xù)性檢驗(yàn)的候選關(guān)鍵幀均認(rèn)為與當(dāng)前幀構(gòu)成閉環(huán)。

閉環(huán)關(guān)鍵幀選取完畢后，依次與當(dāng)前關(guān)鍵幀使用BoW搜索匹配特征點(diǎn)，若匹配點(diǎn)數(shù)小于30，則剔除此閉環(huán)關(guān)鍵幀，繼續(xù)下一幀的匹配。匹配成功后，按照文獻(xiàn)[32]的方法計(jì)算閉環(huán)幀到當(dāng)前幀的相似變換群，即sim(3)變換，此時(shí)便得到了閉環(huán)關(guān)鍵幀與當(dāng)前幀的轉(zhuǎn)換關(guān)系S，通過轉(zhuǎn)化關(guān)系再次搜索匹配點(diǎn)并使用局部光束法平差優(yōu)化S。最后，調(diào)整當(dāng)前幀及其相連關(guān)鍵幀的位姿與地圖點(diǎn)，更新關(guān)鍵幀鏈接關(guān)系并傳遞調(diào)整位姿，構(gòu)建Essential Graph進(jìn)行全局位姿優(yōu)化(圖2(c))并調(diào)整相應(yīng)地圖點(diǎn)。

3 試驗(yàn)結(jié)果和分析

3.1 試驗(yàn)設(shè)計(jì)

本文試驗(yàn)選取了兩組數(shù)據(jù)均采集自車載Ladybug3相機(jī)，分別稱為Kashiwa和Omiya圖像序列。Ladybug拍攝魚眼影像大小為1616×1232像素，像元分辨率為0.009 mm，顏色分辨率為8 bit量化的RGB色彩空間。魚眼相機(jī)的焦距為3.3 mm，投影中心相互距離約40 mm；共有6個(gè)魚眼相機(jī)，5個(gè)相機(jī)用于360°水平成像，1個(gè)相機(jī)指向天空。全景影像球面成像于距離球心20 m處。拼接后的全景影像大小為4000×2000像素。

Kashiwa圖像序列包含498張全景影像，攝影間隔約2 m；Omiya圖像序列包含7464張全景影像，拍攝間隔約為1 m。由于天空上沒有特征點(diǎn)，匹配在獨(dú)立的水平魚眼相機(jī)中執(zhí)行，而F矩陣計(jì)算、光束法平差都是在統(tǒng)一的全景坐標(biāo)系中進(jìn)行。參考值由高精度的GPS/IMU組合導(dǎo)航系統(tǒng)獲得，且GPS經(jīng)過CORS站的事后改正，其絕對(duì)定位精度高于10 cm。GPS/IMU數(shù)據(jù)不參與SLAM流程。本文同時(shí)采用兩種精度評(píng)價(jià)方法進(jìn)行評(píng)價(jià)，首先按照經(jīng)典的SLAM算法，給定初始位置，姿態(tài)與尺度信息將相對(duì)位姿統(tǒng)一到給定的坐標(biāo)系下，之后不再給出任何控制信息，并計(jì)算絕對(duì)定位均方誤差以衡量SLAM系統(tǒng)的實(shí)時(shí)動(dòng)態(tài)定位精度?？紤]到在測(cè)繪中的應(yīng)用，全局定位精度更受關(guān)注，因此本文選取均勻分布的少量GPS點(diǎn)作為控制點(diǎn)，根據(jù)7參數(shù)相似變換將所有的SLAM后得到的外方位元素轉(zhuǎn)換到絕對(duì)坐標(biāo)系中，并計(jì)算所有相片外方位元素與參考值間的絕對(duì)值，作為精度評(píng)定指標(biāo)。本文的SLAM系統(tǒng)運(yùn)行在Ubantu16.04環(huán)境下,使用Qt5.8作為集成開發(fā)環(huán)境。CPU為Intel Core i7-7700HQ@2.8 GHz，內(nèi)存為8 GB，執(zhí)行效率約每秒1幀。

3.2 試驗(yàn)結(jié)果和分析

3.2.1 相機(jī)檢校結(jié)果

表1比較了全景相機(jī)中魚眼鏡頭的檢校結(jié)果。采用63個(gè)均勻分布的校準(zhǔn)點(diǎn)，根據(jù)不同的檢校模型，解算對(duì)應(yīng)的模型參數(shù)，并評(píng)估了檢校中誤差。本文方法的精度為0.56像素，明顯高于文獻(xiàn)[33]提出的通用相機(jī)檢校模型，并后繼保證了優(yōu)于一個(gè)像素的核線精度。

表1 本文使用的魚眼相機(jī)檢校方法檢校結(jié)果與文獻(xiàn)[33]對(duì)比

3.2.2 特征提取與匹配結(jié)果

圖3所示為Ladybug數(shù)據(jù)集使用三幀同時(shí)匹配的結(jié)果，由于魚眼相機(jī)變形較大，距離相機(jī)越近的地物變形越大，可以看出，特征點(diǎn)基本分布于距相機(jī)相對(duì)較遠(yuǎn)的地物上。圖中3號(hào)相機(jī)只匹配出了極少的特征點(diǎn)。這表明基于單相機(jī)SLAM容易受到匹配條件的困擾而追蹤失敗。而本文使用的多鏡頭組合式全景相機(jī)可依賴其余鏡頭捕捉到的足夠的特征點(diǎn)完成跟蹤和后繼平差。此外，由于本文根據(jù)幾何關(guān)系設(shè)定了恰當(dāng)?shù)乃阉鲄^(qū)間(算法1)，0號(hào)相機(jī)和4號(hào)相機(jī)的同名點(diǎn)也能夠進(jìn)行正確匹配。

圖3 連續(xù)三幀圖像所匹配的ORB特征點(diǎn)Fig.3 Matched ORB features on three adjacent frames

3.2.3 閉環(huán)探測(cè)結(jié)果

如圖4所示，藍(lán)色連線表示所探測(cè)的閉環(huán)，Kashiwa數(shù)據(jù)集共探測(cè)出52個(gè)閉環(huán)，Omiya數(shù)據(jù)集共探測(cè)出203個(gè)閉環(huán)。在SLAM系統(tǒng)中，一旦成功探測(cè)出閉環(huán)，將進(jìn)行相似變換關(guān)系的計(jì)算，傳播位姿并更新連接關(guān)系，連接關(guān)系更新后，便可避免重復(fù)的路徑中對(duì)于冗余的閉環(huán)探測(cè)過程(藍(lán)線所示)，重復(fù)路徑中的關(guān)鍵幀將更新與先前幀的相連關(guān)系，并統(tǒng)一參與位姿圖的優(yōu)化。

3.2.4 定位結(jié)果

表2為Kashiwa和Omiya圖像序列使用全景SLAM跟蹤所得位姿精度。僅用前兩幀的GPS、IMU數(shù)據(jù)作為SLAM軌跡的對(duì)齊，其他所有的GPS/IMU數(shù)據(jù)作為參考和精度評(píng)價(jià)。Kashiwa序列(1 km，498張全景影像)定位平均精度達(dá)到30 cm。Kashiwa序列具有很好的共視條件，不使用閉環(huán)糾正其精度依舊能達(dá)到46 cm。Omiya序列(9 km，7492張影像)僅在圖4所示處存在閉環(huán)，且運(yùn)行里程較長，因此閉環(huán)約束的范圍有限，無閉環(huán)糾正精度為42.7 m，加入閉環(huán)后精度為19 m。

事實(shí)上，在測(cè)繪中更加關(guān)注靜態(tài)的全局定位精度，而非機(jī)器人領(lǐng)域的動(dòng)態(tài)當(dāng)前定位精度。因此本文分別在Kashiwa與Omiya圖像序列上分別加入3個(gè)與15個(gè)均勻分布的GPS控制點(diǎn)進(jìn)行7參數(shù)轉(zhuǎn)換與整體平差，將SLAM軌跡納入到絕對(duì)坐標(biāo)系下，并統(tǒng)計(jì)更符合實(shí)際應(yīng)用的定位精度。如表3所示，兩套數(shù)據(jù)集的定位精度分別達(dá)到13 cm與16 cm。在角度上，車輛的姿態(tài)基本僅在水平方向上有旋轉(zhuǎn)，誤差主要集中在水平旋轉(zhuǎn)角Rz上。

表2 Kashiwa圖像序列與Omiya圖像序列數(shù)據(jù)集使用全景SLAM跟蹤所得結(jié)果，僅用前兩幀GPS/IMU數(shù)據(jù)對(duì)齊SLAM軌跡

Tab.2 Pose and map point location accuracy of the trajectories of Kashiwa and Omiya sequences, aligned by the given initial pose and scale

數(shù)據(jù)定位中誤差/m地圖點(diǎn)殘差/m位置/m姿態(tài)/(°)DxDyDzRxRyRzKashiwa加閉環(huán)0.30340.77200.14000.23220.01000.620.743.75無閉環(huán)0.46420.85430.47220.49110.01050.650.854.77Omiya加閉環(huán)19.529625.138414.231412.72644.11221.252.143.85無閉環(huán)33.058640.240620.990124.05798.57333.912.463.21

表3 Kashiwa圖像序列與Omiya圖像序列使用全景SLAM跟蹤所得結(jié)果，用最小數(shù)量均勻分布的GPS點(diǎn)7參數(shù)變換對(duì)齊SLAM軌跡

Tab.3 Pose and map point location accuracy of the trajectories of Kashiwa and Omiya sequences, minimum number of even distributed GPS points are used to align SLAM trajectories

數(shù)據(jù)定位中誤差/m地圖點(diǎn)殘差/m位置/m姿態(tài)/(°)DxDyDzRxRyRzKashiwa加閉環(huán)0.13400.30050.07860.108 0.01020.610.743.23 無閉環(huán)0.15680.32780.10570.11540.01020.620.813.01 Omiya加閉環(huán)0.16380.69690.11210.09990.00010.120.332.85 無閉環(huán)0.16810.73500.13540.09970.00010.910.461.991

特征法SLAM可得到稀疏地圖點(diǎn)云，如圖5所示。由于沒有實(shí)際的控制點(diǎn)評(píng)價(jià)物方定位精度，本文采用誤差傳播的方式，利用攝站定位誤差估計(jì)物方點(diǎn)誤差。表2中的地圖點(diǎn)殘差是與GPS/IMU作為真值前方交會(huì)出的地圖點(diǎn)坐標(biāo)進(jìn)行比較的結(jié)果。

圖5給出了Kashiwa與Omiya序列的使用全景SLAM系統(tǒng)得到的全局地圖全覽(包括關(guān)鍵幀的位姿及其連接關(guān)系與地圖點(diǎn)分布情況示意圖)，圖中Kashiwa序列(圖5(a))當(dāng)前幀為接近終點(diǎn)的第492幀，在無閉環(huán)糾正的情況下，尺度的漂移非常小。這歸功于全景相機(jī)的360°成像。圖5中綠色的線條顯示了關(guān)鍵幀的連接關(guān)系，同時(shí)也是位姿優(yōu)化中重要的圖連接關(guān)系。

圖6表示通過給定初始位姿態(tài)與方向，將SLAM的軌跡通過7參數(shù)轉(zhuǎn)換到地理參考坐標(biāo)系下與GPS的軌跡對(duì)比。其中，藍(lán)色曲線為GPS參考位置，紅色曲線為全景SLAM所跟蹤出的位姿結(jié)果?？梢钥闯?，Kahiswa序列由于極好的共視關(guān)系，跟蹤定位位置與GPS參考位置近乎完全重合，Omiya序列由于運(yùn)行里程較長、共視關(guān)系較差且無較好的閉環(huán)約束，出現(xiàn)了一定程度的尺度漂移。然而，在加入極少量(15個(gè))控制點(diǎn)后，定位精度變具有明顯的改善，與GPS參考軌跡一致(圖6(d))。

圖4 兩套數(shù)據(jù)集上所探測(cè)出的閉環(huán)Fig.4 Detected loops on two datasets

圖5 Kashiwa與Omiya序列使用全景SLAM系統(tǒng)跟蹤后得到的全局地圖總覽(關(guān)鍵幀及其連接關(guān)系，地圖點(diǎn)分布情況)Fig.5 Overall of global map of Kashiwa and Omiya sequences by panoramic SLAM system (status of current frame, tracked map points, tracked key frames and their connections)

圖6 兩個(gè)區(qū)域的SLAM結(jié)果以及加入3/15個(gè)控制點(diǎn)后的定位結(jié)果分別與GPS參考的對(duì)比Fig. 6 The comparison between GPS reference and SLAM results before and after GCP alignment

3.3 待改進(jìn)之處

本文采用BoW詞袋模型并參考ORB-SLAM閉環(huán)探測(cè)的方法對(duì)全景圖像序列進(jìn)行閉環(huán)探測(cè)。文獻(xiàn)[34]指出在復(fù)雜場(chǎng)景中閉環(huán)探測(cè)具有相當(dāng)?shù)碾y度，ORB-SLAM使用的閉環(huán)探測(cè)方法具有很大的局限性。這在全景SLAM中問題更加突出。如圖7所示，除了球面投影形變，地面物體的分布也發(fā)生了劇烈的變化。不過，歸功于全景相機(jī)的大范圍視場(chǎng)，未加入閉環(huán)約束的位姿跟蹤已可達(dá)到較為滿意的效果。

圖7 反向行駛拍攝的全景圖像Fig.7 Two panoramic images shot in the reverse directions

4 結(jié)束語

本文提出了基于多鏡頭組合式全景相機(jī)的全自動(dòng)SLAM系統(tǒng)。該系統(tǒng)能夠?qū)崿F(xiàn)全景圖像系列的定位和稀疏地圖重建，其定位精度接近GPS參考精度。相對(duì)于國際上主流的基于傳統(tǒng)平面相機(jī)的各類SLAM系統(tǒng)，如Mono-SLAM、Stereo-SLAM以及RGB-D SLAM，本文提出的全景SLAM可作為良好的補(bǔ)充，輔助或取代昂貴的GPS/IMU導(dǎo)航系統(tǒng)，特別是在GPS信號(hào)失鎖時(shí)。后期的工作將集中于全景相機(jī)影像的閉合探測(cè)，以增強(qiáng)幾何約束，得到更好的定位精度。