PnP概述

PnP(Perspective-n-Point)是求解3D到2D點(diǎn)的對(duì)應(yīng)方法。它描述了當(dāng)知道n個(gè)3D空間點(diǎn)及其位置，如何估計(jì)相機(jī)的位姿。如果兩張圖像中的一張?zhí)卣鼽c(diǎn)3D位置已知，那么至少需要3個(gè)點(diǎn)對(duì)(以及至少一個(gè)額外驗(yàn)證點(diǎn)驗(yàn)證結(jié)果)就可以計(jì)算相機(jī)的運(yùn)動(dòng)。

PnP的應(yīng)用范圍很廣比如兩階段法的6D姿態(tài)估計(jì)以及視覺SLAM等等。
特征點(diǎn)的3D位置可以由三角化或者RGB-D相機(jī)的深度圖確定，當(dāng)然還有其他方法。

PnP數(shù)學(xué)模型

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

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

已知:n個(gè)點(diǎn)在世界坐標(biāo)系下的坐標(biāo) $P_{1}$ 、 $P_{2}$ 、......、 $P_{i}$ 、......、 $P_{n}$
??對(duì)應(yīng)像素的坐標(biāo) $p_{1}$ 、 $p_{2}$ 、......、 $p_{i}$ 、......、 $p_{n}$
??相機(jī)內(nèi)參 $K$
求解:相機(jī)坐標(biāo)系( $O_{c}X_{c}Y_{c}Z_{c}$ )相對(duì)于世界坐標(biāo)系( $O_{w}X_{w}Y_{w}Z_{w}$ )的位姿，公式中(1)中的[R t]
$\begin{bmatrix} X_{c}\\ Y_{c}\\ Z_{c}\\ \end{bmatrix} = [R\ t]\begin{bmatrix} X_{w}\\ Y_{w}\\ Z_{w}\\ \end{bmatrix} (1)$

PnP求解方法

DLT直接線性變換
P3P三對(duì)點(diǎn)估計(jì)位姿
EPnP(Efficient Pnp)
BA(Bundle Adjustment)光速法平差

DLT直接線性變換法

假設(shè)：相機(jī)經(jīng)過標(biāo)定，也就是相機(jī)的內(nèi)參K已知。
已知：空間中的3D點(diǎn)坐標(biāo)： $\begin{bmatrix} X_{w} & Y_{w} &Z_{w} \\ \end{bmatrix}^{T}$ 齊次坐標(biāo)表示為 $\begin{bmatrix} X_{w} & Y_{w} &Z_{w} & 1 \\ \end{bmatrix}^{T}$
投影點(diǎn)的坐標(biāo)： $\begin{bmatrix} u & v \\ \end{bmatrix}^{T}$ 齊次坐標(biāo)的表示為： $\begin{bmatrix} u & v & 1\\ \end{bmatrix}^{T}$
詳解的內(nèi)參矩陣： $K$
求解：相機(jī)外參 $R、t$
以下在Ipad上進(jìn)行手寫推倒

注意實(shí)際的R和t還應(yīng)該乘以 $K^{-1}$
字有點(diǎn)丑，大家見諒吧!!!
$\color{red}{這里我有個(gè)問題:}$ 我推倒的時(shí)候只推出了 $R=UV^{T}$ ,沒有推出正負(fù)解，很多博客這里給的是正負(fù)解， $R=\pm UV^{T}$ ，這里面怎么出現(xiàn)的負(fù)解呢？

EPnP

原論文:EPnP: An Accurate O(n) Solution to the PnP Problem

EPnP的特點(diǎn)

EPnP的復(fù)雜度是 $O(n)$ ,其他算法的復(fù)雜度基本上是 $O(n^{3})$ ，所以對(duì)于特征點(diǎn)較多的PnP問題，非常高效。
核心思想是將3D點(diǎn)表示為4個(gè)控制點(diǎn)的組合，優(yōu)化也只針對(duì)4個(gè)控制點(diǎn)，所以速度很快，在求解 $Mx=0$ 時(shí)，最多考慮了4個(gè)奇異向量，因此精度也很高。

步驟

