PnP问题求解

1.直接线性变换（DLT）
对某空间点P = (X, Y, Z, 1)T，在某图像中对应点为 x1 = (u1, v1, 1)T，则有
$$s\left(\begin{array}{l}{u}_1\\ v_1\\ 1\end{array}\right)=\left(\begin{array}{llll}{t}_1& t_2& t_3& t_4\\ t_5& t_6& t_7& t_8\\ t_9& t_{10}& t_{11}& t_{12}\end{array}\right)\left(\begin{array}{l}X\\ Y\\ Z\\ 1\end{array}\right)$$
用最后一行消去s，得
$$u_1=\frac{t_{1}X+t_{2}Y+t_{3}Z+t_4}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}}{v}_1=\frac{t_{5}X+t_{6}Y+t_{7}Z+t_8}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}}$$
定义$${\mathbf{t}}_1={\left(t_1,t_2,t_3,t_4\right)}^T,{\mathbf{t}}_2={\left(t_5,t_6,t_7,t_8\right)}^T,{\mathbf{t}}_3={\left(t_9,t_{10},t_{11},t_{12}\right)}^T$$
则有
$$\begin{array}{l}{\mathbf{t}}_1^T\mathbf{P}-{\mathbf{t}}_3^T\mathbf{P}{u}_1=0\\ {\mathbf{t}}_2^T\mathbf{P}-{\mathbf{t}}_3^T\mathbf{P}{v}_1=0\end{array}$$
因此一对特征点提供了两个方程， 也就是两个约束，由于t共12个自由度，因此最少需要6对特征点，便可解出t。
当匹配点数大于六对时，便可以用奇异值分解求其最小二乘解。
2.P3P
仅需要三对匹配点，其输入数据为三对3D-2D匹配点，对于3D点A，B，C，其对应2D匹配点为a,b,c。

这里之前看的一些博客可能写成了▲Oab相似于▲OAB，看了好久才明白，AB的深度是未知的，如果默认平行的话相当于世界点全部映在了成像平面的平行平面上，后续余弦定理求解并不依赖于相似，只需要∠Oab＝∠OAB即可，即

记x=OA/OC,y=OB/OC，得

记v = AB²/OC²,uv = BC²/OC²,wv = AC²/OC²，则有

以v化简得

其中，A,B,C,a,b,c的坐标是已知的，因此cos⟨a, b⟩, cos⟨b, c⟩, cos⟨a, c⟩，v = AB²/OC²,uv = BC²/OC²,wv = AC²/OC²是可以求出的，于是便得到关于x和y的二元二次方程，通过吴消元法解出。类似于通过本质矩阵E求解，P3P方法最多会得到四个解，可以通过验证点来判断。但P3P只能利用三个点的信息，多出来的匹配点的信息无法利用，并且如果3D或2D点受噪声影响，或存在错误匹配，则算法失效。