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

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

一種相機(jī)位姿魯棒估計(jì)方法

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

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

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

位姿估計(jì);樣本點(diǎn)對;絕對定向;共線約束

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

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

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

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

1 位姿估計(jì)

1.1 問題陳述

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

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

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

由余弦定理有：

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

圖1 位姿估計(jì)示意圖

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

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

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

1.2 三點(diǎn)迭代算法

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

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

令

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

將此方程組寫成矩陣形式

由于

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

令

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

這樣,迭代計(jì)算公式可寫成如下形式

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

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

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

1.3 絕對定向

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

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

此處的2個(gè)點(diǎn)集已消除了比例變換,即λ= 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)行整理,得

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

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

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

1.4 迭代優(yōu)化

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

式中：FΔx、FΔy即為像點(diǎn)橫、縱坐標(biāo)的實(shí)際值與理論值的偏差;x、y為中心化的像點(diǎn)坐標(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)化為線性最小二乘問題。以各樣本點(diǎn)對根據(jù)三點(diǎn)直接算法求得的位姿初值為基礎(chǔ),根據(jù)線性化后的式(13)構(gòu)造約束方程組,用線性最小二乘方法解該方程組得到位姿校正量,以此更新位姿參數(shù);多次迭代直至校正量足夠小,從而得到位姿的最優(yōu)解。

2 實(shí)驗(yàn)結(jié)果

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

2.1 仿真實(shí)驗(yàn)

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

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

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

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

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

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

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

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

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

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

上述解算過程使算法對外點(diǎn)具有很強(qiáng)的抗性,保證了算法的魯棒性。

2.2 實(shí)際數(shù)據(jù)實(shí)驗(yàn)

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

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

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

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

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

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

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

3 結(jié) 論

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

References)

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

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