对于经特征匹配得到的两图像间配对的特征点,可恢复得到两帧图像间相机的运动。
对极约束
名词解释
如上图,对于两帧图像 I 1 、 I 2 I_1、I_2 I1、I2,P在其上投影分别为 p 1 、 p 2 p_1、p_2 p1、p2。相机的中心分别为 O 1 、 O 2 O_1、O_2 O1、O2,则有如下定义:
- 极平面(Epipolar plane) :由 O 1 、 O 2 、 P O_1、O_2、P O1、O2、P组成的平面
- 极点(Epipoles) : O 1 O 2 O_1O_2 O1O2连线同像平面 I 1 、 I 2 I_1、I_2 I1、I2的交点 e 1 、 e 2 e_1、e_2 e1、e2
- 基线 : O 1 O 2 O_1O_2 O1O2连线
- 极线(Epipolar line) :极平面同像平面的交线 p 1 e 1 、 p 2 e 2 p_1e_1、p_2e_2 p1e1、p2e2,记为 l 1 、 l 2 l_1、l_2 l1、l2
记两帧图像间的变换为 T 12 T_{12} T12,实际求解中,特征点 p 1 、 p 2 p_1、p_2 p1、p2通过特征匹配得到, P 、 e 1 、 e 2 P、e_1、e_2 P、e1、e2未知,待求变换 T 12 T_{12} T12。
像素坐标
对于世界坐标下某点
P
=
[
X
Y
Z
]
T
P=\begin{bmatrix}X&Y&Z\end{bmatrix}^T
P=[XYZ]T,由针孔相机模型可知其在图像坐标系下的像素坐标位置:
s
1
p
1
=
K
P
s
2
p
2
=
K
(
R
P
+
t
)
s_1p_1 = KP\qquad s_2p_2=K(RP+t)
s1p1=KPs2p2=K(RP+t)
其中,
K
K
K为相机内参矩阵,
R
、
t
R、t
R、t为两帧图像间的旋转、平移变换。
齐次坐标
通常采用齐次坐标表示像素坐标,也即一个向量同它自身乘以任意非零整数意义相同。通常用于表达一个投影变换。如
s
1
p
1
s_1p_1
s1p1同
p
1
p_1
p1成投影关系,他们在齐次坐标下意义相同。称其为尺度意义下相等,记作:
s
p
≃
p
sp\simeq p
sp≃p
则可得:
p
1
≃
K
P
p
2
≃
K
(
R
P
+
t
)
p_1\simeq KP\qquad p_2\simeq K(RP+t)
p1≃KPp2≃K(RP+t)
取归一化平面上坐标
x
1
,
x
2
x_1,x_2
x1,x2:
x
1
=
K
−
1
p
1
x
2
=
K
−
1
p
2
x_1=K^{-1}p_1\qquad x_2=K^{-1}p_2
x1=K−1p1x2=K−1p2
则可得:
x
2
≃
R
x
1
+
t
x_2 \simeq Rx_1+t
x2≃Rx1+t
对极约束
对上式两侧左乘
t
∧
t^\wedge
t∧再左乘
x
2
T
x_2^T
x2T:
t
∧
x
2
≃
t
∧
R
x
1
x
2
T
t
∧
x
2
≃
x
2
T
t
∧
R
x
1
t^\wedge x_2\simeq t^\wedge Rx_1\\ x_2^Tt^\wedge x_2\simeq x_2^T t^\wedge Rx_1
t∧x2≃t∧Rx1x2Tt∧x2≃x2Tt∧Rx1
对于上述公式左侧向量
t
∧
x
2
t^\wedge x_2
t∧x2方向同
t
t
t以及
x
2
x_2
x2垂直,故而再和
x
2
x_2
x2进行内积计算时,结果为
0
0
0,由此对其进行公式简化:
x
2
T
t
∧
R
x
1
=
0
x_2^T t^\wedge Rx_1 = 0
x2Tt∧Rx1=0
带入
p
1
、
p
2
p_1、p_2
p1、p2:
p
2
T
K
−
T
t
∧
R
K
−
1
p
1
=
0
p_2^TK^{-T}t^\wedge RK^{-1}p_1=0
p2TK−Tt∧RK−1p1=0
称其为对极约束,其中同时包含了旋转和平移两部分。其物理意义为刻画了
O
1
、
O
2
、
P
O_1、O_2、P
O1、O2、P共面的事实。
为简化公式,定义两个矩阵:基础矩阵(Fundamental Matrix)F和本质矩阵(Essential Matrix)E:
E
=
t
∧
R
F
=
K
−
T
E
K
−
1
E=t^\wedge R\qquad F=K^{-T}EK^{-1}
E=t∧RF=K−TEK−1
则有:
x
2
T
E
x
1
=
0
p
2
T
F
p
1
=
0
x_2^TEx_1=0\qquad p_2^TFp_1=0
x2TEx1=0p2TFp1=0
由此,相机位姿估计问题可分为如下两步:
- 根据匹配特征的像素位置求E或F
- 根据E或F求R,t
本质矩阵
本质矩阵 E = t ∧ R E=t^\wedge R E=t∧R是一个 3 × 3 3\times 3 3×3的矩阵,满足如下规则约束:
- 本质矩阵E的尺度等价:E乘以任意非零常数后,对极约束依然满足
- 本质矩阵E的内在性质:E的奇异值必然是 [ σ σ 0 ] T \begin{bmatrix}\sigma&\sigma&0\end{bmatrix}^T [σσ0]T形式的
- 由于尺度等价性,E只有5个自由度( t ∧ R t^\wedge R t∧R为6自由度)
八点法求解
本质矩阵E为 3 × 3 3\times3 3×3的矩阵,具有九个维度。此处仅考虑尺度等价性,则可用8对点估计本质矩阵E。
对于一对匹配的特征点,设其归一化坐标为
x
1
=
[
u
1
v
1
1
]
x_1=\begin{bmatrix}u_1&v_1&1\end{bmatrix}
x1=[u1v11]和
x
2
=
[
u
2
v
2
1
]
x_2=\begin{bmatrix}u_2&v_2&1\end{bmatrix}
x2=[u2v21],则可根据对极约束得到:
[
u
2
v
2
1
]
[
e
1
e
2
e
3
e
4
e
5
e
6
e
7
e
8
e
9
]
[
u
1
v
1
1
]
=
0
\begin{bmatrix}u_2&v_2&1\end{bmatrix}\begin{bmatrix}e_1&e_2&e_3\\e_4&e_5&e_6\\e_7&e_8&e_9\end{bmatrix}\begin{bmatrix}u_1\\v_1\\1\end{bmatrix}=0
[u2v21]⎣⎡e1e4e7e2e5e8e3e6e9⎦⎤⎣⎡u1v11⎦⎤=0
对本质矩阵E,将其展开写为向量形式:
e
=
[
e
1
e
2
e
3
e
4
e
5
e
6
e
7
e
8
e
9
]
T
\mathscr{e}=\begin{bmatrix}e_1&e_2&e_3&e_4&e_5&e_6&e_7&e_8&e_9\end{bmatrix}^T
e=[e1e2e3e4e5e6e7e8e9]T
则对极约束可写为关于
e
e
e的线性形式:
[
u
2
u
1
u
2
v
1
u
2
v
2
u
1
v
2
v
1
v
2
u
1
v
1
1
]
⋅
e
=
0
\begin{bmatrix}u_2u_1&u_2v_1&u_2&v_2u_1&v_2v_1&v_2&u_1&v_1&1\end{bmatrix}\cdot \mathscr{e}=0
[u2u1u2v1u2v2u1v2v1v2u1v11]⋅e=0
同样,对所有8对匹配特征点做上述计算,则可得到如下线性方程组:
[
u
2
1
u
1
1
u
2
1
v
1
1
u
2
1
v
2
1
u
1
1
v
2
1
v
1
1
v
2
1
u
1
1
v
1
1
1
u
2
2
u
1
2
u
2
2
v
1
2
u
2
2
v
2
2
u
1
2
v
2
2
v
1
2
v
2
2
u
1
2
v
1
2
1
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
u
2
8
u
1
8
u
2
8
v
1
8
u
2
8
v
2
8
u
1
8
v
2
8
v
1
8
v
2
8
u
1
8
v
1
8
1
]
[
e
1
e
2
e
3
e
4
e
5
e
6
e
7
e
8
e
9
]
=
0
\begin{bmatrix} u_2^1u_1^1&u_2^1v_1^1&u_2^1&v_2^1u_1^1&v_2^1v_1^1&v_2^1&u_1^1&v_1^1&1\\ u_2^2u_1^2&u_2^2v_1^2&u_2^2&v_2^2u_1^2&v_2^2v_1^2&v_2^2&u_1^2&v_1^2&1\\ \vdots&\vdots&\vdots&\vdots&\vdots&\vdots&\vdots&\vdots&\vdots\\ u_2^8u_1^8&u_2^8v_1^8&u_2^8&v_2^8u_1^8&v_2^8v_1^8&v_2^8&u_1^8&v_1^8&1 \end{bmatrix}\begin{bmatrix}e_1\\e_2\\e_3\\e_4\\e_5\\e_6\\e_7\\e_8\\e_9\end{bmatrix}=0
⎣⎢⎢⎢⎡u21u11u22u12⋮u28u18u21v11u22v12⋮u28v18u21u22⋮u28v21u11v22u12⋮v28u18v21v11v22v12⋮v28v18v21v22⋮v28u11u12⋮u18v11v12⋮v1811⋮1⎦⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡e1e2e3e4e5e6e7e8e9⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=0
其中,
u
i
v
i
u^iv^i
uivi表示第
i
i
i对匹配的特征点。当系数矩阵满秩(Rank=8)时,E的各个元素可求解。此时e构成一条线,同尺度等价性一致。
通过求解上述线性方程组,即可得到本质矩阵E。
SVD分解
针对估计所得的本质矩阵E,计算对应的相机运动 R 、 t R、t R、t,应使用SVD分解进行求得。有关SVD分解的基础内容,可以学习如下文章:奇异值分解(SVD)
设本质矩阵E的SVD如下:
E
=
U
Σ
V
T
E=U\Sigma V^T
E=UΣVT
根据SVD定义:矩阵
U
、
V
U、V
U、V为正交阵,
Σ
\Sigma
Σ为奇异值矩阵。由本质矩阵的内在性质知:
Σ
=
d
i
a
g
(
σ
,
σ
,
0
)
\Sigma=\mathrm{diag}(\sigma, \sigma, 0)
Σ=diag(σ,σ,0),则在SVD分解中,任意本质矩阵E具有两个可能的
t
、
R
t、R
t、R与之对应(此处直接给出了最终解,未推导):
t
1
∧
=
U
R
Z
(
π
2
)
Σ
U
T
R
1
=
U
R
Z
T
(
π
2
)
V
T
t
2
∧
=
U
R
Z
(
−
π
2
)
Σ
U
T
R
2
=
U
R
Z
T
(
−
π
2
)
V
T
t_1^\wedge=UR_Z(\frac{\pi}{2})\Sigma U^T\qquad R_1=UR_Z^T(\frac{\pi}{2})V^T\\ t_2^\wedge=UR_Z(-\frac{\pi}{2})\Sigma U^T\qquad R_2=UR_Z^T(-\frac{\pi}{2})V^T\\
t1∧=URZ(2π)ΣUTR1=URZT(2π)VTt2∧=URZ(−2π)ΣUTR2=URZT(−2π)VT
其中,
R
Z
(
π
2
)
R_Z(\frac{\pi}{2})
RZ(2π)表示绕Z轴旋转
π
2
\frac{\pi}{2}
2π得到旋转矩阵。同时由于E和
−
-
−E等价,对任意一个
t
t
t取负,可得相同结果。则存在四组可能的解:
如上图,用蓝色线表示相机,红色点(蓝线同黑线交点)表示空间点在相机上的投影。上述四种解中,仅第一种解中P在两个相机中都具备正向深度,故而可排除其余四种。
内在性质
根据线性方程组求解得到的E可能不满足其内在性质,也即奇异值不一定为 [ σ σ 0 ] T \begin{bmatrix}\sigma&\sigma&0\end{bmatrix}^T [σσ0]T形式。
通常,刻意将
Σ
\Sigma
Σ矩阵调整为上述形式,也即对八点法求得的E进行SVD分解后,得到的奇异值矩阵
Σ
=
d
i
a
g
(
σ
1
,
σ
2
,
σ
3
)
\Sigma=\mathrm{diag}(\sigma_1, \sigma_2, \sigma_3)
Σ=diag(σ1,σ2,σ3),假设
σ
1
≥
σ
2
≥
σ
3
\sigma_1\ge \sigma_2\ge \sigma_3
σ1≥σ2≥σ3,则取:
E
=
U
d
i
a
g
(
σ
1
+
σ
2
2
,
σ
1
+
σ
2
2
,
0
)
V
T
E=U\:\mathrm{diag}(\frac{\sigma_1+\sigma_2}{2}, \frac{\sigma_1+\sigma_2}{2}, 0)\:V^T
E=Udiag(2σ1+σ2,2σ1+σ2,0)VT
也即将求得的矩阵投影至E所在的流形上。
八点法存在的问题
尺度不确定性
由于E本身具备尺度等价性,分解计算得到的 R 、 t R、t R、t也具备尺度等价性。此时由于 R ∈ S O ( 3 ) R\in SO(3) R∈SO(3)存在约束,故而认为 t t t具备一个尺度。由此,对t进行归一化处理,使其长度为1。由于归一化,将直接导致单目视觉尺度不确定。
初始化纯旋转
若相机在初始化时发生的运动为纯旋转,也即由E分解得到的 R 、 t R、t R、t中 t = 0 t=0 t=0时,将导致无法直接求解 R R R
多于8对点时
当给定的匹配特征点多于8对时,可使用最小二乘计算对极约束,记系数矩阵为
A
A
A则有:
A
e
=
0
A\mathscr{e}=0
Ae=0
对于八点法,
A
A
A大小为
8
×
9
8\times 9
8×9。多于8对点时可以构造超定方程:
min
e
∥
A
e
∥
2
2
=
min
e
e
T
A
T
A
e
\min_e\begin{Vmatrix}A\mathscr{e}\end{Vmatrix}_2^2=\min_ee^TA^TAe
emin∥∥Ae∥∥22=emineTATAe
从而,可以求解最小二乘意义下的E矩阵。当存在误匹配问题时,则可采用**随机采样一致性(RANSAC)**代替最小二乘进行计算。
单应矩阵
单应矩阵(Homography)H用于描述两平面间的映射关系。若场景内特征点都落至某一平面上(墙、地面),则可使用单应性估计运动。
对于像平面
I
1
、
I
2
I_1、I_2
I1、I2上匹配的两特征点
p
1
、
p
2
p_1、p_2
p1、p2,若特征点落在某平面P上:
n
T
P
+
d
=
0
−
n
T
P
d
=
1
n^TP+d=0\\ -\frac{n^TP}{d}=1
nTP+d=0−dnTP=1
其中
n
n
n为平面的法向量,d为截距。同样在相机平面中,存在投影关系:
p
2
≃
K
(
R
P
+
t
)
≃
K
(
R
P
+
t
⋅
1
)
≃
K
(
R
P
+
t
⋅
(
−
n
T
P
d
)
)
≃
K
(
R
−
t
n
T
d
)
P
≃
K
(
R
−
t
n
T
d
)
K
−
1
P
1
\begin{aligned} p_2\simeq& K\Bigl(RP+t\Bigr)\\ \simeq& K\Bigl(RP+t\cdot1\Bigr)\\ \simeq& K\Bigl(RP+t\cdot(-\frac{n^TP}{d})\Bigr)\\ \simeq& K\Bigl(R-\frac{tn^T}{d}\Bigr)P\\ \simeq& K\Bigl(R-\frac{tn^T}{d}\Bigr)K^{-1}P_1 \end{aligned}
p2≃≃≃≃≃K(RP+t)K(RP+t⋅1)K(RP+t⋅(−dnTP))K(R−dtnT)PK(R−dtnT)K−1P1
由此得到对应匹配特征点间的变换关系:
H
=
K
(
R
−
t
n
T
d
)
K
−
1
H=K\Bigl(R-\frac{tn^T}{d}\Bigr)K^{-1}
H=K(R−dtnT)K−1
称矩阵H为单应矩阵,为一个
3
×
3
3\times 3
3×3的矩阵。
将
p
1
、
p
2
p_1、p_2
p1、p2的归一化像素坐标带入:
p
2
≃
H
p
1
[
u
2
v
2
1
]
≃
[
h
1
h
2
h
3
h
4
h
5
h
6
h
7
h
8
h
9
]
[
u
1
v
1
1
]
p_2\simeq Hp_1\\ \begin{bmatrix}u_2\\v_2\\1\end{bmatrix}\simeq \begin{bmatrix}h_1&h_2&h_3\\h_4&h_5&h_6\\h_7&h_8&h_9\end{bmatrix} \begin{bmatrix}u_1\\v_1\\1\end{bmatrix}
p2≃Hp1⎣⎡u2v21⎦⎤≃⎣⎡h1h4h7h2h5h8h3h6h9⎦⎤⎣⎡u1v11⎦⎤
符号
≃
\simeq
≃表示了尺度意义下的相等,故而矩阵H乘以任意非零常数依旧等价于其本身。实际处理中,取
h
9
=
1
h_9=1
h9=1从而展开:
h
1
u
1
+
h
2
v
1
+
h
3
−
h
7
u
1
u
2
−
h
8
v
1
u
2
=
u
2
h
4
u
1
+
h
5
v
1
+
h
6
−
h
7
u
1
v
2
−
h
8
v
1
v
2
=
v
2
h_1u_1+h_2v_1+h_3-h_7u_1u_2-h_8v_1u_2=u_2\\ h_4u_1+h_5v_1+h_6-h_7u_1v_2-h_8v_1v_2=v_2
h1u1+h2v1+h3−h7u1u2−h8v1u2=u2h4u1+h5v1+h6−h7u1v2−h8v1v2=v2
也即,一堆匹配特征点可以提供两个约束条件,则自由度为8的单应矩阵H只需要4对点即可求解得到:
[
u
1
1
v
1
1
1
0
0
0
−
u
1
1
u
2
1
−
v
1
1
u
2
1
0
0
0
u
1
1
v
1
1
1
−
u
1
1
v
2
1
−
v
1
1
v
2
1
u
1
2
v
1
2
1
0
0
0
−
u
1
2
u
2
2
−
v
1
2
u
2
2
0
0
0
u
1
2
v
1
2
1
−
u
1
2
v
2
2
−
v
1
2
v
2
2
u
1
3
v
1
3
1
0
0
0
−
u
1
3
u
2
3
−
v
1
3
u
2
3
0
0
0
u
1
3
v
1
3
1
−
u
1
3
v
2
3
−
v
1
3
v
2
3
u
1
4
v
1
4
1
0
0
0
−
u
1
4
u
2
4
−
v
1
4
u
2
4
0
0
0
u
1
4
v
1
4
1
−
u
1
4
v
2
4
−
v
1
4
v
2
4
]
[
h
1
h
2
h
3
h
4
h
5
h
6
h
7
h
8
]
=
[
u
2
1
v
2
1
u
2
2
v
2
2
u
2
3
v
2
3
u
2
4
v
2
4
]
\begin{bmatrix} u_1^1&v_1^1&1&0&0&0&-u_1^1u_2^1&-v_1^1u_2^1\\ 0&0&0&u_1^1&v_1^1&1&-u_1^1v_2^1&-v_1^1v_2^1\\ u_1^2&v_1^2&1&0&0&0&-u_1^2u_2^2&-v_1^2u_2^2\\ 0&0&0&u_1^2&v_1^2&1&-u_1^2v_2^2&-v_1^2v_2^2\\ u_1^3&v_1^3&1&0&0&0&-u_1^3u_2^3&-v_1^3u_2^3\\ 0&0&0&u_1^3&v_1^3&1&-u_1^3v_2^3&-v_1^3v_2^3\\ u_1^4&v_1^4&1&0&0&0&-u_1^4u_2^4&-v_1^4u_2^4\\ 0&0&0&u_1^4&v_1^4&1&-u_1^4v_2^4&-v_1^4v_2^4\\ \end{bmatrix}\begin{bmatrix}h_1\\h_2\\h_3\\h_4\\h_5\\h_6\\h_7\\h_8\end{bmatrix}=\begin{bmatrix}u_2^1\\v_2^1\\u_2^2\\v_2^2\\u_2^3\\v_2^3\\u_2^4\\v_2^4\end{bmatrix}
⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡u110u120u130u140v110v120v130v140101010100u110u120u130u140v110v120v130v1401010101−u11u21−u11v21−u12u22−u12v22−u13u23−u13v23−u14u24−u14v24−v11u21−v11v21−v12u22−v12v22−v13u23−v13v23−v14u24−v14v24⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡h1h2h3h4h5h6h7h8⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡u21v21u22v22u23v23u24v24⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
计算得到单应矩阵H后,应同本质矩阵一样,进一步分解计算
R
、
t
R、t
R、t
小结
在2D-2D的情况下,仅知道图像坐标间对应关系:
- 当特征点在平面上(俯视、仰视),使用H计算得到 R 、 t R、t R、t
- 否则,使用E\F计算 R 、 t R、t R、t
- 获得 R 、 t R、t R、t后,采用三角化计算其深度信息。