多鏡頭組合式相機的全景SLAM

季順平，秦梓杰

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

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

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

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

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

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

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

1.1 多鏡頭組合式全景相機模型

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

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

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

x′=mRiKi(x)+Ti

(1)

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

(2)

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

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

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

(3)

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

1.2 魚眼相機的檢校

內(nèi)參函數(shù)Ki可采用不同的相機檢校模型。魚眼鏡頭的內(nèi)參數(shù)同樣需要精確的檢校才能保證在一系列坐標(biāo)轉(zhuǎn)換中保持高精度。文獻[28]提出了一種通用魚眼相機檢校方法并得到廣泛的應(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是魚眼相機坐標(biāo)；θ和φ分別是天頂角和水平角。lk、mk、ik、jk為多項式系數(shù)。試驗表明，基于該方法檢校魚眼相機并生成核線立體像對，其核線誤差約1～1.5像素。為使得核線誤差降低到1像素以下，本文對改模型加以改進，提出一種更為精確的魚眼相機檢校方法。如式(5)所示

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

(5)

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

1.3 全景相機單像解析和前方交會

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

(6)

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

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

(7)

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

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

(8)

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

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

(9)

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

(10)

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

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

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

(11)

式中

(12)

(13)

e對于地圖點的雅克比矩陣為

(14)

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

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

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

(15)

圖2 構(gòu)建的優(yōu)化圖(三角形節(jié)點代表位姿，圓形節(jié)點代表地圖點，邊代表誤差項)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個線程并行工作，分別為跟蹤(tracking)，局部地圖構(gòu)建(local mapping)與閉環(huán)(loop closing)。跟蹤線程主要負責(zé)系統(tǒng)初始化，地圖點跟蹤，關(guān)鍵幀選取與位姿優(yōu)化等功能，局部地圖構(gòu)建線程主要負責(zé)特征點三角化，誤匹配點與冗余關(guān)鍵幀剔除，局部地圖優(yōu)化等功能，閉環(huán)線程主要負責(zé)閉環(huán)探測，閉環(huán)關(guān)鍵幀位姿改正與全局地圖優(yōu)化等功能。以下將逐一介紹各模塊的算法策略。

2.1 初始化

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

2.2 地圖點跟蹤

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

算法1 跟蹤地圖點

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

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

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

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

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

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

(16)

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

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

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

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

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

2.3 關(guān)鍵幀選取

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

(4) 連續(xù)性檢驗：認為孤立的候選組具有很強的不確定性。

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

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

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

3.1 試驗設(shè)計

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

Kashiwa圖像序列包含498張全景影像，攝影間隔約2 m；Omiya圖像序列包含7464張全景影像，拍攝間隔約為1 m。由于天空上沒有特征點，匹配在獨立的水平魚眼相機中執(zhí)行，而F矩陣計算、光束法平差都是在統(tǒng)一的全景坐標(biāo)系中進行。參考值由高精度的GPS/IMU組合導(dǎo)航系統(tǒng)獲得，且GPS經(jīng)過CORS站的事后改正，其絕對定位精度高于10 cm。GPS/IMU數(shù)據(jù)不參與SLAM流程。本文同時采用兩種精度評價方法進行評價，首先按照經(jīng)典的SLAM算法，給定初始位置，姿態(tài)與尺度信息將相對位姿統(tǒng)一到給定的坐標(biāo)系下，之后不再給出任何控制信息，并計算絕對定位均方誤差以衡量SLAM系統(tǒng)的實時動態(tài)定位精度?？紤]到在測繪中的應(yīng)用，全局定位精度更受關(guān)注，因此本文選取均勻分布的少量GPS點作為控制點，根據(jù)7參數(shù)相似變換將所有的SLAM后得到的外方位元素轉(zhuǎn)換到絕對坐標(biāo)系中，并計算所有相片外方位元素與參考值間的絕對值，作為精度評定指標(biāo)。本文的SLAM系統(tǒng)運行在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 試驗結(jié)果和分析

3.2.1 相機檢校結(jié)果

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

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

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

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

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

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

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

3.2.4 定位結(jié)果

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

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

表2 Kashiwa圖像序列與Omiya圖像序列數(shù)據(jù)集使用全景SLAM跟蹤所得結(jié)果，僅用前兩幀GPS/IMU數(shù)據(jù)對齊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地圖點殘差/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點7參數(shù)變換對齊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地圖點殘差/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可得到稀疏地圖點云，如圖5所示。由于沒有實際的控制點評價物方定位精度，本文采用誤差傳播的方式，利用攝站定位誤差估計物方點誤差。表2中的地圖點殘差是與GPS/IMU作為真值前方交會出的地圖點坐標(biāo)進行比較的結(jié)果。

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

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

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

圖5 Kashiwa與Omiya序列使用全景SLAM系統(tǒng)跟蹤后得到的全局地圖總覽(關(guān)鍵幀及其連接關(guā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 兩個區(qū)域的SLAM結(jié)果以及加入3/15個控制點后的定位結(jié)果分別與GPS參考的對比Fig. 6 The comparison between GPS reference and SLAM results before and after GCP alignment

3.3 待改進之處

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

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

4 結(jié)束語

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