基础矩阵(Fundamental Matrix)F 和本质矩阵(Essential Matrix)E
重新仔细看了高博的十四讲对极几何部分,整理一下思路。
首先是外积的定义,三维向量a和向量b做外积,a^b可以转换为3*3的矩阵与b做内积.
a
×
b
=
[
i
j
k
a
1
a
2
a
3
b
1
b
2
b
3
]
=
[
a
2
b
3
−
a
3
b
2
a
3
b
1
−
a
1
b
3
a
1
b
2
−
a
2
b
1
]
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
b
≜
a
∧
b
\boldsymbol{a} \times \boldsymbol{b}=\left[\begin{array}{ccc} \boldsymbol{i} & \boldsymbol{j} & \boldsymbol{k} \\ a_{1} & a_{2} & a_{3} \\ b_{1} & b_{2} & b_{3} \end{array}\right]=\left[\begin{array}{c} a_{2} b_{3}-a_{3} b_{2} \\ a_{3} b_{1}-a_{1} b_{3} \\ a_{1} b_{2}-a_{2} b_{1} \end{array}\right]=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right] \boldsymbol{b} \triangleq \boldsymbol{a}^{\wedge} \boldsymbol{b}
a×b=⎣⎡ia1b1ja2b2ka3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b≜a∧b
对世界坐标为P的空间点,其在两成像平面坐标为p1,p2,有
s
1
p
1
=
K
P
,
s
2
p
2
=
K
(
R
P
+
t
)
s_{1} \boldsymbol{p}_{1}=\boldsymbol{K} \boldsymbol{P}, \quad s_{2} \boldsymbol{p}_{2}=\boldsymbol{K}(\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t})
s1p1=KP,s2p2=K(RP+t)
齐次坐标下为
p
1
=
K
P
,
p
2
=
K
(
R
P
+
t
)
\boldsymbol{p}_{1}=\boldsymbol{K} \boldsymbol{P}, \quad \boldsymbol{p}_{2}=\boldsymbol{K}(\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t})
p1=KP,p2=K(RP+t)
取
x
1
=
K
−
1
p
1
,
x
2
=
K
−
1
p
2
\boldsymbol{x}_{1}=\boldsymbol{K}^{-1} \boldsymbol{p}_{1}, \quad \boldsymbol{x}_{2}=\boldsymbol{K}^{-1} \boldsymbol{p}_{2}
x1=K−1p1,x2=K−1p2
于是有
x
2
=
R
x
1
+
t
\boldsymbol{x}_{2}=\boldsymbol{R} \boldsymbol{x}_{1}+\boldsymbol{t}
x2=Rx1+t
接下来是整合式子的部分,为了将式子整合成一边为0的乘积形式,首先利用外积,两边同时左乘t^
t
∧
x
2
=
t
∧
R
x
1
\boldsymbol{t}^{\wedge} \boldsymbol{x}_{2}=\boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{x}_{1}
t∧x2=t∧Rx1
再左乘x2的转置
x
2
T
t
∧
x
2
=
x
2
T
t
∧
R
x
1
\boldsymbol{x}_{2}^{T} \boldsymbol{t}^{\wedge} \boldsymbol{x}_{2}=\boldsymbol{x}_{2}^{T} \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{x}_{1}
x2Tt∧x2=x2Tt∧Rx1
t与x2的外积垂直于t和x2,因此其与x2做内积等于0,即等式两边等于0.
x
2
T
t
∧
R
x
1
=
0
\boldsymbol{x}_{2}^{T} \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{x}_{1}=0
x2Tt∧Rx1=0
代入原式,便得到了对极约束,其几何意义为两相机的中心和点P共面,且包含了相机内参和相机位姿的信息。
p
2
T
K
−
T
t
∧
R
K
−
1
p
1
=
0
\boldsymbol{p}_{2}^{T} \boldsymbol{K}^{-T} \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{K}^{-1} \boldsymbol{p}_{1}=0
p2TK−Tt∧RK−1p1=0
便有了本质矩阵E和基础矩阵F的定义:
E
=
t
∧
R
\boldsymbol{E}=\boldsymbol{t}^{\wedge} \boldsymbol{R}
E=t∧R
F
=
K
−
T
E
K
−
1
\boldsymbol{F}=\boldsymbol{K}^{-T} \boldsymbol{E} \boldsymbol{K}^{-1}
F=K−TEK−1
因此,有
x
2
T
E
x
1
=
p
2
T
F
p
1
=
0
\boldsymbol{x}_{2}^{T} \boldsymbol{E} \boldsymbol{x}_{1}=\boldsymbol{p}_{2}^{T} \boldsymbol{F} \boldsymbol{p}_{1}=0
x2TEx1=p2TFp1=0
位姿t与R的内积就可以转换成t转变的33的矩阵与R相乘,结果是一个33的矩阵,也就是本质矩阵E。
至此,相机位姿估计问题可以转变为:
1.根据配对点的像素位置,求出 E 或者 F;
2.根据 E 或者 F,求出 R, t。
在求解本质矩阵E的时候,由于其等式等于0,乘以任意常数仍应等于0,因此本质矩阵E具有尺度不变性,原本3*3应有9个自由度,由于尺度不变性使得8对点即可解出E(若考虑R和t的各三个自由度,只需3+3-1=5对点,但因为非线性的性质,会有一定的麻烦)。
一个3×3的矩阵是本质矩阵的充要条件是它的奇异值中有两个相等而第三个是0。
由于t^是反对称矩阵,根据反对称矩阵的性质,有
t
∧
=
k
U
Z
U
T
{t}^{\wedge}=k U Z U^{T}
t∧=kUZUT
其中,
Z
=
[
0
1
0
−
1
0
0
0
0
0
]
Z=\left[\begin{array}{ccc} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & 0 \end{array}\right]
Z=⎣⎡0−10100000⎦⎤
令
W
=
[
0
−
1
0
1
0
0
0
0
1
]
W=\left[\begin{array}{ccc} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{array}\right]
W=⎣⎡010−100001⎦⎤
于是有
Z
=
diag
(
1
,
1
,
0
)
W
Z=\operatorname{diag}(1,1,0) W
Z=diag(1,1,0)W
E的奇异值分解为
E
=
U
Σ
V
T
\boldsymbol{E}=\boldsymbol{U} \boldsymbol{\Sigma} \boldsymbol{V}^{T}
E=UΣVT
因为本质矩阵E的奇异值特性,因此Z与∑ W只相差了一个常数的尺度。
因此,在齐次坐标下
t
∧
=
U
Z
U
T
or
t
∧
=
−
U
Z
U
T
R
=
U
W
V
T
or
R
=
U
W
T
V
T
{t}^{\wedge}=U Z U^{T}\text { or } {t}^{\wedge}=-U Z U^{T} \quad R=U W V^{T} \text { or } R=U W^{T} V^{T}
t∧=UZUT or t∧=−UZUTR=UWVT or R=UWTVT
共四组解,但只有一组解为正。由此便由本质矩阵E解出相机位姿。
在ORB-SLAM3的初始化中,TwoViewReconstruction部分通过TwoViewReconstruction::ComputeF21用直接线性变换方法求解F矩阵,并进行打分,进而估计初始的相机位姿。