PnP
算法是指已知3D点及其在相机上的投影，求相机姿态的问题。
投影方程可以表示为：
λ
[
u
v
1
]
=
K
[
R
t
]
[
x
y
z
1
]
\lambda \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = K \begin{bmatrix} R & t \end{bmatrix}\begin{bmatrix} x \\ y \\ z\\1 \end{bmatrix}
λ
⎣
⎡
u
v
1
⎦
⎤
=
K
[
R
t
]
⎣
⎢
⎢
⎡
x
y
z
1
⎦
⎥
⎥
⎤
这里的
K
为相机内参矩阵，是已知的。我们要做的就是从
n
对这样的对应关系中恢复出相机姿态。在SLAM应用中，系统初始化后，后面的跟踪就是基于求解
PnP
问题。
1、求解
PnP
问题的
DLT
解法
首先，将
[
R
t
]
\begin{bmatrix} R & t \end{bmatrix}
[
R
t
]
看作是一个包含12个未知量的
3x4
矩阵，而将投影方程消去尺度因子
λ
\lambda
λ
后，可以获得两个方程，在联立不少于6个的投影方程后，即可求出12个未知量的最小二乘解，最后恢复出
[
R
t
]
\begin{bmatrix} R & t \end{bmatrix}
[
R
t
]
。
2、求解
PnP
问题的
EPnP
解法
它的思路是先将世界坐标系下的三维点转换到相机坐标系下，再用
icp
的方式得到相机位姿。但是，如何在位姿未知的情况下转换？这里引入了控制点的概念，所有的三维点都可以由四个控制点表示，在世界坐标系和相机坐标系下只有基底的变化，系数是相同的。而这就要求获得控制点在相机坐标系下的坐标，这里同样利用的是投影方程联立求解。
以上具体推到过程参考：
https://zhuanlan.zhihu.com/p/58648937
https://zhuanlan.zhihu.com/p/59070440