1.在世界坐標(biāo)系下確定4個(gè)控制點(diǎn) $c^{w}_{j},j=1,2,3,4$ ,理論上可以任意取這四個(gè)控制點(diǎn)，只要不共面就行(因?yàn)楣裁鏌o法組成坐標(biāo)系),但原論文給了一種方法，取所有點(diǎn)的質(zhì)心為 $c^{w}_{1}$ 并作為原點(diǎn)，通過主成分分析PCA得到另外的三個(gè)點(diǎn) $c^{w}_{2}$ 、 $c^{w}_{3}$ 、 $c^{w}_{4}$ 建立坐標(biāo)系。
2.已知參考點(diǎn)(特征點(diǎn))在世界坐標(biāo)系的坐標(biāo)， $P^{w}_{i},j=1,...,n$ ,以及控制點(diǎn)在世界坐標(biāo)系下的坐標(biāo)，計(jì)算權(quán)重因子 $\alpha$
3.計(jì)算四個(gè)控制點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo) $c^{c}_{j},j=1,2,3,4$ （核心）
4.計(jì)算參考點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo) $P^{c}_{i},j=1,...,n$
5.根據(jù)ICP方法，計(jì)算R，t。

EPnP方法示意圖

理論推倒

1.控制點(diǎn)及齊次重心坐標(biāo)系

這里實(shí)際上是步驟2，為了表述清晰我先說明。
這里為什么叫Homogeneous Barycentric Coordinates(HB)呢？是因?yàn)槭褂貌襟E1的方法進(jìn)行了控制點(diǎn)的選取，那么EPnP算法可以將參考點(diǎn)的坐標(biāo)表示為控制點(diǎn)坐標(biāo)的加權(quán)和：
$P^{w}_{i}=\sum_{j=1}^{4}\alpha_{ij}c^{w}_{j},\sum_{j=1}^{4}\alpha_{ij}=1 \ \ \ (1)$
其中， $\alpha_{ij}$ 是HB坐標(biāo)，一旦控制點(diǎn)確定后， $\alpha_{ij}$ 是唯一確定的。
在攝像頭坐標(biāo)系中存在同樣的加權(quán)關(guān)系：
$P^{c}_{i}=\sum_{j=1}^{4}\alpha_{ij}c^{c}_{j}\ \ \ (2)$
那么為什么在攝像頭坐標(biāo)系中存在同樣的加權(quán)關(guān)系，這里對(duì)(2)進(jìn)行手寫推導(dǎo)：

公式2推動(dòng)導(dǎo)

這里考慮一下為什么要四個(gè)控制點(diǎn)，要知道 $P^{w}_{i}$ 是非齊次的3D坐標(biāo)， $P^{w}_{i}\in R^{3}$ ,假設(shè)3個(gè)控制點(diǎn)滿足條件那么
$P^{w}_{i}=\begin{bmatrix} x^{w}_{i} \\ y^{w}_{i}\\ z^{w}_{i}\\ \end{bmatrix}=\begin{bmatrix} c^{w}_{1} &c^{w}_{2} & c^{w}_{3} \\ \end{bmatrix}\begin{bmatrix} \alpha_{i1}\\ \alpha_{i2}\\ \alpha_{i3}\\ \end{bmatrix},\sum_{j=1}^{4}\alpha_{ij}=1$
一共是4個(gè)方程，而未知數(shù)是3個(gè)，這是一個(gè)超定方程組，只存在最小二乘意義上的解。換句話，在一般情形下，不存在精確滿足4個(gè)方程的解。按照同樣的思路，把4個(gè)控制點(diǎn)時(shí)的約束寫成矩陣形式：
$\begin{bmatrix} P^{w}_{i}\\ 1 \\ \end{bmatrix}=C\begin{bmatrix} \alpha_{i1}\\ \alpha_{i2}\\ \alpha_{i3}\\ \alpha_{i4}\\ \end{bmatrix}=\begin{bmatrix} c^{w}_{1} &c^{w}_{2} & c^{w}_{3} & c^{w}_{4} \\ 1& 1 &1& 1 \\ \end{bmatrix}\begin{bmatrix} \alpha_{i1}\\ \alpha_{i2}\\ \alpha_{i3}\\ \alpha_{i4}\\ \end{bmatrix} \ \ \ (3)$

通過上面的推導(dǎo)可以發(fā)現(xiàn)， $\alpha_{ij}$ 在世界坐標(biāo)系和相機(jī)坐標(biāo)系下相同，這就意味著，我們可以在世界坐標(biāo)系下求出 $\alpha_{ij}$ ，然后應(yīng)用在相機(jī)坐標(biāo)系下。根據(jù)公式（3），我們也可以得到 $\alpha_{ij}$ 的計(jì)算方法：
$\begin{bmatrix} \alpha_{i1}\\ \alpha_{i2}\\ \alpha_{i3}\\ \alpha_{i4}\\ \end{bmatrix}_{4\times 1}=\begin{bmatrix} c^{w}_{1} &c^{w}_{2} & c^{w}_{3} & c^{w}_{4} \\ 1& 1 &1& 1 \\ \end{bmatrix}^{-1}_{4\times 4}\begin{bmatrix} P^{w}_{i} \\ 1\\ \end{bmatrix}_{4\times 1}=C^{-1}\begin{bmatrix} P^{w}_{i} \\ 1\\ \end{bmatrix} \ \ \ (4)$

