P3P 是另一种解 PnP 的方法。它仅使用三对匹配点，对数据要求较少。
P3P 需要利用给定的三个点的几何关系。它的输入数据为三对 3D-2D 匹配点。记 3D点为 A , B , C A, B, C A,B,C， 2D 点为 a , b , c a, b, c a,b,c，其中小写字母代表的点为大写字母在相机成像平面上的投影。此外， P3P 还需要使用一对验证点，以从可能的解出选出正确的那一个（类似于对极几何情形）。记验证点对为 D − d D−d D−d，相机光心为 O O O。请注意，我们知道的是 A , B , C A, B, C A,B,C 在世界坐标系中的坐标，而不是在相机坐标系中的坐标。一旦 3D 点在相机坐标系下的坐标能够算出，我们就得到了 3D-3D 的对应点，把 PnP 问题转换为了 ICP 问题。
显然，三角形之间存在对应关系：
Δ O a b − Δ O A B , Δ O b c − Δ O B C , Δ O a c − Δ O A C \Delta O a b-\Delta O A B, \quad \Delta O b c-\Delta O B C, \quad \Delta O a c-\Delta O A C ΔOab−ΔOAB,ΔObc−ΔOBC,ΔOac−ΔOAC
来考虑 O a b Oab Oab 和 O A B OAB OAB 的关系，利用余弦定理，有：
O A 2 + O B 2 − 2 O A ⋅ O B ⋅ cos ⟨ a , b ⟩ = A B 2 O A^{2}+O B^{2}-2 O A \cdot O B \cdot \cos \langle a, b\rangle=A B^{2} OA2+OB2−2OA⋅OB⋅cos⟨a,b⟩=AB2
对于其他两个三角形亦有类似性质，于是有：
O
A
2
+
O
B
2
−
2
O
A
⋅
O
B
⋅
cos
⟨
a
,
b
⟩
=
A
B
2
O A^{2}+O B^{2}-2 O A \cdot O B \cdot \cos \langle a, b\rangle=A B^{2}
OA2+OB2−2OA⋅OB⋅cos⟨a,b⟩=AB2
O
B
2
+
O
C
2
−
2
O
B
⋅
O
C
⋅
cos
⟨
b
,
c
⟩
=
B
C
2
O B^{2}+O C^{2}-2 O B \cdot O C \cdot \cos \langle b, c\rangle=B C^{2}
OB2+OC2−2OB⋅OC⋅cos⟨b,c⟩=BC2
O
A
2
+
O
C
2
−
2
O
A
⋅
O
C
⋅
cos
⟨
a
,
c
⟩
=
A
C
2
O A^{2}+O C^{2}-2 O A \cdot O C \cdot \cos \langle a, c\rangle=A C^{2}
OA2+OC2−2OA⋅OC⋅cos⟨a,c⟩=AC2
对上面三式全体除以 O C 2 O C^{2} OC2，并且记 x = O A / O C , y = O B / O C x=O A / O C, y=O B / O C x=OA/OC,y=OB/OC，得：
x
2
+
y
2
−
2
x
y
cos
⟨
a
,
b
⟩
=
A
B
2
/
O
C
2
x^{2}+y^{2}-2 x y \cos \langle a, b\rangle=A B^{2} / O C^{2}
x2+y2−2xycos⟨a,b⟩=AB2/OC2
y
2
+
1
2
−
2
y
cos
⟨
b
,
c
⟩
=
B
C
2
/
O
C
2
y^{2}+1^{2}-2 y \cos \langle b, c\rangle=B C^{2} / O C^{2}
y2+12−2ycos⟨b,c⟩=BC2/OC2
x
2
+
1
2
−
2
x
cos
⟨
a
,
c
⟩
=
A
C
2
/
O
C
2
x^{2}+1^{2}-2 x \cos \langle a, c\rangle=A C^{2} / O C^{2}
x2+12−2xcos⟨a,c⟩=AC2/OC2
记 v = A B 2 / O C 2 , u v = B C 2 / O C 2 , w v = A C 2 / O C 2 v=A B^{2} / O C^{2}, u v=B C^{2} / O C^{2}, w v=A C^{2} / O C^{2} v=AB2/OC2,uv=BC2/OC2,wv=AC2/OC2，有：
x
2
+
y
2
−
2
x
y
cos
⟨
a
,
b
⟩
−
v
=
0
x^{2}+y^{2}-2 x y \cos \langle a, b\rangle-v=0
x2+y2−2xycos⟨a,b⟩−v=0
y
2
+
1
2
−
2
y
cos
⟨
b
,
c
⟩
−
u
v
=
0
y^{2}+1^{2}-2 y \cos \langle b, c\rangle-u v=0
y2+12−2ycos⟨b,c⟩−uv=0
x
2
+
1
2
−
2
x
cos
⟨
a
,
c
⟩
−
w
v
=
0
x^{2}+1^{2}-2 x \cos \langle a, c\rangle-w v=0
x2+12−2xcos⟨a,c⟩−wv=0
我们可以把第一个式子中的 v v v 放到等式一边，并代入第 2 ， 3 2， 3 2，3 两式，得：
(
1
−
u
)
y
2
−
u
x
2
−
cos
⟨
b
,
c
⟩
y
+
2
u
x
y
cos
⟨
a
,
b
⟩
+
1
=
0
(1-u) y^{2}-u x^{2}-\cos \langle b, c\rangle y+2 u x y \cos \langle a, b\rangle+1=0
(1−u)y2−ux2−cos⟨b,c⟩y+2uxycos⟨a,b⟩+1=0
(
1
−
w
)
x
2
−
w
y
2
−
cos
⟨
a
,
c
⟩
x
+
2
w
x
y
cos
⟨
a
,
b
⟩
+
1
=
0
(1-w) x^{2}-w y^{2}-\cos \langle a, c\rangle x+2 w x y \cos \langle a, b\rangle+1=0
(1−w)x2−wy2−cos⟨a,c⟩x+2wxycos⟨a,b⟩+1=0
( x , y x,y x,y是未知量， u , w , c o s ⟨ a , b ⟩ , c o s ⟨ b , c ⟩ , c o s ⟨ a , c ⟩ u,w,cos ⟨a, b⟩, cos ⟨b, c⟩ , cos ⟨a, c⟩ u,w,cos⟨a,b⟩,cos⟨b,c⟩,cos⟨a,c⟩是已知量)
注意这些方程中的已知量和未知量。由于我们知道 2 D 2D 2D 点的图像位置，三个余弦角 c o s ⟨ a , b ⟩ , c o s ⟨ b , c ⟩ , c o s ⟨ a , c ⟩ cos ⟨a, b⟩, cos ⟨b, c⟩ , cos ⟨a, c⟩ cos⟨a,b⟩,cos⟨b,c⟩,cos⟨a,c⟩ 是已知的。同时， u = B C 2 / A B 2 , w = A C 2 / A B 2 u=B C^{2} / A B^{2}, w=A C^{2} / A B^{2} u=BC2/AB2,w=AC2/AB2 可以通过 A , B , C A,B,C A,B,C 在世界坐标系下的坐标算出，变换到相机坐标系下之后，并不改变这个比值。该式中的 x , y x, y x,y 是未知的，随着相机移动会发生变化。因此，该方程组是关于 x , y x,y x,y 的一个二元二次方程（多项式方程）。解析地求解该方程组是一个复杂的过程，需要用吴消元法。
这里不展开对该方程解法的介绍，感兴趣的读者请参照。类似于分解 E E E 的情况，该方程最多可能得到四个解，但我们可以用验证点来计算最可能的解，得到 A , B , C A, B, C A,B,C 在相机坐标系下 3 D 3D 3D坐标。然后，根据 3 D − 3 D 3D-3D 3D−3D的点对，计算相机的运动 R , t R,t R,t。
从 P 3 P P3P P3P 的原理上可以看出，为了求解 P n P PnP PnP，我们利用了三角形相似性质，求解投影点 a , b , c a, b, c a,b,c 在相机坐标系下的 3D 坐标，最后把问题转换成一个 3D 到 3D 的位姿估计问题。
后文将看到，带有匹配信息的 3D-3D 位姿求解非常容易，所以这种思路是非常有效的。其他的一些方法，例如 EPnP，亦采用了这种思路。然而， P3P 也存在着一些问题：
- P3P 只利用三个点的信息。当给定的配对点多于 3 组时，难以利用更多的信息。
- 如果 3D 点或 2D 点受噪声影响，或者存在误匹配，则算法失效。
所以后续人们还提出了许多别的方法，如 EPnP、 UPnP 等。它们利用更多的信息，而且用迭代的方式对相机位姿进行优化，以尽可能地消除噪声的影响。不过，相对于 P3P 来说，原理会更加复杂一些，所以我们建议读者阅读原始的论文，或通过实践来理解 PnP 过程。
在 SLAM 当中，通常的做法是先使用 P3P/EPnP 等方法估计相机位姿，然后构建最小二乘优化问题对估计值进行调整（Bundle Adjustment）。在相机运动足够连续时，也可以假设相机不动或匀速运动，用推测值作为初始值进行优化。
视觉SLAM十四讲
3409
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
