文章目录
- [1. 前言](#1. 前言)
- [1. 直接线性变换DLT](#1. 直接线性变换DLT)
- [2 求解P3P](#2 求解P3P)
1. 前言
透视n点（Perspective-n-Point，PnP）问题是计算机视觉领域的经典问题，用于求解3D-2D的点运动。换句话说，当知道 N N N个世界坐标系中3D空间点的坐标以及它们在图像上的投影点像素坐标时，可以使用PnP算法来估计相机在世界坐标系的姿态。P3P是最简化的PnP形式，即最少只需3个点即可估计当前的相机姿态（解不唯一）。
总体来说，PnP的求解方法有P3P、直接线性变换（Direct Linear Transformation，DLT）、EPnP（Efficient PnP）和UPnP等。此外，还有非线性优化 解法，通过构建最小二乘问题并迭代求解，即万金油式的光束平差法（Bundle Adjustment，BA） 。
1. 直接线性变换DLT
假设有世界坐标系中的3D点 P = [ X , Y , Z , 1 ] T P=[X, Y, Z, 1]^T P=[X,Y,Z,1]T，在图像 I 1 I_1 I1中对应的投影像素点为 x 1 = [ u 1 , v 1 , 1 ] T x_1=[u_1, v_1, 1]^T x1=[u1,v1,1]T，根据相机小孔成像模型有：
s [ u 1 v 1 1 ] = [ R ∣ t ] P = [ t 1 t 2 t 3 t 4 t 5 t 6 t 7 t 8 t 9 t 10 t 11 t 12 ] [ X Y Z 1 ] s \begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix}= \begin{bmatrix} R | t \end{bmatrix} P= \begin{bmatrix} 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{bmatrix} \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix} s u1v11 =[R∣t]P= t1t5t9t2t6t10t3t7t11t4t8t12 XYZ1
其中 s = Z s=Z s=Z，利用最后一行将其消去有：
{ s u 1 = t 1 X + t 2 Y + t 3 Z + t 4 s v 1 = t 5 X + t 6 Y + t 7 Z + t 8 s = t 9 X + t 10 Y + t 11 Z + t 12 ⇒ { u 1 = t 1 X + t 2 Y + t 3 Z + t 4 t 9 X + t 10 Y + t 11 Z + t 12 v 1 = t 5 X + t 6 Y + t 7 Z + t 8 t 9 X + t 10 Y + t 11 Z + t 12 \begin{cases} s u_1 = t_1 X + t_2 Y + t_3 Z + t_4\\ s v_1 = t_5 X + t_6 Y + t_7 Z + t_8\\ s = t_9 X + t_{10} Y + t_{11} Z + t_{12} \end{cases} \Rightarrow \begin{cases} 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}} \\ \end{cases} \\ ⎩ ⎨ ⎧su1=t1X+t2Y+t3Z+t4sv1=t5X+t6Y+t7Z+t8s=t9X+t10Y+t11Z+t12⇒{u1=t9X+t10Y+t11Z+t12t1X+t2Y+t3Z+t4v1=t9X+t10Y+t11Z+t12t5X+t6Y+t7Z+t8
为简化表示，定义 [ R ∣ t ] [R|t] [R∣t]的行向量为（用粗体符号表示）：
{ t 1 = [ t 1 , t 2 , t 3 , t 4 ] T t 2 = [ t 5 , t 6 , t 7 , t 8 ] T t 3 = [ t 9 , t 10 , t 11 , t 12 ] T \begin{cases} \bold{t_1} = [t_1, t_2, t_3, t_4]^T \\ \bold{t_2} = [t_5, t_6, t_7, t_8]^T \\ \bold{t_3} = [t_9, t_{10}, t_{11}, t_{12}]^T \end{cases} ⎩ ⎨ ⎧t1=[t1,t2,t3,t4]Tt2=[t5,t6,t7,t8]Tt3=[t9,t10,t11,t12]T
带入上面的公式有：
{ u 1 = t 1 T P t 3 T P v 1 = t 2 T P t 3 T P ⇒ { t 1 T P − t 3 T P u 1 = 0 t 2 T P − t 3 T P v 1 = 0 \begin{cases} u_1 = \frac{\bold{t_1}^T P}{\bold{t_3}^T P} \\ v_1 = \frac{\bold{t_2}^T P}{\bold{t_3}^T P} \\ \end{cases} \Rightarrow \begin{cases} \bold{t_1}^T P - \bold{t_3}^T P u_1 = 0 \\ \bold{t_2}^T P - \bold{t_3}^T P v_1 = 0 \end{cases} {u1=t3TPt1TPv1=t3TPt2TP⇒{t1TP−t3TPu1=0t2TP−t3TPv1=0
其中， t \bold{t} t是待求变量，每个特征点提供了两个关于 t \bold{t} t的线性约束。若给出 N N N个特征点，则可以列出如下线性方程组：
P 1 T 0 − u 1 P 1 T 0 P 1 T − v 1 P 1 T ⋮ ⋮ ⋮ P N T 0 − u N P N T 0 P N T − v N P N T \] \[ t 1 t 2 t 3 \] = 0 \\begin{bmatrix} P_1\^T \& 0 \& -u_1 P_1\^T \\\\ 0 \& P_1\^T \& -v_1 P_1\^T \\\\ \\hdashline \\vdots \& \\vdots \& \\vdots \\\\ \\hdashline P_N\^T \& 0 \& -u_N P_N\^T \\\\ 0 \& P_N\^T \& -v_N P_N\^T \\\\ \\end{bmatrix} \\begin{bmatrix} t_1 \\\\ t_2 \\\\ t_3 \\end{bmatrix} =0 P1T0⋮PNT00P1T⋮0PNT−u1P1T−v1P1T⋮−uNPNT−vNPNT t1t2t3 =0 ## 2 求解P3P  假设 A , B , C A,B,C A,B,C为世界坐标系中的三个点， a , b , c a,b,c a,b,c为这三个点在相机像平面的对应投影点，则我们可以得到三角形之间的对应关系： Δ O a b − Δ O A B , Δ O b c − Δ O B C , Δ O a c − Δ O A C (1) \\Delta Oab - \\Delta OAB, \\Delta Obc - \\Delta OBC, \\Delta Oac - \\Delta OAC \\tag{1} ΔOab−ΔOAB,ΔObc−ΔOBC,ΔOac−ΔOAC(1) 根据余弦定理，可以得到三个方程： { O A 2 + O B 2 − 2 O A ⋅ O B ⋅ c o s ⟨ a , b ⟩ = A B 2 O B 2 + O C 2 − 2 O B ⋅ O C ⋅ c o s ⟨ b , c ⟩ = B C 2 O A 2 + O C 2 − 2 O A ⋅ O C ⋅ c o s ⟨ a , c ⟩ = A C 2 (2) \\begin{cases} {OA}\^2 + {OB}\^2 - 2 OA \\cdot OB \\cdot cos \\langle a,b \\rangle = {AB}\^2 \\\\ {OB}\^2 + {OC}\^2 - 2 OB \\cdot OC \\cdot cos \\langle b,c \\rangle = {BC}\^2 \\\\ {OA}\^2 + {OC}\^2 - 2 OA \\cdot OC \\cdot cos \\langle a,c \\rangle = {AC}\^2 \\end{cases} \\tag{2} ⎩ ⎨ ⎧OA2+OB2−2OA⋅OB⋅cos⟨a,b⟩=AB2OB2+OC2−2OB⋅OC⋅cos⟨b,c⟩=BC2OA2+OC2−2OA⋅OC⋅cos⟨a,c⟩=AC2(2) 上述等式除以 O C 2 {OC}\^2 OC2，并记 x = O A / O C , y = O B / O C x=OA/OC, y=OB/OC x=OA/OC,y=OB/OC，有： { x 2 + y 2 − 2 x y c o s ⟨ a , b ⟩ = A B 2 / O C 2 y 2 + 1 2 − 2 y c o s ⟨ b , c ⟩ = B C 2 / O C 2 x 2 + 1 2 − 2 x c o s ⟨ a , c ⟩ = A C 2 / O C 2 (3) \\begin{cases} x\^2 + y\^2 - 2xycos \\langle a,b \\rangle = {AB}\^2 / {OC}\^2 \\\\ y\^2 + 1\^2 - 2ycos \\langle b,c \\rangle = {BC}\^2 / {OC}\^2 \\\\ x\^2 + 1\^2 - 2xcos \\langle a,c \\rangle = {AC}\^2 / {OC}\^2 \\end{cases} \\tag{3} ⎩ ⎨ ⎧x2+y2−2xycos⟨a,b⟩=AB2/OC2y2+12−2ycos⟨b,c⟩=BC2/OC2x2+12−2xcos⟨a,c⟩=AC2/OC2(3) 令 v = A B 2 / O C 2 , u = B C 2 / A B 2 , w = A C 2 / A B 2 v={AB}\^2/{OC}\^2,u={BC}\^2/{AB}\^2,w={AC}\^2/{AB}\^2 v=AB2/OC2,u=BC2/AB2,w=AC2/AB2，则上式变为： { x 2 + y 2 − 2 x y c o s ⟨ a , b ⟩ − v = 0 y 2 + 1 2 − 2 y c o s ⟨ b , c ⟩ − u v = 0 x 2 + 1 2 − 2 x c o s ⟨ a , c ⟩ − w v = 0 (4) \\begin{cases} x\^2 + y\^2 - 2xycos \\langle a,b \\rangle - v = 0 \\\\ y\^2 + 1\^2 - 2ycos \\langle b,c \\rangle - uv = 0\\\\ x\^2 + 1\^2 - 2xcos \\langle a,c \\rangle - wv = 0 \\end{cases} \\tag{4} ⎩ ⎨ ⎧x2+y2−2xycos⟨a,b⟩−v=0y2+12−2ycos⟨b,c⟩−uv=0x2+12−2xcos⟨a,c⟩−wv=0(4) 关于 a , b , c a,b,c a,b,c的三个余弦角 c o s ⟨ a , b ⟩ , c o s ⟨ b , c ⟩ , c o s ⟨ a , c ⟩ cos \\langle a,b \\rangle,cos \\langle b,c \\rangle,cos \\langle a,c \\rangle cos⟨a,b⟩,cos⟨b,c⟩,cos⟨a,c⟩，以及 u , w u,w u,w可以直接计算出。将(4)中的第一个等式分别带入第二和第三个等式，得到关于 x , y x,y x,y的一个二元二次方程（多项式方程）： ( 1 − u ) y 2 − u x 2 − c o s ⟨ b , c ⟩ y + 2 u x y c o s ⟨ a , b ⟩ + 1 = 0 ( 1 − w ) x 2 − w y 2 − c o s ⟨ a , c ⟩ x + 2 w x y c o s ⟨ a , b ⟩ + 1 = 0 (1-u)y\^2-ux\^2-cos \\langle b,c \\rangle y+2uxycos \\langle a,b \\rangle +1 = 0 \\\\ (1-w)x\^2-wy\^2-cos \\langle a,c \\rangle x+2wxycos \\langle a,b \\rangle +1 = 0 (1−u)y2−ux2−cos⟨b,c⟩y+2uxycos⟨a,b⟩+1=0(1−w)x2−wy2−cos⟨a,c⟩x+2wxycos⟨a,b⟩+1=0