论文：EPnP: Accurate Non-Iterative O(n) Solution to the PnP Problem
参考资料：
[1] Lepetit V , Moreno-Noguer F , Fua P . EPnP: An AccurateO(n) Solution to the PnP Problem[J]. international journal of computer vision, 2009, 81(2):155-166.
[2] PnP问题之EPnP解法
[3] EPnP算法
[4] 一起学ORBSLAM2（9）ORBSLAM的PNP解决方案
问题模型
已知世界坐标下的 n n n个3D点，以及它们在一张图像上对应的2D像素点，还有相机的内参数。
求解相机坐标系到世界坐标系的 R \mathbf{R} R, t \mathbf{t} t.
按照高翔博士在《视觉SLAM十四讲》中的阐述，该问题属于PnP问题，即3D到2D点对运动的求解问题。
EPnP方法求解
(1)一般情况的参数化
定义世界坐标系下的
n
n
n个3D点：
p
i
,
i
=
1
,
.
.
.
,
n
(1)
p_i,\quad i=1,...,n \tag{1}
pi,i=1,...,n(1)
定义世界坐标系下的4个控制点：
c
j
,
j
=
1
,
.
.
.
,
4
(2)
c_j,\quad j=1,...,4 \tag{2}
cj,j=1,...,4(2)
世界坐标系下的三维点可以用4个控制点的线性组合表示：
p
i
w
=
∑
j
=
1
4
α
i
j
c
j
w
with
∑
j
=
1
4
α
i
j
=
1
\mathbf{p}_{i}^{w}=\sum_{j=1}^{4} \alpha_{i j} \mathbf{c}_{j}^{w} \quad \text { with } \quad \sum_{j=1}^{4} \alpha_{i j}=1
piw=j=1∑4αijcjw with j=1∑4αij=1
其中的
α
i
j
\alpha_{i j}
αij 称为均质重心坐标（ homogeneous barycentric coordinates）, 一组
[
α
i
1
,
α
i
2
,
α
i
3
,
α
i
4
]
\left[\alpha_{i 1}, \alpha_{i 2}, \alpha_{i 3}, \alpha_{i 4}\right]
[αi1,αi2,αi3,αi4] 能够唯一地确定对应的三维点
p
i
p_{i}
pi, 因此世界坐标系下
p
i
w
p_{i}^{w }
piw可以表示成如下齐次坐标公式:
[
p
i
w
1
]
=
[
c
1
w
c
2
w
c
3
w
c
4
w
1
1
1
1
]
[
α
i
1
α
i
2
α
i
3
α
i
4
]
=
C
[
α
i
1
α
i
2
α
i
3
α
i
4
]
\left[\begin{array}{c} \mathbf{p}_{i}^{w} \\ 1 \end{array}\right]=\left[\begin{array}{cccc} \mathbf{c}_{1}^{w} & \mathbf{c}_{2}^{w} & \mathbf{c}_{3}^{w} & \mathbf{c}_{4}^{w} \\ 1 & 1 & 1 & 1 \end{array}\right]\left[\begin{array}{l} \alpha_{i 1} \\ \alpha_{i 2} \\ \alpha_{i 3} \\ \alpha_{i 4} \end{array}\right]=\mathbf{C}\left[\begin{array}{l} \alpha_{i 1} \\ \alpha_{i 2} \\ \alpha_{i 3} \\ \alpha_{i 4} \end{array}\right]
[piw1]=[c1w1c2w1c3w1c4w1]⎣⎢⎢⎡αi1αi2αi3αi4⎦⎥⎥⎤=C⎣⎢⎢⎡αi1αi2αi3αi4⎦⎥⎥⎤
假设此时相机在世界坐标系下的位姿为
[
R
,
t
]
[R, t]
[R,t], 4个控制点可以从世界坐标系转到相机坐标系下:
c
j
c
=
[
R
∣
t
]
[
c
j
w
1
]
\mathbf{c}_{j}^{c}=[\mathbf{R} \mid \mathbf{t}]\left[\begin{array}{c} \mathbf{c}_{j}^{w} \\ 1 \end{array}\right]
cjc=[R∣t][cjw1]
同理，世界坐标系下的三维点
p
i
w
p_{i}^{w}
piw 也能转到相机坐标系下并与4个控制点产生关联：
p
i
c
=
[
R
∣
t
]
[
p
i
w
1
]
=
[
R
∣
t
]
[
∑
j
=
1
4
α
i
j
c
j
w
∑
j
=
1
4
α
i
j
]
=
∑
j
=
1
4
α
i
j
[
R
,
t
]
[
c
j
w
1
]
=
∑
j
=
1
4
α
i
j
c
j
c
\mathbf{p}_{i}^{c}=[\mathbf{R} \mid \mathbf{t}]\left[\begin{array}{c}\mathbf{p}_{i}^{w} \\ 1\end{array}\right]=[\mathbf{R} \mid \mathbf{t}]\left[\begin{array}{c}\sum_{j=1}^{4} \alpha_{i j} \mathbf{c}_{j}^{w} \\ \sum_{j=1}^{4} \alpha_{i j}\end{array}\right]=\sum_{j=1}^{4} \alpha_{i j}[\mathbf{R}, \mathbf{t}]\left[\begin{array}{c}\mathbf{c}_{j}^{w} \\ 1\end{array}\right]=\sum_{j=1}^{4} \alpha_{i j} \mathbf{c}_{j}^{c}
pic=[R∣t][piw1]=[R∣t][∑j=14αijcjw∑j=14αij]=∑j=14αij[R,t][cjw1]=∑j=14αijcjc
经过上面的参数化定义，可以发现一个重要特点：HB坐标 α i j \alpha_{ij} αij在世界坐标系和相机坐标系的数值是相同的，那么假如我们能够通过世界坐标系下的三维点求解出各个点的 [ α i 1 , α i 2 , α i 3 , α i 4 ] [\alpha_{i1},\alpha_{i2},\alpha_{i3},\alpha_{i4}] [αi1,αi2,αi3,αi4]，我们即可通过上式求出相机坐标系下的4个控制点坐标，后面求解相机在世界坐标系下的位姿问题，就转变为：已知 c c c^c cc和 c w c^w cw，求解两组空间三维点间的位姿变换(3D-3D)，理所当然可以使用ICP方法求解。
(2)控制点的选择- -choose_control_points()函数
第一个控制点
c
1
w
c_1^w
c1w需要选择观察点集
{
p
i
,
i
=
1
,
.
.
.
,
n
}
\{p_i,\quad i=1,...,n\}
{pi,i=1,...,n}的质心：
c
1
w
=
1
n
∑
i
=
1
n
p
i
w
\mathbf{c}_{1}^{w}=\frac{1}{n} \sum_{i=1}^{n} \mathbf{p}_{i}^{w}
c1w=n1i=1∑npiw
计算其它三个控制点，
c
2
w
,
c
3
w
,
c
4
w
c_2^w,c_3^w,c_4^w
c2w,c3w,c4w通过PCA分解得到，定义
n
×
3
n\times 3
n×3维矩阵A：
A
=
[
(
p
1
w
−
c
1
w
)
⊤
(
p
2
w
−
c
1
w
)
⊤
⋮
(
p
n
w
−
c
1
w
)
⊤
]
\mathbf{A}=\left[\begin{array}{c} \left(\mathbf{p}_{1}^{w}-\mathbf{c}_{1}^{w}\right)^{\top} \\ \left(\mathbf{p}_{2}^{w}-\mathbf{c}_{1}^{w}\right)^{\top} \\ \vdots \\ \left(\mathbf{p}_{n}^{w}-\mathbf{c}_{1}^{w}\right)^{\top} \end{array}\right]
A=⎣⎢⎢⎢⎢⎡(p1w−c1w)⊤(p2w−c1w)⊤⋮(pnw−c1w)⊤⎦⎥⎥⎥⎥⎤
利用SVD分解获得
A
T
A
A^TA
ATA的主分量（注：主成分分析概念点击参考网址），类似齐次线性最小二乘法求解的过程，从而获取
A
T
A
A^TA
ATA的特征值
[
λ
1
,
λ
2
,
λ
3
]
[\lambda_1,\lambda_2,\lambda_3]
[λ1,λ2,λ3]及其对应的特征向量
v
1
,
v
2
,
v
3
\mathbf{v_1},\mathbf{v_2},\mathbf{v_3}
v1,v2,v3，所以剩下的三个控制点
c
2
w
,
c
3
w
,
c
4
w
c_2^w,c_3^w,c_4^w
c2w,c3w,c4w分别是：
{
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
+
λ
s
n
v
3
\left\{\begin{array}{l} \mathbf{c}_{2}^{w=}=\mathbf{c}_{1}^{w}+\sqrt{\frac{\lambda_{1}}{n}} \mathbf{v}_{\mathbf{1}} \\ \mathbf{c}_{3}^{w}=\mathbf{c}_{1}^{w}+\sqrt{\frac{\lambda_{2}}{n}} \mathbf{v}_{2} \\ \mathbf{c}_{4}^{w}=\mathbf{c}_{1}^{w}+\sqrt{\frac{\lambda_{s}}{n}} \mathbf{v}_{3} \end{array}\right.
⎩⎪⎪⎪⎨⎪⎪⎪⎧c2w==c1w+nλ1v1c3w=c1w+nλ2v2c4w=c1w+nλsv3
确定4个控制点具体坐标后，将观察点集
{
p
i
,
i
=
1
,
.
.
.
,
n
}
\{p_i,\quad i=1,...,n\}
{pi,i=1,...,n}转到HB坐标系下：
[
α
i
1
α
i
2
α
i
3
α
i
4
]
=
C
−
1
[
p
i
w
1
]
\left[\begin{array}{c} \alpha_{i 1} \\ \alpha_{i 2} \\ \alpha_{i 3} \\ \alpha_{i 4} \end{array}\right]=\mathbf{C}^{-1}\left[\begin{array}{c} \mathbf{p}_{i}^{w} \\ 1 \end{array}\right]
⎣⎢⎢⎡αi1αi2αi3αi4⎦⎥⎥⎤=C−1[piw1]
(3)求控制点在相机坐标系的坐标c j c _{j}^{c} jc
由相机的投影棋型：
s
[
u
i
1
]
=
K
p
i
c
=
K
∑
j
=
1
4
α
i
j
c
j
c
s\left[\begin{array}{c} \mathbf{u}_{i} \\ 1 \end{array}\right]=\mathbf{K} \mathbf{p}_{i}^{c}=\mathbf{K} \sum_{j=1}^{4} \alpha_{i j} c_{j}^{c}
s[ui1]=Kpic=Kj=1∑4αijcjc
矩阵参数貝体化:
s
[
u
i
v
i
1
]
=
[
f
u
0
u
c
0
f
v
v
c
0
0
1
]
∑
j
=
1
4
α
i
j
[
x
j
c
y
j
e
z
j
e
]
s\left[\begin{array}{c} u_{i} \\ v_{i} \\ 1 \end{array}\right]=\left[\begin{array}{ccc} f_{u} & 0 & u_{c} \\ 0 & f_{v} & v_{c} \\ 0 & 0 & 1 \end{array}\right] \sum_{j=1}^{4} \alpha_{i j}\left[\begin{array}{l} x_{j}^{c} \\ y_{j}^{e} \\ z_{j}^{e} \end{array}\right]
s⎣⎡uivi1⎦⎤=⎣⎡fu000fv0ucvc1⎦⎤j=1∑4αij⎣⎡xjcyjezje⎦⎤
上式可以消去
s
s
s后得到两个线性方程：
∑
j
=
1
4
(
α
i
j
f
u
x
j
c
+
α
i
j
(
u
c
−
u
i
)
z
j
c
)
=
0
∑
j
=
1
4
(
α
i
j
f
v
y
j
c
+
α
i
j
(
v
c
−
v
i
)
z
j
c
)
=
0
\begin{aligned} \sum_{j=1}^{4}\left(\alpha_{i j} f_{u} x_{j}^{c}+\alpha_{i j}\left(u_{c}-u_{i}\right) z_{j}^{c}\right) &=0 \\ \sum_{j=1}^{4}\left(\alpha_{i j} f_{v} y_{j}^{c}+\alpha_{i j}\left(v_{c}-v_{i}\right) z_{j}^{c}\right) &=0 \end{aligned}
j=1∑4(αijfuxjc+αij(uc−ui)zjc)j=1∑4(αijfvyjc+αij(vc−vi)zjc)=0=0
其中
α
i
j
\boldsymbol{\alpha}_{i \boldsymbol{j}}
αij, 相机内部参数,
[
u
i
,
v
i
]
\left[\boldsymbol{u}_{i}, \boldsymbol{v}_{\boldsymbol{i}}\right]
[ui,vi] 为已知量，未知量为控制点在相机坐标系下的坐标
[
x
j
c
,
y
j
c
,
z
j
c
]
\left[x_{j}^{c}, y_{j}^{c}, z_{j}^{c}\right]
[xjc,yjc,zjc]。 把所有的
n
n
n 个相机坐标系下的观察点都串联起来，得到线性方程:
M
x
=
0
\mathbf{M x}=\mathbf{0}
Mx=0
展开矩阵M有：
[
α
11
f
u
0
α
11
(
u
c
−
u
1
)
…
α
14
f
u
0
α
14
(
u
c
−
u
1
)
0
α
11
f
v
α
11
(
v
c
−
v
1
)
…
0
α
14
f
v
α
14
(
v
c
−
v
1
)
⋮
⋮
⋮
α
n
1
f
u
0
α
n
1
(
u
c
−
u
n
)
…
α
n
4
f
u
0
α
n
4
(
u
c
−
u
n
)
0
α
n
1
f
v
α
n
1
(
v
c
−
v
n
)
…
0
α
n
4
f
v
α
n
4
(
v
c
−
v
n
)
]
[
x
1
c
y
1
c
z
1
c
⋮
x
4
c
y
4
c
z
4
c
]
=
0
\left[\begin{array}{ccccccc} \alpha_{11} f_{u} & 0 & \alpha_{11}\left(u_{c}-u_{1}\right) & \ldots & \alpha_{14} f_{u} & 0 & \alpha_{14}\left(u_{c}-u_{1}\right) \\ 0 & \alpha_{11} f_{v} & \alpha_{11}\left(v_{c}-v_{1}\right) & \ldots & 0 & \alpha_{14} f_{v} & \alpha_{14}\left(v_{c}-v_{1}\right) \\ \vdots & & & & \vdots & & \vdots \\ \alpha_{n 1} f_{u} & 0 & \alpha_{n 1}\left(u_{c}-u_{n}\right) & \ldots & \alpha_{n 4} f_{u} & 0 & \alpha_{n 4}\left(u_{c}-u_{n}\right) \\ 0 & \alpha_{n 1} f_{v} & \alpha_{n 1}\left(v_{c}-v_{n}\right) & \ldots & 0 & \alpha_{n 4} f_{v} & \alpha_{n 4}\left(v_{c}-v_{n}\right) \end{array}\right]\left[\begin{array}{c} x_{1}^{c} \\ y_{1}^{c} \\ z_{1}^{c} \\ \vdots \\ x_{4}^{c} \\ y_{4}^{c} \\ z_{4}^{c} \end{array}\right]=\mathbf{0}
⎣⎢⎢⎢⎢⎢⎡α11fu0⋮αn1fu00α11fv0αn1fvα11(uc−u1)α11(vc−v1)αn1(uc−un)αn1(vc−vn)…………α14fu0⋮αn4fu00α14fv0αn4fvα14(uc−u1)α14(vc−v1)⋮αn4(uc−un)αn4(vc−vn)⎦⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡x1cy1cz1c⋮x4cy4cz4c⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=0
上式中未知量
x
=
[
c
1
c
T
,
c
2
c
T
,
c
3
c
T
,
c
4
c
T
]
T
\mathbf{x}=\left[c_{1}^{c T}, c_{2}^{c T}, c_{3}^{c T}, c_{4}^{c T}\right]^{T}
x=[c1cT,c2cT,c3cT,c4cT]T 是12维向量,
M
M
M 是
2
n
×
12
2 n \times 12
2n×12 维矩阵，它的解可以表达为
x
=
∑
i
=
1
N
β
i
v
i
\mathbf{x}=\sum_{i=1}^{N} \beta_{i} \mathbf{v}_{i}
x=i=1∑Nβivi
其中12维向量
V
i
\mathbf{V}_{i}
Vi 为
M
\mathbf{M}
M 的右奇异向量（不知道为啥ORB-SLAM代码里取的是左奇异向量）, 对应 的奇异值为
0
0
0。 具体的求解步骤为：求解
M
⊤
M
\mathbf{M}^{\top} \mathbf{M}
M⊤M 的特征值和特征向量，特征值为0的特征向量为
V
i
\mathbf{V}_{i}
Vi。
根据约束 ∣ ∣ c i c − c j c ∣ ∣ 2 = ∣ ∣ c i w − c j w ∣ ∣ 2 ||\mathbf{c}^c_i-\mathbf{c}^c_j||^2=||\mathbf{c}^w_i-\mathbf{c}^w_j||^2 ∣∣cic−cjc∣∣2=∣∣ciw−cjw∣∣2 ,求解 β k \beta_k βk。其中 ∣ ∣ c i w − c j w ∣ ∣ 2 ||\mathbf{c}^w_i-\mathbf{c}^w_j||^2 ∣∣ciw−cjw∣∣2为世界坐标系下4个控制点之间的距离的平方，可以直接直接计算出来，是一个6维( C 4 2 = 6 C_4^2=6 C42=6)的向量。 N N N为 M T M M^TM MTM的0特征值的个数，根据论文描述，由于尺度不确定性，因此 N N N的维度至少为1，而且如果相机是正投影而不是仿射变换，则 N N N的维度变为4，因此如果焦距很小的话就只有1个0特征值，如果焦距长度越来越大，他就越趋向于正投影， N N N的维度就慢慢趋向于4，而由于有噪声的原因，特征值可能不是完全是0，而是一个足够小的数。在ORB-SLAM中计算了 N = 1 − 4 N=1-4 N=1−4的所有可能，然后计算反投影误差取最小的反投影误差对应的 N N N值作为正确的 N N N。
a. 论文中的解法：
下面是Copy自：https://zhuanlan.zhihu.com/p/59070440
Case N = 1 N =1 N=1
这是最简单的情况，上式这时变成了 x = β v \mathbf{x}=\beta\mathbf{v} x=βv。这时候把控制点在摄像机坐标系和世界坐标系的坐标联系起来：控制点间距在两种坐标系之下是相等的
∥ c i c − c j c ∥ 2 = ∥ c i w − c j w ∥ ∥ 2 \left\|\mathbf{c}_{i}^{c}-\mathbf{c}_{j}^{c}\right\|^{2}=\left\|\mathbf{c}_{i}^{w}-\mathbf{c}_{j}^{w \|}\right\|^{2} ∥∥cic−cjc∥∥2=∥∥∥ciw−cjw∥∥∥∥2
由此，引入一个约束。
∥ β v [ θ ] − β v [ j ∥ 2 = ∥ c i w − c j w ∥ 2 \left\|\beta \mathbf{v}^{[\theta]}-\beta \mathbf{v}^{[j}\right\|^{2}=\left\|c_{i}^{w}-c_{j}^{w}\right\|^{2} ∥∥∥βv[θ]−βv[j∥∥∥2=∥∥ciw−cjw∥∥2
其中， v [ i ] \mathbf{v}^{[i]} v[i]表示 v \mathbf{v} v中第 i i i个控制点所占据的3个元素组成的向量。4个控制点，可以得到 β \beta β的一个闭式解。
Case N = 2 \boldsymbol{N}=\mathbf{2} N=2
此时,上式变为 x = β 1 v 1 + β 2 v 2 \mathbf{x}=\boldsymbol{\beta}_{1} \mathbf{v}_{1}+\beta_{2} \mathbf{v}_{\mathbf{2}} x=β1v1+β2v2, 带入 (12) 式
∥ ( β 1 v 1 [ 1 + β 2 v 2 [ i ] ) − ( β 1 v 1 [ j + β 2 v 2 [ j ] ) ∥ 2 = ∥ c i w − c j w ∥ 2 \left\|\left(\beta_{1} \mathbf{v}_{1}^{[1}+\beta_{2} \mathbf{v}_{2}^{[i]}\right)-\left(\beta_{1} \mathbf{v}_{1}^{[j}+\beta_{2} \mathbf{v}_{2}^{[j]}\right)\right\|^{2}=\left\|\mathbf{c}_{i}^{w}-\mathbf{c}_{j}^{w}\right\|^{2} ∥∥∥(β1v1[1+β2v2[i])−(β1v1[j+β2v2[j])∥∥∥2=∥∥ciw−cjw∥∥2
将上式展开
∥ β 1 ( v 1 [ d − v 1 [ j ] ) ⏟ s 1 + β 2 ( v 2 [ i ] − v 2 [ j ] ) ⏟ s 2 ∥ 2 = ∥ c i w − c j u ∥ 2 ⏟ c β 1 2 S 1 T S 1 + 2 β 1 β 2 S 1 T S 2 + β 2 2 S 2 T S 2 = c \begin{array}{c} \|\beta_{1} \underbrace{\left(\mathbf{v}_{1}^{[\mathbf{d}}-\mathbf{v}_{1}^{[j]}\right)}_{\mathbf{s}_{1}}+\beta_{2} \underbrace{\left(\mathbf{v}_{2}^{[i]}-\mathbf{v}_{2}^{[j]}\right)}_{\mathbf{s}_{2}}\|^{2}=\underbrace{\left\|\mathbf{c}_{i}^{w}-\mathbf{c}_{j}^{u}\right\|^{2}}_{c} \\ \beta_{1}{ }^{2} \mathbf{S}_{1}^{T} \mathbf{S}_{1}+2 \beta_{1} \beta_{2} \mathbf{S}_{1}^{T} \mathbf{S}_{2}+\beta_{2}{ }^{2} \mathbf{S}_{2}{ }^{T} \mathbf{S}_{2}=c \end{array} ∥β1s1 (v1[d−v1[j])+β2s2 (v2[i]−v2[j])∥2=c ∥∥ciw−cju∥∥2β12S1TS1+2β1β2S1TS2+β22S2TS2=c
引入三个中间变量
β 11 = β 1 2 , β 22 = β 22 2 , β 12 = β 1 β 2 \beta_{11}=\beta_{1}^{2}, \beta_{22}=\beta_{22}^{2}, \beta_{12}=\beta_{1} \beta_{2} β11=β12,β22=β222,β12=β1β2
上式就变成了线性方程（中间变量法，可以将非线性方程转换成线性方程解算）。4个控制点可以构造出6个线性方程，组成
L β = ρ \mathbf{L} \boldsymbol{\beta}=\boldsymbol{\rho} Lβ=ρ
其中, β = [ β 11 , β 12 , β 22 ] T , L \boldsymbol{\beta}=\left[\beta_{11}, \beta_{12}, \beta_{22}\right]^{T}, \mathbf{L} β=[β11,β12,β22]T,L 中为 式子左边的系救，大小为 6 × 3 。 ρ \mathbf{6} \times \mathbf{3}。 \boldsymbol{\rho} 6×3。ρ 中为式子右边的系數, 大小为 6 × 1 \mathbf{6} \times \mathbf{1} 6×1。 解出 β \boldsymbol{\beta} β 后, 可以获得两组 β 1 , β 2 \boldsymbol{\beta}_{\mathbf{1}}, \boldsymbol{\beta}_{2} β1,β2的解。再加上一个条件，控制点在摄像机的前端，即 c j c \mathbf{c}^c_j cjc的 z \mathbf{z} z分量要大于0，从而 β 1 , β 2 \boldsymbol{\beta}_{\mathbf{1}}, \boldsymbol{\beta}_{2} β1,β2唯一确定。
Case N = 3 \boldsymbol{N}=\mathbf{3} N=3
与Case N = 2 \boldsymbol{N}=\mathbf{2} N=2 的解法相同, 这里的 β = [ β 11 , β 12 , β 13 , β 22 , β 23 , β 33 ] T , L \boldsymbol{\beta}=\left[\beta_{11}, \beta_{12}, \beta_{13}, \beta_{22}, \beta_{23}, \beta_{33}\right]^{\boldsymbol{T}}, \mathbf{L} β=[β11,β12,β13,β22,β23,β33]T,L的大小为 6 × 6 \mathbf{6} \times \mathbf{6} 6×6。
case N = 4 \boldsymbol{N}=\mathbf{4} N=4
再次使用Case N = 2 \boldsymbol{N}=\mathbf{2} N=2的解法试一下，第 i i i个控制点在相机坐标系下的坐标可表示为：
c i c = β 1 v 1 [ i ] + β 2 v 2 [ i ] + β 3 v 3 [ i ] + β 4 v 4 [ i ] \mathbf{c}_{i}^{c}=\beta_{1} \mathbf{v}_{1}^{[i]}+\beta_{2} \mathbf{v}_{2}^{[i]}+\beta_{3} \mathbf{v}_{3}^{[i]}+\beta_{4} \mathbf{v}_{4}^{[i]} cic=β1v1[i]+β2v2[i]+β3v3[i]+β4v4[i]
将其代入约束等式中
∥ β 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 = ∥ c i c − c j c ∥ 2 \left\|\beta_{1} \mathbf{v}_{1}^{[i]}+\beta_{2} \mathbf{v}_{2}^{[i]}+\beta_{3} \mathbf{v}_{3}^{[i]}+\beta_{4} \mathbf{v}_{4}^{[i]}-\left(\beta_{1} \mathbf{v}_{1}^{[j]}+\beta_{2} \mathbf{v}_{2}^{[j]}+\beta_{3} \mathbf{v}_{3}^{[j]}+\beta_{4} \mathbf{v}_{4}^{[j]}\right)\right\|^{2}=\left\|\mathbf{c}_{i}^{c}-\mathbf{c}_{j}^{c}\right\|^{2} ∥∥∥β1v1[i]+β2v2[i]+β3v3[i]+β4v4[i]−(β1v1[j]+β2v2[j]+β3v3[j]+β4v4[j])∥∥∥2=∥∥cic−cjc∥∥2
这样的约束等式有6条, 将6条等式以矩阵形式表达
L 6 × 10 β 10 × 1 = ρ 6 × 1 \mathbf{L}_{6 \times 10} \boldsymbol{\beta}_{10 \times 1}=\boldsymbol{\rho}_{6 \times 1} L6×10β10×1=ρ6×1
其中 β = [ β 11 , β 12 , β 22 , β 13 , β 23 , β 33 , β 14 , β 24 , β 34 , β 44 ] ⊤ \boldsymbol{\beta}=\left[\beta_{11}, \beta_{12}, \beta_{22}, \beta_{13}, \beta_{23}, \beta_{33}, \beta_{14}, \beta_{24}, \beta_{34}, \beta_{44}\right]^{\top} β=[β11,β12,β22,β13,β23,β33,β14,β24,β34,β44]⊤
β i j = β i β j , L \beta_{i j}=\beta_{i} \beta_{j}, \quad \mathbf{L} βij=βiβj,L 的大小为 6 × 10 6 \times 10 6×10, 将其中一条约束等式展开如下
∥ β 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 \left\|\beta_{1} \mathbf{v}_{1}^{[i]}+\beta_{2} \mathbf{v}_{2}^{[i]}+\beta_{3} \mathbf{v}_{3}^{[i]}+\beta_{4} \mathbf{v}_{4}^{[i]}-\left(\beta_{1} \mathbf{v}_{1}^{[j]}+\beta_{2} \mathbf{v}_{2}^{[j]}+\beta_{3} \mathbf{v}_{3}^{[j]}+\beta_{4} \mathbf{v}_{4}^{[j]}\right)\right\|^{2} ∥∥∥β1v1[i]+β2v2[i]+β3v3[i]+β4v4[i]−(β1v1[j]+β2v2[j]+β3v3[j]+β4v4[j])∥∥∥2
= ∥ β 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 =\|\beta_{1} \underbrace{\left(\mathbf{v}_{1}^{[i]}-\mathbf{v}_{1}^{[j]}\right)}_{\mathbf{s}_{1}}+\beta_{2} \underbrace{\left(\mathbf{v}_{2}^{[i]}-\mathbf{v}_{2}^{[j]}\right)}_{\mathbf{s}_{2}}+\beta_{3} \underbrace{\left(\mathbf{v}_{3}^{[i]}-\mathbf{v}_{3}^{[j]}\right)}_{\mathbf{s}_{3}}+\beta_{4} \underbrace{\left(\mathbf{v}_{4}^{[i]}-\mathbf{v}_{\mathbf{4}}^{[j]}\right)}_{\mathbf{s}_{4}}\|^{2} =∥β1s1 (v1[i]−v1[j])+β2s2 (v2[i]−v2[j])+β3s3 (v3[i]−v3[j])+β4s4 (v4[i]−v4[j])∥2
= β 11 s 1 ⊤ s 1 + 2 β 12 s 1 ⊤ s 2 + β 22 s 2 ⊤ s 2 + 2 β 13 s 1 ⊤ s 3 + 2 β 23 s 2 ⊤ s 3 + β 33 s 3 ⊤ s 3 + =\beta_{11} \mathbf{s}_{1}^{\top} \mathbf{s}_{1}+2 \beta_{12} \mathbf{s}_{1}^{\top} \mathbf{s}_{2}+\beta_{22} \mathbf{s}_{2}^{\top} \mathbf{s}_{2}+2 \beta_{13} \mathbf{s}_{1}^{\top} \mathbf{s}_{3}+2 \beta_{23} \mathbf{s}_{2}^{\top} \mathbf{s}_{3}+\beta_{33} \mathbf{s}_{3}^{\top} \mathbf{s}_{3}+ =β11s1⊤s1+2β12s1⊤s2+β22s2⊤s2+2β13s1⊤s3+2β23s2⊤s3+β33s3⊤s3+
2 β 14 s 1 ⊤ s 4 + 2 β 24 s 2 ⊤ s 4 + 2 β 34 s 3 ⊤ s 4 + β 44 s 4 ⊤ s 4 \mathbf{2} \beta_{14} \mathbf{s}_{1}^{\top} \mathbf{s}_{4}+2 \beta_{24} \mathbf{s}_{2}^{\top} \mathbf{s}_{4}+2 \beta_{34} \mathbf{s}_{3}^{\top} \mathbf{s}_{4}+\beta_{44} \mathbf{s}_{4}^{\top} \mathbf{s}_{4} 2β14s1⊤s4+2β24s2⊤s4+2β34s3⊤s4+β44s4⊤s4
显然，由于未知数 β 10 × 1 \mathbf{\beta}_{10 \times 1} β10×1是10维向量，但是我们只能找到6条约束等式，因此无法得到唯一解。这次，就不能直接使用Case N = 2 , N = 3 \boldsymbol{N}=\mathbf{2},\boldsymbol{N}=\mathbf{3} N=2,N=3的解法了。因为，方程的数量少于待求解变量。看一下中间变量法，引入中间变量的同时，也增加了变量的个数。比如，Case N = 3 \boldsymbol{N}=\mathbf{3} N=3的时候，引入中间变量后，变量个数由3个变成了6个。多出了3个变量 β a b \beta_{ab} βab。原文[1]提到，再引入约束，使用"Reinearization"的方法求解。（好难的~~~看不懂）
b. ORB-SLAM解法(其实ORB-SLAM是搬了OpenCV求解EPnP的代码)
它是用一种近似求解的方法，虽然原理看不懂, 但是实现起来非常简单
近似求解 β 1 , β 2 , β 3 , β 4 \beta_{1}, \beta_{2}, \beta_{3}, \beta_{4} β1,β2,β3,β4, 记 L = [ l 1 , l 2 , l 3 , l 4 , l 5 , l 6 , l 7 , l 8 , l 9 , l 10 ] \mathbf{L}=\left[\mathbf{l}_{1}, \mathbf{l}_{2}, \mathbf{l}_{3}, \mathbf{l}_{4}, \mathbf{l}_{5}, \mathbf{l}_{6}, \mathbf{l}_{7}, \mathbf{l}_{8}, \mathbf{l}_{9}, \mathbf{l}_{10}\right] L=[l1,l2,l3,l4,l5,l6,l7,l8,l9,l10], 也就
是把 L \mathbf{L} L 写成了列向量的形式。
[ l 1 , l 2 , l 3 , l 4 , l 5 , l 6 , l 7 , l 8 , l 9 , l 10 ] [ β 11 β 12 β 22 β 13 β 23 β 33 β 14 β 24 β 34 β 44 ] = ρ \left[\mathbf{l}_{1}, \mathbf{l}_{2}, \mathbf{l}_{3}, \mathbf{l}_{4}, \mathbf{l}_{5}, \mathbf{l}_{6}, \mathbf{l}_{7}, \mathbf{l}_{8}, \mathbf{l}_{9}, \mathbf{l}_{10}\right]\left[\begin{array}{c} \beta_{11} \\ \beta_{12} \\ \beta_{22} \\ \beta_{13} \\ \beta_{23} \\ \beta_{33} \\ \beta_{14} \\ \beta_{24} \\ \beta_{34} \\ \beta_{44} \end{array}\right]=\boldsymbol{\rho} [l1,l2,l3,l4,l5,l6,l7,l8,l9,l10]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡β11β12β22β13β23β33β14β24β34β44⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=ρ
Case N = 4 \boldsymbol{N}=\mathbf{4} N=4
β 11 l 1 + β 12 l 2 + β 13 l 4 + β 14 l 7 = ρ \beta_{11} \mathbf{l}_{1}+\beta_{12} \mathbf{l}_{2}+\beta_{13} \mathbf{l}_{4}+\beta_{14} \mathbf{l}_{7}=\boldsymbol{\rho} β11l1+β12l2+β13l4+β14l7=ρ
Case N = 2 \boldsymbol{N}=\mathbf{2} N=2
β 11 l 1 + β 12 l 2 + β 22 l 3 = ρ \beta_{11} \mathbf{l}_{1}+\beta_{12} \mathbf{l}_{2}+\beta_{22} \mathbf{l}_{3}=\boldsymbol{\rho} β11l1+β12l2+β22l3=ρ
Case N = 3 \boldsymbol{N}=\mathbf{3} N=3
β 11 l 1 + β 12 l 2 + β 22 l 3 + β 13 l 4 + β 23 l 5 = ρ \beta_{11} \mathbf{l}_{1}+\beta_{12} \mathbf{l}_{2}+\beta_{22} \mathbf{l}_{3}+\beta_{13} \mathbf{l}_{4}+\beta_{23} \mathbf{l}_{5}=\rho β11l1+β12l2+β22l3+β13l4+β23l5=ρ
Case N = 1 \boldsymbol{N}=\mathbf{1} N=1
最简单的情况, 跟论文方法一样求闭式解，可以根据以上三种方法求得, β 1 , β 2 , β 3 , β 4 \beta_{1}, \beta_{2}, \beta_{3}, \beta_{4} β1,β2,β3,β4 的初值, 值得一提的是, N = 1 , 2 , 3 \boldsymbol{N}=\mathbf{1}, \mathbf{2}, \mathbf{3} N=1,2,3 的情况中, 剩余的 β \boldsymbol{\beta} β 直接补 0
通过以上任一种方法都能求得 β 1 , β 2 , β 3 , β 4 \beta_1, \beta_2, \beta_3,\beta_4 β1,β2,β3,β4初值，它的数值直接影响到最终求解到的相机位姿的精度，因此我们需要使用高斯牛顿法对其进行进一步优化提高精度：
可以再次利用前面的控制点间距约束等式来获取优化残差项：
展开式子:
β
∗
=
arg
min
β
∑
(
i
,
j
)
,
s.t.
i
<
j
∥
Error
i
,
j
(
β
)
∥
2
Error
i
,
j
(
β
)
=
∥
c
i
c
−
c
j
c
∥
2
⏟
c
c
−
∥
c
i
w
−
c
j
w
∥
2
⏟
c
w
\begin{array}{l} \begin{aligned} \boldsymbol{\beta}^{*}=\underset{\boldsymbol{\beta}}{\arg \min } \sum_{(i, j), \text { s.t. } i<j}\left\|\operatorname{Error}_{i, j}(\boldsymbol{\beta})\right\|^{2} \\ \text { Error }_{i, j}(\boldsymbol{\beta})=\underbrace{\left\|\mathbf{c}_{i}^{c}-\mathbf{c}_{j}^{c}\right\|^{2}}_{c c}-\underbrace{\left\|\mathbf{c}_{i}^{w}-\mathbf{c}_{j}^{w}\right\|^{2}}_{c w} \end{aligned}\\ \end{array}
β∗=βargmin(i,j), s.t. i<j∑∥Errori,j(β)∥2 Error i,j(β)=cc
∥∥cic−cjc∥∥2−cw
∥∥ciw−cjw∥∥2
展开式子：
c
c
=
∥
(
β
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
=
∥
β
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
\begin{aligned} c c &=\left\|\left(\beta_{1} \mathbf{v}_{1}^{[i]}+\beta_{2} \mathbf{v}_{2}^{[i]}+\beta_{3} \mathbf{v}_{3}^{[i]}+\beta_{4} \mathbf{v}_{4}^{[i]}\right)-\left(\beta_{1} \mathbf{v}_{1}^{[j]}+\beta_{2} \mathbf{v}_{2}^{[j]}+\beta_{3} \mathbf{v}_{3}^{[j]}+\beta_{4} \mathbf{v}_{4}^{[j]}\right)\right\|^{2} \\ &=\|\beta_{1} \underbrace{\left(\mathbf{v}_{1}^{[i]}-\mathbf{v}_{1}^{[j]}\right)}_{\mathbf{s}_{1}}+\beta_{2} \underbrace{\left(\mathbf{v}_{2}^{[i]}-\mathbf{v}_{2}^{[j]}\right)}_{\mathbf{S}_{2}}+\beta_{3} \underbrace{\left(\mathbf{v}_{3}^{[i]}-\mathbf{v}_{3}^{[j]}\right)}_{\mathbf{s}_{3}}+\beta_{4} \underbrace{\left(\mathbf{v}_{4}^{[i]}-\mathbf{v}_{4}^{[j]}\right)}_{\mathbf{s}_{4}}\|^{2} \end{aligned}
cc=∥∥∥(β1v1[i]+β2v2[i]+β3v3[i]+β4v4[i])−(β1v1[j]+β2v2[j]+β3v3[j]+β4v4[j])∥∥∥2=∥β1s1
(v1[i]−v1[j])+β2S2
(v2[i]−v2[j])+β3s3
(v3[i]−v3[j])+β4s4
(v4[i]−v4[j])∥2
=
β
1
2
S
1
T
S
1
+
β
1
β
2
S
1
T
S
2
+
β
1
β
3
S
1
T
S
3
+
β
1
β
4
S
1
T
S
4
+
β
1
β
2
S
2
T
S
1
+
β
2
2
S
2
T
S
2
+
β
2
β
3
S
2
T
S
3
+
β
2
β
4
S
2
T
S
4
+
β
1
β
3
S
3
T
S
1
+
β
2
β
3
S
3
T
S
2
+
β
3
2
S
3
T
S
3
+
β
3
β
4
S
3
T
S
4
+
β
1
β
4
S
4
T
S
1
+
β
2
β
4
S
4
T
S
2
+
β
3
β
4
S
4
T
S
3
+
β
4
2
S
4
T
S
4
=
β
1
2
S
1
T
S
1
+
2
β
1
β
2
S
1
T
S
2
+
2
β
1
β
3
S
1
T
S
3
+
2
β
1
β
4
S
1
T
S
4
+
β
2
2
S
2
T
S
2
+
2
β
2
β
3
S
2
T
S
3
+
2
β
2
β
4
S
2
T
S
4
+
β
3
2
S
3
T
S
3
+
2
β
3
β
4
S
3
T
S
4
+
β
4
2
S
4
T
S
4
\begin{array}{l} =\beta_{1}^{2} \mathbf{S}_{1}^{T} \mathbf{S}_{1}+\beta_{1} \beta_{2} \mathbf{S}_{1}^{T} \mathbf{S}_{2}+\beta_{1} \beta_{3} \mathbf{S}_{1}^{T} \mathbf{S}_{3}+\beta_{1} \beta_{4} \mathbf{S}_{1}^{T} \mathbf{S}_{4} \\ +\beta_{1} \beta_{2} \mathbf{S}_{2}^{T} \mathbf{S}_{1}+\beta_{2}{ }^{2} \mathbf{S}_{2}{ }^{T} \mathbf{S}_{2}+\beta_{2} \beta_{3} \mathbf{S}_{2}{ }^{T} \mathbf{S}_{3}+\beta_{2} \beta_{4} \mathbf{S}_{2}{ }^{T} \mathbf{S}_{4} \\ +\beta_{1} \beta_{3} \mathbf{S}_{3}{ }^{T} \mathbf{S}_{1}+\beta_{2} \beta_{3} \mathbf{S}_{3}{ }^{T} \mathbf{S}_{2}+\beta_{3}{ }^{2} \mathbf{S}_{3}{ }^{T} \mathbf{S}_{3}+\beta_{3} \beta_{4} \mathbf{S}_{3}{ }^{T} \mathbf{S}_{4} \\ +\beta_{1} \beta_{4} \mathbf{S}_{4}{ }^{T} \mathbf{S}_{1}+\beta_{2} \beta_{4} \mathbf{S}_{4}{ }^{T} \mathbf{S}_{2}+\beta_{3} \beta_{4} \mathbf{S}_{4}{ }^{T} \mathbf{S}_{3}+\beta_{4}{ }^{2} \mathbf{S}_{4}{ }^{T} \mathbf{S}_{4} \\ =\beta_{1}{ }^{2} \mathbf{S}_{1}{ }^{T} \mathbf{S}_{1}+2 \beta_{1} \beta_{2} \mathbf{S}_{1}{ }^{T} \mathbf{S}_{2}+2 \beta_{1} \beta_{3} \mathbf{S}_{1}{ }^{T} \mathbf{S}_{3}+2 \beta_{1} \beta_{4} \mathbf{S}_{1}^{T} \mathbf{S}_{4} \\ +\beta_{2}{ }^{2} \mathbf{S}_{2}{ }^{T} \mathbf{S}_{2}+2 \beta_{2} \beta_{3} \mathbf{S}_{2}{ }^{T} \mathbf{S}_{3}+2 \beta_{2} \beta_{4} \mathbf{S}_{2}{ }^{T} \mathbf{S}_{4} \\ +\beta_{3}{ }^{2} \mathbf{S}_{3}{ }^{T} \mathbf{S}_{3}+2 \beta_{3} \beta_{4} \mathbf{S}_{3}{ }^{T} \mathbf{S}_{4} \\ +\beta_{4}{ }^{2} \mathbf{S}_{4}{ }^{T} \mathbf{S}_{4} \end{array}
=β12S1TS1+β1β2S1TS2+β1β3S1TS3+β1β4S1TS4+β1β2S2TS1+β22S2TS2+β2β3S2TS3+β2β4S2TS4+β1β3S3TS1+β2β3S3TS2+β32S3TS3+β3β4S3TS4+β1β4S4TS1+β2β4S4TS2+β3β4S4TS3+β42S4TS4=β12S1TS1+2β1β2S1TS2+2β1β3S1TS3+2β1β4S1TS4+β22S2TS2+2β2β3S2TS3+2β2β4S2TS4+β32S3TS3+2β3β4S3TS4+β42S4TS4
利用Gauss-Newton法进行非线性优化时，需要求残差项Error(
β
\beta
β)对于参数项
β
\beta
β的雅克比矩阵：
J
i
,
j
=
∂
[
Error
i
,
j
(
β
)
]
∂
β
=
[
2
β
1
S
1
T
S
1
+
2
β
2
S
1
T
S
2
+
2
β
3
S
1
T
S
3
+
2
β
4
S
1
T
S
4
2
β
1
S
1
T
S
2
+
2
β
2
S
2
T
S
2
+
2
β
3
S
2
T
S
3
+
2
β
4
S
2
T
S
4
2
β
1
S
1
T
S
3
+
2
β
2
S
2
T
S
3
+
2
β
3
S
3
T
S
3
+
2
β
4
S
3
T
S
4
2
β
1
S
1
T
S
4
+
2
β
2
S
2
T
S
4
+
2
β
3
S
3
T
S
4
+
2
β
4
S
4
T
S
4
]
\begin{aligned} \mathbf{J}_{i, j}=& \frac{\partial\left[\operatorname{Error}_{i, j}(\beta)\right]}{\partial \beta} \\ &=\left[\begin{array}{l} 2 \beta_{1} \mathbf{S}_{1}^{T} \mathbf{S}_{1}+2 \beta_{2} \mathbf{S}_{1}^{T} \mathbf{S}_{2}+2 \beta_{3} \mathbf{S}_{1}^{T} \mathbf{S}_{3}+2 \beta_{4} \mathbf{S}_{1}^{T} \mathbf{S}_{4} \\ 2 \beta_{1} \mathbf{S}_{1}{ }^{T} \mathbf{S}_{2}+2 \beta_{2} \mathbf{S}_{2}{ }^{T} \mathbf{S}_{2}+2 \beta_{3} \mathbf{S}_{2}{ }^{T} \mathbf{S}_{3}+2 \beta_{4} \mathbf{S}_{2}{ }^{T} \mathbf{S}_{4} \\ 2 \beta_{1} \mathbf{S}_{1}{ }^{T} \mathbf{S}_{3}+2 \beta_{2} \mathbf{S}_{2}{ }^{T} \mathbf{S}_{3}+2 \beta_{3} \mathbf{S}_{3}{ }^{T} \mathbf{S}_{3}+2 \beta_{4} \mathbf{S}_{3}{ }^{T} \mathbf{S}_{4} \\ 2 \beta_{1} \mathbf{S}_{1}{ }^{T} \mathbf{S}_{4}+2 \beta_{2} \mathbf{S}_{2}{ }^{T} \mathbf{S}_{4}+2 \beta_{3} \mathbf{S}_{3}{ }^{T} \mathbf{S}_{4}+2 \beta_{4} \mathbf{S}_{4}{ }^{T} \mathbf{S}_{4} \end{array}\right] \end{aligned}
Ji,j=∂β∂[Errori,j(β)]=⎣⎢⎢⎡2β1S1TS1+2β2S1TS2+2β3S1TS3+2β4S1TS42β1S1TS2+2β2S2TS2+2β3S2TS3+2β4S2TS42β1S1TS3+2β2S2TS3+2β3S3TS3+2β4S3TS42β1S1TS4+2β2S2TS4+2β3S3TS4+2β4S4TS4⎦⎥⎥⎤
将六个小雅克比
J
i
,
j
\boldsymbol{J}_{i, j}
Ji,j 合成为
6
×
4
\mathbf{6} \times \mathbf{4}
6×4 的大雅克比
J
=
[
J
1
,
2
J
1
,
3
J
1
,
4
J
2
,
3
J
2
,
4
J
3
,
4
]
\mathbf{J}=\left[\begin{array}{l} \mathbf{J}_{1,2} \\ \mathbf{J}_{1,3} \\ \mathbf{J}_{1,4} \\ \mathbf{J}_{2,3} \\ \mathbf{J}_{2,4} \\ \mathbf{J}_{3,4} \end{array}\right]
J=⎣⎢⎢⎢⎢⎢⎢⎡J1,2J1,3J1,4J2,3J2,4J3,4⎦⎥⎥⎥⎥⎥⎥⎤
记残差为
r
=
[
Error
1
,
2
(
β
)
Error
1
,
3
(
β
)
Error
1
,
4
(
β
)
Error
2
,
3
(
β
)
Error
2
,
4
(
β
)
Error
3
,
4
(
β
)
]
\mathbf{r}=\left[\begin{array}{c} \operatorname{Error}_{1,2}(\boldsymbol{\beta}) \\ \operatorname{Error}_{1,3}(\boldsymbol{\beta}) \\ \operatorname{Error}_{1,4}(\boldsymbol{\beta}) \\ \operatorname{Error}_{2,3}(\boldsymbol{\beta}) \\ \operatorname{Error}_{2,4}(\boldsymbol{\beta}) \\ \operatorname{Error}_{3,4}(\boldsymbol{\beta}) \end{array}\right]
r=⎣⎢⎢⎢⎢⎢⎢⎡Error1,2(β)Error1,3(β)Error1,4(β)Error2,3(β)Error2,4(β)Error3,4(β)⎦⎥⎥⎥⎥⎥⎥⎤
那么，增量方程为
J
T
J
δ
β
=
−
J
T
r
(
22
)
\mathbf{J}^{T} \mathbf{J} \delta \beta=-\mathbf{J}^{T} \mathbf{r}(\mathbf{2} 2)
JTJδβ=−JTr(22)
使用QR分解求解线性方程组，得到增量
δ
β
\delta \beta
δβ，更新方程
β
=
β
+
δ
β
(
23
)
\boldsymbol{\beta}=\boldsymbol{\beta}+\boldsymbol{\delta} \boldsymbol{\beta}(\mathbf{2 3})
β=β+δβ(23)
(4)计算相机位姿
- 计算控制点在相机坐标系下的坐标 ：
c i c = ∑ j = 1 N β k v k [ i ] , i = 1 , 2 , 3 , 4 \mathbf{c}_{i}^{c}=\sum_{j=1}^{N} \beta_{k} \mathbf{v}_{k}^{[i]}, i=1,2,3,4 cic=j=1∑Nβkvk[i],i=1,2,3,4 - 计算3D参考点在相机参考坐标系的坐标。
p i c = ∑ j = 1 4 α i j c j c \mathbf{p}_{i}^{c}=\sum_{j=1}^{4} \alpha_{i j} \mathbf{c}_{j}^{c} pic=j=1∑4αijcjc - 计算
{
p
i
w
}
i
=
1
,
2
,
…
,
n
\left\{\mathbf{p}_{i}^{w}\right\}_{i=1,2, \ldots, n}
{piw}i=1,2,…,n 的重心
p
0
w
\mathbf{p}_{0}^{w}
p0w 和矩阵
A
\mathbf{A}
A
p 0 w = 1 n ∑ i = 1 n p i w A = [ ( p 1 w − p 0 w ) ⊤ ( p 2 w − p 0 w ) ⊤ ⋮ ( p n w − p 0 w ) ⊤ ] \begin{array}{c} \mathbf{p}_{0}^{w}=\frac{1}{n} \sum_{i=1}^{n} \mathbf{p}_{i}^{w} \\ \mathbf{A}=\left[\begin{array}{c} \left(\mathbf{p}_{1}^{w}-\mathbf{p}_{0}^{w}\right)^{\top} \\ \left(\mathbf{p}_{2}^{w}-\mathbf{p}_{0}^{w}\right)^{\top} \\ \vdots \\ \left(\mathbf{p}_{n}^{w}-\mathbf{p}_{0}^{w}\right)^{\top} \end{array}\right] \end{array} p0w=n1∑i=1npiwA=⎣⎢⎢⎢⎢⎡(p1w−p0w)⊤(p2w−p0w)⊤⋮(pnw−p0w)⊤⎦⎥⎥⎥⎥⎤ - 计算
{
p
i
c
}
i
=
1
,
2
,
…
,
n
\left\{\mathbf{p}_{i}^{c}\right\}_{i=1,2, \ldots, n}
{pic}i=1,2,…,n 的重心
p
0
c
\mathbf{p}_{0}^{c}
p0c 和矩阵
B
\mathbf{B}
B.
p 0 c = 1 n ∑ i = 1 n p i c \mathbf{p}_{0}^{c}=\frac{1}{n} \sum_{i=1}^{n} \mathbf{p}_{i}^{c} p0c=n1i=1∑npic
B = [ ( p 1 c − p 0 c ) ⊤ ( p 2 c − p 0 c ) ⊤ ⋮ ( p n c − p 0 c ) ⊤ ] \mathbf{B}=\left[\begin{array}{c} \left(\mathbf{p}_{1}^{c}-\mathbf{p}_{0}^{c}\right)^{\top} \\ \left(\mathbf{p}_{2}^{c}-\mathbf{p}_{0}^{c}\right)^{\top} \\ \vdots \\ \left(\mathbf{p}_{n}^{c}-\mathbf{p}_{0}^{c}\right)^{\top} \end{array}\right] B=⎣⎢⎢⎢⎢⎡(p1c−p0c)⊤(p2c−p0c)⊤⋮(pnc−p0c)⊤⎦⎥⎥⎥⎥⎤ - 计算矩阵
H = B ⊤ A \mathbf{H}=\mathbf{B}^{\top} \mathbf{A} H=B⊤A - 计算
H
\mathbf{H}
H 的SVD分解:
H = U Σ V ⊤ \mathbf{H}=\mathbf{U} \mathbf{\Sigma} \mathbf{V}^{\top} H=UΣV⊤ - 计算旋转
R
\mathbf{R}
R
R = U V ⊤ \mathbf{R}=\mathbf{U} \mathbf{V}^{\top} R=UV⊤
如果 ∣ R ∣ < 0 |\mathbf{R}|<\mathbf{0} ∣R∣<0, 那么 R ( 2 , : ) = − R ( 2 , : ) \mathbf{R}(2,:)=-\mathbf{R}(\mathbf{2},:) R(2,:)=−R(2,:) - 计算位姿中的平移
t
\mathbf{t}
t
t = p 0 c − R p 0 w \mathbf{t}=\mathbf{p}_{0}^{c}-\mathbf{R} \mathbf{p}_{0}^{w} t=p0c−Rp0w
在ORB-SLAM2方案中，会在当前帧中随机选取多组观测点（每组4个点），将其放入EPnP求解器中求解出相机当前在世界坐标系下的位姿，然后通过RANSAC算法选取最优的一组位姿，并且剔除外点，具体请参考ORB-SLAM源代码.
扫一扫
2387
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
