一種相機位姿魯棒估計方法

楊阿華, 李學軍, 劉 濤, 劉春曉

(1.裝備學院研究生管理大隊,北京101416; 2.裝備學院信息裝備系,北京101416; 3.重慶北碚區軍事代表室,重慶400700)

一種相機位姿魯棒估計方法

楊阿華1, 李學軍2, 劉 濤2, 劉春曉3

(1.裝備學院研究生管理大隊,北京101416; 2.裝備學院信息裝備系,北京101416; 3.重慶北碚區軍事代表室,重慶400700)

針對位姿估計算法對魯棒性和精度要求高的特點,提出了一種魯棒的高精度位姿估計方法：該方法以3個樣本點對為基礎,首先假設各空間點到投影中心的距離近似相等,然后根據像點與空間點間的幾何約束關系建立方程組并推導迭代公式,進而迭代求解各空間點到投影中心的距離;以此為基礎,提出了一種最小二乘絕對定向方法來獲取相機的初始位姿。最后根據空間點與像點之間的共線約束進行迭代優化,得到最終的相機位姿。測試表明,該方法在滿足假設的條件下能很好地收斂,并達到較高的解算精度。

位姿估計;樣本點對;絕對定向;共線約束

位姿估計是攝影測量學、計算機圖形學、機器人導航、無人機空中自主加油以及空間交會對接等領域中的一個核心問題。位姿估計是指求解2個三維坐標系之間的相對位置與姿態,包括3個沿坐標軸方向的平移量和3個繞坐標軸的旋轉角?；谔卣鼽c的單目視覺位姿估計方法是利用單個相機對目標上3個以上的特征點成像,然后利用特征點成像坐標和特征點間的幾何位置關系及相機參數解算相機與目標之間的相對位姿。由于其結構簡單和易于實現,該方法成為視覺測量領域的研究熱點之一,并廣泛應用于三維測量[1-2]和三維拼接等領域中。

基于特征點的單目視覺位姿估計是經典的PnP(perspective-n-point)問題[3],即由透視N點來求解各點到投影中心的距離,該問題是在1981年首先由Fischler等人[4]提出,隨后眾多國內外學者給出了大量的PnP問題求解方法,這些方法一般可分為非迭代方法(閉式解法)和迭代方法(數值解法)2大類。

非迭代方法通過構造多項式方程組來解算相對位姿,通常適用于特征點數目較少的情況(如3～5個點),同時推導出了很多種閉式解法[5-6],也得出了位姿解的數量和唯一解條件等重要結論。該類方法具有運算量小、實時性好等優點,但受圖像誤差影響較大且精度不高,而且對野值(錯誤的特征點對應)比較敏感,只可用于迭代方法的初始值計算。

雖然利用3～5個特征點就可以完成位姿估計,但很多學者還是希望利用更多數量特征點,通過引入冗余來克服圖像噪聲和野值的影響,提高位姿解的精度。一般做法是采用多點迭代方法[7-9]求解PnP問題,將其表示為受約束的非線性優化問題。該類方法通常適用于特征點數目較多的情況,而且對噪聲和野值具有較好的魯棒性,其思路一般是首先構造目標函數,然后利用優化理論最小化目標函數得出位姿解。

1 位姿估計

1.1 問題陳述

已知地面控制點p,其世界坐標為(x,y,z), p點在相片上的投影點為p′,假設相機已經進行了內標定,即相機的內參數和畸變系數已知,由像素坐標和相機內參數,通過內定向可以得到p′在相機坐標系下的坐標(x′,y′,z′)。稱p和p′為1個樣本點對。

位姿估計是在已知若干樣本點對后,求解相機坐標系相對于世界坐標系的位置和姿態的過程。如圖1所示,x、y、z和x′、y′、z′分別為世界坐標系和相機坐標系3軸,ABCD為成像平面,空間3點p1、p2、p3對應的像點為p′1、p′2、p′3, 3個空間點的投影射線向量分別為、,記對應的單位向量為V1、V2、V3,有

