2D-3D匹配问题的PnP算法对比：DLT，P3P，EPnP

一. 问题定义

首先需要清楚什么叫做PnP(Perspective-n-Point)呢？是为了解决什么问题？
已知信息：

• n个3D点在A坐标系(可以认为是世界坐标系)的坐标$\{p_1,p_2,...,p_n\}$，以及这些3D点投影在图像上的2D点在图像坐标系的坐标$\{u_1,u_2,...,u_n\}$

• 这n个3D参考点和图像上2D投影点的的匹配关系（3D位置通常由三角化或者RGBD的深度图确定，对于双目或RGBD的里程计，可以直接用PnP估计相机运动，而单目视觉里程计需要先初始化）

• 相机内参K，畸变系数

待求变量：

• A坐标系(可以是世界坐标系，也可以是另一个相机坐标系)到相机坐标系的位姿变换，即旋转和平移

插图来自opencv官网。这里的相机坐标系，x轴指向有，y轴指向下，z轴指向前方。

为什么要用3D-2D的方法进行姿态估计？
这种方法不需要对极约束，有可以在很少的匹配点中获得较好的运动估计。

可选方法以及前提条件：
这类问题有很多种求解方法，比如P3P，直接线性变换(DLT)，EPnP，UPnP等等，还可以用非线性优化来构建最小二乘问题来迭代求解，即光束平差法(BA)。

选项	DLT	P3P	EPnP	非线性优化(BA)
求解思路	构建增广矩阵(R|t)，通过投影矩阵构建12维的方程组	通过三个点对的相似三角形和余弦定理联立方程	通过控制点来变换，复杂度为O(n)	把相机位姿和空间点位置都看成优化变量，最小化重投影误差
概况	至少要6个点对(12个未知数，每个点对构建两个方程)，大于6个时，可以用SVD等对超定方程求最小二乘解	需要4个点对，3个用来求解，一个点对用来验证，返回唯一的解	对于有噪音的特征点效果较好；求的是闭式解，不需要迭代和初始估计值
不足	忽略了T矩阵之间的联系，求出的R不一定满足SO(3)，需要找到旋转矩阵对它近似	难以运用更多匹配点对的信息，遇到噪声或误匹配就会失效。需要4个点对，且这些点要是共面的		可能陷入局部最小

二. 直接线性变换DLT

已知一个3D点在世界坐标系的齐次坐标，其对应的2D投影点的齐次坐标，相机内参（不知道也可以，可以用PnP去估计K,R,t, 未知数多一点结果稍微差一点），那么就可以写出3D点到2D点的投影：
$$\lambda \left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}R|t\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right)$$
在这个方法中，忽略了旋转矩阵自身的正交约束，R和t相当于就是12个未知数，$(R|t)$就是3*4的矩阵。
这时就可以自己动动手，把这个式子全部展开，然后消去$\lambda$项，写成矩阵形式：
$$\left(\begin{array}{cccccccccccc}X{f}_x& Y{f}_x& Z{f}_x& f_x& 0& 0& 0& 0& X{c}_x-ux& Y{c}_x-uY& Z{c}_x-uZ& c_x-u\\ 0& 0& 0& 0& X{f}_y& Y{f}_y& Z{f}_y& f_y& X{c}_y-vx& Y{c}_y-vY& Z{c}_y-vZ& c_y-v\end{array}\right)\left(\begin{array}{c}{a}_1\\ a_2\\ .\\ .\\ .\\ a_{12}\end{array}\right)=0$$
可以看到一个点对可以提供2个方程，那么12个未知数就至少需要6个点对。

当超过6个点对时，就会出现超定，这时就要用SVD求解：阅读相关

这里存在一个问题，假设未知数的时候，12个未知数之间没有相互关系，求出来的旋转矩阵不一定能满足正定的要求。所以此时还要找一个最好的旋转矩阵跟它近似，这里可以使用QR分解，相当于把结果从矩阵空间重新投影到SE(3)流形上，转换成旋转和平移两部分。
$$R\leftarrow (R{R}^T{)}^{-\frac{1}{2}}R$$

参考资料：

三. P3P

已知信息：

• A.B.C三点在世界坐标系下的坐标 ，就可以求出AB，BC，AC的长度

• $\angle BOA(\alpha ),\angle AOC(\beta ),\angle AOB(\gamma )$：这三个角度是已知的量，我个人是这么理解的，首先不能直接用OA，OB，OC加余弦定理来求，因为并不知道光心O在世界坐标系下的位置。在已知3D点世界坐标和2D点匹配的情况下，可以画出上图，可以发现，2D点和对应的相机坐标系中的点也可以形成相同的角度，这样，已知内参和像素坐标就可以求出夹角。

  此时，可以先把像素坐标转化为归一化后的相机坐标系坐标，比如$(\frac{x}{z},\frac{y}{z},1)=(\frac{u}{f_x}-\frac{c_x}{f_x},\frac{v}{f_y}-\frac{c_y}{f_y},1)$，然后我们就可以求从光心O到这个归一化坐标点的模长，然后把这个向量长度归为单位长度，即让${}^k{X}_1^s{,}^k{X}_2^s{,}^k{X}_3^s$都是从光心指向相机坐标点的单位向量。有了单位向量，直接做内积，就可以求出余弦值了，就可以得到三个角度。

