胡釗政 ，李 娜，夏克文

（1.武漢理工大學ITS研究中心，湖北 武漢 430063；2.河北工業(yè)大學信息工程學院，天津 300401）

透視三點問題(Perspective-Three-Point, P3P)是計算機視覺與攝影測量學領(lǐng)域經(jīng)典問題，在目標定位、測量、虛擬現(xiàn)實、姿態(tài)估計等方向有重要和廣泛的應用價值。P3P問題定義為：已知攝像機內(nèi)參數(shù)矩陣，給定三點（又稱控制點）在某參考坐標系的相對位置及其圖像對應點，計算三控制點到攝像機光心的距離[1-14]。P3P是n點透視問題(PnP)中的其中一類問題，即n=3的情況。與其他PnP問題想比，P3P所需輸入最少，應用更廣泛。自其被提出以來，一直受到研究者們的重視。目前針對P3P問題的研究主要集中在兩個方面：即P3P問題的具體解法與P3P問題解的分布情況。例如，P3P問題可以轉(zhuǎn)化成以控制點距離為未知元的三組三元二次方程組的求解問題[1]，進而可以轉(zhuǎn)成一元四次方程的求解問題[2]。Xu等[3]提出P3P問題的遞歸解法，Li和Xu等[4-5]提出P3P問題的穩(wěn)定解法。許多學者研究了P3P存在多解的一般性條件。例如，Wolfe等[6]給出了P3P多解現(xiàn)象的幾何解釋，并指出P3P最多可以有4個不同的解，但大多數(shù)情況下有兩個解。蘇成和徐迎慶[7]給出了判斷P3P正解數(shù)目的充要條件。張彩霞和胡占義[8-9]討論了危險圓柱的情況。Gao等[10-11]運用Wu-Ritt零點分解方法研究P3P多解問題。Wu和Hu[12]通過計算旋轉(zhuǎn)矩陣與平移矢量來求解P3P問題。符德林和馬文鵬[13]擴展了P3P問題多解的判別條件適用范圍。文中提出了透視三點問題貝葉斯新解法(Bayesian Perspective-Three- Point, BP3P)，為P3P問題的求解與分析提供了一條新的途徑。算法不僅能求解P3P問題，還能對其解的分布情況進行分析。該算法將P3P問題轉(zhuǎn)化成支撐平面計算問題。支撐平面的計算可以利用三控制點產(chǎn)生的角度，長度比例等約束條件，通過貝葉斯理論轉(zhuǎn)化成最大似然概率估計問題。算法通過搜索高斯半球面來計算最大似然概率，確定支撐平面結(jié)構(gòu)信息，完成P3P問題的求解，并對P3P多解現(xiàn)象進行分析。文中還將三控制點約束推廣至一般性平面幾何約束，實現(xiàn)單視圖視覺定位，使所提的視覺定位算法具有較好的通用性。

1 P3P問題與支撐平面

1.1 支撐平面與P3P問題之間的關(guān)系

文中定義支撐平面(Support Plane)為三控制點所唯一確定的平面。針對支撐平面與P3P問題，有如下引理。

引理1確定支撐平面（即平面法向量和距離已知）是求解P3P問題的充要條件。

證明：a）必要條件證明

根據(jù)P3P問題的定義，每個控制點應滿足如下約束條件：

其中，第一個公式表示反向投影射線（見圖1所示虛線），它可以通過控制點對應的圖像與攝像機內(nèi)參數(shù)矩陣K計算[14]。其中，λ為尺度因子。第二個公式表示控制點與攝像機光心之間的距離。若P3P問題已解，則控制點距離已知。通過式(1)可計算控制點三維坐標，并唯一確定支撐平面。必要條件得證。

圖1 控制點X，圖像點x，支撐平面(N, d)與攝像機坐標系之間的幾何關(guān)系

b）充分條件證明

每個控制點同時在其反向投影射線與支撐平面上，因此滿足：

