文章目录
- 1 问题引入
- 2 求解P3P
1 问题引入
透视n点（Perspective-n-Point，PnP）问题是计算机视觉领域的经典问题，用于求解3D-2D的点运动。换句话说，当知道n个3D空间点坐标以及它们在图像上的投影点坐标时，可以使用PnP算法来估计相机的姿态（或者说3D点在相机坐标系下的姿态）。P3P是最简化的PnP形式，即最少只需3个点即可估计当前的相机姿态（解不唯一）。
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