2.控制點(diǎn)的選擇

這里實(shí)際上是步驟1
原則上，只要控制點(diǎn)滿足C可逆且不共面就可以，3D參考點(diǎn)集為 $\left\{ P_{i}^{w},i=1,2,...,n \right\}$ ，選擇3D參考點(diǎn)的重心為第一個(gè)控制點(diǎn)：
$c_{1}^{w}=\frac{1}{n}\sum_{i=1}^{n}P_{i}^{w}$
對(duì)參考點(diǎn)進(jìn)行重心化，得到矩陣A：
$A=\begin{bmatrix} P_{1}^{w^{T}}-c_{1}^{w^{T}} \\ ...\\ P_{n}^{w^{T}}-c_{n}^{w^{T}}\\ \end{bmatrix}$

計(jì)算 $A^{T}A$ 的三個(gè)特征值 $\lambda_{1},\lambda_{2},\lambda_{3}$ 對(duì)應(yīng)的特征向量 $v_{1},v_{2},v_{3}$ ,
那么剩余的三個(gè)控制點(diǎn)可以按照下面的公式來確定：
$\left\{\begin{matrix} c_{2}^{w}=c_{1}^{w} + \sqrt{\lambda_{1}}v_{1} \\ c_{3}^{w}=c_{1}^{w} + \sqrt{\lambda_{2}}v_{2}\\ c_{4}^{w}=c_{1}^{w} + \sqrt{\lambda_{3}}v_{3}\\ \end{matrix}\right.$

世界坐標(biāo)系下控制點(diǎn)的計(jì)算：第一步找到點(diǎn)云的重心作為坐標(biāo)系的原點(diǎn)，然后通過主成分分析(PCA)確定坐標(biāo)軸的三個(gè)方向。

3.計(jì)算控制點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)

手推公式：

圖中最后的矩陣，齊次重心坐標(biāo) $\alpha_{ij}$ ，相機(jī)內(nèi)參數(shù)和2D投影的像素坐標(biāo)都是已知量，未知量是4個(gè)控制點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)。共12個(gè)位置參數(shù)，一個(gè)像點(diǎn)可以列2個(gè)方程，n 個(gè)像點(diǎn)可以列出2n 個(gè)方程。
$M_{2n\times12}X_{12 \times 1} \ \ \ (5)$

公式(5)中即為4個(gè)待求的3D控制點(diǎn)坐標(biāo)，共有12個(gè)未知數(shù)維度是12×1。M的大小為 $2n\times12$ ，類比于前面講的DLT方法，可以直接進(jìn)行SVD分解。

$M=U \sum V^{T},O(n^{3})$

這里計(jì)算一下直接對(duì)M進(jìn)行SVD分解的復(fù)雜度 $O(SVD(M))=2n \times 2n \times 2n=8n^{3}=O(n^{3})$ ,這里解釋一下由于矩陣的乘法先進(jìn)行每一行與每一列相乘，假設(shè)是第一行與第一列，那么會(huì)有2nx2n個(gè)參數(shù)，一共有2n行，所以是2nx2nx2n。
EPnP采用了一種復(fù)雜度更低更為高效的方法，即對(duì) $M^{T}M$ 進(jìn)行特征值分解：

這里的復(fù)雜度為O(n),計(jì)算公式為2nx12x12。
由此可以解出

X=\sum_{i=1}^{N}\beta_{i}v_{i} \ \ \ \ (6)

(6)式中，

v_{i}

是M的N個(gè)零特征值對(duì)應(yīng)的特征向量

\color{red}{(這塊是為什么不太懂？懂得可以評(píng)論！?。?！)}

。對(duì)于第i個(gè)控制點(diǎn)-：

c_{i}^{c}=\sum_{k=1}^{N}\beta_{k}v_{k}^{[i]} \ \ \ (7)

可以寫成展開形式