最终目的：
通过余弦定理，我们可以得到OA，OB，OC的长度，也就可以得到ABC三个点在相机坐标系下的坐标。之后就可以转化成了3D(相机系)-3D(世界系) 的点对问题，来计算出相机在世界坐标系下的位姿R,t。步骤如下：

由上面的已知条件（绿色），我们要计算的就是图中的s1,s2,s3。
从第一个式子开始，定义$u=\frac{s_2}{s_1},v=\frac{s_3}{s_1}$并代入：
$a^2=s_1^2(u^2+v^2-2uv\cos \alpha )$, 这样可以得到$s_1^2=\frac{a^2}{u^2+v^2-2uv\cos \alpha }$.
同理，把u和v代入第二和第三个式子可以得到$s_1^2$的另外两种表示，代入后就得到了一个四次多项式：

求出v就可以得到$s_1,s_2,s_3$，这里存在一个问题，就是会有4组解，所以需要另外的措施消除这种模糊性：

• 使用已知的从GPS得到的近似解
• 使用第四个点对进行验证，得到唯一的解

举一个四组解的例子：三个夹角都相等，3D点互相的距离也都相等

此时，ABC三点在相机和世界坐标系下的坐标都已知，可以根据此求解3D-3D的ICP问题，从而求解出相机的位姿。

四. EPnP

对应论文《EPnP: Accurate Non-Iterative O(n) Solution to the PnP Problem》

把2D图像点通过内参转换到相机坐标系的3D点，然后用ICP来解3D-3D的变换就可以得到位姿。核心是：求解3D参考点在相机坐标系下的坐标

解决方法是：使用4个控制点作为参考基准，让所有世界坐标系下的3D点都可以表示为这4个参考点的线性组合，权重值和为1。这样，就可以用原来世界坐标系（或相机坐标系）下的4个控制点来表示所有的世界坐标系（或相机坐标系）下的3D点。核心就变成了：求解4个控制点在相机坐标系下的坐标。

• 控制点的选取
  原本控制点可以随机选取，为了有一个稳定结果，论文推荐了下面这个固定的选法：其中一个控制点为世界坐标系中所有点的质心，再用所有3D点坐标减去重心坐标，做成$A^{T}A$的形式，再用SVD求出三个主方向，并加在重心坐标上得到其他三个控制点。
  4个控制点在世界坐标系下的坐标为$c_j^w,j=1,...,4$，这四个点是可以通过3D点直接求出来的。令这4个控制点在相机坐标系下的坐标为$c_j^c,j=1,...,4$，注意对于不同的3D点，每一个点所对应的坐标权重值是不一样的。

• 同一个3D点对应的相机坐标系和世界坐标系下的4个控制点的权重系数关系
  权重${\alpha }_{ij}$是相同的，证明过程如下：
  $${\mathbf{p}}_i^c=R_{cw}{P}_i^w+t=R_{cw}\sum _{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^w+\sum _{j=1}^4{\alpha }_{ij}t=\sum _{j=1}^4{\alpha }_{ij}(R_{cw}{c}_i^w+t)=\sum _{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^c$$

为了求相机坐标系下控制点的坐标，可以根据相机模型写出下面的方程：
$$\forall i,w_i\left(\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right)=K{\mathbf{p}}_i^c=\left(\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right)\sum _{j=1}^4{\alpha }_{ij}\left(\begin{array}{c}{x}_j^c\\ y_j^c\\ z_j^c\end{array}\right)$$

这里的$K$是内参矩阵，每一个控制点的相机系坐标为$c_j^c=[x_j^c,y_j^c,z_j^c{]}^T,j=1,...4$。这里的$w_i$就是深度，而且有$w_i={\sum }_{j=1}^4{\alpha }_{ij}{z}_j^c$. 把前两行减去最后一行，可以得到：
$$\begin{array}{l}{\sum }_{j=1}^4{\alpha }_{ij}{f}_x{x}_j^c+{\alpha }_{ij}(c_x-u_i)z_j^c=0\\ {\sum }_{j=1}^4{\alpha }_{ij}{f}_y{y}_j^c+{\alpha }_{ij}(c_y-v_i)z_j^c=0\end{array}$$

这个方程组中，有四个控制点的相机坐标，也就是有12个未知数，可以提取出未知数$\mathbf{x}$变为列向量，写成如下矩阵形式。$M$的维度是$2n\ast 12$，$n$是点对个数。
$$M\mathbf{x}=0$$
具体的求解可以看下面的epnp的第二个参考链接。

此时权重系数已知，控制点的相机系坐标也已知，就可以恢复出3D点在相机坐标系下的位置了，3D点的世界坐标一开始就知道，所以就可以利用ICP求解位姿了。

为什么算法的复杂度是$O(n)$?
体现在求解$M\mathbf{x}=0$时，求$\mathbf{x}$时要先求维度为$12\ast 12$的$M^{T}M$，这里的复杂度就是$O(n)$。

---

参考：
DLT： https://zhuanlan.zhihu.com/p/58648937
P3P：https://zhuanlan.zhihu.com/p/140077137
Bonn大学P3P课件：https://www.ipb.uni-bonn.de/html/teaching/msr2-2020/sse2-15-p3p.pdf
EpnP：https://blog.csdn.net/zkk9527/article/details/107939991
https://zhuanlan.zhihu.com/p/59070440