其中，N與d分別為支撐平面對應的單位法向量與距離（見圖1）。若已知支撐平面，則可通過上式計算控制點三維坐標，并計算控制點與攝像機光心之間的距離，從而求解P3P問題。充分條件得證。

引理2P3P問題的多解對應為支撐平面的多解。

證明：若P3P問題存在多解對應為支撐平面的多解，即每組解對應不同的支撐平面。

通過上述兩個引理，可將P3P問題轉(zhuǎn)化成支撐平面求解問題。下文將提出BP3P算法來求解支撐平面。

1.2 從三控制點產(chǎn)生幾何約束

假定小孔成像攝像機模型，根據(jù)射影幾何理論，空間平面與其圖像之間幾何關(guān)系可以用單應矩陣來描述[14]：

舊時史書和方志的這種編纂方式，自有它存在的合理性，今人也不用過多苛責，但這一體例有與生俱來的重大缺陷，也就是人物形象的類型化，語言表述的格套化，對此，在方志學素有心得的章學誠也清醒地意識到這個問題：“行皆曾、史，學皆程、朱，文皆馬、班，品皆夷、惠，魚魚鹿鹿，何以辨真?zhèn)卧?？”?］《修志十議》，844“稱許之間，漫無區(qū)別，學皆伏、鄭，才盡班、揚，吏必龔、黃，行惟曾、史。且其文字之體，尤不可通，或如應酬膚語，或如案牘文移，泛填排偶之辭，間雜帖括之句，循名按實，開卷茫然?！保?］《永清縣志闕訪列傳序例》，776

其中，x與X分別為平面上的點與其對應的圖像點（以齊次坐標表示），單應矩陣為一3×3的滿秩矩陣。若已知單應矩陣，則可以通過圖像點來計算該點在平面上的坐標。根據(jù)計算出來的坐標值，可以進一步計算出平面上的諸如長度、角度、面積、曲率等更多的幾何屬性。單應矩陣可以通過攝像機內(nèi)參數(shù)矩陣與平面結(jié)構(gòu)信息（平面法向量與距離）計算得到。計算方法可參考Liebowitz和Zisserman[15]提出的分層重建方法或Zhang[16]提出的基于世界坐標系設定方法。需要指出的是，如果平面法向量已知而平面距離未知，可假設平面距離為單位一，這樣計算出來的物理平面上的坐標值及長度等幾何屬性與真實值在相差一個比例因子下相等。但是一些幾何屬性如角度、長度比例等與真實值相等（參考分層重建理論中的相似重建與歐氏重建的區(qū)別[14]）。若平面法向量已知，則可以利用參考長度確定平面距離（見后文所述）。因此，在平面法向量計算中主要考慮諸如角度，長度比例這些在相似變換中保持恒定的幾何屬性。因為P3P問題中攝像機內(nèi)參數(shù)已知，因此這些平面幾何屬性的計算完全依賴于平面法向量。從而，可以用Q(N)來表示從給定法向量N計算出來的平面幾何屬性。

通過控制點坐標可計算三段線段長度（即點與點之間的距離），記為Di(i=1, 2, 3)。根據(jù)3個距離可計算三組長度比例：

同樣的，亦可通過余弦公式計算得到3個角度。這三組長度比例與3個角度可以通過P3P問題的已知條件直接計算得到，因此它們是場景先驗知識。利用這六組先驗知識可對支撐平面產(chǎn)生六組幾何約束，其數(shù)學表達如下：

Qi(N)表示從平面法向量N計算得到的第i個平面幾何屬性值，ui為上述長度比例或角度，它們的差值di(N)定義為平面法向量N對應的校正誤差。迫使校正誤差為零，即可對支撐平面的法向量產(chǎn)生約束。

2 P3P問題貝葉斯解法(BP3P)

2.1 最大似然概率模型

將平面法向量計算轉(zhuǎn)化為最大化條件概率問題。即給定六組幾何約束，計算產(chǎn)生最大條件概率的平面法向量，其數(shù)學表達如下：

