讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196
文末正下方中心提供了本人
联系方式，
点击本人照片即可显示
W
X
→
官方认证
{\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}
文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证
一、前言
通过前面的博客，我们可以知道如下两个公式(如果为同一相机,则其相机内参
K
1
=
K
2
\mathbf K_1=\mathbf K_2
K1=K2):
E
=
t
∧
R
=
t
×
R
p
0
T
E
p
1
=
0
(01)
\tag{01} \color{blue} \mathbf E=\mathbf t ^{\wedge} \mathbf R=\mathbf t\times \mathbf R~~~~~~~~~\color{blue} p_0^T \mathbf Ep_1=0
E=t∧R=t×R p0TEp1=0(01)
F
=
(
K
0
−
T
E
K
1
−
1
)
v
o
T
F
v
1
=
0
(02)
\tag{02} \color{blue} \mathbf F=(\mathbf K_0^{-T} \mathbf E\mathbf K_1^{-1}) ~~~~~~~~~~ \color{blue} v_o^T\mathbf Fv_1=0
F=(K0−TEK1−1) voTFv1=0(02)其上的
p
0
,
p
1
p_0,p_1
p0,p1 是图像坐标，
v
0
,
v
1
v_0,v_1
v0,v1 是像素坐标;
E
\mathbf E
E 表示本质矩阵 Essential,
F
\mathbf F
F 表示基本矩阵 Fundamental。一般在程序的运行过程中，相机内参是移植的，另外在 ORB-SLAM2 源码中
K
1
=
K
2
\mathbf K_1=\mathbf K_2
K1=K2，因为是同一相机。也就是已知相机内参的情况下，一般会先计算出本质矩阵，然后通过本质矩阵求解
R
t
\mathbf R\mathbf t
Rt。求解本质矩阵的公式如下(同一相机)：
E
=
K
T
F
K
(03)
\tag{03} \color{blue} \mathbf E= \mathbf K^T \mathbf F \mathbf K
E=KTFK(03)
二、反对称矩阵性质
再进行公式推导之前，我们需要了解一下反对称矩阵的一些性质(这里只列举部分→与后续公式推导相关的)。
性质 ( 1 ) : \color{red}{性质(1):} 性质(1): 当 A \mathbf A A为 n n n 实反对称矩阵时，那么存在 X T A X = 0 \mathbf{ X^TAX=0} XTAX=0, 其中 X \mathbf X X 为 n n n 阶矩阵( ∀ X ∈ R n \forall \mathbf X \in R^{n} ∀X∈Rn)。
性质 ( 2 ) : \color{red}{性质(2):} 性质(2): 实反对称矩阵的特征值为 0 或者虚数。
性质
(
3
)
:
\color{red}{性质(3):}
性质(3): 若
A
\mathbf A
A为
n
n
n 阶实反对称矩阵，则存在矩阵
n
n
n 阶正交矩阵
Γ
\mathbf \Gamma
Γ 使得：
Γ
T
A
Γ
=
[
0
m
1
−
m
1
0
⋱
0
m
r
−
m
r
0
⋱
0
m
n
−
m
n
0
]
,
\mathbf \Gamma^T \mathbf A \mathbf \Gamma=\left[\begin{array}{cccccccc} 0 & m_{1} & & & & & & \\ -m_{1} & 0 & & & & & & \\ & & \ddots & & & & & \\ & & & 0 & m_{r} & & & \\ & & & -m_{r} & 0 & & & \\ & & & & & \ddots & & \\ & & & & & & 0 & m_{n} \\ & & & & & & -m_{n} & 0 \end{array}\right],
ΓTAΓ=⎣
⎡0−m1m10⋱0−mrmr0⋱0−mnmn0⎦
⎤, 上式中
±
b
i
\pm b_i
±bi 是
A
\mathbf A
A 的全部非零特征值，
i
∈
{
1
,
2
,
3
⋯
}
i \in \{1,2,3 \cdots \}
i∈{1,2,3⋯}。
A
\mathbf A
A 的秩
R
(
A
)
=
2
r
R(\mathbf A)=2r
R(A)=2r。其上矩阵
Γ
T
A
Γ
\mathbf \Gamma^T \mathbf A \mathbf \Gamma
ΓTAΓ 称为实反对称矩阵
A
\mathbf A
A 在正交相似下的标准型。
性质 ( 4 ) : \color{red}{性质(4):} 性质(4):
三、基础推导
首先这里我们作如下假设(偏移矩阵与旋转矩阵):
t
=
(
t
1
,
t
2
,
t
3
)
R
=
(
r
1
,
r
2
,
r
3
)
(04)
\tag{04} \color{blue} \mathbf t=(t_1,t_2,t_3)~~~~~~~~~~~~~\mathbf R=(\mathbf r_1,\mathbf r_2,\mathbf r_3)
t=(t1,t2,t3) R=(r1,r2,r3)(04)那么我们根据公式(1)可得：
E
=
t
∧
R
=
(
t
∧
r
1
,
t
∧
r
2
,
t
∧
r
3
)
(05)
\tag{05} \color{blue} \mathbf E=\mathbf t^{\wedge} \mathbf R=\mathbf(\mathbf t^{\wedge} \mathbf r_1,\mathbf t^{\wedge}\mathbf r_2,\mathbf t^{\wedge}\mathbf r_3)
E=t∧R=(t∧r1,t∧r2,t∧r3)(05)根据前面博客的推论，我们知道
,
E
,\mathbf E
,E 的奇异矩阵必是
d
i
a
g
(
σ
1
,
σ
2
,
0
)
=
(
t
1
2
+
t
2
2
+
t
3
2
,
t
1
2
+
t
2
2
+
t
3
2
,
0
)
diag(\sigma_{1}, \sigma_{2}, 0)=(t_{1}^{2}+t_{2}^{2}+t_{3}^{2}, t_{1}^{2}+t_{2}^{2}+t_{3}^{2}, 0)
diag(σ1,σ2,0)=(t12+t22+t32,t12+t22+t32,0)。另外根据反对真矩阵的性质: 反对称矩阵矩阵一定正交于
d
i
a
g
(
m
1
D
,
m
2
D
,
m
3
D
,
0
,
0
,
⋯
0
)
diag(m_1D,m_2D,m_3D,0,0,\cdots0)
diag(m1D,m2D,m3D,0,0,⋯0),
D
D
D 的注解如下：
D
=
(
0
1
−
1
0
)
,
m
i
≠
0
,
i
=
1
,
2
,
3
⋯
(06)
\tag{06} \color{blue} \mathbf D=\left(\begin{array}{cc} 0 & 1 \\ -1 & 0 \end{array}\right), m_{i} \neq 0, i=1, 2, 3\cdots
D=(0−110),mi=0,i=1,2,3⋯(06)根据反对称 性质(3) ，然后等式的两边都进行转置:
t
∧
=
Γ
N
Γ
T
\mathbf t^{\wedge}=\mathbf {\Gamma N\Gamma}^T
t∧=ΓNΓT，这里的
Γ
\mathbf \Gamma
Γ 为正交矩阵，
N
\mathbf N
N的格式如下：
N
=
[
0
−
m
0
m
0
0
0
0
0
]
(07)
\tag{07} \color{blue} \mathbf N=\left[\begin{array}{ccc} 0 & -m & 0 \\ m & 0 & 0 \\ 0 & 0 & 0 \end{array}\right]
N=⎣
⎡0m0−m00000⎦
⎤(07)把
t
∧
=
Γ
N
Γ
T
\mathbf t^{\wedge}=\mathbf \Gamma \mathbf N\mathbf \Gamma^T
t∧=ΓNΓT 代入
E
=
t
∧
R
\mathbf E =\mathbf t^{\wedge} \mathbf R
E=t∧R 可得：
E
=
t
∧
R
=
Γ
N
Γ
T
R
(08)
\tag{08} \color{blue} \mathbf E =\mathbf t^{\wedge} \mathbf R=\mathbf {\Gamma N \Gamma}^T\mathbf R
E=t∧R=ΓNΓTR(08)
E
T
E
=
R
T
Γ
N
T
N
Γ
T
R
(09)
\tag{09} \color{blue} \mathbf {E}^T{\mathbf E}=\mathbf {R}^T \mathbf \Gamma \mathbf N^T\mathbf N\mathbf \Gamma^T\mathbf R
ETE=RTΓNTNΓTR(09)又:
N
T
N
=
d
i
a
g
(
m
2
,
m
2
,
0
)
(10)
\tag{10} \color{blue} \mathbf N^T\mathbf N=diag(m^2,m^2,0)
NTN=diag(m2,m2,0)(10)这个时候可以很明显的看到
d
i
a
g
(
m
2
,
m
2
,
0
)
diag(m^2,m^2,0)
diag(m2,m2,0) 与奇异值矩阵是十分相似的，根据奇异值分解的原理
A
=
U
Σ
V
T
\mathbf A=\mathbf U \mathbf \Sigma \mathbf V^{T}
A=UΣVT，如果
A
T
A
=
V
Σ
2
V
T
\mathbf A^T\mathbf A=\mathbf V \mathbf \Sigma^2 \mathbf V^{T}
ATA=VΣ2VT，特征向量组成
V
\mathbf V
V矩阵； 如果
A
A
T
=
U
Σ
2
U
T
\mathbf A\mathbf A^T=\mathbf U\mathbf \Sigma^2 \mathbf U^{T}
AAT=UΣ2UT，特征向量组成
U
\mathbf U
U 矩阵。那么我们现在对本质
E
T
E
\mathbf E^T\mathbf E
ETE 进行特征值分解：
E
T
E
=
V
Σ
2
V
T
=
R
T
Γ
N
T
N
Γ
T
R
=
（
R
T
Γ
)
(
N
T
N
)
(
Γ
T
R
)
(11)
\tag{11} \color{blue} \mathbf E^T\mathbf E=\mathbf V \mathbf \Sigma^2 \mathbf V^{T}=\mathbf {R}^T \mathbf \Gamma \mathbf N^T\mathbf N\mathbf \Gamma^T\mathbf R=（\mathbf {R}^T \mathbf \Gamma) (\mathbf N^T\mathbf N) (\mathbf \Gamma^T\mathbf R)
ETE=VΣ2VT=RTΓNTNΓTR=（RTΓ)(NTN)(ΓTR)(11)可以知道
E
T
E
\mathbf E^T\mathbf E
ETE 的特征值为
N
T
N
=
d
i
a
g
(
m
2
,
m
2
,
0
)
\mathbf N^T\mathbf N=diag(m^2,m^2,0)
NTN=diag(m2,m2,0), 其对应的特征向量为
V
=
R
T
Γ
=
(
V
1
,
V
2
,
V
3
)
\mathbf V=\mathbf R^T \mathbf \Gamma=(V_1,V_2,V_3)
V=RTΓ=(V1,V2,V3)。这里的
V
\mathbf V
V 也是。这样我们得到
Σ
=
d
i
a
g
(
∣
m
∣
,
∣
m
∣
,
0
)
(12)
\tag{12} \color{blue} \mathbf \Sigma=diag(|m|,|m|,0)
Σ=diag(∣m∣,∣m∣,0)(12)
N
T
N
=
Σ
2
(13)
\tag{13} \color{blue} \mathbf N^T\mathbf N= \mathbf \Sigma^2
NTN=Σ2(13)
对于(07)式子,其可以写成
N
=
W
Σ
\mathbf N=\mathbf W \mathbf \Sigma
N=WΣ:(这里的
Σ
\mathbf \Sigma
Σ 为正交矩阵)
N
=
W
Σ
=
[
0
−
1
0
1
0
0
0
0
1
]
[
m
0
0
0
m
0
0
0
0
]
(14)
\tag{14} \color{blue} \mathbf N=\mathbf W \mathbf \Sigma=\left[\begin{array}{ccc} 0 & - 1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{ccc} m & 0 & 0 \\ 0 & m & 0 \\ 0 & 0 & 0 \end{array}\right]
N=WΣ=⎣
⎡010−100001⎦
⎤⎣
⎡m000m0000⎦
⎤(14)
注意
:
\color{red}{注意:}
注意:其上的
Σ
\mathbf \Sigma
Σ 为正交矩阵，那么他的逆就等于他的转置。后续会利用到这个性质。另外，可以发现，上式还可以写成:
N
=
−
W
T
Σ
=
[
0
1
0
−
1
0
0
0
0
−
1
]
[
m
0
0
0
m
0
0
0
0
]
(15)
\tag{15} \color{blue} \mathbf N=-\mathbf W^T \mathbf \Sigma=\left[\begin{array}{ccc} 0 & 1 & 0\\ -1 & 0 & 0 \\ 0 & 0 & -1 \end{array}\right]\left[\begin{array}{ccc} m & 0 & 0 \\ 0 & m & 0 \\ 0 & 0 & 0 \end{array}\right]
N=−WTΣ=⎣
⎡0−1010000−1⎦
⎤⎣
⎡m000m0000⎦
⎤(15)那么我们就要就要注意了，后续再计算
R
t
\mathbf R \mathbf t
Rt的时候，其结果是否受
W
\mathbf W
W 影响，如果与其相关，则我们需要分情况进行讨论。另外，需要注意到，
Σ
=
d
i
a
g
(
∣
m
∣
,
∣
m
∣
,
0
)
\mathbf \Sigma=diag(|m|,|m|,0)
Σ=diag(∣m∣,∣m∣,0) 因为奇异值是大于0的。但是 m 的正负值是不确定的，所以我们需要分两种情况来进行讨论，即 m>0 与 m0)
情形一: 以(14)式为基准(
N
=
W
Σ
\mathbf N= \mathbf W \mathbf \Sigma
N=WΣ)
根据前面的推导
N
=
W
Σ
\mathbf N= \mathbf W \mathbf \Sigma
N=WΣ。结合我们前面的
V
=
R
T
Γ
=
(
V
1
,
V
2
,
V
3
)
\mathbf V=\mathbf R^T \mathbf \Gamma=(V_1,V_2,V_3)
V=RTΓ=(V1,V2,V3), 以及 (08) 式可以得出：
E
V
=
Γ
N
Γ
T
R
V
=
Γ
N
Γ
T
R
R
T
Γ
=
Γ
N
=
Γ
W
Σ
(16)
\tag{16} \color{blue} \mathbf E \mathbf V=\mathbf {\Gamma N \Gamma}^T\mathbf R \mathbf V= \mathbf {\Gamma N \Gamma}^T\mathbf R \mathbf R^T \mathbf \Gamma=\mathbf \Gamma \mathbf N=\mathbf \Gamma \mathbf {W \Sigma}
EV=ΓNΓTRV=ΓNΓTRRTΓ=ΓN=ΓWΣ(16)另外结合
E
=
U
Σ
V
T
\mathbf E=\mathbf U \mathbf \Sigma \mathbf V^{T}
E=UΣVT,两边同时乘
V
\mathbf V
V, 可以得到
E
V
=
U
Σ
\mathbf E \mathbf V= \mathbf U \Sigma
EV=UΣ。那么结合(16)式可以得到：
E
V
=
U
Σ
=
Γ
N
=
Γ
W
Σ
(17)
\tag{17} \color{blue} \mathbf E \mathbf V= \mathbf U \Sigma=\mathbf \Gamma \mathbf N=\mathbf \Gamma \mathbf {W \Sigma}
EV=UΣ=ΓN=ΓWΣ(17)
U
=
Γ
W
⟹
Γ
=
U
W
T
\color{blue} \mathbf U=\mathbf \Gamma \mathbf W~~\Longrightarrow ~~\mathbf \Gamma = \mathbf U\mathbf W^T
U=ΓW ⟹ Γ=UWT 前面通过(11)式得到
V
=
R
T
Γ
\mathbf V=\mathbf R^T \mathbf \Gamma
V=RTΓ，结合上式可以得到如下结论(
W
−
1
=
W
T
\mathbf W^{-1}=\mathbf W^T
W−1=WT，正交矩阵性质)：
R
=
Γ
V
T
=
U
W
T
V
T
(18)
\tag{18} \color{blue} \mathbf R= \mathbf \Gamma \mathbf V^T=\mathbf U\mathbf W^T\mathbf V^T
R=ΓVT=UWTVT(18)
我们前面已经假设
t
∧
=
Γ
N
Γ
T
\mathbf t^{\wedge}=\mathbf {\Gamma N\Gamma}^T
t∧=ΓNΓT，以及(17)式中
U
=
Γ
W
\mathbf U=\mathbf \Gamma \mathbf W
U=ΓW, 那么进一步推导可以的获得:
t
∧
=
Γ
N
Γ
T
=
U
W
T
N
W
U
T
=
U
W
T
W
Σ
W
U
T
=
U
Σ
W
U
T
(19)
\tag{19} \color{blue} \mathbf t^{\wedge}=\mathbf {\Gamma N\Gamma}^T=\mathbf U \mathbf W^T \mathbf N \mathbf WU^T=\mathbf U \mathbf W^T \mathbf W \mathbf \Sigma \mathbf WU^T=\mathbf U \mathbf \Sigma \mathbf WU^T
t∧=ΓNΓT=UWTNWUT=UWTWΣWUT=UΣWUT(19)
情形二: 以(15)式为基准(
N
=
−
W
T
Σ
\mathbf N= -\mathbf W^T \mathbf \Sigma
N=−WTΣ)
根据上面的推导，我们知道情形一与情形二他们主要在于
N
\mathbf N
N 的区别，进一步其实是在于
W
\mathbf W
W 与
−
W
T
-\mathbf W^T
−WT 的区别，所以只需要把情形一中的
W
\mathbf W
W 替换成
−
W
T
-\mathbf W^T
−WT 即可，故:
R
=
U
W
T
V
T
=
U
(
−
W
T
)
T
V
T
=
−
U
W
V
T
(20)
\tag{20} \color{blue} \mathbf R= \mathbf U\mathbf W^T\mathbf V^T= \mathbf U (-\mathbf W^T)^T\mathbf V^T= -\mathbf U \mathbf W \mathbf V^T
R=UWTVT=U(−WT)TVT=−UWVT(20)
t
∧
=
U
Σ
W
U
T
=
U
Σ
(
−
W
T
)
U
T
=
−
U
Σ
W
T
U
T
\color{blue} \mathbf t^{\wedge}=\mathbf U \mathbf \Sigma \mathbf W\mathbf U^T=\mathbf U \mathbf \Sigma (-\mathbf W^T)\mathbf U^T=-\mathbf U \mathbf \Sigma \mathbf W^T\mathbf U^T
t∧=UΣWUT=UΣ(−WT)UT=−UΣWTUT
五、情况一(m0)中情形二，也就是式(20)结果中的
t
∧
\mathbf t^{\wedge}
t∧ 变号即可。如下：
R
=
−
U
W
V
T
(25)
\tag{25} \color{blue} \mathbf R= -\mathbf U \mathbf W \mathbf V^T
R=−UWVT(25)
t
∧
=
U
Σ
W
T
U
T
\color{blue} \mathbf t^{\wedge}=\mathbf U \mathbf \Sigma \mathbf W^T\mathbf U^T
t∧=UΣWTUT
六、求解偏移矩阵t
根据上述的推导，我们共获得了4组解，从解可以看出，只需要对 E \mathbf E E 做奇异值分解，即可获得 U , Σ , V \mathbf U, \mathbf \Sigma, \mathbf V U,Σ,V 从而轻松求解出 R \mathbf R R 以及 , t ∧ \mathbf t^{\wedge} t∧。由于本质矩阵 E \mathbf E E 具备尺度等价性质，那么我们先对 E \mathbf E E 进行特征值分解，分解之后获得 U , Σ , V \mathbf U, \mathbf \Sigma, \mathbf V U,Σ,V， 然后令其中的 Σ = d i a g ( 1 , 1 , 0 ) \mathbf \Sigma=diag(1,1,0) Σ=diag(1,1,0) 再重新与 U , V \mathbf U,\mathbf V U,V 组合，根据前面的公式求解 R \mathbf R R 以及 , t ∧ \mathbf t^{\wedge} t∧。
那么现在我们就需要根据
t
∧
\mathbf t^{\wedge}
t∧ 求解出
t
\mathbf t
t，进一步分析，我们以(19)式中的
t
∧
=
U
Σ
W
U
T
\mathbf t^{\wedge}=\mathbf U \mathbf \Sigma \mathbf W\mathbf U^T
t∧=UΣWUT 为例进行分析，其中的
U
=
(
u
1
,
u
2
,
u
3
)
\mathbf U=(u_1,u_2,u_3)
U=(u1,u2,u3)。那么:
t
∧
=
[
u
⃗
1
,
u
⃗
2
,
u
⃗
3
]
[
1
1
0
]
[
0
−
1
0
1
0
0
0
0
1
]
[
u
1
T
u
2
T
u
3
T
]
=
u
2
u
1
T
−
u
1
u
2
T
(26)
\tag{26} \color{blue} \mathbf {t}^{\wedge}=\left[\vec{u}_{1}, \vec{u}_{2}, \vec{u}_{3}\right]\left[\begin{array}{ccc} 1 & \\ & 1 & \\ & & 0 \end{array}\right]\left[\begin{array}{ccc} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{l} {u}_{1}^T \\ {u}_{2}^T \\ {u}_{3}^T \end{array}\right]=u_2u_1^T-u_1u_2^T
t∧=[u
1,u
2,u
3]⎣
⎡110⎦
⎤⎣
⎡010−100001⎦
⎤⎣
⎡u1Tu2Tu3T⎦
⎤=u2u1T−u1u2T(26)令
u
1
,
u
2
,
u
3
u_1,u_2,u_3
u1,u2,u3 构成右手坐标系，即
d
e
t
(
U
)
=
1
det(U)=1
det(U)=1，又如果 x,y向量正交，则x y内积为0，即x的转置乘y为0，根据该性质我们可以得出:
t
∧
u
1
=
u
2
t
∧
u
2
=
−
u
1
t
∧
u
3
=
0
(27)
\tag{27} \color{blue} \mathbf t^{\wedge}u_1=u_2~~~~~~~\mathbf t^{\wedge}u_2=-u_1~~~~~~~\mathbf t^{\wedge}u_3=0
t∧u1=u2 t∧u2=−u1 t∧u3=0(27)
然后再根据性质 a,b向量叉乘，获得垂直与a,b的新向量，进行如下推导：
{
(
t
−
u
3
)
∧
u
1
=
t
∧
u
1
+
u
3
∧
u
1
=
u
2
−
u
3
×
u
1
=
u
2
−
u
2
=
0
(
t
−
u
3
)
∧
u
2
=
t
∧
u
2
+
u
3
∧
u
2
=
−
u
1
+
u
3
×
u
2
=
−
u
1
+
u
1
=
0
(
t
−
u
3
)
∧
u
3
=
t
∧
u
3
+
u
3
∧
u
3
=
0
−
u
3
×
u
3
=
0
−
0
=
0
(28)
\tag{28} \color{blue} \left\{\begin{array}{l} (\mathbf t-u_3)^{\wedge}u_1=\mathbf t^{\wedge}u_1+u_3^{\wedge}u_1=u_2-u_3\times u_1=u_2-u_2=\mathbf0\\ (\mathbf t-u_3)^{\wedge}u_2=\mathbf t^{\wedge}u_2+u_3^{\wedge}u_2=-u_1+u_3\times u_2=-u_1+u_1=\mathbf0\\ (\mathbf t-u_3)^{\wedge}u_3=\mathbf t^{\wedge}u_3+u_3^{\wedge}u_3=\mathbf0-u_3\times u_3=\mathbf0-\mathbf0=\mathbf0\\ \end{array}\right.
⎩
⎨
⎧(t−u3)∧u1=t∧u1+u3∧u1=u2−u3×u1=u2−u2=0(t−u3)∧u2=t∧u2+u3∧u2=−u1+u3×u2=−u1+u1=0(t−u3)∧u3=t∧u3+u3∧u3=0−u3×u3=0−0=0(28)
又因为
u
1
,
u
2
,
u
3
u_1,u_2,u_3
u1,u2,u3 为非零列列向量，故只有
t
−
u
3
=
0
\mathbf t-u_3=0
t−u3=0 时，才能满足上述要求，所以
t
=
u
3
\mathbf t=u_3
t=u3。由此可见，
t
\mathbf t
t 为
E
E
E 左奇异值矩阵的最后一列。
七、源码注释
其代码的主调函数为 Initializer.cc文件中的 Initializer::ReconstructF() 函数，该函数调用 DecomposeE(E21,R1,R2,t) 函数，该为核心部分，展示如下：
/**
* @brief 分解Essential矩阵得到R,t
* 分解E矩阵将得到4组解，这4组解分别为[R1,t],[R1,-t],[R2,t],[R2,-t]
* 参考：Multiple View Geometry in Computer Vision - Result 9.19 p259
* @param[in] E 本质矩阵
* @param[in & out] R1 旋转矩阵1
* @param[in & out] R2 旋转矩阵2
* @param[in & out] t 平移向量，另外一个取相反数
*/
void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)
{
// 对本质矩阵进行奇异值分解
//准备存储对本质矩阵进行奇异值分解的结果
cv::Mat u,w,vt;
//对本质矩阵进行奇异值分解
cv::SVD::compute(E,w,u,vt);
// 左奇异值矩阵U的最后一列就是t，对其进行归一化
u.col(2).copyTo(t);
t=t/cv::norm(t);
// 构造一个绕Z轴旋转pi/2的旋转矩阵W，按照下式组合得到旋转矩阵 R1 = u*W*vt
//计算完成后要检查一下旋转矩阵行列式的数值，使其满足行列式为1的约束
cv::Mat W(3,3,CV_32F,cv::Scalar(0));
W.at(0,1)=-1;
W.at(1,0)=1;
W.at(2,2)=1;
//计算
R1 = u*W*vt;
//旋转矩阵有行列式为+1的约束，所以如果算出来为负值，需要取反
if(cv::determinant(R1)0.7*maxGood)
nsimilar++;
if(nGood2>0.7*maxGood)
nsimilar++;
if(nGood3>0.7*maxGood)
nsimilar++;
if(nGood4>0.7*maxGood)
nsimilar++;
// Step 4.4 四个结果中如果没有明显的最优结果，或者没有足够数量的三角化点，则返回失败
// 条件1: 如果四组解能够重建的最多3D点个数小于所要求的最少3D点个数（mMinGood），失败
// 条件2: 如果存在两组及以上的解能三角化出 >0.7*maxGood的点，说明没有明显最优结果，失败
if(maxGood1)
{
return false;
}
// Step 4.5 选择最佳解记录结果
// 条件1: 有效重建最多的3D点，即maxGood == nGoodx，也即是位于相机前方的3D点个数最多
// 条件2: 三角化视差角 parallax 必须大于最小视差角 minParallax，角度越大3D点越稳定
//看看最好的good点是在哪种解的条件下发生的
if(maxGood==nGood1)
{
//如果该种解下的parallax大于函数参数中给定的最小值
if(parallax1>minParallax)
{
// 存储3D坐标
vP3D = vP3D1;
// 获取特征点向量的三角化测量标记
vbTriangulated = vbTriangulated1;
// 存储相机姿态
R1.copyTo(R21);
t1.copyTo(t21);
// 结束
return true;
}
}else if(maxGood==nGood2)
{
if(parallax2>minParallax)
{
vP3D = vP3D2;
vbTriangulated = vbTriangulated2;
R2.copyTo(R21);
t1.copyTo(t21);
return true;
}
}else if(maxGood==nGood3)
{
if(parallax3>minParallax)
{
vP3D = vP3D3;
vbTriangulated = vbTriangulated3;
R1.copyTo(R21);
t2.copyTo(t21);
return true;
}
}else if(maxGood==nGood4)
{
if(parallax4>minParallax)
{
vP3D = vP3D4;
vbTriangulated = vbTriangulated4;
R2.copyTo(R21);
t2.copyTo(t21);
return true;
}
}
// 如果有最优解但是不满足对应的parallax>minParallax，那么返回false表示求解失败
return false;
}
八、结语
从上述的分析中，我们知道从 Essential 中恢复 Rt 存在四种解，那么那一组解才是最合适的呢?源码中是对每一个解进行分析，检测点是不是都在两个相机的前方，并且统计重投影误差较小的点的个数，找出4个解中统计得到点数最多的的那个解作为最终的解。其代码具体的实现，我们接下会进行讲解。
本文内容来自计算机视觉life ORB-SLAM2 课程课件
