基于已知兩個方向角的相對位姿標定

陸義云

（南京郵電大學 自動化學院，江蘇 南京 210046）

0 引言

多個點的PnP問題已經得到很廣泛的研究，對于所謂的PnP問題[3]，在計算機視覺一般描述為：給定一個世界坐標系中的已知點集和它們在像平面對應的投影，求在世界坐標系和攝像機坐標系之間的變換矩陣（3個旋轉參變量和3個平移參變量）。由于PnP問題在目標跟蹤、物體識別、視覺導航等許多領域有著廣泛的應用前景，因此一直是計算機視覺中的一個重要問題。目前，在計算機視覺方面，對于PnP問題的研究存在許多方法，比較流行的有P3P問題和P4P問題,對于P3P問題解的研究也取得了眾多的成果。在國內，2006年吳毅紅等利用正交變換下求解正解等價于求解基于變換定義下的旋轉矩陣和平移向量；不過吳毅紅等只指出了解的上限，沒有作詳細的證明；在國外，Zuzana Kekelova[15]等人對于基于未知焦距的P3P問題也作了研究[13]。對于P4P問題的研究，T.Pajdla[13]等人提出了基于未知焦距的P4P方法，Bujnak M[14]等人提出了基于未知焦距和徑向畸變的P4P問題的方法。但無論是基于3點的PnP問題還是基于4點的PnP問題，它們有一個共同的缺點，就是要求的計算量都比較大，運算速度比較慢，此外還會受到各種條件的影響。本論文中提供了一種基于2點的PnP的方法來求解相機的旋轉矩陣和平行向量。

本文中，研究已知兩個方向下相對位姿標定問題的計算。它主要是受到手機重力感應技術和裝有慣性測量單元（IMU）的智能手機（如NOKIA, iPhone）啟發。在智能手機中主要是由加速度計測量地球重力矢量測量到兩個方向角度（如在iPhone中采用的加速度計是3軸加速度計，分別X，Y和Z軸，這3個軸構成的立體空間以偵測你在iPhone上各種動作，實際應用時通常以這3個軸或任意兩個軸所構成的角度來計算iPhone傾斜的角度[0]）。本文中給定的P2P問題，最終轉化為求解多項式方程組的問題，通過利用Grobner基法[2，9]求解。

1 相機標定

攝像機的基本成像模型，通常稱為基本針孔模型，由三維空間到平面的中心投影變換所給出，令空間點Oc是投影中心，它到像平面π的距離為f，空間點X在平面π上的投影（或像）u是以點Oc為端點并經過Xw的射線與平面π的交點[1,11]，則由空間點Xw到圖像點u的投影矩陣可以寫成：

P是3×4投影矩陣，其可表示成 ：

其中，λ為深度因子，K為攝像機內參矩陣，t=[tx,ty,tz]T為平移向量，R是一個3×3的正交矩陣，對于攝像機內參矩陣K，可以寫成：

式（3）中fu為圖像橫坐標軸尺度因子，fv為圖像縱坐標軸尺度因子。s為歪斜因子，表示兩個坐標軸的垂直程度，（u0,v0）表示主點橫坐標和縱坐標。

對于旋轉矩陣R，在三維空間中，可以用歐拉角（α、β、γ）來表示，其中α（也稱roll）為繞Z軸旋轉角，γ（pitch）為繞X軸旋轉角，β（yaw）為繞Y軸旋轉角，R可以寫成：

其中：繞Z坐標軸旋轉矩陣為：

繞X坐標軸旋轉矩陣為：

繞Y坐標軸旋轉矩陣為：

現在基于重力感應的手機（如iphone，NOKIA等）可以利用加速度計測量地球重力矢量來測量到兩個方向角度，即可測量到繞Z坐標軸的旋轉角α和繞X坐標軸旋轉角γ。對于一個P2P問題的攝像機，其繞X和Z這兩個坐標軸轉動的角度為Roll和Pitch，即垂直方向已知，求攝像機的絕對位置。因此在式（4）中β是唯一的參數。

對于尺度因子λ的消去，通常采用反對稱矩陣[u]×與方程相乘的方法，因此可以得到：

其中反對稱矩陣為：

因此式（9）可以寫成：

在式(10)中，由于對稱矩陣的秩為2，因此在（11）式得到的3個多項式方程中，其中2個是線性不相關的。在這3個多項式方程組中，含有q2，q，tx，ty，tz這幾個變量。在此，可以運用Grobner基的方法求解多項式方程組。

2 Grobner 基求解多項式方程組

代數方程組的求解是許多領域中經常要處理的問題，而當變元很多時,求解過程往往比較困難。為此，我們可以使用Grobner基[2-3,5]的方法求解代數方程組。

將多項式加上“=0”即變成方程組，所以方程組和多項式的關系是非常密切，為了解多元多次方程組的問題，我們可以考慮將它轉化為多項式問題。同時有必要定義個一個多元多次多項式中每個單項的排列次序，即單項式的大小[4,6,8-9,11]。在這方面一般定義方法為：

定義：

定義兩個單項式的大小順序為x1＞x2＞…xn，再者，同一未知數定義xm+1＞xm＞…x2＞x＞1，我們稱α＞β，這種次序大小稱為字典序（簡稱lex order）。

定義：設K是一個域，k[x1,x2,…,xn]為系數屬于K的n元多次多項式所成集合，即：

（a）0∈I；

（b）若f,g∈I，則f+g∈I；

（c）若f,g∈I，且則。

定義：

lp(f)=X a1，即lp(f)表示f的首項冪積；

lc(f)=a1，即lc(f)表示f的首項系數；

lt(f)=a1X a1，即lt(f)表示f的首項；