直接求解式(6)較困難，可利用貝葉斯公式將式(6)轉(zhuǎn)化為：

假設各幾何約束之間條件獨立，可將上式進一步轉(zhuǎn)化成：

2.2 高斯似然概率模型

為了對似然概率進行準確的建模，提出了以下3個基本準則：

（1）校正誤差決定準則。似然概率隨著絕對校正誤差增加而降低。當校正誤差為零，則似然概率應取得最大值；

（2）幾何約束對等性準則。若所有的幾何約束被完全滿足時（即校正誤差為零），應產(chǎn)生相等的似然概率；

（3）歸一化準則。即考慮到幾何約束的表達形式、單位、尺度等差異，應將校正誤差進行歸一化處理。

基于上述準則，提出利用歸一化高斯函數(shù)似然概率建模方法，其數(shù)學表達如下：

其中，校正誤差用式(5)中ui（非零值）進行歸一化以滿足準則三。不難發(fā)現(xiàn)，式(10)可以滿足準則一。為了滿足準則二，所有幾何約束應所對應的高斯函數(shù)應取相同的均方差，即σi=σj=σ，實際應用中可根據(jù)需求選擇合適的均方差。從而有：

將式(11)代入式(9)可得：

2.3 最大似然搜索

將式(12)代入式(6)可得：

文中采取全局搜索算法來求解式(13)。因為三維空間中單位法向量對應為三維高斯球面(Gaussian Sphere)上的一點，以此可將搜索區(qū)間確定為高斯球面?？紤]到只有在攝像機前面的目標才能出現(xiàn)在圖像中（即深度值為正的約束），可將搜索區(qū)間減半，在高斯半球面上搜索(Gaussian Hemisphere)。具體實現(xiàn)如下：首先在高斯半球面上均勻抽樣[17]，每一個抽樣點都對應一單位法向量。然后利用式(13)計算每個抽樣點對應的似然概率，最后計算出最大似然概率對應的平面法向量即為支撐平面的法向量。需要指出的是，最大似然搜索能獲得P3P的一個解，如果要獲得P3P問題的多解，則需對計算出來的似然概率圖進行相應的分析（見實驗部分相關(guān)討論）。

2.4 計算平面距離與控制點距離

若平面法向量已知，可假定平面距離為單位一，在相差一個尺度因子β下計算控制點三維坐標：

比較式(14)與(2)可知：X=βX′，且β為支撐平面的距離。利用任意兩控制點的實際距離Di，可計算尺度因子β：

其中，為從式(14)計算的三維坐標得到的兩個控制點之間的距離。確定尺度因子β，即可計算控制點實際距離完成P3P問題求解。

2.5 一般性幾何約束的視覺定位

可以將BP3P算法推廣用以處理更一般的幾何約束，實現(xiàn)對平面目標的準確定位。實際上，文中算法僅要求場景的幾何約束可以表達成式(5)的形式，就可以利用所提算法計算支撐平面，進行三維定位。例如，算法可以利用已知的角度，長度比例，甚至更一般性的幾何約束條件，通過歸一化高斯函數(shù)來對似然概率建模，計算最大似然概率，求解平面法向量。因此，三控制點僅是幾何約束條件中的一個特例。除三控制點幾何約束之外，算法可以處理更為一般性的幾何約束，具有較好的通用性。考慮到實際環(huán)境中幾何約束的多樣性，文中所提BP3P算法具有更好的實用性與靈活性。

3 實驗結(jié)果與分析

文中設計了三組實驗來對所提BP3P算法進行驗證。第一組實驗中，考慮到計算結(jié)果如平面法向量與控制點距離的基準值(Ground Truth)很難獲取，文中采用Zhang[16]標定算法中所用到的標定模板作為支撐平面，通過在平面上隨機選擇3個不共線的網(wǎng)格點作為三控制點（見圖2中Δ標記所示）。選擇標定模板的主要原因有二：

圖2 在平面網(wǎng)格中隨機選擇3個非共線控制點X1, X2, X3

