楊阿華, 李學(xué)軍, 劉 濤, 劉春曉

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

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

楊阿華1, 李學(xué)軍2, 劉 濤2, 劉春曉3

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

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

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

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

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

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

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

1 位姿估計

1.1 問題陳述

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

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

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

由余弦定理有：

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

圖1 位姿估計示意圖

根據(jù)空間點、像點和投影中心間的共線關(guān)系有

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

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

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次迭代后的結(jié)果。

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

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

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

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

1.3 絕對定向

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

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

此處的2個點集已消除了比例變換,即λ= 1。在此基礎(chǔ)上,本文通過分解旋轉(zhuǎn)矩陣,根據(jù)優(yōu)化目標(biāo)構(gòu)造線性約束方程,采用線性最小二乘優(yōu)化方法求解各未知量。

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

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

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

引入輔助參數(shù)u、v、w,設(shè)：

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

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

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

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

1.4 迭代優(yōu)化

為了減小樣本點坐標(biāo)的隨機誤差對解算結(jié)果造成的影響,以共線方程為基礎(chǔ),最終得到誤差函數(shù)為：

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

以最小化像平面重投影誤差作為優(yōu)化目標(biāo),即

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

2 實驗結(jié)果

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

2.1 仿真實驗

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

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

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

圖2 位姿解算結(jié)果

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

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

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

圖3 位姿解算結(jié)果

圖4 位姿解算結(jié)果

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

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

2.2 實際數(shù)據(jù)實驗

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

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

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

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

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

表1 測試點集空間坐標(biāo)解算誤差 m

圖5 三維重建結(jié)果

3 結(jié) 論

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

References)

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

[2]肖永亮,蘇顯渝,薛俊鵬,等.基于凸松弛全局優(yōu)化算法的視覺測量位姿估計[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-),男,博士研究生.主要研究方向：計算機圖形,圖像處理.李學(xué)軍,男,教授,博士生導(dǎo)師.