上式中， $v_{k}^{[i]}$ 是特征向量 $v_{k}$ 的第i個(gè)子向量，一共四個(gè)控制點(diǎn)，所以是四個(gè)。
通過對(duì) $M^{T}M$ 進(jìn)行特征值分解我們能夠求出N個(gè) $V_{k}$ 。但還需要求出 $\left\{\beta_{k} \right\}, k=1,2,3,...,N$ 。才能最終求出在相機(jī)坐標(biāo)系下的控制點(diǎn)坐標(biāo)。
在原始論文中指出， $M^{T}M$ 特征值的個(gè)數(shù)與點(diǎn)對(duì)的數(shù)量以及焦距有關(guān)，EPnP算法建議只考慮N=1, 2, 3, 4的情況。

控制點(diǎn)在相機(jī)坐標(biāo)系和世界坐標(biāo)系的相對(duì)位置關(guān)系是不會(huì)發(fā)生改變的，引入相對(duì)位置約束條件：
$\left\| c_{i}^{c}-c_{j}^{c}\right\|=\left\|c_{i}^{w}-c_{j}^{w} \right\| \ \ (8)$

該公式的含義是在4個(gè)控制點(diǎn)中任取兩個(gè)點(diǎn)，一個(gè)為i，一個(gè)為j，進(jìn)行相對(duì)位置關(guān)系計(jì)算。
將式(7)代入式(8)中：
$\left\| \sum_{k=1}^{N}\beta_{k}v_{k}^{[i]}-\sum_{k=1}^{N}\beta_{k}v_{k}^{[j]} \right\|=\left\|c_{i}^{w}-c_{j}^{w} \right\| \ \ \ (9)$
對(duì)于4個(gè)控制點(diǎn)，根據(jù)排列組合可以得到 $C_{4}^{2}$ 個(gè)這樣的方程,分別是1-2，1-3，1-4，2-3， 2-4，3-4。

對(duì)N=1，N=2，N=3，N=4的情況進(jìn)行手寫推導(dǎo)：

在N=4的情況下，有10個(gè)未知數(shù)，6個(gè)方程。在Opencv中實(shí)現(xiàn)EPnP算法很簡(jiǎn)單：

solvePnP(pts_3d, pts_2d, K, Mat(), r, t, CV_EPNP); // 調(diào)用OpenCV 的 PnP 求解，可選擇EPNP，DLS等方法,默認(rèn)采用迭代法（最小化重投影）

在OpenCV中開源代碼并沒按照上述4種情況的方法去求解，而是采用了近似的解法，具體的可以去看一下源碼。
值得說明的是，在代碼中 $L$ 和 $\beta$ 的排序有點(diǎn)不同，但不影響求解只要 $L$ 和 $\beta$ 的順序?qū)?yīng)即可，以 $\beta$ 為例說明。

Opencv的解法：
因?yàn)?img class="math-inline" src="https://math.jianshu.com/math?formula=%5Cbeta_%7B11%7D" alt="\beta_{11}" mathimg="1">、 $\beta_{12}$ 、 $\beta_{13}$ 、 $\beta_{14}$ 、 $\beta_{22}$ 、 $\beta_{23}$ 、 $\beta_{24}$ 、 $\beta_{33}$ 、 $\beta_{34}$ 、 $\beta_{44}$ 這10個(gè)未知數(shù)是相關(guān)的，所以我們只需求出 $\beta_{11}$ 、 $\beta_{12}$ 、 $\beta_{13}$ 、 $\beta_{14}$ ，就能從中解出 $\beta_{1}$ 、 $\beta_{2}$ 、 $\beta_{3}$ 、 $\beta_{4}$ 的值。

在OpenCV的源碼中取的0,1,3,6列組成了新的矩陣，,然后進(jìn)行SVD分解。由于，我們寫出的公式跟代碼列出的公式順序不一樣。因此我們對(duì)應(yīng)的選擇的0,1,2,3列組成了新的矩陣 $L_{6\times 4}$ 。

上述方程可以列出6個(gè)，所以表示如下：
$\begin{bmatrix} S_{1}^{T}S1& 2S_{1}^{T}S2 & 2S_{1}^{T}S3 & 2S_{1}^{T}S4 \\ \end{bmatrix}\begin{bmatrix} \beta_{11} \\ \beta_{12} \\ \beta_{13} \\ \beta_{14} \end{bmatrix}=c \ \ (10)$
上述方程可以列出6個(gè)，因?yàn)閕與j的不同組合有6種，所以表示如下：
$L_{6 \times 4}\beta_{4 \times 1}=\rho_{6 \times 1} \ \ (11)$
這有變成了與N=2一樣的問題，只不過N=2的未知數(shù)是三個(gè)，這里面是4個(gè)，同理可以用SVD方法求解，得到 $\beta_{11}$ 、 $\beta_{12}$ 、 $\beta_{13}$ 、 $\beta_{14}$ ，就可以求出 $\beta_{1}$ 、 $\beta_{2}$ 、 $\beta_{3}$ 、 $\beta_{4}$

