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

季順平，秦梓杰

武漢大學遙感信息工程學院，湖北 武漢 430079

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

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

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

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

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

1 全景相機的成像、檢校與優化模型

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

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

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

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

x′=mRiKi(x)+Ti

(1)

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

(2)

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

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

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

(3)

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

1.2 魚眼相機的檢校

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

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

(5)

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

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

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

(6)

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

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

(7)

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

當已知相機位姿的初始值以及全景球面上的特征點所對應的地圖點時，采用最小二乘法構建目標函數使球面重投影誤差最小

(8)

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

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

(9)

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

(10)

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

1.4 全景相機的非線性優化方法

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

(11)

式中

(12)

(13)

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

(14)

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

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

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

(15)

圖2 構建的優化圖(三角形節點代表位姿，圓形節點代表地圖點，邊代表誤差項)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流程

系統將分為3個線程并行工作，分別為跟蹤(tracking)，局部地圖構建(local mapping)與閉環(loop closing)。跟蹤線程主要負責系統初始化，地圖點跟蹤，關鍵幀選取與位姿優化等功能，局部地圖構建線程主要負責特征點三角化，誤匹配點與冗余關鍵幀剔除，局部地圖優化等功能，閉環線程主要負責閉環探測，閉環關鍵幀位姿改正與全局地圖優化等功能。以下將逐一介紹各模塊的算法策略。

2.1 初始化

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

2.2 地圖點跟蹤

地圖點的跟蹤是SLAM自動化的關鍵環節。在算法1中，列出了全景相機地圖跟蹤的算法。

算法1 跟蹤地圖點

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

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

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

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

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

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

(16)

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

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

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

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

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

2.3 關鍵幀選取

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

算法2 關鍵幀選取的判別規則

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

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

(3) 當前幀跟蹤到的地圖點數小于上一幀跟蹤到的點數的85%且局部地圖構建線程空閑。

(4) 當前幀跟蹤到的地圖點數小于上一幀跟蹤到的50%，中斷局部地圖構建并插入關鍵幀。

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

2.4 局部地圖構建

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

2.5 閉環探測與全局優化

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

算法3 閉環探測流程

檢測到新關鍵幀插入：

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

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

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

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

(5) 通過連續性檢驗的候選關鍵幀均認為與當前幀構成閉環。

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

3 試驗結果和分析

3.1 試驗設計

本文試驗選取了兩組數據均采集自車載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。由于天空上沒有特征點，匹配在獨立的水平魚眼相機中執行，而F矩陣計算、光束法平差都是在統一的全景坐標系中進行。參考值由高精度的GPS/IMU組合導航系統獲得，且GPS經過CORS站的事后改正，其絕對定位精度高于10 cm。GPS/IMU數據不參與SLAM流程。本文同時采用兩種精度評價方法進行評價，首先按照經典的SLAM算法，給定初始位置，姿態與尺度信息將相對位姿統一到給定的坐標系下，之后不再給出任何控制信息，并計算絕對定位均方誤差以衡量SLAM系統的實時動態定位精度。考慮到在測繪中的應用，全局定位精度更受關注，因此本文選取均勻分布的少量GPS點作為控制點，根據7參數相似變換將所有的SLAM后得到的外方位元素轉換到絕對坐標系中，并計算所有相片外方位元素與參考值間的絕對值，作為精度評定指標。本文的SLAM系統運行在Ubantu16.04環境下,使用Qt5.8作為集成開發環境。CPU為Intel Core i7-7700HQ@2.8 GHz，內存為8 GB，執行效率約每秒1幀。

3.2 試驗結果和分析

3.2.1 相機檢校結果

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

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

3.2.2 特征提取與匹配結果

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

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

3.2.3 閉環探測結果

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

3.2.4 定位結果

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

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

表2 Kashiwa圖像序列與Omiya圖像序列數據集使用全景SLAM跟蹤所得結果，僅用前兩幀GPS/IMU數據對齊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

數據定位中誤差/m地圖點殘差/m位置/m姿態/(°)DxDyDzRxRyRzKashiwa加閉環0.30340.77200.14000.23220.01000.620.743.75無閉環0.46420.85430.47220.49110.01050.650.854.77Omiya加閉環19.529625.138414.231412.72644.11221.252.143.85無閉環33.058640.240620.990124.05798.57333.912.463.21

表3 Kashiwa圖像序列與Omiya圖像序列使用全景SLAM跟蹤所得結果，用最小數量均勻分布的GPS點7參數變換對齊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

數據定位中誤差/m地圖點殘差/m位置/m姿態/(°)DxDyDzRxRyRzKashiwa加閉環0.13400.30050.07860.108 0.01020.610.743.23 無閉環0.15680.32780.10570.11540.01020.620.813.01 Omiya加閉環0.16380.69690.11210.09990.00010.120.332.85 無閉環0.16810.73500.13540.09970.00010.910.461.991

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

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

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

圖4 兩套數據集上所探測出的閉環Fig.4 Detected loops on two datasets

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

3.3 待改進之處

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

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

4 結束語

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