P3P 問題是經(jīng)典的多視圖幾何問題之一，其中標(biāo)定的相機(jī)的絕對位姿由三個 2D-3D 對應(yīng)關(guān)系決定。由于這是許多視覺系統(tǒng)的關(guān)鍵（例如定位和SfM），因此過去有很多研究關(guān)注于如何開發(fā)更快、更穩(wěn)定的P3P算法。雖然當(dāng)前SOTA的求解器既非常快又穩(wěn)定，但仍然存在可能崩潰的配置。本文將問題代數(shù)化為尋找兩個圓錐的交點(diǎn)。通過這個方式，我們能夠分析表征多項(xiàng)式系統(tǒng)的實(shí)根，并為每個問題實(shí)例采用量身定制的解決方案。這導(dǎo)出了一個快速穩(wěn)定的P3P求解器，它能夠正確解決其它方法可能會失敗的情況。實(shí)驗(yàn)評估表明，該方法在速度和成功率方面都優(yōu)于當(dāng)前的SOTA方法。

1 什么是P3P問題

PnP是指根據(jù)2D-3D對應(yīng)關(guān)系集合估計(jì)相機(jī)絕對位姿，集合最小的情況是P3P問題。P3P是將2D-3D對應(yīng)關(guān)系通過相機(jī)內(nèi)參轉(zhuǎn)換為3D-3D對應(yīng)關(guān)系進(jìn)行求解。給定世界坐標(biāo)系中的3個3D點(diǎn)以及它們對應(yīng)的歸一化圖像點(diǎn)，兩個點(diǎn)集通過剛體變換關(guān)聯(lián)：

其中是某個正數(shù)。P3P的目標(biāo)是求解其中的旋轉(zhuǎn)和平移。

2 P3P問題發(fā)展史

P3P作為一個幾何問題歷史悠久，比計(jì)算機(jī)視覺領(lǐng)域的出現(xiàn)都要早很久。早在1773年Lagrange就在研究這個問題，Lagrange證明該問題最多可能有4個實(shí)數(shù)解，可以轉(zhuǎn)化為4次多項(xiàng)式問題求解。大約1個世紀(jì)后，1841年德國數(shù)學(xué)家Grunert重新研究了該問題，給出了一種直接求解方法。20世紀(jì)早期，該問題在攝影測量領(lǐng)域內(nèi)受到關(guān)注，但主要關(guān)注點(diǎn)在于精調(diào)而不是從頭求解。Finstenvalder和Scheufele在1937年證明P3P問題只需要找到1個三次多項(xiàng)式的1個根和2個二次多項(xiàng)式的根。該問題后來在1981年Fischler和Bolles的RANSAC論文重新露面，由于RANSAC的成功，該問題也開始受到很大的關(guān)注。

根據(jù)最后需要求解的一元多項(xiàng)式的階次，P3P問題可分為兩大類：求解1個四次方程和求解1個三次方程。

最近的大多數(shù)工作關(guān)注于將P3P問題轉(zhuǎn)化為求解四次方程問題。Gao等人在2003年用吳零點(diǎn)分解法第一次給出了P3P的完成解析解。Kneip等人2011年提出了一種直接由計(jì)算相機(jī)絕對位置和旋轉(zhuǎn)的方式求解P3P問題的方法，避免了特征值分解或奇異值分解。Ke等人2017年提出用相應(yīng)的幾何約束確定相機(jī)的旋轉(zhuǎn)。Banno和Nakano分別于2018和2019提出了P3P的直接求解法，通過估計(jì)中間坐標(biāo)系中的距離，使得旋轉(zhuǎn)矩陣可以形式化為距離的線性表示。