Gauss-Newton(高斯-牛頓法)優(yōu)化參數(shù) $\beta$
如果大家需要經(jīng)常研究?jī)?yōu)化問題，那么我強(qiáng)烈建議不要僅僅去看高翔老師的視覺SLAM14講，特別是搞研究的，一定要去看最優(yōu)化理論，推薦陳寶林老師的書,我在后續(xù)的博客可能也會(huì)從數(shù)學(xué)的角度寫一些基礎(chǔ)的算法。

進(jìn)入正題：
優(yōu)化目標(biāo)：縮小兩個(gè)坐標(biāo)系下控制點(diǎn)間距差。
優(yōu)化的目標(biāo)函數(shù)：
$Error(\beta)_{ij}=\left\|c_{i}^{c}-c_{j}^{c} \right\|^{2}-\left\|c_{i}^{w}-c_{j}^{w} \right\|^{2} \ \ (12)$

$\beta^{*}=arg min_{\beta}\sum_{(i,j)s.t.i<j}^{}\left\|Error_{ij}(\beta) \right\|^{2} \ (13)$

這是一個(gè)無約束的非線性最優(yōu)化問題，Gauss-Newton求解式，首先求解 $Error(\beta)$ 相對(duì)于 $\beta$ 的雅克比矩陣。

$J_{ij}$ 的維度為1×4，將6個(gè)小雅克比矩陣 $J_{ij}$ 合成為6×4的大雅克比矩陣
$J=\begin{bmatrix} J_{12}\\ J_{13}\\ J_{14}\\ J_{23}\\ J_{24}\\ J_{34} \end{bmatrix}$ ，記殘差為 $e=Error=\begin{bmatrix} Error_{12}(\beta)\\ Error_{13}(\beta)\\ Error_{14}(\beta)\\ Error_{23}(\beta)\\ Error_{24}(\beta)\\ Error_{34}(\beta) \end{bmatrix}$
增量方程：
$J^{T}J\delta \beta=-J^{T}e$
$\delta\beta$ 的求解在OpenCV中沒有采用 $\delta\beta=-(J^{T}J)^{-1}J^{T}e$ 的方式求解，而是對(duì) $J\delta\beta=-e$ 進(jìn)行QR分解，從而得到 $\delta\beta$ 。
因?yàn)镴是一個(gè)超定矩陣，求線性最小二乘問題時(shí)，正規(guī)方程的解是不穩(wěn)定的，所以用QR分解。
之后更新 $\beta$ , $\beta:=\beta+\delta\beta$

至此我們通過N=1，N=2，N=3，N=4可以確定四組 $\beta$ 和 $v$ ,最后通過公式（6）計(jì)算控制點(diǎn)在相機(jī)坐標(biāo)下的坐標(biāo)。 $\color{red}{(具體選擇哪種情況需要根據(jù)恢復(fù)影像的外方位元素后，計(jì)算的反投影誤差決定！，不太理解，歡迎討論)}$

4.求解R，t（ICP方法）

轉(zhuǎn)變成了已知一組3D點(diǎn)在不同坐標(biāo)系下的坐標(biāo)求位姿R，t的問題，也就是典型的ICP問題。
ICP（迭代最近點(diǎn)）算法推導(dǎo)詳解這篇文章進(jìn)行了詳細(xì)推導(dǎo)與解釋。

參考文章

https://zhuanlan.zhihu.com/p/361791835
https://blog.csdn.net/jessecw79/article/details/82945918#control_points_64
[泡泡機(jī)器人公開課]第三十九課：PnP 算法簡(jiǎn)介&代碼解析-柴政

色偷偷精品伊人,欧洲久久精品,欧美综合婷婷骚逼,国产AV主播,国产最新探花在线,九色在线视频一区,伊人大交九欧美,1769亚洲,黄色成人av

PnP算法詳解（超詳細(xì)公式推導(dǎo)）

PnP算法詳解（超詳細(xì)公式推導(dǎo)）

PnP概述

PnP數(shù)學(xué)模型

PnP求解方法

DLT直接線性變換法