PnP(Perspective-n-Point)問題的幾何結構如圖1所示，給定3D點的坐標、對應2D點坐標以及內參矩陣，求解相機的位姿。

數學語言描述如下：

圖1.PnP幾何結構

1.直接線性變換法(Direct Linear Transform，DLT)

假設：攝像機已經校準過了。 已知：

求解相機的外參：R、t 透視投影模型為：

每組3D-2D匹配點對應兩個方程，一共有12個未知數，至少需要6組匹配點。 設有N組匹配點，則：

上式寫成矩陣形式： AF=0 當N=6時，可以直接求解線性方程組。

因此, 旋轉矩陣, 平移矩陣求得:

2.P3P

P3P問題是已知三個3D目標點與其2D投影之間的對應關系，來確定標定相機的位姿問題。

圖2.兩點約束

注：直接線性變換法，只考慮了線性意義下的最優解，沒有考慮幾何約束。而P3P考慮了三角約束，給出三角約束意義下的最優解

2.1 Zero Structure for the P3P Equation System

文章[1]：Complete Solution Classification for the Perspective-Three-Point Problem

圖3.三點約束 對于公式（16）的變量有一些真實的約束：

圖4.三點約束--重定義邊長 公式(17)消去C、v，得ES：

2.2 PST

文章[2]：A Stable Direct Solution of Perspective-Three-Point Problem 使用相似三角形，利用幾何約束來減少未知參數的個數，把P3P方程組轉化為四次方程，該稱為透視相似三角形方法(Perspective Similar Triangle ，PST)。

（1）P3P問題轉為PST問題

圖5.P3P幾何結構

（2）PST的求解

圖6.PST幾何結構 約束1：相似三角形對應邊成比例

所以：

（3）PST多解和缺解問題

由PST(perspective similar Triangle)求解，可得方程組等效轉換為四次多項式

多解問題： 由于存在多組解，相機位姿不能從3點集唯一確定的。解的個數直接對應于四次多項式實根的個數。要得到唯一的解，至少還應引入一點，構建2個三角形，進行求解。另一種方法是RANSAC算法，該算法將點集劃分為3個點子集，檢查這些子集的一致性。 RANSAC算法參考文獻：Random Sample Consensus: A Paradigm for Model Fitting with Apphcatlons to Image Analysis and Automated Cartography 缺解問題：

缺解問題是由P3P的固有結構決定的，其他P3P方法，如迭代解法、幾何解法和分類法，也有同樣的問題。

3.RPnP

文章[3]：A Robust O(n) Solution to the Perspective-n-Point Problem

下面來看一下，如何建立新的正交坐標系，以及如何求解正交坐標系到相機坐標系之間[R T]。

3.1確定旋轉軸

當確定旋轉軸時，只需求解剩余的旋轉和三個平移參數，減少了未知變量的數量，來提高方程組的數值精度。

3.2求解旋轉角和平移矢量的方程

相機坐標系與新坐標系繞之間的旋轉矩陣：

其中,

3.3獲取相機的位姿

再獲取到相機坐標系與新坐標系繞之間的旋轉和平移矩陣，進而可直接相機坐標系與世界坐標系繞之間旋轉和平移矩陣，即相機的位姿。

審核編輯：郭婷