PnP算法详解（超详细公式推导）

博主缺粉丝希望大家能给个关注！！！

PnP概述

PnP(Perspective-n-Point)是求解3D到2D点的对应方法。它描述了当知道n个3D空间点及其位置，如何估计相机的位姿。如果两张图像中的一张特征点3D位置已知，那么至少需要3个点对(以及至少一个额外验证点验证结果)就可以计算相机的运动。

• PnP的应用范围很广比如两阶段法的6D姿态估计以及视觉SLAM等等。
• 特征点的3D位置可以由三角化或者RGB-D相机的深度图确定，当然还有其他方法。

PnP数学模型

PnP问题的几何结构如下图所示，给定3D点的坐标以及对应2D点的坐标以及内参矩阵，求解相机的姿态。

已知:n个点在世界坐标系下的坐标$P_1$、$P_2$、…、$P_i$、…、$P_n$
  对应像素的坐标$p_1$、$p_2$、…、$p_i$、…、$p_n$
  相机内参$K$
求解:相机坐标系($O_c{X}_c{Y}_c{Z}_c$)相对于世界坐标系($O_w{X}_w{Y}_w{Z}_w$)的位姿，公式中(1)中的[R t]
[ X c Y c Z c ] = [ R   t ] [ X w Y w Z w ] ( 1 )

$$\begin{bmatrix} X_{c}\\ Y_{c}\\ Z_{c}\\ \end{bmatrix}$$ = [R\ t] $$\begin{bmatrix} X_{w}\\ Y_{w}\\ Z_{w}\\ \end{bmatrix}$$ (1) ⎣⎡​Xc​Yc​Zc​​⎦⎤​=[R t]⎣⎡​Xw​Yw​Zw​​⎦⎤​(1)

PnP求解方法

• DLT直接线性变换
• P3P三对点估计位姿
• EPnP(Efficient Pnp)
• BA(Bundle Adjustment)光速法平差

DLT直接线性变换法

假设：相机经过标定，也就是相机的内参K已知。
已知：空间中的3D点坐标： [ X w Y w Z w ] T

$$\begin{bmatrix} X_{w} & Y_{w} &Z_{w} \\ \end{bmatrix}$$ ^{T} [Xw​​Yw​​Zw​​]T 齐次坐标表示为${\left[\begin{array}{cccc}{X}_w& Y_w& Z_w& 1\end{array}\right]}^T$
投影点的坐标：$ $$\begin{bmatrix} u & v \ \end{bmatrix}$$ ^{T} $ 齐次坐标的表示为：${\left[\begin{array}{ccc}u& v& 1\end{array}\right]}^T$
详解的内参矩阵：$K$
求解：相机外参$R、t$
以下在Ipad上进行手写推倒

注意实际的R和t还应该乘以$K^{-1}$
字有点丑，大家见谅吧!!!
$这里我有个问题:$我推倒的时候只推出了$R=U{V}^T$,没有推出正负解，很多博客这里给的是正负解，$R=\pm U{V}^T$，这里面怎么出现的负解呢？

EPnP

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

EPnP的特点

• EPnP的复杂度是$O(n)$,其他算法的复杂度基本上是$O(n^3)$，所以对于特征点较多的PnP问题，非常高效。
• 核心思想是将3D点表示为4个控制点的组合，优化也只针对4个控制点，所以速度很快，在求解 $Mx=0$时，最多考虑了4个奇异向量，因此精度也很高。

步骤

1.在世界坐标系下确定4个控制点$c_j^w,j=1,2,3,4$,理论上可以任意取这四个控制点，只要不共面就行(因为共面无法组成坐标系),但原论文给了一种方法，取所有点的质心为$c_1^w$并作为原点，通过主成分分析PCA得到另外的三个点$c_2^w$、$c_3^w$、$c_4^w$建立坐标系。
2.已知参考点(特征点)在世界坐标系的坐标，$P_i^w,j=1,...,n$,以及控制点在世界坐标系下的坐标，计算权重因子$\alpha $
3.计算四个控制点在相机坐标系下的坐标$c_j^c,j=1,2,3,4$（核心）
4.计算参考点在相机坐标系下的坐标$P_i^c,j=1,...,n$
5.根据ICP方法，计算R，t。

理论推倒

1.控制点及齐次重心坐标系