3個向量對應的夾角分別為α、β、γ,則cosα= V1·V2,同理可得cosβ、cosγ。

由余弦定理有：

式中：d1、d2、d3為3個空間點相互之間的距離,可以由空間點的坐標求得;l1、l2、l3為投影中心o′到空間點的距離。

圖1 位姿估計示意圖

根據空間點、像點和投影中心間的共線關系有

式中：R、T分別為旋轉矩陣和平移向量。若已知l1、l2、l3,則由式(2)來確定R、T是一個絕對定向的過程。因此,位姿估計的關鍵是確定參數l1、l2、l3。

遵循一般的求解流程,本文首先采用一種三點線性迭代算法求解各空間點到投影中心的距離,然后通過絕對定向得到相機位姿初始值;以此為基礎,根據共線約束,以最小化像平面重投影誤差為優化目標,通過非線性迭代優化來獲取位姿的最優解。

1.2 三點迭代算法

采用一種線性迭代的方法來求解各控制點到投影中心的距離li。此種方法只需3個控制點,不涉及線性方程組的求解,解算速度快。

已知3個樣本點對,可以得到如式(1)所示的3個方程。改寫方程組中的方程,以第1式為例,因為,因此,第1式可寫為

令

代入式(5),從而可將式(1)改寫成如下的形式

將此方程組寫成矩陣形式

由于

代入式(6),則可得到求解空間點到投影中心距離li的迭代計算公式

令

式中：l(k)1、l(k)2、l(k)3、n(k)、x(k)為各變量第k次迭代后的結果。

這樣,迭代計算公式可寫成如下形式

在視覺測量領域,控制點相互之間到投影中心的距離差相比于到投影中心的絕對距離一般都較小。利用這一特點,可假設初始迭代時,即當k=0時,l1≈l2≈l3,從而有：

有了上面的初始條件,就可以根據式(7)對待求的li進行迭代解算。當為某一預設的門檻)時,迭代結束。測試表明,迭代5次左右即可結束。

相比文獻[10]中提到的方法,此種方法更為簡單,高效。值得注意的是,若3個樣本點共線或近似共線,則迭代過程不收斂,一般的做法是從所有樣本點中取3點,保證3點在圖像平面上所成三角形面積最大。

1.3 絕對定向

在獲取了各空間點到投影中心的距離后,根據式(2)求解R、T就是一個絕對定向問題,即確定2個空間點集間的旋轉、平移、比例變換關系,優化目標可表示為

式中：λ為比例系數。常用的絕對定向方法有四元數法[11-12]和SVD法[13-14]等。

此處的2個點集已消除了比例變換,即λ= 1。在此基礎上,本文通過分解旋轉矩陣,根據優化目標構造線性約束方程,采用線性最小二乘優化方法求解各未知量。

由于旋轉矩陣是正交矩陣,因此R可用反對稱矩陣表示為

設p′i=liVi=(x′iy′iz′i),pi=(xiyizi), T=(t1t2t3),因此式(2)可寫為

將S代入式(9),并按已知量(pi、p′)和未知量(a、b、c、t1、t2、t3)進行整理,得

引入輔助參數u、v、w,設：

代入式(10),并進行整理,得

一個樣本點對對應一個式(9)所示的方程,進而得到3個式(11)中的方程,則N個樣本點對可以構造包含3N個方程的約束方程組,運用線性最小二乘方法求解該方程組,可以得到6個未知數a、b、c、u、v、w的最優解。旋轉矩陣由式(8)得到,保證所得矩陣為正交矩陣;平移向量元素t1、t2、t3可由式(12)得到

文獻[11]和文獻[13]的方法需首先將點集中心化來消除平移變換,然后再單獨解算旋轉變換參數;上述方法無須中心化,而是同時求解平移和旋轉變換參數,計算量也相對較小。

如果采用1.2節的迭代方法,則可由3個樣本點對解算l1、l2、l3,進而得到9個由式(11)所確定的約束方程。采用最小二乘法解該方程組,并結合式(12),即可求得相機位姿的初始值。三點迭代算法加上絕對定向,即構成了位姿估計的三點直接算法。