與基于四次方程的方法不同，基于三次方程的方法在P3P問題的文獻(xiàn)中沒有得到太多關(guān)注。自Finstenvalder和Scheufele在1937年的工作以來，Grafarend等人在1989年也使用了三次方程，他們試圖將（3）簡化為齊次形式，然后使用與Finstenvalder和Scheufele相同的技術(shù)求解。Haralick等人在1991年回顧了P3P問題的主要基于三次方程的解法，并討論了數(shù)值精度。最近，Persson和Nordberg在2018年展示了關(guān)于尋找旋轉(zhuǎn)和平移的更多細(xì)節(jié)，并提出了一種使用三次方程的一個根的有效算法，該方法比以前的方法有更好的數(shù)值精度，并且更快。

3 P3P問題轉(zhuǎn)化為兩個圓錐曲線相交問題

參照Persson和Nordberg在2018年的解法，為了消除旋轉(zhuǎn)和平移參數(shù)，如圖1所示，有如下約束：

根據(jù)余弦定理，

其中。我們的目標(biāo)是找到的解，從而求解旋轉(zhuǎn)和平移。我們可以假設(shè)，不然3D點(diǎn)就和相機(jī)中心一樣了。將(3)中前兩個式子除以第三個式子，并通過變量替換，可以得到以下二元二次方程組：

其中

現(xiàn)在問題變?yōu)榍髢蓚€二次方程的實(shí)數(shù)解，也就是說找到兩個圓錐曲線的實(shí)數(shù)交點(diǎn)。

4 本文的方法

本文的方法的基本思路也是求解兩個圓錐曲線的相交問題。(4)和(5)的兩個二次方程可以重寫為：

其中為的矩陣。為了找到交點(diǎn)，首先構(gòu)造一個矩陣

交點(diǎn)可通過構(gòu)建一個與真正的解相交的退化圓錐曲線找到。退化圓錐曲線由以下命題給出：

命題1（退化圓錐曲線，見《計(jì)算機(jī)視覺中的多視圖幾何》）如果矩陣 不滿秩，則圓錐曲線退化。退化點(diǎn)的圓錐曲線是兩條線(秩為2)或一條重復(fù)線(秩為1)，可以寫為：

其中。

退化圓錐曲線被構(gòu)造出來后，可以被分解為（至多）兩條直線(和)，可以進(jìn)一步很容易地與原來的兩個圓錐曲線相交。

4.1 尋找退化圓錐曲線

根據(jù)定理1，退化圓錐曲線需要非滿秩，即行列式為0：

得到關(guān)于的三次方程。求解(11)可以得到的值，并得到矩陣。注意原始方程組的任何解(同時屬于圓錐曲線)也在退化圓錐曲線上。

對于(11)中的每個解，都可以得到一個退化圓錐曲線。根據(jù)(10)，退化圓錐曲線是兩條直線和的組合。那么如何將退化圓錐曲線分解為兩條直線呢？

4.1.1 方法一：直接求解直線

這里展示一種尋找直線的直接方法。假定已經(jīng)找到了一個退化圓錐曲線，寫為如下形式：

由于，假設(shè)，矩陣也可以寫為：

假定，令，則

根據(jù)(14)和(15)可以解出, 進(jìn)而根據(jù)(16)和(17)可求得。在這種情況下，可以得到一對直線和。為了避免的情況，可以找到的絕對值最大的對角線元素，更穩(wěn)定地計(jì)算出直線對。

4.1.2 方法二：通過求兩直線相交求解直線

由于兩直線參數(shù)的叉乘可得交點(diǎn)，可以進(jìn)而從中提取出兩直線。對于交點(diǎn)，這里展示兩種求法：

(1) 零空間法:根據(jù)(10)可知, 交點(diǎn)在的零空間內(nèi)，對于的任意零空間向量，有。我們現(xiàn)在必須找到，使得的尺度與(以及)一致。由于，我們有

結(jié)合(12),(13)和(18)，可以推導(dǎo)出的范數(shù)和的元素之間的關(guān)系：

因此，可以將以正確的尺度適當(dāng)?shù)刂匦驴s放得到交點(diǎn)。

(2) 伴隨矩陣法:矩陣的伴隨矩陣應(yīng)滿足：

