PnP问题应用与一下场景：
已知三维点和对应二维点以及相机相机内参数，可以获取相机外参。
我们介绍其中的一种算法：ePnP
算法流程
1、ePnP算法首先在世界坐标系内寻找4个控制点，记作
C
1
w
,
C
2
w
,
C
3
w
,
C
4
w
C_1^w,C_2^w,C_3^w,C_4^w
C1w,C2w,C3w,C4w，使得：
对于世界坐标系内任意一点
P
1
w
,
P_1^w,
P1w,存在对应的
α
i
=
[
α
i
1
,
α
i
2
,
α
i
3
,
α
i
4
]
T
\alpha_i=[\alpha_{i1},\alpha_{i2},\alpha_{i3},\alpha_{i4}]^T
αi=[αi1,αi2,αi3,αi4]T，满足：
P
i
w
=
∑
j
=
1
4
α
i
j
C
j
w
,
w
i
t
h
∑
j
=
1
4
α
i
j
=
1
P_i^w = \sum_{j=1}^4\alpha_{ij}C_j^w, {\kern 20pt} with\sum_{j=1}^4\alpha_{ij}=1
Piw=j=1∑4αijCjw,withj=1∑4αij=1
世界坐标系上的
P
w
P^w
Pw 经过
R
,
t
R,t
R,t 变换可以得到相机坐标系下的点
P
c
P^c
Pc :
P
i
c
=
R
P
i
w
+
t
=
R
(
∑
j
=
1
4
α
i
j
C
j
w
)
+
t
P_i^c = RP_i^w+t=R\left(\sum_{j=1}^4\alpha_{ij}C_j^w\right) + t
Pic=RPiw+t=R(j=1∑4αijCjw)+t
由于
∑
j
=
1
4
α
i
j
=
1
\sum_{j=1}^4\alpha_{ij}=1
∑j=14αij=1,因此
t
=
∑
j
=
1
4
α
i
j
t
t=\sum_{j=1}^4\alpha_{ij}t
t=∑j=14αijt 带入上式得
(
C
j
w
是世界坐标系下的控制点，
C
j
c
是世界坐标系下的控制点
)
(C_j^w是世界坐标系下的控制点，C_j^c是世界坐标系下的控制点)
(Cjw是世界坐标系下的控制点，Cjc是世界坐标系下的控制点):
P
i
c
=
∑
j
−
1
4
α
i
j
(
R
C
j
w
+
t
)
=
∑
j
−
1
4
α
i
j
C
j
c
,
(
C
j
c
=
R
C
j
w
+
t
)
P_i^c = \sum_{j-1}^4\alpha_{ij}(RC_j^w + t)= \sum_{j-1}^4\alpha_{ij}C_j^c,{\color{red} \left(C_j^c = RC_j^w+t \right)}
Pic=j−1∑4αij(RCjw+t)=j−1∑4αijCjc,(Cjc=RCjw+t)
通过内参矩阵，建立相机坐标系下的点
P
c
P^c
Pc 到像素坐标系下的点
p
p
p 的映射：
p
i
=
s
i
[
u
i
v
i
1
]
=
K
P
i
c
=
[
f
u
0
u
c
0
f
v
v
c
0
0
1
]
∑
j
=
1
4
α
i
j
[
C
x
j
c
C
y
j
c
C
z
j
c
]
p_i = s_i\begin{bmatrix}u_i \\ v_i \\ 1 \end{bmatrix} = KP^c_i=\begin{bmatrix}f_u &0 & u_c \\0& f_v & v_c \\ 0&0&1 \end{bmatrix}\sum_{j=1}^4\alpha_{ij}\begin{bmatrix}C_{xj}^c \\C_{yj}^c \\ C_{zj}^c \end{bmatrix}
pi=si
uivi1
=KPic=
fu000fv0ucvc1
j=1∑4αij
CxjcCyjcCzjc
将公式展开，可得：
∑
j
=
1
4
α
i
j
f
u
C
x
j
c
+
α
i
j
(
u
c
−
u
i
)
C
z
j
c
=
0
∑
j
=
1
4
α
i
j
f
v
C
y
j
c
+
α
i
j
(
v
c
−
v
i
)
C
z
j
c
=
0
\sum_{j=1}^4\alpha_{ij}f_uC^c_{xj} + \alpha_{ij}(u_c-u_i)C_{zj}^c = 0 \\ \sum_{j=1}^4\alpha_{ij}f_vC^c_{yj} + \alpha_{ij}(v_c-v_i)C_{zj}^c = 0
j=1∑4αijfuCxjc+αij(uc−ui)Czjc=0j=1∑4αijfvCyjc+αij(vc−vi)Czjc=0
对于上述公式 只有相机坐标系下的四个控制点
C
j
c
C_j^c
Cjc 未知，每个控制点有三个参数，因此一共有12个未知数。每对点能建立两个方程组，所以，至少要6对点才能进行求解。
f
u
[
α
i
1
α
i
2
α
i
3
α
i
4
]
[
C
x
1
c
C
x
2
c
C
x
3
c
C
x
4
c
]
+
(
u
c
−
u
i
)
[
α
i
1
α
i
2
α
i
3
α
i
4
]
[
C
z
1
c
C
z
2
c
C
z
3
c
C
z
4
c
]
=
0
f
v
[
α
i
1
α
i
2
α
i
3
α
i
4
]
[
C
y
1
c
C
y
2
c
C
y
3
c
C
y
4
c
]
+
(
v
c
−
v
i
)
[
α
i
1
α
i
2
α
i
3
α
i
4
]
[
C
z
1
c
C
z
2
c
C
z
3
c
C
z
4
c
]
=
0
[
f
u
α
i
1
f
u
α
i
2
f
u
α
i
3
f
u
α
i
4
0
0
0
0
(
u
c
−
u
i
)
α
i
1
(
u
c
−
u
i
)
α
i
2
(
u
c
−
u
i
)
α
i
3
(
u
c
−
u
i
)
α
i
4
0
0
0
0
f
v
α
i
1
f
v
α
i
2
f
v
α
i
3
f
v
α
i
4
(
v
c
−
v
i
)
α
i
1
(
v
c
−
v
i
)
α
i
2
(
v
c
−
v
i
)
α
i
3
(
v
c
−
v
i
)
α
i
4
]
[
C
x
1
c
C
x
2
c
C
x
3
c
C
x
4
c
C
y
1
c
C
y
2
c
C
y
3
c
C
y
4
c
C
z
1
c
C
z
2
c
C
z
3
c
C
z
4
c
]
=
0
f_u \begin{bmatrix} \alpha_{i1} & \alpha_{i2} & \alpha_{i3} & \alpha_{i4} \end{bmatrix}\begin{bmatrix} C_{x1}^c \\ C_{x2}^c \\ C_{x3}^c \\ C_{x4}^c\end{bmatrix} + (u_c-u_i) \begin{bmatrix} \alpha_{i1} & \alpha_{i2} & \alpha_{i3} & \alpha_{i4} \end{bmatrix} \begin{bmatrix} C_{z1}^c \\ C_{z2}^c \\ C_{z3}^c \\ C_{z4}^c\end{bmatrix} =0 \\ f_v \begin{bmatrix} \alpha_{i1} & \alpha_{i2} & \alpha_{i3} & \alpha_{i4} \end{bmatrix}\begin{bmatrix} C_{y1}^c \\ C_{y2}^c \\ C_{y3}^c \\ C_{y4}^c\end{bmatrix} + (v_c-v_i) \begin{bmatrix} \alpha_{i1} & \alpha_{i2} & \alpha_{i3} & \alpha_{i4} \end{bmatrix} \begin{bmatrix} C_{z1}^c \\ C_{z2}^c \\ C_{z3}^c \\ C_{z4}^c\end{bmatrix} =0 \\\\\\ \begin{bmatrix} f_u\alpha_{i1} & f_u\alpha_{i2} & f_u\alpha_{i3} & f_u\alpha_{i4} &0&0&0&0&(u_c-u_i) \alpha_{i1} & (u_c-u_i) \alpha_{i2} & (u_c-u_i) \alpha_{i3} & (u_c-u_i) \alpha_{i4} \\0&0&0&0&f_v\alpha_{i1} & f_v\alpha_{i2} & f_v\alpha_{i3} & f_v\alpha_{i4}& (v_c-v_i) \alpha_{i1} & (v_c-v_i) \alpha_{i2} & (v_c-v_i) \alpha_{i3} & (v_c-v_i) \alpha_{i4} \end{bmatrix}\begin{bmatrix}C_{x1}^c \\ C_{x2}^c \\ C_{x3}^c \\ C_{x4}^c\\C_{y1}^c \\ C_{y2}^c \\ C_{y3}^c \\ C_{y4}^c \\ C_{z1}^c \\ C_{z2}^c \\ C_{z3}^c \\ C_{z4}^c\end{bmatrix} =0
fu[αi1αi2αi3αi4]
Cx1cCx2cCx3cCx4c
+(uc−ui)[αi1αi2αi3αi4]
Cz1cCz2cCz3cCz4c
=0fv[αi1αi2αi3αi4]
Cy1cCy2cCy3cCy4c
+(vc−vi)[αi1αi2αi3αi4]
Cz1cCz2cCz3cCz4c
=0[fuαi10fuαi20fuαi30fuαi400fvαi10fvαi20fvαi30fvαi4(uc−ui)αi1(vc−vi)αi1(uc−ui)αi2(vc−vi)αi2(uc−ui)αi3(vc−vi)αi3(uc−ui)αi4(vc−vi)αi4]
Cx1cCx2cCx3cCx4cCy1cCy2cCy3cCy4cCz1cCz2cCz3cCz4c
=0
找到相机坐标系下的控制点后，就能够求得相机外参:
C
j
c
=
R
C
j
w
+
t
[
C
x
j
c
C
y
j
c
C
z
j
c
]
=
[
r
1
r
2
r
3
r
4
r
5
r
6
r
7
r
8
r
9
]
[
C
x
j
w
C
y
j
w
C
z
j
w
]
+
[
t
1
t
2
t
3
]
C
x
j
c
=
r
1
C
x
j
w
+
r
2
C
y
j
w
+
r
3
C
z
j
w
+
t
1
C
y
j
c
=
r
4
C
x
j
w
+
r
5
C
y
j
w
+
r
6
C
z
j
w
+
t
2
C
z
j
c
=
r
7
C
x
j
w
+
r
8
C
y
j
w
+
r
9
C
z
j
w
+
t
3
[
C
x
1
c
C
y
1
c
C
z
1
c
C
x
2
c
C
y
2
c
C
z
2
c
C
x
3
c
C
y
3
c
C
z
3
c
C
x
4
c
C
y
4
c
C
z
4
c
]
=
[
C
x
1
w
C
y
1
w
C
z
1
w
0
0
0
0
0
0
1
0
0
0
0
0
C
x
1
w
C
y
1
w
C
z
1
w
0
0
0
0
1
0
0
0
0
0
0
0
C
x
1
w
C
y
1
w
C
z
1
w
0
0
1
C
x
2
w
C
y
2
w
C
z
2
w
0
0
0
0
0
0
1
0
0
0
0
0
C
x
2
w
C
y
2
w
C
z
2
w
0
0
0
0
1
0
0
0
0
0
0
0
C
x
2
w
C
y
2
w
C
z
2
w
0
0
1
C
x
3
w
C
y
3
w
C
z
3
w
0
0
0
0
0
0
1
0
0
0
0
0
C
x
3
w
C
y
3
w
C
z
3
w
0
0
0
0
1
0
0
0
0
0
0
0
C
x
3
w
C
y
3
w
C
z
3
w
0
0
1
C
x
4
w
C
y
4
w
C
z
4
w
0
0
0
0
0
0
1
0
0
0
0
0
C
x
4
w
C
y
4
w
C
z
4
w
0
0
0
0
1
0
0
0
0
0
0
0
C
x
4
w
C
y
4
w
C
z
4
w
0
0
1
]
[
r
1
r
2
r
3
r
4
r
5
r
6
r
7
r
8
r
9
t
1
t
2
t
3
]
\begin{aligned} &C_j^c = RC_j^w+t\\\\ &\begin{bmatrix}C_{xj}^c \\ C_{yj}^c \\ C_{zj}^c \end{bmatrix} = \begin{bmatrix} r_1 & r_2 & r_3 \\ r_4 & r_5 & r_6 \\r_7 & r_8 & r_9 \end{bmatrix}\begin{bmatrix}C_{xj}^w \\ C_{yj}^w \\ C_{zj}^w \end{bmatrix} + \begin{bmatrix} t_1 \\ t_2 \\ t_3\end{bmatrix} \\\\ &C_{xj}^c = r_1C_{xj}^w + r_2C_{yj}^w + r_3C_{zj}^w + t_1\\ &C_{yj}^c = r_4C_{xj}^w + r_5C_{yj}^w + r_6C_{zj}^w + t_2\\ &C_{zj}^c = r_7C_{xj}^w + r_8C_{yj}^w + r_9C_{zj}^w + t_3\\ \\\\ &\begin{bmatrix}C_{x1}^c \\ C_{y1}^c \\C_{z1}^c \\C_{x2}^c \\ C_{y2}^c \\C_{z2}^c \\C_{x3}^c \\ C_{y3}^c \\C_{z3}^c \\C_{x4}^c \\ C_{y4}^c \\C_{z4}^c\end{bmatrix} = \begin{bmatrix} C_{x1}^w & C_{y1}^w & C_{z1}^w&0&0&0&0&0&0&1&0&0 \\ 0&0&0&C_{x1}^w & C_{y1}^w & C_{z1}^w &0&0&0&0&1&0\\ 0&0&0&0&0&0& C_{x1}^w & C_{y1}^w & C_{z1}^w &0&0&1\\ C_{x2}^w & C_{y2}^w & C_{z2}^w&0&0&0&0&0&0&1&0&0 \\ 0&0&0&C_{x2}^w & C_{y2}^w & C_{z2}^w &0&0&0&0&1&0\\ 0&0&0&0&0&0& C_{x2}^w & C_{y2}^w & C_{z2}^w &0&0&1 \\ C_{x3}^w & C_{y3}^w & C_{z3}^w&0&0&0&0&0&0&1&0&0 \\ 0&0&0&C_{x3}^w & C_{y3}^w & C_{z3}^w &0&0&0&0&1&0\\ 0&0&0&0&0&0& C_{x3}^w & C_{y3}^w & C_{z3}^w &0&0&1\\ C_{x4}^w & C_{y4}^w & C_{z4}^w&0&0&0&0&0&0&1&0&0 \\ 0&0&0&C_{x4}^w & C_{y4}^w & C_{z4}^w &0&0&0&0&1&0\\ 0&0&0&0&0&0& C_{x4}^w & C_{y4}^w & C_{z4}^w &0&0&1 \end{bmatrix} \begin{bmatrix}r_1 \\ r_2 \\ r_3 \\ r_4 \\ r_5 \\ r_6 \\ r_7 \\ r_8 \\ r_9 \\t_1\\t_2\\t_3 \end{bmatrix} \end{aligned}
Cjc=RCjw+t
CxjcCyjcCzjc
=
r1r4r7r2r5r8r3r6r9
CxjwCyjwCzjw
+
t1t2t3
Cxjc=r1Cxjw+r2Cyjw+r3Czjw+t1Cyjc=r4Cxjw+r5Cyjw+r6Czjw+t2Czjc=r7Cxjw+r8Cyjw+r9Czjw+t3
Cx1cCy1cCz1cCx2cCy2cCz2cCx3cCy3cCz3cCx4cCy4cCz4c
=
Cx1w00Cx2w00Cx3w00Cx4w00Cy1w00Cy2w00Cy3w00Cy4w00Cz1w00Cz2w00Cz3w00Cz4w000Cx1w00Cx2w00Cx3w00Cx4w00Cy1w00Cy2w00Cy3w00Cy4w00Cz1w00Cz2w00Cz3w00Cz4w000Cx1w00Cx2w00Cx3w00Cx4w00Cy1w00Cy2w00Cy3w00Cy4w00Cz1w00Cz2w00Cz3w00Cz4w100100100100010010010010001001001001
r1r2r3r4r5r6r7r8r9t1t2t3
控制点选取
原则上，就是只要选择3个线性无关的点，就可以表示任意一个三维点，但由于方程组是4，如何是3个控制点只能求得最小二乘解。论文中给出了具体的选择方法。 3D参考点集为
{
P
i
w
,
i
=
1
,
⋯
,
n
}
\left\{P^w_i,i=1,\cdots,n \right \}
{Piw,i=1,⋯,n}, 选择3D点的中心为第一个控制点:
C
1
w
=
1
n
∑
i
=
1
n
P
i
w
C^w_1=\frac{1}{n}\sum_{i=1}^nP^w_i
C1w=n1i=1∑nPiw
进而得到矩阵:
A
=
[
(
P
1
w
)
T
−
(
C
1
w
)
T
⋯
(
P
n
w
)
T
−
(
C
n
w
)
T
]
A=\begin{bmatrix}{(P_{1}^{w})}^{T} - {(C_{1}^{w})}^{T} \\ \cdots \\ {(P_{n}^{w})}^{T} - {(C_{n}^{w})}^{T} \end{bmatrix}
A=
(P1w)T−(C1w)T⋯(Pnw)T−(Cnw)T
记
A
T
A
A^TA
ATA的特征值为
λ
i
\lambda_i
λi, 特征向量为
V
i
V_i
Vi ,那么剩下的三个点为：
C
w
=
C
1
w
+
λ
i
1
2
V
i
,
i
=
1
,
2
,
3
C^w = C_1^w +\lambda^{\frac{1}{2}}_iV_i,\ i = 1,2,3
Cw=C1w+λi21Vi, i=1,2,3
求解 α \alpha α
联立方程组:
P
i
w
=
α
i
1
C
1
w
+
α
i
2
C
2
w
+
α
i
3
C
3
w
+
α
i
4
C
4
w
[
x
i
y
i
z
i
]
=
α
i
1
[
C
x
1
w
C
y
1
w
C
z
1
w
]
+
α
i
2
[
C
x
2
w
C
y
2
w
C
z
2
w
]
+
α
i
3
[
C
x
3
w
C
y
3
w
C
z
3
w
]
+
α
i
4
[
C
x
4
w
C
y
4
w
C
z
4
w
]
\begin{aligned} &P_i^w = \alpha_{i1}C_1^w+\alpha_{i2}C_2^w+\alpha_{i3}C_3^w+\alpha_{i4}C_4^w \\\\ &\begin{bmatrix} x_i \\y_i\\z_i\end{bmatrix} = \alpha_{i1}\begin{bmatrix} C_{x1}^w \\C_{y1}^w\\C_{z1}^w\end{bmatrix}+\alpha_{i2}\begin{bmatrix} C_{x2}^w \\C_{y2}^w\\C_{z2}^w\end{bmatrix}+\alpha_{i3}\begin{bmatrix} C_{x3}^w \\C_{y3}^w\\C_{z3}^w\end{bmatrix}+\alpha_{i4}\begin{bmatrix} C_{x4}^w \\C_{y4}^w\\C_{z4}^w\end{bmatrix} \end{aligned}
Piw=αi1C1w+αi2C2w+αi3C3w+αi4C4w
xiyizi
=αi1
Cx1wCy1wCz1w
+αi2
Cx2wCy2wCz2w
+αi3
Cx3wCy3wCz3w
+αi4
Cx4wCy4wCz4w
每个点可以得到4个方程组:
x
i
=
α
i
1
C
x
1
w
+
α
i
2
C
x
2
w
+
α
i
3
C
x
3
w
+
α
i
4
C
x
4
w
y
i
=
α
i
1
C
y
1
w
+
α
i
2
C
y
2
w
+
α
i
3
C
y
3
w
+
α
i
4
C
y
4
w
z
i
=
α
i
1
C
z
1
w
+
α
i
2
C
z
2
w
+
α
i
3
C
z
3
w
+
α
i
4
C
z
4
w
1
=
α
i
1
+
α
i
2
+
α
i
3
+
α
i
4
x_i=\alpha_{i1}C_{x1}^w+\alpha_{i2}C_{x2}^w+\alpha_{i3}C_{x3}^w+\alpha_{i4}C_{x4}^w\\ y_i=\alpha_{i1}C_{y1}^w+\alpha_{i2}C_{y2}^w+\alpha_{i3}C_{y3}^w+\alpha_{i4}C_{y4}^w\\ z_i=\alpha_{i1}C_{z1}^w+\alpha_{i2}C_{z2}^w+\alpha_{i3}C_{z3}^w+\alpha_{i4}C_{z4}^w\\ 1= \alpha_{i1}+\alpha_{i2}+\alpha_{i3}+\alpha_{i4}
xi=αi1Cx1w+αi2Cx2w+αi3Cx3w+αi4Cx4wyi=αi1Cy1w+αi2Cy2w+αi3Cy3w+αi4Cy4wzi=αi1Cz1w+αi2Cz2w+αi3Cz3w+αi4Cz4w1=αi1+αi2+αi3+αi4
用矩阵的方式可表示为：
[
x
i
y
i
z
i
1
]
=
[
C
x
1
w
C
x
2
w
C
x
3
w
C
x
4
w
C
y
1
w
C
y
2
w
C
y
3
w
C
y
4
w
C
z
1
w
C
z
2
w
C
z
3
w
C
z
4
w
1
1
1
1
]
[
α
i
1
α
i
2
α
i
3
α
i
4
]
⟶
[
C
x
1
w
C
x
2
w
C
x
3
w
C
x
4
w
C
y
1
w
C
y
2
w
C
y
3
w
C
y
4
w
C
z
1
w
C
z
2
w
C
z
3
w
C
z
4
w
1
1
1
1
]
−
1
[
x
i
y
i
z
i
1
]
=
[
α
i
1
α
i
2
α
i
3
α
i
4
]
\begin{bmatrix} x_i \\ y_i \\ z_i \\ 1 \end{bmatrix} = \begin{bmatrix} C_{x1}^w &C_{x2}^w &C_{x3}^w &C_{x4}^w \\ C_{y1}^w &C_{y2}^w &C_{y3}^w &C_{y4}^w \\ C_{z1}^w &C_{z2}^w &C_{z3}^w &C_{z4}^w \\ 1&1&1&1 \end{bmatrix} \begin{bmatrix} \alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4}\end{bmatrix} \longrightarrow \begin{bmatrix} C_{x1}^w &C_{x2}^w &C_{x3}^w &C_{x4}^w \\ C_{y1}^w &C_{y2}^w &C_{y3}^w &C_{y4}^w \\ C_{z1}^w &C_{z2}^w &C_{z3}^w &C_{z4}^w \\ 1&1&1&1 \end{bmatrix} ^{-1}\begin{bmatrix} x_i \\ y_i \\ z_i \\ 1 \end{bmatrix} = \begin{bmatrix} \alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4}\end{bmatrix}
xiyizi1
=
Cx1wCy1wCz1w1Cx2wCy2wCz2w1Cx3wCy3wCz3w1Cx4wCy4wCz4w1
αi1αi2αi3αi4
⟶
Cx1wCy1wCz1w1Cx2wCy2wCz2w1Cx3wCy3wCz3w1Cx4wCy4wCz4w1
−1
xiyizi1
=
αi1αi2αi3αi4
C++代码实现：https://github.com/ldx-star/ePnP
819
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