这里实际上是步骤2，为了表述清晰我先说明。
这里为什么叫Homogeneous Barycentric Coordinates(HB)呢？是因为使用步骤1的方法进行了控制点的选取，那么EPnP算法可以将参考点的坐标表示为控制点坐标的加权和：
$$P_i^w=\sum _{j=1}^4{\alpha }_{ij}{c}_j^w,\sum _{j=1}^4{\alpha }_{ij}=1(1)$$
其中，${\alpha }_{ij}$是HB坐标，一旦控制点确定后，${\alpha }_{ij}$是唯一确定的。
在摄像头坐标系中存在同样的加权关系：
$$P_i^c=\sum _{j=1}^4{\alpha }_{ij}{c}_j^c(2)$$
那么为什么在摄像头坐标系中存在同样的加权关系，这里对(2)进行手写推导：

这里考虑一下为什么要四个控制点，要知道$P_i^w$是非齐次的3D坐标，$P_i^w\in R^3$,假设3个控制点满足条件那么
P i w = [ x i w y i w z i w ] = [ c 1 w c 2 w c 3 w ] [ α i 1 α i 2 α i 3 ] , ∑ j = 1 4 α i j = 1 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 Piw​=⎣⎡​xiw​yiw​ziw​​⎦⎤​=[c1w​​c2w​​c3w​​]⎣⎡​αi1​αi2​αi3​​⎦⎤​,j=1∑4​αij​=1
一共是4个方程，而未知数是3个，这是一个超定方程组，只存在最小二乘意义上的解。换句话，在一般情形下，不存在精确满足4个方程的解。按照同样的思路，把4个控制点时的约束写成矩阵形式：
$$\left[\begin{array}{c}{P}_i^w\\ 1\end{array}\right]=C\left[\begin{array}{c}{\alpha }_{i1}\\ {\alpha }_{i2}\\ {\alpha }_{i3}\\ {\alpha }_{i4}\end{array}\right]=\left[\begin{array}{cccc}{c}_1^w& c_2^w& c_3^w& c_4^w\\ 1& 1& 1& 1\end{array}\right]\left[\begin{array}{c}{\alpha }_{i1}\\ {\alpha }_{i2}\\ {\alpha }_{i3}\\ {\alpha }_{i4}\end{array}\right](3)$$

通过上面的推导可以发现，${\alpha }_{ij}$在世界坐标系和相机坐标系下相同，这就意味着，我们可以在世界坐标系下求出${\alpha }_{ij}$，然后应用在相机坐标系下。根据公式（3），我们也可以得到${\alpha }_{ij}$ 的计算方法：
[ α i 1 α i 2 α i 3 α i 4 ] 4 × 1 = [ c 1 w c 2 w c 3 w c 4 w 1 1 1 1 ] 4 × 4 − 1 [ P i w 1 ] 4 × 1 = C − 1 [ P i w 1 ]     ( 4 )

$$\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) ⎣⎢⎢⎡​αi1​αi2​αi3​αi4​​⎦⎥⎥⎤​4×1​=[c1w​1​c2w​1​c3w​1​c4w​1​]4×4−1​[Piw​1​]4×1​=C−1[Piw​1​]   (4)

2.控制点的选择

这里实际上是步骤1
原则上，只要控制点满足C可逆且不共面就可以，3D参考点集为 { P i w , i = 1 , 2 , . . . , n } \left\{ P_{i}^{w},i=1,2,...,n \right\} {Piw​,i=1,2,...,n}，选择3D参考点的重心为第一个控制点：
$$c_1^w=\frac{1}{n}\sum _{i=1}^n{P}_i^w$$
对参考点进行重心化，得到矩阵A：
A = [ P 1 w T − c 1 w T . . . P n w T − c n w T ] A=

$$\begin{bmatrix} P_{1}^{w^{T}}-c_{1}^{w^{T}} \\ ...\\ P_{n}^{w^{T}}-c_{n}^{w^{T}}\\ \end{bmatrix}$$ A=⎣⎡​P1wT​−c1wT​...PnwT​−cnwT​​⎦⎤​