1.4 迭代優化

為了減小樣本點坐標的隨機誤差對解算結果造成的影響,以共線方程為基礎,最終得到誤差函數為：

式中：FΔx、FΔy即為像點橫、縱坐標的實際值與理論值的偏差;x、y為中心化的像點坐標;a1、a2、a3、b1、b2、b3、c1、c2、c3為旋轉矩陣元素;t1、t2、t3為平移向量元素。

以最小化像平面重投影誤差作為優化目標,即

根據非線性最小二乘理論可以求解該優化問題。通過將FΔx、FΔy一階泰勒展開,可將其轉化為線性最小二乘問題。以各樣本點對根據三點直接算法求得的位姿初值為基礎,根據線性化后的式(13)構造約束方程組,用線性最小二乘方法解該方程組得到位姿校正量,以此更新位姿參數;多次迭代直至校正量足夠小,從而得到位姿的最優解。

2 實驗結果

本文采用仿真數據和實際數據2種方法來驗證算法的有效性。

2.1 仿真實驗

仿真數據生成：在坐標范圍為x∈[-1 000, 1 000],y∈[-1 000,1 000],z∈[450,500]的立方體內產生10 000個隨機分布的三維點;隨機生成1 000個相機的姿態角及其空間位置,其中3個姿態角限定在[-10°,10°]范圍內,空間位置限定在x∈[-200,200],y∈[-200,200],z∈[-10, 10]的范圍內,以保證在相機視野內有足夠多的空間點,并使視野內的空間點到投影中心的距離相差不大;相機焦距設為6 000像素,分辨率設為4 000像素×3 000像素,由以上數據可以得到每個相機的投影矩陣。根據投影矩陣可以獲得每個在相機視野內的空間點所對應的像點坐標,并在橫縱方向對像點坐標均分別加上均值為0、標準差為σ的高斯隨機噪聲,σ的取值在一定范圍內(小于10),否則即為外點。從中隨機取N個點(N≥3,根據實驗需要確定N)來進行位姿解算。

相機位置誤差通過公式d=‖Ti-T‖/‖T‖來衡量,其中Ti、T分別為解算的相機位置和理想的相機位置;相機姿態誤差通過計算矩陣RiRT對應的3個歐拉角構成的向量的模來衡量,即：r=‖dω dφ dκ‖,Ri、R分別為解算的相機姿態角對應的旋轉矩陣和理想姿態角對應的旋轉矩陣。

圖2為σ=0,1,…,10時,采用本文提出的三點直接算法及DLT(direct linear transform)算法解算位姿的結果,對于三點直接算法和DLT算法,N分別為3和6,即為2種算法最少所需的點數(已經剔除了N=6時控制點近似共面的情況,從而使DLT算法適用)。圖2(a)是位置誤差曲線,圖2(b)是旋轉誤差曲線。每個σ處的誤差均是用1 000個相機進行計算所得的平均結果。從圖2中曲線可知,本文算法的旋轉誤差與DLT算法相近,但位置誤差要優于DLT算法,此外本文算法只需3個樣本點對,而DLT則需要6個樣本點對才能解算。

圖2 位姿解算結果

圖3 為N=15,σ=0,1,…,10時,只采用三點直接算法(此時只需3個點)和三點直接算法加上基于共線約束的非線性迭代優化后的解算結果。圖3(a)是位置誤差曲線,圖3(b)是旋轉誤差曲線。每個σ處的誤差是用1 000個相機進行計算所得的平均結果。從圖3中可以看出,加上非線性優化后,解算精度顯著提高,對隨機誤差的抗性顯著增強。

圖4為σ=1.5時,隨N的增加,采用三點直接算法及直接算法與迭代優化相結合的方法解算位姿的結果。