證明：通過(13)可以得到

與相等。

給定一個矩陣，可以得到。為了避免0元素，可以找到其對角線最大的元素和對應(yīng)的列，交點(diǎn)可以通過將該列除以對角線的平方根得到。

恢復(fù)直線：得到交點(diǎn)之后，根據(jù)其反對稱矩陣

定義一個新的矩陣

結(jié)合(22)和(10)可得。直線對可以通過的行和列得到。

秩為1的情況：如果退化圓錐曲線包括一對重復(fù)的直線，則矩陣的秩為1。在這種情況下，可以直接從一行或一列中恢復(fù)重復(fù)的直線。

4.2 P3P問題求解

退化圓錐曲線中獲得的直線過原二次方程組(7)與(8)的解（交點(diǎn)），因此可以通過求直線與兩個圓錐曲線的交點(diǎn)進(jìn)行求解。假設(shè)第一條直線為：

將(23)代入(5)可得一個關(guān)于的二次方程，至多有2個解。需要注意的是，我們只關(guān)心正的實(shí)數(shù)解。得到后，根據(jù)(23)可以得到。由可得。代入（3）可得關(guān)于的二次方程

由于, 可以得到的解。這種情況下，可以得到的值。由于有一對直線，的解有4種可能。知道后，可以用Gauss-Newton優(yōu)化(3)的平方和對結(jié)果進(jìn)行細(xì)化。之前的工作也使用了類似的細(xì)化方法。

求解旋轉(zhuǎn)和平移：對于每個，首先用(1)消除平移，得到以下方程組

為了找到另一個非共面向量量對應(yīng)關(guān)系，與前人工作一樣，可以使用由三個3D點(diǎn)和圖像點(diǎn)定義的平面的法線（見圖2）。法向量也滿足

其中

結(jié)合(25)和(26)，可以解得旋轉(zhuǎn)

得到旋轉(zhuǎn)后由(1)可以求解平移。

圖2：從向量對應(yīng)關(guān)系到旋轉(zhuǎn)

4.3 可能解的配置分析以及魯棒算法

以上展示了P3P問題求解的一個通用算法，主要包括2個步驟：

求解構(gòu)建退化圓錐曲線(式(9));

將退化圓錐曲線分解為兩條直線，進(jìn)而代入(4)(5)的圓錐曲線求交點(diǎn)

接下來分析解的可能配置，并由此得到魯棒的算法。推薦學(xué)習(xí)3D視覺工坊近期開設(shè)的課程：面向三維視覺算法的C++重要模塊精講：從零基礎(chǔ)入門到進(jìn)階

（11）中三次方程的通解可能有四種情況：三個實(shí)根，一個實(shí)根和兩個復(fù)根，一個實(shí)根和一個重根、一個實(shí)三重根。這四種可能性對應(yīng)于直線和交點(diǎn)的不同情況。在本文的例子中，第一個圓錐曲線（4）是不定的，第二個圓錐曲線（5）是雙曲線（b>0）。不失一般性，假設(shè)第一個是橢圓，并在圖3中展示兩個圓錐曲線的可能相對位置。簡要起見，以圖3b作為示例。兩個圓錐曲線有四個實(shí)交點(diǎn)，特征三次方程有三個實(shí)根，每個實(shí)根對應(yīng)于一對實(shí)直線。每對實(shí)直線在四個實(shí)交點(diǎn)處與兩個圓錐曲線中的任何一個相交（圖3b）。在其他情況下也存在類似的情況。

圖3：雙曲線和橢圓相對位置的八種可能情況的圖解。具有不同顏色的一對線對應(yīng)于不同的三次根。(a)-(c): 兩個圓錐曲線分別具有零、四和兩個實(shí)交點(diǎn)的一般情況。(d)-(f): 兩個圓錐曲線相切的臨界情況，分別有一個二重交點(diǎn)、兩個二重交點(diǎn)、一個二重交點(diǎn)和兩個交點(diǎn)。(g)-(h): 兩個圓錐曲線彼此密切（兩個圓錐在切點(diǎn)具有相同的曲率），分別有一個三重交點(diǎn)和一個四重交點(diǎn)