（1）可以準確地標定出攝像機的內(nèi)參數(shù)，并將其作為P3P問題所需的部分輸入條件；

（2）可以將標定結(jié)果的攝像機外參數(shù)（如旋轉(zhuǎn)矩陣和平移矢量）作為基準值，對計算結(jié)果進行驗證。利用Nikon Coolpix 4100數(shù)碼相機從不同角度和位置拍攝標定模板的照片。部分圖像如圖3所示，圖像分辨率為1600×1200（像素）。利用Zhang[16]平面模板標定算法計算得到攝像機焦距為4175.2，主點位置在 [874, 677]T（單位皆為像素）。

圖3 平面網(wǎng)格模板在不同位置與角度下拍攝的圖像

首先用文中BP3P算法計算支撐平面在不同圖像中的法向量。在高斯半球上抽取200×400個采樣點來離散化搜索區(qū)間，通過搜索最大似然概率來計算平面法向量，并將其與基準值進行比較。用計算得到的法向量與基準法向量之間的夾角來表示法向量計算誤差。計算結(jié)果見表1所示。其中，第二行與第三行分別為文中算法計算得到的支撐平面單位法向量和從攝像機標定結(jié)果中計算出來的基準值，第四行為兩個法向量對應的角度誤差。從表1計算結(jié)果可以得到：四幅圖像中計算出來的法向量與基準值最大誤差為0.33°，平均誤差為0.25°，表明計算結(jié)果較穩(wěn)定和準確。

表1 支撐平面法向量計算結(jié)果

根據(jù)表1中計算出來的法向量，通過任意兩個控制點之間的實際距離作為參考長度來計算支撐平面的距離，從而確定支撐平面。最后利用式(2)來計算每一個控制點在攝像機坐標系的三維坐標Xi(i=1,2,3)，最后計算控制點在四幅圖像中與攝像機關(guān)心的距離。計算結(jié)果見表2所示。表2中分別用表示文中算法計算的距離與距離基準值，并用表示距離計算誤差。根據(jù)表2中結(jié)果可得：控制點與攝像機光心的距離分布在1.0～2.2m之間，距離最大計算誤差為0.8cm，平均誤差為0.4cm。這些計算結(jié)果表明BP3P算法具有較好的精度和穩(wěn)定性。

表2 控制點與攝像機光心的距離計算結(jié)果(cm)

實驗中還利用BP3P算法來分析與求解P3P多解問題。如引理2所述，P3P多解對應為支撐平面的多解。考慮一個包含兩個解的P3P問題[6]，利用BP3P算法進行展示。如圖4所示，圖4(a)為原始圖像，圖4(b)為從6個幾何約束計算得到的似然概率圖。圖中用灰度值表示似然概率，灰度值越大，似然概率越大。從圖中可以明顯觀察到似然概率有兩個峰值區(qū)域。圖4(c)為對似然概率圖進行分析的結(jié)果。利用閾值將似然概率圖進行分割以凸顯似然概率最大的30個平面法向量的位置，可以發(fā)現(xiàn)它們分布于兩個區(qū)域。在兩個區(qū)域中分別計算局部最大似然概率，可以求解支撐平面法向量的兩組解，分別為[0.0699-0.8883 0.4540]T，[-0.0917 0.8019 0.5904]T。它們在似然概率圖中對應的位置見“+”所標記。根據(jù)計算的兩組平面法向量，可以獲得P3P問題的兩組解。因此，BP3P算法不僅可以求解P3P問題，同時還能對其多解情況進行分析。

圖4 利用BP3P算法分析P3P的多解問題

