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

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

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

其中是某個(gè)正數。P3P的目標是求解其中的旋轉和平移。

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

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

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

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

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

根據余弦定理，

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

其中

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

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

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

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

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

其中。

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

根據定理1，退化圓錐曲線(xiàn)需要非滿(mǎn)秩，即行列式為0：

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

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

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

由于，假設，矩陣也可以寫(xiě)為：

假定，令，則

\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},

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

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

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

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

因此，可以將以正確的尺度適當地重新縮放得到交點(diǎn)。

(2) 伴隨矩陣法:矩陣的伴隨矩陣應滿(mǎn)足：

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

與相等。

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

恢復直線(xiàn)：得到交點(diǎn)之后，根據其反對稱(chēng)矩陣

定義一個(gè)新的矩陣

結合(22)和(10)可得。直線(xiàn)對可以通過(guò)的行和列得到。

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

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

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

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

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

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

其中

結合(25)和(26)，可以解得旋轉

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

圖2：從向量對應關(guān)系到旋轉

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

• 求解構建退化圓錐曲線(xiàn)(式(9));
• 將退化圓錐曲線(xiàn)分解為兩條直線(xiàn)，進(jìn)而代入(4)(5)的圓錐曲線(xiàn)求交點(diǎn)