三次方程的根、線的數(shù)量和交點(diǎn)之間的關(guān)系如表1所示。

表1：三次方程的根、退化圓錐曲線的直線數(shù)和兩個圓錐曲線交點(diǎn)之間的關(guān)系。1D、1T和1Q分別表示一個二重交點(diǎn)、一個三重交點(diǎn)和一個四重交點(diǎn)

假設(shè)三次方程（11）可以寫成

進(jìn)行變量替換可得關(guān)于的沮喪三次方程(沒有二次項(xiàng))

其中

(30)的判別式為：

若,（30）有三個不同的實(shí)根，其對應(yīng)于情況（a）和（b）。對于情況（a），由于所有的實(shí)根都不對應(yīng)于任何實(shí)交點(diǎn)，我們可以選擇三個根中的任何一個。使用三角解找到根,該根可以對應(yīng)于一對實(shí)線，也可以不對應(yīng)于實(shí)線。如果選擇的實(shí)根給出一對實(shí)線，可以在驗(yàn)證第一條直線沒有實(shí)交點(diǎn)后跳過第二條直線。對于情況（b），三個根中的任何一個都將產(chǎn)生一對實(shí)線和四個實(shí)交點(diǎn)。

若,（30）具有一個實(shí)根和兩個共軛復(fù)根，這對應(yīng)于情況（c）。使用Cardano公式可以找到實(shí)根，并且可以得到一對實(shí)直線。如果其中第一條直線與圓錐曲線有實(shí)交點(diǎn)，則可以跳過第二條直線。否則，如果第一條直線沒有實(shí)交點(diǎn)，就需要檢查第二條直線。

若且,則（30）有一個單實(shí)根和一個重實(shí)根，這對應(yīng)于情況（d）、（e）和（f）。對于情況（d），可以看到重實(shí)根在虛直線中產(chǎn)生。因此，對于這種情況，可以使用單實(shí)數(shù)根。

若且，則。在這種情況下，方程（30）有三重根，這對應(yīng)于情況（g）和（h）。三次方的實(shí)根給出了一對實(shí)線，并且可以很容易地找到實(shí)交點(diǎn)。

基于以上分析和表1，可以注意到，可以使用任何一對實(shí)線來恢復(fù)實(shí)交點(diǎn)，并且除了情況（d）之外，三次方程的任何實(shí)根都對應(yīng)于一對實(shí)直線。只需要在且時避免使用重實(shí)根。另一方面，如果，則這對直線和圓錐曲線之間一定有雙重交點(diǎn)。為了避免重復(fù)的解，需要檢查直線和圓錐曲線之間的交點(diǎn)。整個過程如算法1所示。推薦學(xué)習(xí)3D視覺工坊三維點(diǎn)云課程：三維點(diǎn)云處理：算法與實(shí)戰(zhàn)匯總

5 實(shí)驗(yàn)

5.1 消融實(shí)驗(yàn)

本文提取直線的幾種方法對比

直接法速度最快，但伴隨矩陣法最穩(wěn)定。實(shí)踐中推薦用伴隨矩陣法。

5.2 與SOTA方法對比

5.2.1 數(shù)值穩(wěn)定性

圖4：高斯核平滑直方圖，在無噪聲數(shù)據(jù)上運(yùn)行 100,000 次運(yùn)行的旋轉(zhuǎn)和平移誤差之和。測量估計(jì)和GT之間誤差的 L1 范數(shù)。

可以看到所有方法的都是數(shù)值穩(wěn)定的。Nakano(rp)表示帶root polishing(利用Gauss-Newton方法優(yōu)化根)，該方法更多的分布在小誤差范圍。

表2：誤差的平均值、中位數(shù)和最大值。粗體表示最好的結(jié)果