假設域K上的多項式方程組Pi(X)=0,i=1, 2, …,m，要求解這樣一個方程組，需要解決一些問題：該方程組有多少解，怎么求解。考慮由該方程組的多項式生成一組理想，計算某字典序下的Grobner基G，則上述的多項式方程組問題可以通過G來求解，下面給出方程組的求解方法，設給定域K[X]中的一個方程組：

Pi(X)=0,i=1, 2, …,m；

選取字典序x1＞x2＞…xn，計算理想[2]＜p1,p2,…pm＞ 的Grobner基G={G1,G2, …,Gr}，對G中的多項式適當排序后，存在Gi∈G,與mi使得lp（Gi）=，i=1,2,…,n,則必有n≤r，在字典序[2]x1＞x2＞…x下，lp（G）=，Gn的其它項也必含xn，即Gn是xn的一元多項式。同理Gn-1僅xn-1,xn類推下去，可知G包含如下多項式方程組：

由最后一個方程Gn=0解一組xn的解（an1,an2,…,anl），再將每個ani(i=1, 2, …,l)代入Gn-1以及其它G中僅含xn-1，xn的多項式Gj=（xn-1,xn）=0,…,可得xn-1的方程組Gn-1=（xn-1,ai）=0，Gj=（xn-1,anj）=0, …，仍然是一個一元多項式方程組，解之又可得到對應xn-1的解，繼續下去即可得到原方程組的全部解。

3 仿真及分析

本文運用Grobner基算法，采用的圖像如圖1所示，使用iphon4手機拍攝測試圖片，在拍攝測試圖片時，經過一定計算可得，攝像機繞Z坐標軸選擇角度約為 pitch=-139.41°，繞 X坐標軸旋轉角度約為roll=25.83°，建立如圖1所示的三維坐標系，選取3組2D-3D對應點，圖像點 A坐標（129.1875,634.9875）對應空間點坐標（130,0,0）。圖像 C 點（664.1250,350.1250）對應空間點坐標為（130,66,0），圖像 D 點坐標（1424.9755，1769.3000）對應空間點為（0,66,146），其中D點作為驗證點，此時已知相機的內部參數矩陣為：

選取A點和C點作為測試點，利用Groebner基求可得解約為：

即此時繞Y坐標軸旋轉角度約為yaw=-25.42，此時旋轉矩陣為：

平移向量為T=[10.5492 14.4406 181.9821]

將D點代入到其中驗證可得D點的重投影在圖像上的坐標為（1420.9671,1768.2998），即所得點的坐標在誤差允許的范圍之內。

圖1 測試用圖

4 結論

計算機視覺中，PnP問題的應用越來越廣泛，本文中利用一些設備來獲取兩個角度，并利用2組2D-3D的對應點來解決PnP問題，與傳統的PnP問題相比，其在求解速度上有了很大的提高，而且更方便，現在的智能手機幾乎都安裝有慣性測量單元，其利用重力感應能測量手機的在空間中方位，此外在一些導航設備（如GPS）上都有應用。

計算機視覺中，PnP問題的應用越來越廣泛，因此能否快速且有效得解決PnP 問題對計算機視覺中許多問題起著很重要的影響（如三維重建等），本文中利用一些設備來獲取兩個角度，并利用2組2D-3D的對應點來求解旋轉矩陣和平移向量，與傳統的求解PnP問題相比，其在求解速度上有了很大的提高。隨著社會的發展，智能手機的運用也越來越廣泛，現在的智能手機幾乎都安裝有慣性測量單元，其利用重力感應能測量手機的在空間中方位，并且現在的許多手機游戲都用到了慣性測量單元，此外在一些導航設備（如GPS）上都有應用。

[1]馬頌德,張正友.計算機視覺[M].北京:科學出版社,1998.

[2]劉木蘭.Grobner基理論及其應用[M].北京:科學出版社,2000.

[3]湯建良,高小山.透視n點定位（PnP）問題研究[D].中國科學院研究生院,2004.

[4]李源順.淺談多元多次方程[J].數學傳播,2003.

[5]吳福朝.計算機視覺中的數學方法[M].北京:科學出版社,2008.

[6]劉侍剛,吳成柯.高精度三維重建技術的研究[D].西安:西安電子科技大學,2005.

[7]彭豐富.Grobner基與約化的研究及其應用[D].長沙:中南大學,2001.

[8]D.Martinec,T.Pajdla.Robust Rotataion and Translation Estimation in Multivie Reconstruction[J].In CVPR,2007.

[9]Bruno Buchberger,Grobner Bases.A Short Introduction for Systems Theorists[J].Computer Aided Systems Theory EUROCAST,2001.

[10]Zuzana Kukelova.MartinBujnak.Tomas Pajdla.Closed-form solutions to the minimal absolute pose problems with known vertical direction.[J]. In:ACCV 2,Vol.6493Springer:216-229.

[11]Martin Byrod ,Klas Josephson.Kalle Astrom Fast and Stable Polynomial Equation Solving and Its Application to Computer Vision [J]. Int J Computer Vision,2009,84:237-256.

[12]B.Buchberger.An Algorithmical Criterion for the Solvability of Algebraic Systems of Equations (German)[C].Aequationes Mathematicae,1970,4(3):374-383.

[13]M. Bujnak,Z.Kukelova,T.Pajdla.A general solution to the P4P problem for camera with unknown focal length[C]. CVPR 2008, Anchorage, Alaska, USA, June 2008.

[14]Bujnak M,Kukelova,Z.,Pajdla,T.A general solution to the P4P problem for camera with unknown focal length[C]. In Proceedings of CVPR. 2008.

[15]Kukelova Z.,Bujnak M.,Pajdla T.Closed-form solutions to the minimal absolute pose problems with known vertical direction[C].ACCV 2010, Queenstown, NZ,2010,November:8-12.