PnP问题求解

PnP算法是指已知3D点及其在相机上的投影，求相机姿态的问题。
投影方程可以表示为：
$\lambda \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = K \begin{bmatrix} R & t \end{bmatrix}\begin{bmatrix} x \\ y \\ z\\1 \end{bmatrix}$
这里的K为相机内参矩阵，是已知的。我们要做的就是从n对这样的对应关系中恢复出相机姿态。在SLAM应用中，系统初始化后，后面的跟踪就是基于求解PnP问题。

1、求解PnP问题的DLT解法
首先，将 $\begin{bmatrix} R & t \end{bmatrix}$看作是一个包含12个未知量的3x4矩阵，而将投影方程消去尺度因子 $\lambda$后，可以获得两个方程，在联立不少于6个的投影方程后，即可求出12个未知量的最小二乘解，最后恢复出 $\begin{bmatrix} R & t \end{bmatrix}$。

2、求解PnP问题的EPnP解法
它的思路是先将世界坐标系下的三维点转换到相机坐标系下，再用icp的方式得到相机位姿。但是，如何在位姿未知的情况下转换？这里引入了控制点的概念，所有的三维点都可以由四个控制点表示，在世界坐标系和相机坐标系下只有基底的变化，系数是相同的。而这就要求获得控制点在相机坐标系下的坐标，这里同样利用的是投影方程联立求解。

总结：感觉上EPnP就是在DLT上套了一层控制点马甲。
以上具体推到过程参考：
https://zhuanlan.zhihu.com/p/58648937
https://zhuanlan.zhihu.com/p/59070440