第二組實驗中，對BP3P算法進行推廣，利用一般性幾何約束實現(xiàn)視覺定位。如圖5所示，圖5(a)為利用Nikon D700拍攝的原始圖像數(shù)據(jù)，圖像分辨率為2218×1416，攝像機的焦距為1369.0（單位皆為像素）?？紤]到實際場景中有各種各樣的幾何約束，實驗中很難對它們進行一一探討，因此文中采用兩種代表性的幾何約束（即已知的角度與長度約束）對算法進行驗證。實際上，很多特定的幾何約束也可以進一步轉(zhuǎn)化成角度或者長度的形式（如上文所討論的P3P問題）。如圖5(b)所示，通過墻面上標準A4紙上點與線之間的關(guān)系得到三組幾何約束：即L1與L2，L3與L4之間的夾角，以由M1，M2和M3，M4所確定的線段長度比例（見圖5(b)所示）。算法首先利用這三組約束條件來求解A4紙所在平面的單位法向量。

圖5 一般性幾何約束用于視覺定位

如圖6所示，圖6(a)～(c)分別表示3個幾何約束所產(chǎn)生的似然概率圖，其中圖像灰度值越大表示似然概率越大。從圖6(a)～(c)可以觀察到：三組幾何約束產(chǎn)生的似然概率是非均勻分布，并且各個約束對應的概率分布圖皆不相同。圖6(d)表示三組幾何約束產(chǎn)生的聯(lián)合似然概率，在圖右下角可很明顯觀察到一個極大概率分布區(qū)域。圖6(e)顯示似然概率最大的30個平面法向量對應位置，可以觀察到它們共同分布在一狹小區(qū)域（見白色所標記的區(qū)域）。圖6(f)顯示了似然概率最大的平面法向量所在的位置（用‘+’標記），其對應的平面法向量為 [0.0993 0.1679 0.9808]T。

圖6 利用BP3P算法從三組一般性幾何約束條件計算平面法向量

以A4紙長度（圖7中P1～P2距離）作為參考值計算實際平面距離，然后計算平面上點的三維坐標。實驗中，從六組線段中抽取一定數(shù)目的點進行定位。此外，還對A4紙的4個頂點進行定位（見圖中所標記P1～P4四點）。它們在攝像機坐標系中對應的三維坐標如圖7所示。利用A4紙張的標準尺寸（210mm×297mm）對算法計算結(jié)果進行驗證。利用計算出來的三維坐標計算P2～P3，P3～P4，以及P1～P4之間的距離，分別為206.7mm, 294.6mm, 211.8mm。與A4紙張的標準尺寸比較，絕對誤差分別為 3.3mm, 2.4mm,1.8mm，對應的相對誤差水平分別為1.6%,0.8%,0.9%。這些結(jié)果表明計算結(jié)果較準確，所提BP3P算法能被推廣，用以處理一般性幾何約束，實現(xiàn)準確的視覺定位。

圖7 平面點三維定位結(jié)果(mm)（P1～P4為A4紙4個角點）

第三組實驗中，對BP3P進一步推廣與驗證，利用一般性幾何約束實現(xiàn)室內(nèi)環(huán)境下的視覺定位。室內(nèi)尺寸為6.5m×6.5m，其中墻上所掛白板平面的高度為1.2m。如圖8(a)所示，左圖為利用Coolpix 4100數(shù)碼相機拍攝的原始圖像數(shù)據(jù)，圖像分辨率為2248×1712，攝像機的焦距為2356.7（單位皆為像素）。從場景中選取已知的三組長度比例及3個角度作為一般性幾何約束條件。其中，三組長度比例分別由圖中4個點（見圖中Δ標記）所確定的長度定義。3個角度為三條虛線（如圖所示）所確定，分別為90°, 90°, 0°。這些約束條件產(chǎn)生的似然概率如圖8(b)所示，采用的高斯面分辨率為200×400。通過搜索最大似然概率，計算對應的平面法向量為[-0.5030.112-0.857]T（見圖8(a)中‘+’所標記的位置）。而通過場景中的平行線等信息計算出來的平面法向量為[- 0.5000.092-0.861]T[14]。兩者之間的夾角誤差僅為1.2°，表明了文中所提的算法具有較好的精度與實用性。

圖8 利用一般性幾何約束進行室內(nèi)視覺定位

4 結(jié)論