從圖4中可以看出,當N＞7,隨樣本點數的增加,解算精度并沒有顯著提高。所以直接與迭代結合方法對樣本點數不是很敏感,一般N＞7即可達到較高的精度;此外,三點直接算法隨N的增加,精度也有所改善,這是由于在實現三點算法時,采用的是從N個樣本點中取3個點,并保證3點所成三角形面積最大,即保證3個點分布范圍盡量廣,從而有效改善了三點算法的解算精度;從這一結果也可以看出,對于三點算法,3點分布越廣,越有利于提高解算的精度。

圖3 位姿解算結果

圖4 位姿解算結果

由以上數據可知,本文方法僅需3點即可達到較高的解算精度,可將其與RANSAC(Random sample consensus)算法結合。對于有少量明顯誤點的樣本點集,從點集中隨機選取不共線的3點。利用直接解算與迭代優化的方法求解位姿,并根據其他樣本點對的像平面重投影誤差來判斷解算結果的正確性：若誤差在合理范圍內的樣本數達到一定比例,則認為是合理解;多次重復上述過程,并選取均方重投影誤差最小的合理解作為最終的結果,同時剔除重投影誤差較大的樣本點對。

上述解算過程使算法對外點具有很強的抗性,保證了算法的魯棒性。

2.2 實際數據實驗

采用威海天福山一塊面積為2 km2的試驗場,場內布有179個基本控制點。從中選取20個控制點作為測試點集,其余的作為訓練集。用飛機對場區進行航拍,得到了5條航線、共115幅航空影像;所用相機為哈蘇H4D-60,鏡頭焦距為50 mm,分辨率為8 956像素×6 708像素。影像的航向重疊率約為60%,旁向重疊率大于20%,影像空間分辨率約為0.05 m/像素,其中有107幅影像上分布了不少于3個的訓練控制點(很多影像上僅有3個控制點),某些區域由于樹木的遮擋,控制點無法清晰辨識;通過標定實驗得到了相機的內參數和畸變系數。

人工拾取每幅影像中控制點的相片坐標,利用上述直接與迭代結合的方法,解算每幅有效影像對應的相機位姿。以這些位姿為基礎,利用同名射線相交法(空間前方交會)反算測試點集中控制點的空間坐標,表1是解算結果與真實結果間的誤差比較。

從表1數據中可以看出,大部分點的解算誤差都在0.1 m(2個像素)以內,可見解算的各相機方位參數精度較高;個別點(如162)距離誤差較大(0.830),這是因為引起誤差的原因不僅包括相機位姿解算的誤差,還包括相機標定誤差、控制點像坐標拾取誤差及控制點空間坐標的量測誤差等。

以解算的相機位姿為基礎,通過提取相鄰航片的同名像點,并利用射線相交法解算每對同名點的空間坐標,進而得到覆蓋全場區的空間點云,對點云進行三角構網、紋理映射,得到場區的三維地形,如圖5所示。圖5(a)是三角構網后的地形圖,圖5(b)是紋理映射后的三維景觀圖。

從圖5中可看到高度真實感的三維場景,這些都得益于高精度的相機位姿參數,從而進一步證明了所提算法的有效性。

表1 測試點集空間坐標解算誤差 m

圖5 三維重建結果

3 結 論

本文所提位姿估計方法精度高、魯棒性高、計算量小,最少只需3個控制點即可進行解算。由于所需的點數少,所以算法對外點(誤差很大的點)的抗性很強,通過與RANSAC算法結合,可以有效地消除少量外點的影響。所提直接最小二乘絕對定向方法解算精度高,可以達到直接與迭代相結合方法同等的精度。在攝影測量領域,布設、測量地面控制點費時費力,而此算法只需3個控制點即可解算,減少了外業工作量。傳統的DLT算法對于平面結構(控制點共面)不適用,并且需要6個控制點才能進行解算,與之相比,本文算法具有更廣泛的適用性。不足之處是,此方法只適用于3個控制點到投影中心距離相差不大的情況,航空攝影測量中一般都滿足這一條件。對于近景攝影測量,若控制點在景深方向的分布范圍太廣,則此算法不再適用。

References)

[1]黃邦奎,劉震,張文軍.多傳感器線結構光視覺測量系統全局校準[J].光電子·激光,2011,22(12)：1816-1820.

