讲解关于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→官方认证
一、前言
通过前面的文章，知道了如何求解Homography 以及 Fundamental 矩阵。但是这是几个中间过程而已，求解 Homography 以及 Fundamental 是为了进一步求解旋转矩阵
R
R
R 以及偏移矩阵
t
t
t, 通过 Homography 求解
R
t
Rt
Rt 主要有以下两个方式：
F
a
u
g
e
r
a
s
S
V
D
−
b
a
s
e
d
d
e
c
o
m
p
o
s
i
t
i
o
n
\color{blue}Faugeras SVD-based decomposition
FaugerasSVD−baseddecomposition：Motion and Structure from Motion in a Piecewise Planar Environment
Z
h
a
n
g
S
V
D
−
b
a
s
e
d
d
e
c
o
m
p
o
s
i
t
i
o
n
\color{blue} Zhang SVD-based decomposition
ZhangSVD−baseddecomposition：D Reconstruction Based on Homography Mapping
该篇博客主要讲解上面之中的 Faugeras SVD-based decomposition。在进行详细讲解之前，大家需要了解以下相机成像的原理，这里简单给个图示，不再进行详细的讲解
二、适用场景
根据两帧图像恢复相机 R t Rt Rt 的方式有很多种，比如前面说到本质矩阵，基本矩阵，以及该博客讲解的单应矩阵，这么多的方式中，选用那种才是最合适的呢? 我们先来说一下 Homography：
1、两帧图像中的特征点共面(下左图所示)
2、相机发生纯旋转，即偏移矩阵t=0(下右图所示)
以上两种情况就适合从单应矩阵 Homography 进行求解。具体缘由在接下来的第二篇博客中有具体的介绍。
三、公式推导
首先设三维空间的一个点
X
\mathbf{X}
X, 以及该点再图像平面的坐标为
x
\mathbf x
x(假设焦距 f 为1),如下：
X
=
(
X
Y
Z
)
T
x
=
(
x
y
1
)
T
(1)
\tag{1} \color{blue} \mathbf{X}=\left(\begin{array}{lll} X & Y & Z \end{array}\right)^{T}~~~~~~~~~~~~~~~~~~~~~~~~~\mathbf{x}=\left(\begin{array}{lll} x & y & 1 \end{array}\right)^{T}
X=(XYZ)T x=(xy1)T(1) 那么根据相机的成像原理(可以参考图一),其存在如下关系：
X
x
=
Y
y
=
Z
(2)
\tag{2} \color{blue} \frac{X}{x}=\frac{Y}{y}=Z
xX=yY=Z(2)
令第一个相机坐标为世界坐标
\color{red}令第一个相机坐标为世界坐标
令第一个相机坐标为世界坐标，设相机投影模型(E表示单位矩阵,R|T表示把旋转矩阵与偏移矩阵按列拼接起来)
P
1
=
K
1
[
E
∣
0
]
P
2
=
K
2
[
R
∣
t
]
(3)
\tag{3} \color{blue} P_1=K_1[E|0]~~~~~~~~~~~~P_2=K_2[R|t]
P1=K1[E∣0] P2=K2[R∣t](3)设3D点所在的世界平面方程方程，与法向量
n
\mathbf n
n 如下(单位向量)，其中的
d
d
d 表示坐标系原点到平面的距离，
平面方程：
a
X
+
b
Y
+
c
Z
=
d
\color{blue} 平面方程：~~~~~a X+b Y+c Z=d
平面方程： aX+bY+cZ=d
法向量：
n
=
(
a
b
c
)
T
\color{blue} 法向量：\mathbf n = \left(\begin{array}{lll} a & b & c \end{array}\right)^{T}
法向量：n=(abc)T
根据以上两个式子
:
n
T
X
=
d
\color{blue} 根据以上两个式子:~~~~~~\mathbf {n}^T \mathbf{X}=d
根据以上两个式子: nTX=d那么带入平面上的点
X
\mathbf{X}
X ，其平面方程可以是表示为:
1
d
n
T
X
=
1
(4)
\tag{4} \color{blue} \frac{1}{d} \mathbf{n}^{T} \mathbf{X}=1
d1nTX=1(4)那么我们设平面上的点
X
1
\mathbf{X_1}
X1满足上式(4)(以第一个相机坐标为世界坐标)，也就是：
1
=
1
d
n
T
X
1
(5)
\tag{5} \color{blue} 1 = \frac{1}{d} \mathbf{n}^{T} \mathbf{X_1}
1=d1nTX1(5)对于两个相机坐标系点的关系 如下:
X
2
=
R
X
1
+
t
(6)
\tag{6} \color{blue} \mathbf{X}_{2}=R \mathbf{X}_{1}+\mathbf{t}
X2=RX1+t(6)那么我们把(5)式乘到(6)式的
t
\mathbf t
t 后面，可以得到如下：
X
2
=
(
R
+
1
d
t
n
T
)
X
1
(7)
\tag{7} \color{blue} \mathbf{X}_{2}=\left(R+\frac{1}{d} \mathbf{t n}^{T}\right) \mathbf{X}_{1}
X2=(R+d1tnT)X1(7)根据单应性变换，对图像齐次坐标
x
=
(
x
y
1
)
T
\mathbf{x}=\left(\begin{array}{lll}x & y & 1\end{array}\right)^{T}
x=(xy1)T和齐次单应矩阵
H
H
H 那么两个图像的坐标满足
s
x
2
=
H
x
1
s\mathbf x_2=H\mathbf x_1
sx2=Hx1 ,这里的
s
s
s 可以理解为缩放，根据相机投影模型：
x
1
=
K
1
X
1
x
2
=
K
2
X
2
(8)
\tag{8} \color{blue} \mathbf{x}_{1}=K_{1} \mathbf{X}_{1}~~~~~~~~~~~\mathbf{x}_{2}=K_{2} \mathbf{X}_{2}
x1=K1X1 x2=K2X2(8)我们可以推导出
X
2
=
K
2
−
1
H
K
1
X
1
(9)
\tag{9} \color{blue} \mathbf{X}_{2}=K_{2}^{-1} H K_{1} \mathbf{X}_{1}
X2=K2−1HK1X1(9)
根据 (7), (9) 我们可以得到(如果是同一个相机，这里的
K
1
=
K
2
\mathbf K_1=\mathbf K_2
K1=K2)
A
=
d
R
+
t
n
T
=
d
K
2
−
1
H
K
1
(10)
\tag{10} \color{blue} A=dR+\mathbf{t n}^{T}=dK_{2}^{-1} H K_{1}
A=dR+tnT=dK2−1HK1(10)上面的公式我们可以求得
H
H
H 矩阵与
R
t
Rt
Rt 的对应关系(略过)，对
A
A
A 进行奇异值分解，如下
A
=
U
Λ
V
T
Λ
=
diag
(
d
1
,
d
2
,
d
3
)
(11)
\tag{11} \color{blue} A=U \Lambda V^{T}~~~~~~~~~\Lambda=\operatorname{diag}\left(d_{1}, d_{2}, d_{3}\right)
A=UΛVT Λ=diag(d1,d2,d3)(11)另外由于
U
,
V
U,V
U,V 满足
(
其中
U
T
U
=
V
T
V
=
E
)
\left(其中 U^{T} U=V^{T} V=E\right)
(其中UTU=VTV=E)，可以得到：
Λ
=
U
T
A
V
=
d
U
T
R
V
+
(
U
T
t
)
(
V
T
n
)
T
(12)
\tag{12} \color{blue} \Lambda=U^{T} A V=d U^{T} R V+\left(U^{T} \mathbf{t}\right)\left(V^{T} \mathbf{n}\right)^{T}
Λ=UTAV=dUTRV+(UTt)(VTn)T(12)
U
,
V
U,V
U,V为正交矩阵，也就是说其行列式为
±
1
\pm 1
±1，即
s
=
det
(
U
)
=
det
(
V
)
s=\operatorname{det}(U)= \operatorname{det}(V)
s=det(U)=det(V), 且
s
2
=
1
s^2=1
s2=1， 令:
{
R
′
=
s
U
T
R
V
t
′
=
U
T
t
n
′
=
V
T
n
d
′
=
s
d
s
=
det
(
U
)
det
(
V
)
⟹
Λ
=
[
d
1
0
0
0
d
2
0
0
0
d
3
]
=
d
′
R
′
+
t
′
n
′
T
(13)
\tag{13} \color{blue} \left\{\begin{array}{l} R^{\prime}=s U^{T} R V \\ \mathbf{t}^{\prime}=U^{T} \mathbf{t} \\ \mathbf{n}^{\prime}=V^{T} \mathbf{n} \\ d^{\prime}=s d \\ s=\operatorname{det}(U) \operatorname{det}(V) \end{array}\right. \Longrightarrow~~~~~~~~~~~ \Lambda=\left[\begin{array}{ccc} d_{1} & 0 & 0 \\ 0 & d_{2} & 0 \\ 0 & 0 & d_{3} \end{array}\right]=d^{\prime} R^{\prime}+\mathbf t^{\prime} \mathbf{n}^{\prime T}
⎩
⎨
⎧R′=sUTRVt′=UTtn′=VTnd′=sds=det(U)det(V)⟹ Λ=⎣
⎡d1000d2000d3⎦
⎤=d′R′+t′n′T(13) 其上
V
T
V_T
VT 是行列式为
±
1
\pm 1
±1 的正交矩阵，那么其也是一个旋转矩阵(可以参考该片博客末尾史上最简SLAM零基础解读(1) - 旋转平移矩阵→欧式变换推导),该矩阵作用于单位法向量
n
n
n,也就是对齐进行旋转，也就是说
n
′
n'
n′ 也是单位向量。取基底
e
1
=
(
1
,
0
，
0
)
T
,
e
1
=
(
1
,
0
，
0
)
T
,
e
1
=
(
1
,
0
，
0
)
T
\mathbf e_1=(1,0，0)^T,\mathbf e_1=(1,0，0)^T,\mathbf e_1=(1,0，0)^T
e1=(1,0，0)T,e1=(1,0，0)T,e1=(1,0，0)T ，设单位向量
n
′
=
(
x
1
,
x
2
,
x
3
)
T
\mathbf n'=(x_1, x_2, x_3)^T
n′=(x1,x2,x3)T, 则
n
′
e
=
(
x
1
e
1
+
x
2
e
2
+
x
3
e
3
)
\mathbf n'\mathbf e=(x_{1} \mathbf{e}_{1}+x_{2} \mathbf{e}_{2}+x_{3} \mathbf{e}_{3})
n′e=(x1e1+x2e2+x3e3) 根据式子 (13) 可以得到：
[
d
1
e
1
d
2
e
2
d
3
e
3
]
=
[
d
′
R
′
e
1
d
′
R
′
e
2
d
′
R
′
e
3
]
+
[
t
′
x
1
e
1
t
′
x
2
e
2
t
′
x
3
e
3
]
(14)
\tag{14} \color{blue} \left[\begin{array}{lll} d_{1} \mathbf{e}_{1} & d_{2} \mathbf{e}_{2} & d_{3} \mathbf{e}_{3} \end{array}\right]=\left[\begin{array}{llll} d^{\prime} R^{\prime} \mathbf{e}_{1} & d^{\prime} R^{\prime} \mathbf{e}_{2} & d^{\prime} R^{\prime} \mathbf{e}_{3} \end{array}\right]+\left[\begin{array}{lll} \mathbf{t}^{\prime} x_{1} \mathbf e_1 & \mathbf{t}^{\prime} x_{2} \mathbf e_2 & \mathbf{t}^{\prime} x_{3} \mathbf e_3 \end{array}\right]
[d1e1d2e2d3e3]=[d′R′e1d′R′e2d′R′e3]+[t′x1e1t′x2e2t′x3e3](14)
[
d
1
e
1
d
2
e
2
d
3
e
3
]
=
[
d
′
R
′
e
1
d
′
R
′
e
2
d
′
R
′
e
3
]
+
[
t
′
x
1
t
′
x
2
t
′
x
3
]
(15)
\tag{15} \color{blue} \left[\begin{array}{lll} d_{1} \mathbf{e}_{1} & d_{2} \mathbf{e}_{2} & d_{3} \mathbf{e}_{3} \end{array}\right]=\left[\begin{array}{llll} d^{\prime} R^{\prime} \mathbf{e}_{1} & d^{\prime} R^{\prime} \mathbf{e}_{2} & d^{\prime} R^{\prime} \mathbf{e}_{3} \end{array}\right]+\left[\begin{array}{lll} \mathbf{t}^{\prime} x_{1} & \mathbf{t}^{\prime} x_{2} & \mathbf{t}^{\prime} x_{3} \end{array}\right]
[d1e1d2e2d3e3]=[d′R′e1d′R′e2d′R′e3]+[t′x1t′x2t′x3](15)即可推导出如下3式：
d
1
e
1
=
d
′
R
′
e
1
+
t
′
x
1
d
2
e
2
=
d
′
R
′
e
2
+
t
′
x
2
d
3
e
3
=
d
′
R
′
e
3
+
t
′
x
3
(16)
\tag{16} \color{blue} \begin{aligned} d_{1} \mathbf{e}_{1} &=d^{\prime} R^{\prime} \mathbf{e}_{1}+\mathbf{t}^{\prime} x_{1} \\ d_{2} \mathbf{e}_{2} &=d^{\prime} R^{\prime} \mathbf{e}_{2}+\mathbf{t}^{\prime} x_{2} \\ d_{3} \mathbf{e}_{3} &=d^{\prime} R^{\prime} \mathbf{e}_{3}+\mathbf{t}^{\prime} x_{3} \end{aligned}
d1e1d2e2d3e3=d′R′e1+t′x1=d′R′e2+t′x2=d′R′e3+t′x3(16)
注意到
n
n
n 是单位法向量，
V
V
V 是旋转矩阵，则
n
′
n′
n′ 也是单位法向量，则有
Σ
i
3
=
x
i
2
=
1
Σ^3_i=x^2_i=1
Σi3=xi2=1。对(16)的三个式子消去t′得
d
′
R
′
(
x
2
e
1
−
x
1
e
2
)
=
d
1
x
2
e
1
−
d
2
x
1
e
2
d
′
R
′
(
x
3
e
2
−
x
2
e
3
)
=
d
2
x
3
e
2
−
d
3
x
2
e
3
d
′
R
′
(
x
1
e
3
−
x
3
e
1
)
=
d
3
x
1
e
3
−
d
1
x
3
e
1
(17)
\tag{17} \color{blue} \begin{array}{l} d^{\prime} R^{\prime}\left(x_{2} \mathbf{e}_{1}-x_{1} \mathbf{e}_{2}\right)=d_{1} x_{2} \mathbf{e}_{1}-d_{2} x_{1} \mathbf{e}_{2} \\ d^{\prime} R^{\prime}\left(x_{3} \mathbf{e}_{2}-x_{2} \mathbf{e}_{3}\right)=d_{2} x_{3} \mathbf{e}_{2}-d_{3} x_{2} \mathbf{e}_{3} \\ d^{\prime} R^{\prime}\left(x_{1} \mathbf{e}_{3}-x_{3} \mathbf{e}_{1}\right)=d_{3} x_{1} \mathbf{e}_{3}-d_{1} x_{3} \mathbf{e}_{1} \end{array}
d′R′(x2e1−x1e2)=d1x2e1−d2x1e2d′R′(x3e2−x2e3)=d2x3e2−d3x2e3d′R′(x1e3−x3e1)=d3x1e3−d1x3e1(17)根据前面类似的结论，可以知道
R
′
R'
R′ 为旋转矩阵，满足
R
′
T
R
′
=
E
R'^TR'=E
R′TR′=E。也就是
∣
∣
R
′
X
∣
∣
=
∣
∣
X
∣
∣
||R'X||=||X||
∣∣R′X∣∣=∣∣X∣∣，对上式两边取二f范数可得：
{
(
d
′
2
−
d
2
2
)
x
1
2
+
(
d
′
2
−
d
1
2
)
x
2
2
=
0
(
d
′
2
−
d
3
2
)
x
2
2
+
(
d
′
2
−
d
2
2
)
x
3
2
=
0
(
d
′
2
−
d
1
2
)
x
3
2
+
(
d
′
2
−
d
3
2
)
x
1
2
=
0
(18)
\tag{18} \color{blue} \left\{\begin{array}{l} \left(d^{\prime 2}-d_{2}^{2}\right) x_{1}^{2}+\left(d^{\prime 2}-d_{1}^{2}\right) x_{2}^{2}=0 \\ \left(d^{\prime 2}-d_{3}^{2}\right) x_{2}^{2}+\left(d^{\prime 2}-d_{2}^{2}\right) x_{3}^{2}=0 \\ \left(d^{\prime 2}-d_{1}^{2}\right) x_{3}^{2}+\left(d^{\prime 2}-d_{3}^{2}\right) x_{1}^{2}=0 \end{array}\right.
⎩
⎨
⎧(d′2−d22)x12+(d′2−d12)x22=0(d′2−d32)x22+(d′2−d22)x32=0(d′2−d12)x32+(d′2−d32)x12=0(18)上式是一个关于
x
1
2
、
x
2
2
、
x
3
2
x_1^2、x_2^2、x_3^2
x12、x22、x32 的一个齐次线性方程，又根据
n
′
=
(
x
1
,
x
2
,
x
3
)
T
n'=(x_1, x_2, x_3)^T
n′=(x1,x2,x3)T 为单位向量，也就是
x
1
2
+
x
2
2
+
x
3
2
=
∑
i
=
1
3
x
i
2
=
1
x_1^2+x_2^2+x_3^2=\sum_{i=1}^3x_i^2=1
x12+x22+x32=∑i=13xi2=1，且系数矩阵的秩小于3，需要求该线性方程的非零解，其行列式必定为0，也就有：
(
d
′
2
−
d
1
2
)
(
d
′
2
−
d
2
2
)
(
d
′
2
−
d
3
2
)
=
0
(19)
\tag{19} \color{blue} \left(d^{\prime 2}-d_{1}^{2}\right)\left(d^{\prime 2}-d_{2}^{2}\right)\left(d^{\prime 2}-d_{3}^{2}\right)=0
(d′2−d12)(d′2−d22)(d′2−d32)=0(19)
情形一:(
d
′
=
±
d
1
\color{red}d'=\pm d_1
d′=±d1)
如果
d
=
±
d
1
d=\pm d_1
d=±d1 带入(18)式，其为一解为
x
1
=
x
2
=
x
3
=
0
x_1=x_2=x_3=0
x1=x2=x3=0，很明显其不满足
∑
i
=
1
3
x
i
2
=
1
\sum_{i=1}^3x_i^2=1
∑i=13xi2=1, 说明其解为无效解。
情形二:(
d
′
=
±
d
3
\color{red}d'=\pm d_3
d′=±d3)
如果
d
=
±
d
3
d=\pm d_3
d=±d3 带入(18)式，其为一解为
x
1
=
x
2
=
x
3
=
0
x_1=x_2=x_3=0
x1=x2=x3=0，很明显其不满足
∑
i
=
1
3
x
i
2
=
1
\sum_{i=1}^3x_i^2=1
∑i=13xi2=1, 说明其解为无效解。
四、齐次方程求解
根据上面我们已经讨论了情形一与情形二，都是比较简单的，但是很明显都是无效解的，倒是还有一种情形没有讨论，那就是(
d
=
±
d
2
d=\pm d_2
d=±d2),针对于这该情形，会出现好几种情况，现在分别对其进行讨论,当
d
′
=
±
d
2
d'=\pm d_2
d′=±d2 时，带入(18）式，并且根据
∑
i
=
1
3
x
i
2
=
1
\sum_{i=1}^3x_i^2=1
∑i=13xi2=1 可以获得(前面令
n
′
=
(
x
1
,
x
2
,
x
3
)
\mathbf n'=(x_1,x_2,x_3)
n′=(x1,x2,x3))
{
x
1
=
ε
1
d
1
2
−
d
2
2
d
1
2
−
d
3
2
x
2
=
0
x
3
=
ε
3
d
2
2
−
d
3
2
d
1
1
−
d
3
2
ε
1
,
ε
3
=
±
1
(20)
\tag{20} \color{blue} \left\{\begin{array}{l} x_{1}=\varepsilon_{1} \sqrt{\frac{d_{1}^{2}-d_{2}^{2}}{d_{1}^{2}-d_{3}^{2}}} \\ x_{2}=0 \\ x_{3}=\varepsilon_{3} \sqrt{\frac{d_{2}^{2}-d_{3}^{2}}{d_{1}^{1}-d_{3}^{2}}} \end{array}~~~~~~~~~~~ \varepsilon_{1}, \varepsilon_{3}=\pm 1\right.
⎩
⎨
⎧x1=ε1d12−d32d12−d22
x2=0x3=ε3d11−d32d22−d32
ε1,ε3=±1(20)然后我们在此基础上，再根据
d
1
≥
d
2
≥
d
3
d_1\geq d_2\geq d_3
d1≥d2≥d3 分多种情况进行讨论。主要分为三种情况：
(
1
)
d
1
≠
d
2
≠
d
3
(
2
)
d
1
=
d
2
≠
d
3
或者
d
1
≠
d
2
=
d
3
(
3
)
d
1
=
d
2
=
d
3
(21)
\tag{21} \color{blue} (1)d_1≠d_2≠d_3\\ (2)d_1=d_2≠d_3 或者d_1≠d_2=d_3\\(3)d_1=d_2=d_3
(1)d1=d2=d3(2)d1=d2=d3或者d1=d2=d3(3)d1=d2=d3(21)
情况一：(
d
′
=
d
2
>
0
\color{red}d'=d_2>0
d′=d2>0)，满足该条件下，进行细致讨论
(
1
)
当
d
1
≠
d
2
≠
d
3
时
\color{blue} (1)当d_1≠d_2≠d_3时
(1)当d1=d2=d3时
由于
d
′
=
d
2
、
x
2
=
0
d'=d_2、 x_2=0
d′=d2、x2=0,带入(16)中的第二个等式中，可以得到
e
2
=
R
′
e
2
e_2=R'e_2
e2=R′e2 可以得知，
R
′
R'
R′ 是沿着轴
e
2
e_2
e2 旋转的，那么就有：
R
′
=
(
cos
θ
0
−
sin
θ
0
1
0
sin
θ
0
cos
θ
)
R^{\prime}=\left(\begin{array}{ccc} \cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta \end{array}\right)
R′=⎝
⎛cosθ0sinθ010−sinθ0cosθ⎠
⎞其代入(17)式中的第三个式子，可得：
(
cos
θ
0
−
sin
θ
0
1
0
sin
θ
0
cos
θ
)
(
−
x
3
0
x
1
)
=
(
−
d
1
x
3
0
d
3
x
1
)
\left(\begin{array}{ccc} \cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta \end{array}\right)\left(\begin{array}{c} -x_{3} \\ 0 \\ x_{1} \end{array}\right)=\left(\begin{array}{c} -d_{1} x_{3} \\ 0 \\ d_{3} x_{1} \end{array}\right)
⎝
⎛cosθ0sinθ010−sinθ0cosθ⎠
⎞⎝
⎛−x30x1⎠
⎞=⎝
⎛−d1x30d3x1⎠
⎞结合式子(20)可解得：
{
sin
θ
=
d
1
−
d
3
d
2
x
1
x
3
=
ε
1
ε
3
(
d
1
2
−
d
2
2
)
(
d
2
2
−
d
3
2
)
(
d
1
−
d
3
)
d
2
cos
θ
=
d
1
x
3
2
−
d
3
x
1
2
d
2
=
d
2
2
+
d
1
d
3
(
d
1
+
d
3
)
d
2
\left\{\begin{array}{l} \sin \theta=\frac{d_{1}-d_{3}}{d_{2}} x_{1} x_{3}=\varepsilon_{1} \varepsilon_{3} \frac{\sqrt{\left(d_{1}^{2}-d_{2}^{2}\right)\left(d_{2}^{2}-d_{3}^{2}\right)}}{\left(d_{1}-d_{3}\right) d_{2}} \\ \cos \theta=\frac{d_{1} x_{3}^{2}-d_{3} x_{1}^{2}}{d_{2}}=\frac{d_{2}^{2}+d_{1} d_{3}}{\left(d_{1}+d_{3}\right) d_{2}} \end{array}\right.
⎩
⎨
⎧sinθ=d2d1−d3x1x3=ε1ε3(d1−d3)d2(d12−d22)(d22−d32)
cosθ=d2d1x32−d3x12=(d1+d3)d2d22+d1d3由于
n
′
T
n
′
=
1
n'^Tn'=1
n′Tn′=1, (15)式两边同时乘
n
′
n'
n′ 然后位移可以得到:
t
′
=
(
d
1
0
0
0
d
2
0
0
0
d
3
)
(
x
1
0
x
3
)
−
d
2
(
cos
θ
0
−
sin
θ
0
1
0
sin
θ
0
cos
θ
)
(
x
1
0
x
3
)
\mathbf{t}^{\prime}=\left(\begin{array}{ccc} d_{1} & 0 & 0 \\ 0 & d_{2} & 0 \\ 0 & 0 & d_{3} \end{array}\right)\left(\begin{array}{c} x_{1} \\ 0 \\ x_{3} \end{array}\right)-d_{2}\left(\begin{array}{ccc} \cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta \end{array}\right)\left(\begin{array}{c} x_{1} \\ 0 \\ x_{3} \end{array}\right)
t′=⎝
⎛d1000d2000d3⎠
⎞⎝
⎛x10x3⎠
⎞−d2⎝
⎛cosθ0sinθ010−sinθ0cosθ⎠
⎞⎝
⎛x10x3⎠
⎞结合之前式可以推导得出：
t
′
=
(
d
1
−
d
3
)
(
x
1
0
x
3
)
\mathbf{t}^{\prime}=\left(d_{1}-d_{3}\right)\left(\begin{array}{c} x_{1} \\ 0 \\ x_{3} \end{array}\right)
t′=(d1−d3)⎝
⎛x10x3⎠
⎞
(
2
)
当
d
1
=
d
2
≠
d
3
时
\color{blue} (2)当d_1=d_2≠d_3时
(2)当d1=d2=d3时
根据式子(20)，这时有
x
1
=
x
2
=
0
,
x
3
=
±
1
x_1=x_2=0, x_3=\pm1
x1=x2=0,x3=±1, 可得
n
′
=
(
x
1
,
x
2
,
x
3
)
=
(
0
,
0
,
±
1
)
T
\mathbf n'=(x_1,x_2,x_3)=(0,0,\pm1)^T
n′=(x1,x2,x3)=(0,0,±1)T, 结合式子(16), 解为：
{
R
′
=
E
t
′
=
(
d
3
−
d
1
)
n
′
\left\{\begin{array}{l} R^{\prime}=E \\ \mathbf{t}^{\prime}=\left(d_{3}-d_{1}\right) \mathbf{n}^{\prime} \end{array}\right.
{R′=Et′=(d3−d1)n′
(
3
)
当
d
1
=
d
2
=
d
3
时
\color{blue} (3)当d_1=d_2=d_3时
(3)当d1=d2=d3时
这时
x
1
、
x
2
、
x
3
x_1、x_2、x_3
x1、x2、x3 未定义，根据式(16),(17)可得:
{
R
′
=
E
t
′
=
0
\left\{\begin{array}{l} R^{\prime}=E \\ \mathbf{t}^{\prime}=\mathbf{0} \end{array}\right.
{R′=Et′=0
情况二：(
d
′
=
−
d
2
<
0
\color{red}d'=- d_20
d′=d2>0)，情况二：(
d
′
=
−
d
2
<
0
\color{red}d'=- d_20,d'=- d_20,d′=−d2
0
Z_1>0
Z1>0, 有：
n
T
X
1
d
Z
1
=
n
T
x
1
d
>
0
\color{blue} \frac{\mathbf{n}^{T} \mathbf{X}_{1}}{d Z_{1}}=\frac{\mathbf{n}^{T} \mathbf{x}_{1}}{d}>0
dZ1nTX1=dnTx1>0所以剩下的四组解中，只有两组是有效的，另外在论文中提到：当观测到的点不是全部靠近一个相机光心而远离另外一个相机光心，那么只有一个解满足所有的点。如果有
d
′
>
0
d'>0
d′>0, 对于上述两个解 (
n
1
′
,
t
1
′
\mathbf{n}'_1,\mathbf{t}'_1
n1′,t1′) 和 (
n
2
′
,
t
2
′
\mathbf{n}'_2,\mathbf{t}'_2
n2′,t2′)，这两个解有如下关系，也就是两个解是相同的(
本人没有明白所以然
\color{red}本人没有明白所以然
本人没有明白所以然),如下：
{
t
2
′
=
(
d
1
−
d
3
)
n
1
′
n
2
′
=
t
1
′
d
1
−
d
3
\color{blue} \left\{\begin{array}{l} \mathbf{t}_{2}^{\prime}=\left(d_{1}-d_{3}\right) \mathbf{n}_{1}^{\prime} \\ \mathbf{n}_{2}^{\prime}=\frac{\mathbf{t}_{1}^{\prime}}{d_{1}-d_{3}} \end{array}\right.
{t2′=(d1−d3)n1′n2′=d1−d3t1′
注意 \color{red}注意 注意ORB-SLAM2的代码，是在求出所有的8个解之后，对每一个解进行分析，检测点是不是都在两个相机的前方，并且统计重投影误差较小的点的个数，找出8个解中统计得到点数最多的的那个解作为最终的解。其代码实现如下。
六、代码注释
位于 src/Initializer.cc 文件中的 Initializer::ReconstructH() 函数
/**
* @brief 用H矩阵恢复R, t和三维点
* H矩阵分解常见有两种方法：Faugeras SVD-based decomposition 和 Zhang SVD-based decomposition
* 代码使用了Faugeras SVD-based decomposition算法，参考文献
* Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988
*
* @param[in] vbMatchesInliers 匹配点对的内点标记
* @param[in] H21 从参考帧到当前帧的单应矩阵
* @param[in] K 相机的内参数矩阵
* @param[in & out] R21 计算出来的相机旋转
* @param[in & out] t21 计算出来的相机平移
* @param[in & out] vP3D 世界坐标系下，三角化测量特征点对之后得到的特征点的空间坐标
* @param[in & out] vbTriangulated 特征点是否成功三角化的标记
* @param[in] minParallax 对特征点的三角化测量中，认为其测量有效时需要满足的最小视差角（如果视差角过小则会引起非常大的观测误差）,单位是角度
* @param[in] minTriangulated 为了进行运动恢复，所需要的最少的三角化测量成功的点个数
* @return true 单应矩阵成功计算出位姿和三维点
* @return false 初始化失败
*/
bool Initializer::ReconstructH(vector &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated)
{
// 目的 ：通过单应矩阵H恢复两帧图像之间的旋转矩阵R和平移向量T
// 参考 ：Motion and structure from motion in a piecewise plannar environment.
// International Journal of Pattern Recognition and Artificial Intelligence, 1988
// https://www.researchgate.net/publication/243764888_Motion_and_Structure_from_Motion_in_a_Piecewise_Planar_Environment
// 流程:
// 1. 根据H矩阵的奇异值d'= d2 或者 d' = -d2 分别计算 H 矩阵分解的 8 组解
// 1.1 讨论 d' > 0 时的 4 组解
// 1.2 讨论 d' < 0 时的 4 组解
// 2. 对 8 组解进行验证，并选择产生相机前方最多3D点的解为最优解
// 统计匹配的特征点对中属于内点(Inlier)或有效点个数
int N=0;
for(size_t i=0, iend = vbMatchesInliers.size() ; i=d2>=d3
if(d1/d2
关注
打赏
![]()
查看更多评论
- 史上最全slam从零开始-总目录
- (01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解
- 目标检测00-00：mmdetection(Foveabox为例)-目录-史上最新无死角讲解
- 风格迁移1-00：Liquid Warping GAN(Impersonator)-目录-史上最新无死角讲解
- 姿态估计1-00：FSA-Net(头部姿态估算)-目录-史上最新无死角讲解
- 姿态估计0-00：DenseFusion(6D姿态估计)-目录-史上最新无死角讲解
- 行人检测0-00：LFFD-目录-史上最新无死角解读
- 风格迁移0-00：stylegan-目录-史上最全
- 人脸识别0-00：insightFace-目录-史上最全
- (01)ORB-SLAM2源码无死角解析-(65) BA优化(g2o)→闭环线程:Optimizer::OptimizeEssentialGraph→本质图优化
