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

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

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

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

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

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

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

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

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

2.P3P

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

圖2.兩點(diǎn)約束

注：直接線性變換法，只考慮了線性意義下的最優(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.三點(diǎn)約束 對于公式（16）的變量有一些真實(shí)的約束：

圖4.三點(diǎn)約束--重定義邊長 公式(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)換為四次多項(xiàng)式

多解問題： 由于存在多組解，相機(jī)位姿不能從3點(diǎn)集唯一確定的。解的個數(shù)直接對應(yīng)于四次多項(xiàng)式實(shí)根的個數(shù)。要得到唯一的解，至少還應(yīng)引入一點(diǎn)，構(gòu)建2個三角形，進(jìn)行求解。另一種方法是RANSAC算法，該算法將點(diǎn)集劃分為3個點(diǎn)子集，檢查這些子集的一致性。 RANSAC算法參考文獻(xiàn)：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

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

3.1確定旋轉(zhuǎn)軸

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

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

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

其中,

3.3獲取相機(jī)的位姿

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

審核編輯：郭婷