计算$A^{T}A$的三个特征值${\lambda }_1,{\lambda }_2,{\lambda }_3$对应的特征向量$v_1,v_2,v_3$,
那么剩余的三个控制点可以按照下面的公式来确定：
{ c 2 w = c 1 w + λ 1 v 1 c 3 w = c 1 w + λ 2 v 2 c 4 w = c 1 w + λ 3 v 3 \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. ⎩⎨⎧​c2w​=c1w​+λ1​ ​v1​c3w​=c1w​+λ2​ ​v2​c4w​=c1w​+λ3​ ​v3​​

世界坐标系下控制点的计算：第一步找到点云的重心作为坐标系的原点，然后通过主成分分析(PCA)确定坐标轴的三个方向。

3.计算控制点在相机坐标系下的坐标

手推公式：

图中最后的矩阵，齐次重心坐标${\alpha }_{ij}$，相机内参数和2D投影的像素坐标都是已知量，未知量是4个控制点在相机坐标系下的坐标。共12个位置参数，一个像点可以列2个方程，n 个像点可以列出2n 个方程。
$$M_{2n\times 12}{X}_{12\times 1}(5)$$

公式(5)中即为4个待求的3D控制点坐标，共有12个未知数维度是12×1。M的大小为$2n\times 12$，类比于前面讲的DLT方法，可以直接进行SVD分解。

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

这里计算一下直接对M进行SVD分解的复杂度$O(SVD(M))=2n\times 2n\times 2n=8n^3=O(n^3)$,这里解释一下由于矩阵的乘法先进行每一行与每一列相乘，假设是第一行与第一列，那么会有2nx2n个参数，一共有2n行，所以是2nx2nx2n。
EPnP采用了一种复杂度更低更为高效的方法，即对$M^{T}M$进行特征值分解：

这里的复杂度为O(n),计算公式为2nx12x12。
由此可以解出
$$X=\sum _{i=1}^N{\beta }_i{v}_i(6)$$
(6)式中，$v_i$是M的N个零特征值对应的特征向量$(这块是为什么不太懂？懂得可以评论！！！！)$。对于第i个控制点-：
$$c_i^c=\sum _{k=1}^N{\beta }_k{v}_k^{[i]}(7)$$
可以写成展开形式

上式中，$v_k^{[i]}$是特征向量$v_k$的第i个子向量，一共四个控制点，所以是四个。
通过对$M^{T}M$进行特征值分解我们能够求出N个$V_k$ 。但还需要求出 { β k } , k = 1 , 2 , 3 , . . . , N \left\{\beta_{k} \right\}, k=1,2,3,...,N {βk​},k=1,2,3,...,N。才能最终求出在相机坐标系下的控制点坐标。
在原始论文中指出，$M^{T}M$特征值的个数与点对的数量以及焦距有关，EPnP算法建议只考虑N=1, 2, 3, 4的情况。

控制点在相机坐标系和世界坐标系的相对位置关系是不会发生改变的，引入相对位置约束条件：
$$\Vert c_i^c-c_j^c\Vert =\Vert c_i^w-c_j^w\Vert (8)$$

该公式的含义是在4个控制点中任取两个点，一个为i，一个为j，进行相对位置关系计算。
将式(7)代入式(8)中：
$$\Vert \sum _{k=1}^N{\beta }_k{v}_k^{[i]}-\sum _{k=1}^N{\beta }_k{v}_k^{[j]}\Vert =\Vert c_i^w-c_j^w\Vert (9)$$
对于4个控制点，根据排列组合可以得到$C_4^2$个这样的方程,分别是1-2，1-3，1-4，2-3， 2-4，3-4。

对N=1，N=2，N=3，N=4的情况进行手写推导：

在N=4的情况下，有10个未知数，6个方程。在Opencv中实现EPnP算法很简单：

solvePnP(pts_3d, pts_2d, K, Mat(), r, t, CV_EPNP); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法,默认采用迭代法（最小化重投影）
1

在OpenCV中开源代码并没按照上述4种情况的方法去求解，而是采用了近似的解法，具体的可以去看一下源码。
值得说明的是，在代码中$L$和$\beta$ 的排序有点不同，但不影响求解只要 $L$和$\beta$ 的顺序对应即可，以$\beta$ 为例说明。

Opencv的解法：
因为${\beta }_{11}$、${\beta }_{12}$、${\beta }_{13}$、${\beta }_{14}$、${\beta }_{22}$、${\beta }_{23}$、${\beta }_{24}$、${\beta }_{33}$、${\beta }_{34}$、${\beta }_{44}$这10个未知数是相关的，所以我们只需求出${\beta }_{11}$、${\beta }_{12}$、${\beta }_{13}$、${\beta }_{14}$，就能从中解出${\beta }_1$、${\beta }_2$、${\beta }_3$、${\beta }_4$的值。

在OpenCV的源码中取 的0,1,3,6列组成了新的矩阵，,然后进行SVD分解。由于，我们写出的公式跟代码列出的公式顺序不一样。因此我们对应的选择 的0,1,2,3列组成了新的矩阵$L_{6\times 4}$。

上述方程可以列出6个，所以表示如下：
[ S 1 T S 1 2 S 1 T S 2 2 S 1 T S 3 2 S 1 T S 4 ] [ β 11 β 12 β 13 β 14 ] = c    ( 10 )

$$\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) [S1T​S1​2S1T​S2​2S1T​S3​2S1T​S4​]⎣⎢⎢⎡​β11​β12​β13​β14​​⎦⎥⎥⎤​=c  (10)
上述方程可以列出6个，因为i与j的不同组合有6种，所以表示如下：
$$L_{6\times 4}{\beta }_{4\times 1}={\rho }_{6\times 1}(11)$$
这有变成了与N=2一样的问题，只不过N=2的未知数是三个，这里面是4个，同理可以用SVD方法求解，得到${\beta }_{11}$、${\beta }_{12}$、${\beta }_{13}$、${\beta }_{14}$，就可以求出${\beta }_1$、${\beta }_2$、${\beta }_3$、${\beta }_4$