本文提出的求解器在均值誤差和最大誤差上的性能最好，而Nakano(rp)求解器在中值誤差上的性能最好。請注意，基于四次的求解器包含的失敗案例，在此實(shí)驗(yàn)中是刪除了的。

5.2.2 解的對比

表3：與SOTA求解器對比

本文的求解器優(yōu)于現(xiàn)有的方法。對于幾乎所有的試驗(yàn)，都能找到好的解和GT，沒有重復(fù)或錯誤的解。基于四次方程的Ke等人和Kneip等人的方法有很多重復(fù)解和錯誤解。因?yàn)樗麄冇昧怂拇畏匠痰乃?個根(忽略虛部)來找到可能的估計(jì)，提高找到GT的可能性是可以的，但會損失其效率，因?yàn)閷?shí)踐中每個假設(shè)需要用RANSAC進(jìn)行評估。Nakano求解器用的虛部閾值過濾不必要的復(fù)根，但結(jié)果沒有本文的方法好。Ke等人與Kneip等人的方法也能夠用類似的閾值過濾四次方程的根，從而減少重復(fù)和錯誤解。

5.2.3 運(yùn)行時間對比

表4：平均運(yùn)行時間對比，次嘗試，每個嘗試100次

基于三次方程的求解器比基于四次方程的要更高效，所提出的求解器比SOTA方法快15.4%，加速主要原因有兩個：

基于相對位置和判別式的分析，可以快速選擇三次方程的穩(wěn)定根，根據(jù)判別式的符號可以避免沒必要的計(jì)算。

從退化圓錐曲線中恢復(fù)直線的方法比Persson等人的方法更高效。本文用C++復(fù)現(xiàn)的Nakano方法比原始的MATLAB實(shí)現(xiàn)慢，可能是用了不同的矩陣計(jì)算庫導(dǎo)致的。

5.3 失敗案例分析

Persson等人的方法在沒有解的情況下，其判別式非常接近于0。這是因?yàn)榕袆e式為0對應(yīng)于圖3中的(d)-(h)這些臨界情況，由于浮點(diǎn)數(shù)的舍入誤差和數(shù)值不穩(wěn)定性，這些情況下很難恢復(fù)運(yùn)動參數(shù)。本文發(fā)現(xiàn)大多數(shù)失敗案例是(d)和(f)的情形，(e)(g)(h)很少發(fā)生。

與危險(xiǎn)圓柱的關(guān)系

圖5：危險(xiǎn)圓柱是指三個3D點(diǎn)A,B,C為圓形環(huán)繞點(diǎn)的圓柱，其軸垂直于ABC平面

之前有工作指出，如果光心位于危險(xiǎn)圓柱的表面，則P3P問題的解不穩(wěn)定。本文發(fā)現(xiàn)危險(xiǎn)圓柱會導(dǎo)致判別式，即對應(yīng)于圖3中的臨界情形。

5 總結(jié)

本文重新審視了 P3P 問題，特別是研究了基于兩個圓錐曲線相交的解法，類似于 Persson 和 Nordberg的方法。通過分析可能的解的集合并明確枚舉極端情況，能夠設(shè)計(jì)一個快速穩(wěn)定的 P3P 算法。實(shí)驗(yàn)表明，與以前的方法相比，本文的求解器更魯棒且更快。

局限性

本文的方法基于研究兩個圓錐曲線的實(shí)交點(diǎn)。然而，P3P 問題是特殊的，因?yàn)榻鈶?yīng)該是正實(shí)數(shù)。三次方程可能有更多的約束，不幸的是，本文沒有發(fā)現(xiàn)一個很好的方法來做到這一點(diǎn)。請注意，式(4) 和 (5) 可以用同倫延拓來求解，該方法擅長解決大規(guī)模平方系統(tǒng)。但對于 P3P 問題，本文發(fā)現(xiàn)它不如當(dāng)前基本上閉式的求解器有效。