文章目录
- [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