[2]肖永亮,蘇顯渝,薛俊鵬,等.基于凸松弛全局優化算法的視覺測量位姿估計[J].光電子·激光,2011,22(9)：1384-1389.

[3]HARTLEY R I,ZISSERMAN A.Multiple view geometry in computer vision[M].Cambridge：Cambridge University Press,2004：121-132.

[4]FISCHLER M A,BOLLES R C.Random sample consensus：a paradigm for model fitting with applications to image analysis and automated cartography[J].Communications ACM, 1981,24(6)：381-395.

[5]KNEIP L,SCARAMUZZA D,SIEGWART R.A novel parameterization of the perspective-three-point problem for a direct computation of absolute camera position and orientation[C]//Computer Vision and Pattern Recognition (CVPR).2011 IEEE Conference on.Providence.RI：IEEE, 2011：2969-2976.

[6]KUKELOVA Z,BUJNAK M,PAJDLA T.Closed-form solutions to the minimal absolute pose problems with known vertical direction[C]//Computer Vision-ACCV 2010.Berlin Heidelberg：Springer,2011：216-229.

[7]VINCENT L,FRANCESC M N,PASCAL F.EPnP：an accurate O(n)solution to the PnP problem[J].International Journal of Computer Vision,2009,81(2)：155-166.

[8]HESCH J A,ROUMELIOTIS S I.A direct least-squares (DIS)method for PnP[C]//Computer Vision(ICCV).2011 IEEE International Conference on.Barcelona：IEEE,2011：383-390.

[9]OLSSON C,KAHL F,OSKARSSON M.Optimal estimation of perspective camera pose[C]//ICPR.18thInternational Conference on.Hong Kong：IEEE,2006：5-8.

[10]QUAN L,LAN Z.Linear N-point camera pose determination[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,1999,21(8)：774-780.

[11]HORN B K P.Closed-form solution of absolute orientation using unit quaternion[J].J.Optical Soc.Am.,1987,4：629-642.

[12]WALKER M W,SHAO L,VOLZ R A.Estimating 3D location parameters using dual number quaternion[J].CVGIP：Image Understanding,1991,54(3)：358-367.

[13]HORN B K P,HILDEN H M,NEGAHDARIPOUR S. Closed-Form solution of absolute orientation using orthonomal matrices[J].J.Optical Soc.Am.,1988,5：1127-1135.

[14]ARUN K S,HUANG T S,BLOSTEIN S D.A Leastsquares fitting of two 3D point sets[J].IEEE Trans.Pattern Analysis and Machine Intelligence,1987,9：698-700.

(編輯：孫陸青)

A Robust Method for Camera Pose Estimation

YANG Ahua1, LI Xuejun2, LIU Tao2, LIU Chunxiao3
(1.Department of Graduate Management,Equipment Academy,Beijing 101416,China; 2.Department of Information Equipment,Equipment Academy,Beijing 101416,China; 3.Military Representative Office of Chongqing Beibei District,Chongqing 400700,China)

Against the requirement of robustness and accuracy of pose estimation,a robust and accurate pose estimation method is proposed.The method is based on 3 sample point pairs.First,the distance of every spatial point to the projection center is assumed to be nearly the same.Then the equation group,from which the iteration formula is derived,is constructed according to the geometry constraint relationship between the image points and the spatial points.Subsequently,the distance between the spatial points and the projection center is solved iteratively.Based on the above result,a least square absolute orientation method is proposed to solve the initial pose of the camera.Finally, the initial camera pose is optimized iteratively based on the colinearity constraint between the spatial points and the image points.Experiment results illustrate that,in the assumption condition,the proposed method can converge well and achieve high accuracy.

pose estimation;sample point pair;absolute orientation;colinearity constraint

TP 391.41

2095-3828(2014)01-0088-07

ADOI10.3783/j.issn.2095-3828.2014.01.020

2013-05-22

楊阿華(1985-),男,博士研究生.主要研究方向：計算機圖形,圖像處理.李學軍,男,教授,博士生導師.