• Gauss-Newton(高斯-牛顿法)优化参数$\beta$
  如果大家需要经常研究优化问题，那么我强烈建议不要仅仅去看高翔老师的视觉SLAM14讲，特别是搞研究的，一定要去看最优化理论，推荐陈宝林老师的书,我在后续的博客可能也会从数学的角度写一些基础的算法。

进入正题：
优化目标： 缩小两个坐标系下控制点间距差。
优化的目标函数：
$$Error(\beta {)}_{ij}={\Vert c_i^c-c_j^c\Vert }^2-{\Vert c_i^w-c_j^w\Vert }^2(12)$$

$${\beta }^{\ast }=argmi{n}_{\beta }\sum _{(i,j)s.t.i<j}{\Vert Erro{r}_{ij}(\beta )\Vert }^2(13)$$

这是一个无约束的非线性最优化问题，Gauss-Newton求解式，首先求解$Error(\beta )$相对于$\beta$的雅克比矩阵。

$J_{ij}$的维度为1×4，将6个小雅克比矩阵$J_{ij}$合成为6×4的大雅克比矩阵
J = [ J 12 J 13 J 14 J 23 J 24 J 34 ] J=

$$\begin{bmatrix} J_{12}\\ J_{13}\\ J_{14}\\ J_{23}\\ J_{24}\\ J_{34} \end{bmatrix}$$ J=⎣⎢⎢⎢⎢⎢⎢⎡​J12​J13​J14​J23​J24​J34​​⎦⎥⎥⎥⎥⎥⎥⎤​，记残差为$e=Error=\left[\begin{array}{c}Erro{r}_{12}(\beta )\\ Erro{r}_{13}(\beta )\\ Erro{r}_{14}(\beta )\\ Erro{r}_{23}(\beta )\\ Erro{r}_{24}(\beta )\\ Erro{r}_{34}(\beta )\end{array}\right]$
增量方程：
$$J^{T}J\delta \beta =-J^{T}e$$
$\delta \beta$的求解在OpenCV中没有采用$\delta \beta =-(J^{T}J{)}^{-1}{J}^{T}e$的方式求解，而是对$J\delta \beta =-e$进行QR分解，从而得到$\delta \beta$。
因为J是一个超定矩阵，求线性最小二乘问题时，正规方程的解是不稳定的，所以用QR分解。
之后更新$\beta$,$\beta :=\beta +\delta \beta$

至此我们通过N=1，N=2，N=3，N=4可以确定四组$\beta$和$v$,最后通过公式（6）计算控制点在相机坐标下的坐标。$(具体选择哪种情况需要根据恢复影像的外方位元素后，计算的反投影误差决定！，不太理解，欢迎讨论)$

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

转变成了已知一组3D点在不同坐标系下的坐标求位姿R，t的问题，也就是典型的ICP问题。
ICP（迭代最近点）算法推导详解这篇文章进行了详细推导与解释。

参考文章

https://zhuanlan.zhihu.com/p/361791835
https://blog.csdn.net/jessecw79/article/details/82945918#control_points_64
[泡泡机器人公开课]第三十九课：PnP 算法简介&代码解析-柴政