針對計算機視覺領(lǐng)域經(jīng)典的P3P問題，文中提出了P3P問題的貝葉斯新解法(BP3P)，為P3P問題的求解與分析提供了一條新的途徑。首先證明支撐平面是求解P3P問題的充要條件，提出了基于貝葉斯的支撐平面計算方法，將平面法向量估計問題轉(zhuǎn)化成最大似然概率問題。利用高斯函數(shù)來對幾何約束進行似然概率建模，在高斯半球面上搜索最大似然概率，確定支撐平面。用實驗對算法進行驗證，結(jié)果表明BP3P算法可以準確求解P3P問題并分析其多解現(xiàn)象。在一米至兩米距離之內(nèi)，平面法向量與距離平均計算誤差分別為0.25°和0.4cm。實驗中，BP3P算法還被推廣用以處理一般性幾何約束，實現(xiàn)對平面目標的準確定位，從而使所提的視覺定位算法具有較好的通用性。研究結(jié)果具有一定的理論與應用價值。

[1]Fischler M, Bolles R.Random sample consensus [J].Communications of the ACM, 1981, 24(6): 381-395.

[2]Long Quan, Lan Zhongdan.Linear N≥4 point camera pose determination [J].IEEE Transaction on Pattern Analysis and Machine Intelligence, 1999, 21(8):774-780.

[3]Xu De, Li You, Tan Min.A general recursive linear method and unique solution pattern design for the perspective-n-point problem [J].Image and Vision Computation, 2008, 26(6): 740-750.

[4]Li Shiqi, Xu Chi.A stable direct solution of perspective-three-point problem [J].International Journal of Pattern Recognition and Artificial Intelligence, 2011, 25(11): 627-642.

[5]Li Shiqi, Xu Chi, Xie Ming.A Robust O(n) solution to the perspective-three-point problem [J].IEEE Transaction on Pattern Analysis and Machine Intelligence, 2012, 34(7): 1444-1450.

[6]Wolfe W, Mathis D, Sklair C, Magee M.The perspective view of 3 Points [J].IEEE Transaction on Pattern Analysis and Machine Intelligence, 1991, 13(1):66-73.

[7]蘇 成, 徐迎慶.判斷P3P正解數(shù)目的充要條件[J].計算機學報, 1998, 21(12): 1085-1095.

[8]Zhang Caixia, Hu Zhanyi.Why is the danger cylinder dangerous in the P3P problem? [J].Acta Automatica Sinica, 2006, 32(4): 504-511.

[9]張彩霞, 胡占義.P3P問題的多解現(xiàn)象的概率研究[J].軟件學報, 2007, 18(9): 2100-2104.

[10]Gao Xiaoshan, Hou Xiaorong, Tang Jianliang, Cheng Hangfei.Complete solution classification for the perspective-three-point problem [J].IEEE Transaction on Pattern Analysis and Machine Intelligence, 2003,25(8): 930-943.

[11]湯建良.一類P3P問題方程系統(tǒng)的零點分解[J].數(shù)學的實踐與認識, 2004, 34(8): 124-127.

[12]Wu Yihong, Hu Zhanyi.PnP problem revisited [J].Journal of Mathematical Imaging and Vision, 2006,24(1): 131-141.

[13]符德林, 馬文鵬.P3P問題多解條件的補充研究[J].計算機工程與應用, 2011, 47(2): 179-181.

[14]Hartley R, Zisserman A.Multiple view Geometry in Computer vision (2nd Edition) [M].Cambridge, UK:Cambridge University Press, 2004.

[15]Liebowitz D, Zisserman A.Metric rectification for perspective images of planes [C]//Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition, 1998: 482-488.

[16]Zhang Zhengyou.A flexible new technique for camera calibration [J].IEEE Transaction on Pattern Analysis and Machine Intelligence, 2000, 22(11): 1330-1334.

[17]Leopardi P.A partition of the unit sphere into regions of equal area and small diameter [J].Electronic Transactions on Numerical Analysis, 2006, 25(12):309-327.