CVPR2023 | 如何設(shè)計一個更快更魯棒的P3P求解器？（1）

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

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

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

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

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

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

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

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

根據(jù)余弦定理，

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

其中

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

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

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

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

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

其中。

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

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

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

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

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

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

假定，令，則

\tilde{p}_2+\tilde{q}_2& =2c_{12}/c_{11},   \\ \tilde{p}_2\tilde{q}_2& =c_{22}/c_{11},  \\ \tilde{p}_3+\tilde{q}_3& =2c_{13}/c_{11},  \\ \tilde{p}_2\tilde{q}_3+\tilde{p}_3\tilde{q}_2& =2c_{23}/c_{11},

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

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

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

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

因此，可以將以正確的尺度適當?shù)刂匦驴s放得到交點。

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

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

與相等。

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

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

定義一個新的矩陣

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

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

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

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

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

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

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

其中

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

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

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

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

• 求解構(gòu)建退化圓錐曲線(式(9));
• 將退化圓錐曲線分解為兩條直線，進而代入(4)(5)的圓錐曲線求交點