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

數(shù)學(xué)語言描述如下：

圖1.PnP幾何結(jié)構(gòu)

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

假設(shè)：攝像機已經(jīng)校準過了。 已知：

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

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

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

因此, 旋轉(zhuǎn)矩陣, 平移矩陣求得:

2.P3P

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

圖2.兩點約束

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

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 使用相似三角形，利用幾何約束來減少未知參數(shù)的個數(shù)，把P3P方程組轉(zhuǎn)化為四次方程，該稱為透視相似三角形方法(Perspective Similar Triangle ，PST)。

（1）P3P問題轉(zhuǎn)為PST問題

圖5.P3P幾何結(jié)構(gòu)

（2）PST的求解

圖6.PST幾何結(jié)構(gòu) 約束1：相似三角形對應(yīng)邊成比例

所以：

（3）PST多解和缺解問題

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

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

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

3.RPnP

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

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

3.1確定旋轉(zhuǎn)軸

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

3.2求解旋轉(zhuǎn)角和平移矢量的方程

相機坐標系與新坐標系繞之間的旋轉(zhuǎn)矩陣：

其中,

3.3獲取相機的位姿

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

審核編輯：郭婷