1.Overview
1.1 算法输入：
2D-3D关联点对，具体数据结构形式如下所示：
p
a
i
r
<
p
2
d
,
p
3
d
>
pair<p2d, p3d>
pair<p2d,p3d>
1.2 算法输出：
旋转矩阵R（3x3），平移向量T(3x1)，旋转矩阵R表示当前坐标系与目标坐标系之间的旋转，平移向量T表示当前坐标系与目标坐标系之间的平移。在下文中称当前坐标系为世界坐标系，称坐标系为相机坐标系。
2. 符号说明
-
n个3D点在世界坐标系中的坐标，其坐标形式为非齐次坐标
p i w = [ x i w y i w z i w ] , i = 1 , ⋯ n p^w_i = \begin{bmatrix} x_i^w \\ y_i^w\\ z_i^w \end{bmatrix}, i = 1, \cdots n piw=⎣⎡xiwyiwziw⎦⎤,i=1,⋯n -
n个3D点在相机坐标系中的坐标，其坐标形式为非齐次坐标
p i c = [ x i c y i c z i c ] , i = 1 , ⋯ n p^c_i = \begin{bmatrix} x_i^c \\ y_i^c \\ z_i^c \end{bmatrix}, i = 1, \cdots n pic=⎣⎡xicyiczic⎦⎤,i=1,⋯n -
n个2D点坐标，其坐标形式为非齐次坐标
u i = [ u i v i ] , i = 1 , ⋯ n \bf{u_i} = \begin{bmatrix} u_i \\ v_i \end{bmatrix}, i = 1, \cdots n ui=[uivi],i=1,⋯n -
4个控制点在世界坐标系中的坐标，其坐标形式为非齐次坐标
c i w = [ x i w y i w z i w ] , i = 1 , 2 , 3 , 4 c^w_i = \begin{bmatrix} x_i^w \\ y_i^w\\ z_i^w \end{bmatrix}, i = 1,2,3,4 ciw=⎣⎡xiwyiwziw⎦⎤,i=1,2,3,4 -
4个控制点在相机坐标系中的坐标，其坐标形式为非齐次坐标
c i c = [ x i c y i c z i c ] , i = 1 , 2 , 3 , 4 c^c_i = \begin{bmatrix} x_i^c \\ y_i^c \\ z_i^c \end{bmatrix}, i = 1,2,3,4 cic=⎣⎡xicyiczic⎦⎤,i=1,2,3,4
我们用上标 w w w和 c c c来表示该坐标所在坐标系为世界坐标还是相机坐标
3. 理论推导
3.1 控制点的引入
EPnP算法中引入4个控制点，使得世界坐标系每一个3D点坐标都可以被4个控制点坐标的线性组合表示
p
i
w
=
∑
j
=
1
4
a
i
j
c
i
w
(1)
p^w_i = \sum^4_{j=1} a_{ij}c^w_i \tag{1}
piw=j=1∑4aijciw(1)
将4个控制点看成一个坐标系的基，我们称这个坐标系为齐次重心坐标系 (homogeneous barycentric coordinate system)，后续我们将称之为hb坐标，至于为什么叫这个名字将在后面介绍。我们可以得到每个3D点在该坐标系下的hb坐标为
h
b
(
p
i
w
)
=
[
a
i
1
a
i
2
a
i
3
a
i
4
]
(2)
hb(p^w_i) = \begin{bmatrix} a_{i1} \\ a_{i2} \\ a_{i3}\\ a_{i4} \end{bmatrix} \tag{2}
hb(piw)=⎣⎢⎢⎡ai1ai2ai3ai4⎦⎥⎥⎤(2)
3D点相机坐标系下的坐标可由其在世界坐标系下的坐标旋转平移得到
[
p
i
c
1
]
=
[
R
T
0
1
]
[
p
i
w
1
]
(3)
\begin{bmatrix} p^c_i \\ 1 \end{bmatrix} = \begin{bmatrix} R & T \\ 0 & 1 \end{bmatrix} \begin{bmatrix} p^w_i \\ 1 \end{bmatrix} \tag{3}
[pic1]=[R0T1][piw1](3)
将(2)带入(3)
[
p
i
c
1
]
=
[
R
T
0
1
]
∑
j
=
1
4
a
i
j
[
c
i
w
1
]
=
[
R
T
0
1
]
[
∑
j
=
1
4
a
i
j
c
i
w
∑
j
=
1
4
a
i
j
]
(4)
\begin{bmatrix} p^c_i \\ 1 \end{bmatrix} = \begin{bmatrix} R & T \\ 0 & 1 \end{bmatrix} \sum^4_{j=1} a_{ij} \begin{bmatrix} c^w_i \\ 1 \end{bmatrix} = \begin{bmatrix} R & T \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \sum^4_{j=1} a_{ij}c^w_i \\ \sum^4_{j=1} a_{ij} \end{bmatrix} \tag{4}
[pic1]=[R0T1]j=1∑4aij[ciw1]=[R0T1][∑j=14aijciw∑j=14aij](4)
我们将（4）变换为如下形式
[
p
i
c
1
]
=
∑
j
=
1
4
a
i
j
[
R
T
0
1
]
[
c
i
w
1
]
=
∑
j
=
1
4
a
i
j
[
c
i
c
1
]
(5)
\begin{bmatrix} p^c_i \\ 1 \end{bmatrix} = \sum^4_{j=1} a_{ij} \begin{bmatrix} R & T \\ 0 & 1 \end{bmatrix} \begin{bmatrix} c^w_i \\ 1 \end{bmatrix} = \sum^4_{j=1} a_{ij} \begin{bmatrix} c^c_i \\ 1 \end{bmatrix} \tag{5}
[pic1]=j=1∑4aij[R0T1][ciw1]=j=1∑4aij[cic1](5)
我们比较（5）和（1）发现，3D点的坐标在世界坐标系中hb坐标和相机坐标系中的hb坐标是一致的，这一点非常重要
在上述的公式中，我们还可以从（4）中得到
∑
j
=
1
4
a
i
j
=
1
(6)
\sum^4_{j=1} a_{ij} = 1 \tag{6}
j=1∑4aij=1(6)
这是hb坐标的一个限制条件
3.2 hb坐标的计算
我们可以把（1）写成齐次坐标的形式，并用矩阵相乘的方式来表示
[
p
i
w
1
]
=
[
c
1
w
c
2
w
c
3
w
c
4
w
1
1
1
1
]
⏟
C
[
a
i
1
a
i
2
a
i
3
a
i
4
]
(7)
\begin{bmatrix} p^w_i \\ 1 \end{bmatrix} = \underbrace{ \begin{bmatrix} c^w_1 & c^w_2 & c^w_3 & c^w_4\\ 1 &1 & 1& 1 \end{bmatrix} }_{\text{C}} \begin{bmatrix} a_{i1} \\ a_{i2} \\ a_{i3}\\ a_{i4} \end{bmatrix} \tag{7}
[piw1]=C
[c1w1c2w1c3w1c4w1]⎣⎢⎢⎡ai1ai2ai3ai4⎦⎥⎥⎤(7)
我们只需要两边乘以
C
−
1
C^{-1}
C−1即可获得hb坐标
[
a
i
1
a
i
2
a
i
3
a
i
4
]
=
[
c
1
w
c
2
w
c
3
w
c
4
w
1
1
1
1
]
−
1
[
p
i
w
1
]
(8)
\begin{bmatrix} a_{i1} \\ a_{i2} \\ a_{i3}\\ a_{i4} \end{bmatrix} = \begin{bmatrix} c^w_1 & c^w_2 & c^w_3 & c^w_4\\ 1 &1 & 1& 1 \end{bmatrix}^{-1} \begin{bmatrix} p^w_i \\ 1 \end{bmatrix} \tag{8}
⎣⎢⎢⎡ai1ai2ai3ai4⎦⎥⎥⎤=[c1w1c2w1c3w1c4w1]−1[piw1](8)
3.3 控制点的构造
理论上控制点可以随便构造，只需要满足
C
C
C可逆即可，但是为了算法的稳定性，算法的原论文 [1]中提出一种构造控制点的方式，第一个控制点采用所有3D点世界坐标的质心
c
1
w
=
1
n
∑
i
=
1
n
p
i
w
(9)
c^w_1 = \frac 1 n \sum_{i=1}^np^w_i \tag{9}
c1w=n1i=1∑npiw(9)
其余的控制点，我们将选用这些3D点的三个主方向上，需要对这些数据进行主成分分析（PCA）
A
=
[
(
p
1
w
−
c
1
w
)
T
(
p
2
w
−
c
1
w
)
T
⋯
(
p
n
w
−
c
1
w
)
T
]
(10)
\bf{A}= \begin{bmatrix} (p^w_1 - c^w_1)^T \\ (p^w_2 - c^w_1)^T \\ \cdots \\ (p^w_n - c^w_1)^T \end{bmatrix} \tag{10}
A=⎣⎢⎢⎡(p1w−c1w)T(p2w−c1w)T⋯(pnw−c1w)T⎦⎥⎥⎤(10)
计算
A
T
A
\bf{A^T}\bf{A}
ATA的三个特征值
λ
1
,
λ
2
,
λ
3
\lambda_1,\lambda_2,\lambda_3
λ1,λ2,λ3，以及对应三个特征向量
v
1
,
v
2
,
v
3
\bf{v_1},\bf{v_2},\bf{v_3}
v1,v2,v3，那么剩余3个控制点可表示为
{
c
2
w
=
c
1
w
+
λ
1
n
v
1
c
3
w
=
c
1
w
+
λ
2
n
v
2
c
4
w
=
c
1
w
+
λ
1
n
v
3
(11)
\begin{cases} c^w_2 = c^w_1 + \sqrt{ \frac{\lambda_1}{n}} \bf{v_1} \\ c^w_3 = c^w_1 + \sqrt{ \frac{\lambda_2}{n}} \bf{v_2} \\ c^w_4 = c^w_1 + \sqrt{ \frac{\lambda_1}{n}} \bf{v_3} \end{cases} \tag{11}
⎩⎪⎪⎪⎨⎪⎪⎪⎧c2w=c1w+nλ1v1c3w=c1w+nλ2v2c4w=c1w+nλ1v3(11)
上述操作实际上是找到点云的重心，以及点云的三个主方向
3.4 解析求解
在相机投影模型下，有如下等式成立
s
[
u
i
1
]
=
K
p
i
c
=
K
∑
j
=
1
4
a
i
j
c
i
c
(12)
s \begin{bmatrix} \bf{u_i} \\ 1 \end{bmatrix} = K p_i^c = K \sum_{j=1}^4 a_{ij} c^c_i \tag{12}
s[ui1]=Kpic=Kj=1∑4aijcic(12)
我们将（12）展开
s
[
u
i
v
i
1
]
=
[
f
u
0
u
0
0
f
v
v
0
0
0
1
]
⏟
K
∑
j
=
1
4
a
i
j
[
x
j
c
y
j
c
z
j
c
]
(13)
s \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = \underbrace{ \begin{bmatrix} f_u & 0 & u_0 \\ 0 & f_v & v_0 \\ 0 & 0 & 1 \end{bmatrix} }_{\text{K}} \sum_{j=1}^4 a_{ij} \begin{bmatrix} x_j^c \\ y_j^c \\ z_j^c \end{bmatrix} \tag{13}
s⎣⎡uivi1⎦⎤=K
⎣⎡fu000fv0u0v01⎦⎤j=1∑4aij⎣⎡xjcyjczjc⎦⎤(13)
继续展开可以得到
[
s
u
i
s
b
i
s
]
=
[
∑
j
=
1
4
a
i
j
(
f
u
x
j
c
+
u
0
z
j
c
)
∑
j
=
1
4
a
i
j
(
f
v
y
j
c
+
u
0
z
j
c
)
∑
j
=
1
4
a
i
j
z
j
c
]
(14)
\begin{bmatrix} s u_i \\ s b_i \\ s \end{bmatrix} = \begin{bmatrix} \sum_{j=1}^4 a_{ij}(f_u x^c_j + u_0z^c_j) \\ \sum_{j=1}^4 a_{ij}(f_v y^c_j + u_0z^c_j) \\ \sum_{j=1}^4 a_{ij} z^c_j \end{bmatrix} \tag{14}
⎣⎡suisbis⎦⎤=⎣⎢⎡∑j=14aij(fuxjc+u0zjc)∑j=14aij(fvyjc+u0zjc)∑j=14aijzjc⎦⎥⎤(14)
将（14）的第3行带入上面2行可以消去s，就可以构造下面的方程
{
∑
j
=
1
4
a
i
j
[
f
u
x
j
c
+
(
u
0
−
u
i
)
z
j
c
]
=
0
∑
j
=
1
4
a
i
j
[
f
v
y
j
c
+
(
v
0
−
v
i
)
z
j
c
]
=
0
(15)
\begin{cases} \sum_{j=1}^4 a_{ij} \lbrack f_ux_j^c + (u_0-u_i)z^c_j \rbrack = 0\\ \sum_{j=1}^4 a_{ij} \lbrack f_vy_j^c + (v_0-v_i)z^c_j \rbrack = 0\\ \end{cases} \tag{15}
{∑j=14aij[fuxjc+(u0−ui)zjc]=0∑j=14aij[fvyjc+(v0−vi)zjc]=0(15)
一个点我们可以构造如（15）的两个方程，如果是n个点我们可以写成如下矩阵形式
[
a
11
f
u
0
a
11
(
u
0
−
u
1
)
a
12
f
u
0
a
12
(
u
0
−
u
1
)
a
13
f
u
0
a
13
(
u
0
−
u
1
)
a
14
f
u
0
a
14
(
u
0
−
u
1
)
0
a
11
f
v
a
11
(
v
0
−
v
1
)
0
a
12
f
v
a
12
(
v
0
−
v
1
)
0
a
13
f
v
a
13
(
v
0
−
v
1
)
0
a
14
f
v
a
14
(
v
0
−
v
1
)
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
a
n
1
f
u
0
a
n
1
(
u
0
−
u
1
)
a
n
2
f
u
0
a
n
2
(
u
0
−
u
1
)
a
n
3
f
u
0
a
n
3
(
u
0
−
u
1
)
a
n
4
f
u
0
a
n
4
(
u
0
−
u
1
)
0
a
n
1
f
v
a
n
1
(
v
0
−
v
1
)
0
a
n
2
f
v
a
n
2
(
v
0
−
v
1
)
0
a
n
3
f
v
a
n
3
(
v
0
−
v
1
)
0
a
n
4
f
v
a
n
4
(
v
0
−
v
1
)
]
⏟
M
[
x
1
c
y
1
c
z
1
c
x
2
c
y
2
c
z
2
c
x
3
c
y
3
c
z
3
c
x
4
c
y
4
c
z
4
c
]
⏟
x
=
0
\small{ \underbrace{ \begin{bmatrix} a_{11}f_u & 0 & a_{11}(u_0-u_1) & a_{12}f_u & 0 & a_{12}(u_0-u_1) & a_{13}f_u & 0 & a_{13}(u_0-u_1) & a_{14}f_u & 0 & a_{14}(u_0-u_1) \\ 0 & a_{11}f_v & a_{11}(v_0-v_1) & 0 & a_{12}f_v & a_{12}(v_0-v_1) & 0 & a_{13}f_v & a_{13}(v_0-v_1)& 0 & a_{14}f_v & a_{14}(v_0-v_1) \\ \vdots & \vdots & \vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots & \\ a_{n1}f_u & 0 & a_{n1}(u_0-u_1) & a_{n2}f_u & 0 & a_{n2}(u_0-u_1) & a_{n3}f_u & 0 & a_{n3}(u_0-u_1) & a_{n4}f_u & 0 & a_{n4}(u_0-u_1) \\ 0 & a_{n1}f_v & a_{n1}(v_0-v_1) & 0 & a_{n2}f_v & a_{n2}(v_0-v_1) & 0 & a_{n3}f_v & a_{n3}(v_0-v_1)& 0 & a_{n4}f_v & a_{n4}(v_0-v_1) \\ \end{bmatrix} }_{\text{M}} \underbrace{ \begin{bmatrix} x_1^c\\ y_1^c\\ z_1^c\\ x_2^c\\ y_2^c\\ z_2^c\\ x_3^c\\ y_3^c\\ z_3^c\\ x_4^c\\ y_4^c\\ z_4^c\\ \end{bmatrix} }_{\text{x}} =0 }
M
⎣⎢⎢⎢⎢⎢⎡a11fu0⋮an1fu00a11fv⋮0an1fva11(u0−u1)a11(v0−v1)⋮an1(u0−u1)an1(v0−v1)a12fu0⋮an2fu00a12fv⋮0an2fva12(u0−u1)a12(v0−v1)⋮an2(u0−u1)an2(v0−v1)a13fu0⋮an3fu00a13fv⋮0an3fva13(u0−u1)a13(v0−v1)⋮an3(u0−u1)an3(v0−v1)a14fu0⋮an4fu00a14fv⋮0an4fva14(u0−u1)a14(v0−v1)⋮an4(u0−u1)an4(v0−v1)⎦⎥⎥⎥⎥⎥⎤x
⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡x1cy1cz1cx2cy2cz2cx3cy3cz3cx4cy4cz4c⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=0
我们可以上面这个巨大的矩阵等式写成如下简单的形式
M
x
=
0
(16)
\bf{M}\bf{x} = 0 \tag{16}
Mx=0(16)
我们要解出控制点在相机坐标系下的坐标也就是要求
M
\bf{M}
M的零空间，可以用奇异值分解，奇异值分解复杂度过高，我们发现下面这个式子和（16）是同解的
M
T
M
x
=
0
(17)
\bf{M^T}\bf{M}\bf{x} = 0 \tag{17}
MTMx=0(17)
我们只需要求
M
T
M
12
×
12
{\bf{M^T}\bf{M}}_{12 \times12}
MTM12×12特征值为零及其对应特征向量
v
i
,
i
=
1
,
2
⋯
N
v_i, i = 1,2\cdots N
vi,i=1,2⋯N(这里假设值为零的特征值有N个)，则4个控制点在相机坐标系下的坐标可以表示为
N
N
N个
v
i
v_i
vi向量的线性组合
x
=
∑
i
=
1
N
β
i
v
i
(18)
\bf{x} = \sum_{i=1}^N \beta_i v_i \tag{18}
x=i=1∑Nβivi(18)
我们要求出4个控制点在相机坐标系下的坐标，就需要求出解出（18）中的
N
N
N个
β
i
\beta_i
βi的值，但是原论文[1]中作者根据实验得出，N的取值与相机的焦距有关，如下图所示，随着焦距变长，末尾的4个特征值（最小的4个特征值），趋近于0。所以我们在实现的时候直接取最小的4个特征值及其特征值向量（显然由于各种噪声和误差，不可能解出特征值为0），即我们取
N
N
N=4
想要求出
β
1
,
β
2
,
β
3
,
β
4
\beta_1,\beta_2,\beta_3,\beta_4
β1,β2,β3,β4，我们还需要利用一个约束：由于控制点坐标经过旋转和平移从世界坐标系到相机坐标系，控制点之间的欧式距离并不发生改变
∥
c
i
c
−
c
j
c
∥
2
2
=
∥
c
i
w
−
c
j
w
∥
2
2
(19)
\left\|c^c_i - c^c_j\right\|_2^2 = \left\|c^w_i - c^w_j\right\|_2^2 \tag{19}
∥∥cic−cjc∥∥22=∥∥ciw−cjw∥∥22(19)
c
i
c
=
β
1
v
1
[
i
]
+
β
2
v
2
[
i
]
+
β
3
v
3
[
i
]
+
β
4
v
4
[
i
]
(20)
c^c_i = \beta_1 v_1^{[i]} + \beta_2 v_2^{[i]} + \beta_3 v_3^{[i]}+\beta_4 v_4^{[i]} \tag{20}
cic=β1v1[i]+β2v2[i]+β3v3[i]+β4v4[i](20)
这里
v
[
i
]
v^{[i]}
v[i]表示
v
12
×
1
v_{12 \times 1}
v12×1的第
i
i
i个
3
×
1
3 \times 1
3×1的子向量，然后我们将（20）带入（19）得到
∥
β
1
v
1
[
i
]
+
β
2
v
2
[
i
]
+
β
3
v
3
[
i
]
+
β
4
v
4
[
i
]
−
β
1
v
1
[
j
]
−
β
2
v
2
[
j
]
−
β
3
v
3
[
j
]
−
β
4
v
4
[
j
]
∥
2
2
=
∥
c
i
w
−
c
j
w
∥
2
2
\small{ \left\| \beta_1 v_1^{[i]} + \beta_2 v_2^{[i]} + \beta_3 v_3^{[i]}+\beta_4 v_4^{[i]} - \beta_1 v_1^{[j]} - \beta_2 v_2^{[j]} - \beta_3 v_3^{[j]}-\beta_4 v_4^{[j]} \right\|_2^2 = \left\|c^w_i - c^w_j\right\|_2^2 }
∥∥∥β1v1[i]+β2v2[i]+β3v3[i]+β4v4[i]−β1v1[j]−β2v2[j]−β3v3[j]−β4v4[j]∥∥∥22=∥∥ciw−cjw∥∥22
将上面的式子整理一下可以得到
∥
β
1
(
v
1
[
i
]
−
v
1
[
j
]
)
⏟
s
1
+
β
2
(
v
2
[
i
]
−
v
2
[
j
]
)
⏟
s
2
+
β
3
(
v
3
[
i
]
−
v
3
[
j
]
)
⏟
s
3
+
β
4
(
v
4
[
i
]
−
v
4
[
j
]
)
⏟
s
4
∥
2
2
=
∥
c
i
w
−
c
j
w
∥
2
2
(21)
\small{ \left\| \beta_1 \underbrace{(v_1^{[i]}-v_1^{[j]})}_{s_1} + \beta_2 \underbrace{(v_2^{[i]}-v_2^{[j]})}_{s_2}+ \beta_3 \underbrace{(v_3^{[i]}-v_3^{[j]})}_{s_3}+\beta_4 \underbrace{(v_4^{[i]}-v_4^{[j]})}_{s_4} \right\|_2^2 = \left\|c^w_i - c^w_j\right\|_2^2 \tag{21} }
∥∥∥∥∥β1s1
(v1[i]−v1[j])+β2s2
(v2[i]−v2[j])+β3s3
(v3[i]−v3[j])+β4s4
(v4[i]−v4[j])∥∥∥∥∥22=∥∥ciw−cjw∥∥22(21)
根据
i
i
i和
j
j
j的不同取值（21）这样等式可以构造
C
4
2
=
6
C_4^2=6
C42=6个，将（21）展开可得
∥
c
i
w
−
c
j
w
∥
2
2
=
β
1
2
s
1
T
s
1
+
β
1
β
2
s
2
T
s
1
+
β
1
β
3
s
3
T
s
1
+
β
1
β
4
s
4
T
s
1
⋯
+
β
4
β
4
s
4
T
s
4
(22)
\left\|c^w_i - c^w_j\right\|_2^2= \beta_1^2 s_1^Ts_1 + \beta_1 \beta_2 s_2^Ts_1+ \beta_1 \beta_3 s_3^Ts_1 + \beta_1 \beta_4 s_4^Ts_1 \cdots+ \beta_4 \beta_4 s_4^Ts_4 \tag{22}
∥∥ciw−cjw∥∥22=β12s1Ts1+β1β2s2Ts1+β1β3s3Ts1+β1β4s4Ts1⋯+β4β4s4Ts4(22)
将6个等式写成矩阵形式如下所示(其中
β
i
j
=
β
i
β
j
\beta_{ij}=\beta_i\beta_j
βij=βiβj,
s
i
j
s_{ij}
sij表示第
i
i
i个等式的
s
j
s_j
sj)
[
s
11
T
s
11
s
11
T
s
12
s
12
T
s
12
s
11
T
s
13
s
12
T
s
13
s
13
T
s
13
s
11
T
s
14
s
12
T
s
14
s
13
T
s
14
s
14
T
s
14
s
21
T
s
21
s
21
T
s
22
s
22
T
s
22
s
21
T
s
23
s
22
T
s
23
s
23
T
s
23
s
21
T
s
24
s
22
T
s
24
s
23
T
s
24
s
24
T
s
24
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
s
61
T
s
61
s
61
T
s
62
s
62
T
s
62
s
61
T
s
63
s
62
T
s
63
s
63
T
s
63
s
61
T
s
64
s
62
T
s
64
s
63
T
s
64
s
64
T
s
64
]
⏟
L
[
β
11
β
12
β
22
β
13
β
23
β
33
β
14
β
24
β
34
β
44
]
⏟
β
=
[
∥
c
1
w
−
c
2
w
∥
2
2
∥
c
1
w
−
c
2
w
∥
2
2
∥
c
2
w
−
c
2
w
∥
2
2
∥
c
1
w
−
c
3
w
∥
2
2
∥
c
2
w
−
c
3
w
∥
2
2
∥
c
3
w
−
c
3
w
∥
2
2
∥
c
1
w
−
c
4
w
∥
2
2
∥
c
2
w
−
c
4
w
∥
2
2
∥
c
3
w
−
c
4
w
∥
2
2
∥
c
4
w
−
c
4
w
∥
2
2
]
⏟
ρ
\small{ \underbrace{ \begin{bmatrix} s_{11}^Ts_{11}& s_{11}^Ts_{12} & s_{12}^Ts_{12} & s_{11}^Ts_{13} & s_{12}^Ts_{13} & s_{13}^Ts_{13} & s_{11}^Ts_{14} & s_{12}^Ts_{14} & s_{13}^Ts_{14} & s_{14}^Ts_{14} \\ s_{21}^Ts_{21}& s_{21}^Ts_{22} & s_{22}^Ts_{22} & s_{21}^Ts_{23} & s_{22}^Ts_{23} & s_{23}^Ts_{23} & s_{21}^Ts_{24} & s_{22}^Ts_{24} & s_{23}^Ts_{24} & s_{24}^Ts_{24} \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots &\vdots\\ s_{61}^Ts_{61}& s_{61}^Ts_{62} & s_{62}^Ts_{62} & s_{61}^Ts_{63} & s_{62}^Ts_{63} & s_{63}^Ts_{63} & s_{61}^Ts_{64} & s_{62}^Ts_{64} & s_{63}^Ts_{64} & s_{64}^Ts_{64} \\ \end{bmatrix} }_{L} \underbrace{ \begin{bmatrix} \beta_{11} \\ \beta_{12} \\ \beta_{22} \\ \beta_{13} \\ \beta_{23} \\ \beta_{33} \\ \beta_{14} \\ \beta_{24} \\ \beta_{34} \\ \beta_{44} \\ \end{bmatrix} }_{\beta} = \underbrace{ \begin{bmatrix} \left\|c^w_1 - c^w_2 \right\|_2^2 \\ \left\|c^w_1 - c^w_2 \right\|_2^2 \\ \left\|c^w_2 - c^w_2 \right\|_2^2 \\ \left\|c^w_1 - c^w_3 \right\|_2^2 \\ \left\|c^w_2 - c^w_3 \right\|_2^2 \\ \left\|c^w_3 - c^w_3 \right\|_2^2 \\ \left\|c^w_1 - c^w_4 \right\|_2^2 \\ \left\|c^w_2 - c^w_4 \right\|_2^2 \\ \left\|c^w_3 - c^w_4 \right\|_2^2 \\ \left\|c^w_4 - c^w_4 \right\|_2^2 \\ \end{bmatrix} }_{\rho} }
L
⎣⎢⎢⎢⎡s11Ts11s21Ts21⋮s61Ts61s11Ts12s21Ts22⋮s61Ts62s12Ts12s22Ts22⋮s62Ts62s11Ts13s21Ts23⋮s61Ts63s12Ts13s22Ts23⋮s62Ts63s13Ts13s23Ts23⋮s63Ts63s11Ts14s21Ts24⋮s61Ts64s12Ts14s22Ts24⋮s62Ts64s13Ts14s23Ts24⋮s63Ts64s14Ts14s24Ts24⋮s64Ts64⎦⎥⎥⎥⎤β
⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡β11β12β22β13β23β33β14β24β34β44⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=ρ
⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡∥c1w−c2w∥22∥c1w−c2w∥22∥c2w−c2w∥22∥c1w−c3w∥22∥c2w−c3w∥22∥c3w−c3w∥22∥c1w−c4w∥22∥c2w−c4w∥22∥c3w−c4w∥22∥c4w−c4w∥22⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
我们将上面的巨大的矩阵等式下面这种简单的形式
L
β
=
ρ
(23)
L\beta=\rho \tag{23}
Lβ=ρ(23)
取其中几列构造新的方程可以获得原方程的近似解，
β
1
,
β
2
,
β
3
,
β
4
\beta_1,\beta_2,\beta_3,\beta_4
β1,β2,β3,β4的初值，用于后续的高斯牛顿优化的初始值
[
s
11
T
s
11
s
11
T
s
12
s
11
T
s
13
s
11
T
s
14
s
21
T
s
21
s
21
T
s
22
s
21
T
s
23
s
21
T
s
24
⋮
⋮
⋮
⋮
s
61
T
s
61
s
61
T
s
62
s
61
T
s
73
s
71
T
s
74
]
[
β
11
β
12
β
13
β
14
]
=
[
∥
c
1
w
−
c
2
w
∥
2
2
∥
c
1
w
−
c
2
w
∥
2
2
∥
c
2
w
−
c
2
w
∥
2
2
∥
c
1
w
−
c
3
w
∥
2
2
∥
c
2
w
−
c
3
w
∥
2
2
∥
c
3
w
−
c
3
w
∥
2
2
∥
c
1
w
−
c
4
w
∥
2
2
∥
c
2
w
−
c
4
w
∥
2
2
∥
c
3
w
−
c
4
w
∥
2
2
∥
c
4
w
−
c
4
w
∥
2
2
]
(24)
\small{ \begin{bmatrix} s_{11}^Ts_{11}& s_{11}^Ts_{12} & s_{11}^Ts_{13} &s_{11}^Ts_{14} \\ s_{21}^Ts_{21}& s_{21}^Ts_{22} & s_{21}^Ts_{23} &s_{21}^Ts_{24} \\ \vdots & \vdots & \vdots & \vdots & \\ s_{61}^Ts_{61}& s_{61}^Ts_{62} & s_{61}^Ts_{73} &s_{71}^Ts_{74} \\ \end{bmatrix} \begin{bmatrix} \beta_{11} \\ \beta_{12} \\ \beta_{13} \\ \beta_{14} \\ \end{bmatrix} = \begin{bmatrix} \left\|c^w_1 - c^w_2 \right\|_2^2 \\ \left\|c^w_1 - c^w_2 \right\|_2^2 \\ \left\|c^w_2 - c^w_2 \right\|_2^2 \\ \left\|c^w_1 - c^w_3 \right\|_2^2 \\ \left\|c^w_2 - c^w_3 \right\|_2^2 \\ \left\|c^w_3 - c^w_3 \right\|_2^2 \\ \left\|c^w_1 - c^w_4 \right\|_2^2 \\ \left\|c^w_2 - c^w_4 \right\|_2^2 \\ \left\|c^w_3 - c^w_4 \right\|_2^2 \\ \left\|c^w_4 - c^w_4 \right\|_2^2 \\ \end{bmatrix} \tag{24} }
⎣⎢⎢⎡s11Ts11s21Ts21⋮s61Ts61s11Ts12s21Ts22⋮s61Ts62s11Ts13s21Ts23⋮s61Ts73s11Ts14s21Ts24⋮s71Ts74⎦⎥⎥⎤⎣⎢⎡β11β12β13β14⎦⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡∥c1w−c2w∥22∥c1w−c2w∥22∥c2w−c2w∥22∥c1w−c3w∥22∥c2w−c3w∥22∥c3w−c3w∥22∥c1w−c4w∥22∥c2w−c4w∥22∥c3w−c4w∥22∥c4w−c4w∥22⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤(24)
构造下面的目标优化函数
arg min
β
E
r
r
o
r
(
β
)
=
∑
(
L
β
−
ρ
)
2
(25)
\argmin_{\beta} Error(\beta) = \sum (L\beta-\rho)^2 \tag{25}
βargminError(β)=∑(Lβ−ρ)2(25)
记残差项为
r
=
L
β
−
ρ
(26)
\bf{r}= L \beta-\rho \tag{26}
r=Lβ−ρ(26)
优化变量为
x
=
[
β
1
β
2
β
3
β
4
]
\bf{x}= \begin{bmatrix} \beta_1 \\ \beta_2 \\ \beta_3 \\ \beta_4 \\ \end{bmatrix}
x=⎣⎢⎢⎡β1β2β3β4⎦⎥⎥⎤
残差项对优化变量的雅克比矩阵
J
=
∂
r
∂
x
=
L
∂
β
∂
x
=
L
[
∂
β
11
∂
β
1
∂
β
11
∂
β
2
∂
β
11
∂
β
3
∂
β
11
∂
β
4
∂
β
12
∂
β
1
∂
β
12
∂
β
2
∂
β
12
∂
β
3
∂
β
12
∂
β
4
∂
β
22
∂
β
1
∂
β
22
∂
β
2
∂
β
22
∂
β
3
∂
β
22
∂
β
4
∂
β
13
∂
β
1
∂
β
13
∂
β
2
∂
β
13
∂
β
3
∂
β
13
∂
β
4
∂
β
23
∂
β
1
∂
β
23
∂
β
2
∂
β
23
∂
β
3
∂
β
23
∂
β
4
∂
β
33
∂
β
1
∂
β
33
∂
β
2
∂
β
33
∂
β
3
∂
β
33
∂
β
4
∂
β
14
∂
β
1
∂
β
14
∂
β
2
∂
β
14
∂
β
3
∂
β
14
∂
β
4
∂
β
24
∂
β
1
∂
β
24
∂
β
2
∂
β
24
∂
β
3
∂
β
24
∂
β
4
∂
β
34
∂
β
1
∂
β
34
∂
β
2
∂
β
34
∂
β
3
∂
β
34
∂
β
4
∂
β
44
∂
β
1
∂
β
44
∂
β
2
∂
β
44
∂
β
3
∂
β
44
∂
β
4
]
(27)
\bf{J} = \frac {\partial \bf{r}}{\partial \bf{x}} =L \frac {\partial \beta}{\partial \bf{x}} = L \begin{bmatrix} \frac {\partial \beta_{11}}{\partial \beta_1} & \frac {\partial \beta_{11}}{\partial \beta_2} & \frac {\partial \beta_{11}}{\partial \beta_3} & \frac {\partial \beta_{11}}{\partial \beta_4} \\ \frac {\partial \beta_{12}}{\partial \beta_1} & \frac {\partial \beta_{12}}{\partial \beta_2} & \frac {\partial \beta_{12}}{\partial \beta_3} & \frac {\partial \beta_{12}}{\partial \beta_4} \\ \frac {\partial \beta_{22}}{\partial \beta_1} & \frac {\partial \beta_{22}}{\partial \beta_2} & \frac {\partial \beta_{22}}{\partial \beta_3} & \frac {\partial \beta_{22}}{\partial \beta_4} \\ \frac {\partial \beta_{13}}{\partial \beta_1} & \frac {\partial \beta_{13}}{\partial \beta_2} & \frac {\partial \beta_{13}}{\partial \beta_3} & \frac {\partial \beta_{13}}{\partial \beta_4} \\ \frac {\partial \beta_{23}}{\partial \beta_1} & \frac {\partial \beta_{23}}{\partial \beta_2} & \frac {\partial \beta_{23}}{\partial \beta_3} & \frac {\partial \beta_{23}}{\partial \beta_4} \\ \frac {\partial \beta_{33}}{\partial \beta_1} & \frac {\partial \beta_{33}}{\partial \beta_2} & \frac {\partial \beta_{33}}{\partial \beta_3} & \frac {\partial \beta_{33}}{\partial \beta_4} \\ \frac {\partial \beta_{14}}{\partial \beta_1} & \frac {\partial \beta_{14}}{\partial \beta_2} & \frac {\partial \beta_{14}}{\partial \beta_3} & \frac {\partial \beta_{14}}{\partial \beta_4} \\ \frac {\partial \beta_{24}}{\partial \beta_1} & \frac {\partial \beta_{24}}{\partial \beta_2} & \frac {\partial \beta_{24}}{\partial \beta_3} & \frac {\partial \beta_{24}}{\partial \beta_4} \\ \frac {\partial \beta_{34}}{\partial \beta_1} & \frac {\partial \beta_{34}}{\partial \beta_2} & \frac {\partial \beta_{34}}{\partial \beta_3} & \frac {\partial \beta_{34}}{\partial \beta_4} \\ \frac {\partial \beta_{44}}{\partial \beta_1} & \frac {\partial \beta_{44}}{\partial \beta_2} & \frac {\partial \beta_{44}}{\partial \beta_3} & \frac {\partial \beta_{44}}{\partial \beta_4} \\ \end{bmatrix} \tag{27}
J=∂x∂r=L∂x∂β=L⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡∂β1∂β11∂β1∂β12∂β1∂β22∂β1∂β13∂β1∂β23∂β1∂β33∂β1∂β14∂β1∂β24∂β1∂β34∂β1∂β44∂β2∂β11∂β2∂β12∂β2∂β22∂β2∂β13∂β2∂β23∂β2∂β33∂β2∂β14∂β2∂β24∂β2∂β34∂β2∂β44∂β3∂β11∂β3∂β12∂β3∂β22∂β3∂β13∂β3∂β23∂β3∂β33∂β3∂β14∂β3∂β24∂β3∂β34∂β3∂β44∂β4∂β11∂β4∂β12∂β4∂β22∂β4∂β13∂β4∂β23∂β4∂β33∂β4∂β14∂β4∂β24∂β4∂β34∂β4∂β44⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤(27)
增量方程为
J
T
J
Δ
x
=
−
J
T
r
(28)
\bf{J^T}\bf{J} \Delta x = \bf{-J^T} \bf{r} \tag{28}
JTJΔx=−JTr(28)
更新优化变量
x
=
x
+
Δ
x
(28)
\bf{x} = \bf{x} + \Delta x \tag{28}
x=x+Δx(28)
至此，我们已经得到了控制点在相机坐标系的坐标，3D点由于hb坐标在相机坐标系和世界坐标中是一致的，所以我们可以根据（5）还原出每个3D点在相机坐标系中的坐标，这样就把PnP问题转化成了ICP问题，根据ICP问题的算法我们可以解出旋转矩阵
R
R
R和平移向量
T
T
T，本文就不在继续介绍其解法。
4. 参考资料
[1] Lepetit, V.; Moreno-Noguer, F.; Fua, P. Epnp: Efficient perspective-n-point camera pose estimation. International Journal of Computer Vision 2009, 81, 155-166.
[2] PnP问题之EPnP解法
[3] EPnP算法
[4] 高翔, 张涛, 颜沁睿, 刘毅, 视觉SLAM十四讲：从理论到实践, 电子工业出版社, 2017
2044
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
