《视觉SLAM十四讲》学习笔记-对极约束问题

对极约束

问题描述

：求取两帧图像

I

1

,

I

2

2

$I_1, I_2$
之间的运动。设第一帧到第二帧的运动为

R

,

t

⃗

→

$\mathbf{R}, \vec{t}$
，其中心分别为

O

1

,

O

2

2

$O_1, O_2$
，

I

1

1

$I_1$
中有一个点

p

1

1

$p_1$
对应到

I

2

2

$I_2$
的点为

p

2

2

$p_2$
.

$P$
为两个摄像机在远处的交点。

$O_{1} O_{2} P$

O_{1} O_{2} P

$O_1O_2P$
称为极平面(Epipolar plane)，

O

1

O

2

2

$O_1O_2$
连线与相机平面交点称为极点(Epipoles),

O

1

O

2

2

$O_1O_2$
称为基线，

l

1

,

l

2

2

$l_1,l_2$
为极线(Epipolar line).

以第一帧的坐标系作为基准，设

$P$
坐标为

$P = [X, Y, Z]^{⊤}$

P = [X, Y, Z]^{⊤}

$\mathbf{P}=[X, Y, Z]^\top$
,

p

1

,

p

2

2

$p_1, p_2$
位置为：

s 1 p 1 = K P, s 2 p 2 = K (R P + t ⃗)

$s_1p_1=\mathbf{K}\mathbf{P},~s_2p_2=\mathbf{K}(\mathbf{R}\mathbf{P} + \vec{t})$

其中

$\mathbf{K}$
为相机内参,

R

,

t

⃗

→

$\mathbf{R},\vec{t}$
为坐标系的相机运动。转化为齐次等式：

p 1 = K P, p 2 = K (R P + t ⃗)

$p_1=\mathbf{K}\mathbf{P},~p_2=\mathbf{K}(\mathbf{R}\mathbf{P} + \vec{t})$

取

x

⃗

1

=

K

−

1

p

⃗

1

→

−

→

1

$\vec{x}_1=\mathbf{K}^{-1}\vec{p}_1$
,

x

⃗

2

=

K

−

1

p

⃗

2

→

−

→

2

$\vec{x}_2=\mathbf{K}^{-1}\vec{p}_2$
,则有：

x ⃗ 2 = R x ⃗ 1 + t ⃗

$\vec{x}_2 = \mathbf{R}\vec{x}_1 + \vec{t}$

两边左乘

t

⃗

∧

→

∧

$\vec{t}^\wedge$
，有：

t ⃗ \land x ⃗ 2 = t ⃗ \land R x ⃗ 1 (x ⃗ 2 t ⃗ = 0)

$\vec{t}^\wedge\vec{x}_2 = \vec{t}^\wedge \mathbf{R}\vec{x}_1~~(\vec{x}_2 \vec{t}=0)$

两边再左乘

x

⃗

⊤

2

→

⊤

$\vec{x}_2^\top$
：

x ⃗ ⊤ 2 t ⃗ \land x ⃗ 2 = x ⃗ ⊤ 2 t ⃗ \land R x ⃗ 1

$\vec{x}_2^\top\vec{t}^\wedge\vec{x}_2 = \vec{x}_2^\top\vec{t}^\wedge \mathbf{R}\vec{x}_1$

因为

t

⃗

∧

x

⃗

2

→

∧

→

2

$\vec{t}^\wedge\vec{x}_2$
与

t

⃗

∧

→

∧

$\vec{t}^\wedge$
和

x

⃗

2

→

2

$\vec{x}_2$
皆为垂直，所以左侧为0：

x ⃗ ⊤ 2 t ⃗ \land R x ⃗ 1 = 0

$\vec{x}_2^\top\vec{t}^\wedge \mathbf{R}\vec{x}_1 = 0$

再代入

x

1

,

x

2

2

$x_1, x_2$
得到：

(K - 1 p ⃗ 2) ⊤ t ⃗ \land R K - 1 p ⃗ 1 = 0

$(\mathbf{K}^{-1}\vec{p}_2)^\top\vec{t}^\wedge \mathbf{R}\mathbf{K}^{-1}\vec{p}_1 = 0$

即：

p ⃗ ⊤ 2 K - ⊤ t ⃗ \land R K - 1 p ⃗ 1 = 0

$\vec{p}_2^\top \mathbf{K}^{-\top} \vec{t}^\wedge \mathbf{R}\mathbf{K}^{-1}\vec{p}_1 = 0$

此式即为
对极约束
，几何意义为

O

1

,

O

2

,

P

P

$O_1, O_2, P$
共面。

将中间拆为基础矩阵和本质矩阵，可简化约束为:

E = t ⃗ \land R, R = K - T E K - 1, x ⃗ ⊤ 2 E x ⃗ 1 = p ⃗ ⊤ 2 F p ⃗ 1 = 0

$\mathbf{E}=\vec{t}^\wedge\mathbf{R}, \mathbf{R} = \mathbf{K}^{-T}\mathbf{E}\mathbf{K}^{-1}, \vec{x}_2^\top\mathbf{E}\vec{x}_1=\vec{p}_2^\top\mathbf{F}\vec{p}_1=0$

上式中，

$\mathbf{E}$
为
本质矩阵
(Essential Matrix),

$\mathbf{F}$
为
基础矩阵
(Fundamental Matrix).所以相机位势估计问题变为：

根据配对点的像素位置, 求出

$\mathbf{E}$
或

$\mathbf{F}$
;
根据

$\mathbf{E}$
或

$\mathbf{F}$
, 求出

R

,

t

⃗

本质矩阵

$\mathbf{E}$
性质：

尺度等价性：

$\mathbf{E}$
在不同尺度下等价
内在性质：

$\mathbf{E}$
的奇异值必定是

[

ρ

,

ρ

,

0

]
t

⃗

∧

R

如何求解本质矩阵

$\mathbf{E}$
:

方法一：因为

$\mathbf{E}$
有五个自由度，说明可以用五对点来求解

$\mathbf{E}$
。但

$\mathbf{E}$
的内在性质是非线性的，用线性的方程求解会带来问题。
方法二：从尺度等价性出发，用
八对点
来解方程。

八对点求解本质矩阵

$\mathbf{E}$

考虑一对匹配点,它们的归一化坐标为

x

⃗

1

=

[

u

1

,

v

1

,

1

]

⊤

→

[

]

⊤

$\vec{x}_1 = [u_1, v_1, 1]^\top$
,

x

⃗

2

=

[

u

2

,

v

2

,

1

]

⊤

→

[

]

⊤

$\vec{x}_2 = [u_2, v_2, 1]^\top$
，根据对极约束有：

[u 1, v 1, 1] ⊤ ⎡ ⎣ ⎢ e 1 e 4 e 7 e 2 e 5 e 8 e 3 e 6 e 9 ⎤ ⎦ ⎥ [u 2, v 2, 1] ⊤

$[u_1, v_1, 1]^\top \begin{bmatrix} e_1 & e_2 & e_3\\ e_4 & e_5 & e_6\\ e_7 & e_8 & e_9 \end{bmatrix} [u_2, v_2, 1]^\top$

把

$\mathbf{E}$
展开成向量表示，

e

⃗

=

[

e

1

,

⋯

,

e

9

]

⊤

→

[

⋯

]

⊤

$\vec{e}=[e_1,\cdots, e_9]^\top$
,则线性方程为：

[u 1 u 2, u 1 v 2, u 1, v 1 u 2, v 1 v 2, v 1, u 2, v 2, 1] \cdot e ⃗ = 0 ⃗

$[u_1u_2, u_1v_2, u_1, v_1u_2, v_1v_2, v_1, u_2, v_2, 1]\cdot \vec{e} = \vec{0}$

对其他点对，也有类似表示。把这8个点对的方程放在一起可组成一个线性方程：

⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ u 11 u 12 u 21 u 22 u 31 u 32 ⋮ u 81 u 82 u 11 v 12 u 21 v 22 u 31 v 32 ⋮ u 81 v 82 u 11 u 21 u 31 ⋮ u 81 v 11 u 12 v 21 u 22 v 31 u 32 ⋮ v 81 u 82 v 11 v 12 v 21 v 22 v 31 v 32 ⋮ v 81 v 82 v 11 v 21 v 31 ⋮ v 81 u 12 u 22 u 32 ⋮ u 82 v 12 v 22 v 32 ⋮ v 82 111 ⋮ 1 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ e ⃗ = 0 ⃗

$\begin{bmatrix} u_1^1u_2^1 & u_1^1v_2^1& u_1^1& v_1^1u_2^1& v_1^1v_2^1& v_1^1& u_2^1& v_2^1& 1\\ u_1^2u_2^2 & u_1^2v_2^2& u_1^2& v_1^2u_2^2& v_1^2v_2^2& v_1^2& u_2^2& v_2^2& 1\\ u_1^3u_2^3 & u_1^3v_2^3& u_1^3& v_1^3u_2^3& v_1^3v_2^3& v_1^3& u_2^3& v_2^3& 1\\ \vdots & \vdots & \vdots& \vdots & \vdots & \vdots& \vdots& \vdots & \vdots\\ u_1^8u_2^8 & u_1^8v_2^8& u_1^8& v_1^8u_2^8& v_1^8v_2^8& v_1^8& u_2^8& v_2^8& 1 \end{bmatrix} \vec{e}=\vec{0}$

则问题变为：

如何根据已估得的本质矩阵

$\mathbf{E}$
，恢复得到

$\mathbf{R}$
和

t

⃗

→

$\vec{t}$

.

不妨设

$\mathbf{E}$
的SVD分解为:

E = U Σ V ⊤

$\mathbf{E} = \mathbf{U} \mathbf{\Sigma} \mathbf{V}^\top$

其中

$\mathbf{U}$
和

$\mathbf{V}$
为正交阵，

$\mathbf{\Sigma}$
为奇异值矩阵，根据之前的推断

Σ

=

d

i

a

g

(

ρ

,

ρ

,

0

)

(

)

$\Sigma=diag(\rho, \rho, 0)$
,可知对任一

$\mathbf{E}$
存在两个可能的

$\mathbf{R}$
和

t

⃗

→

$\vec{t}$
与之对应：

t ⃗ \land 1 = U R Z (π 2) Σ U ⊤, R 1 = U R ⊤ Z (π 2) V ⊤ t ⃗ \land 2 = U R Z (- π 2) Σ U ⊤, R 2 = U R ⊤ Z (- π 2) V ⊤

$\begin{aligned} \vec{t}_1^\wedge=\mathbf{U}\mathbf{R}_Z(\frac{\pi}{2})\mathbf{\Sigma} \mathbf{U}^\top, ~~\mathbf{R}_1 = \mathbf{U}\mathbf{R}_Z^\top(\frac{\pi}{2}) \mathbf{V}^\top\\ \vec{t}_2^\wedge=\mathbf{U}\mathbf{R}_Z(-\frac{\pi}{2})\mathbf{\Sigma} \mathbf{U}^\top, ~~\mathbf{R}_2 = \mathbf{U}\mathbf{R}_Z^\top(-\frac{\pi}{2}) \mathbf{V}^\top \end{aligned}$

式中

R

Z

(

π

2

)

(

)

$\mathbf{R}_Z(\frac{\pi}{2})$
表示为沿Z轴旋转90度得到的旋转矩阵。此外，由于-

$\mathbf{E}$
和

$\mathbf{E}$
等价，对任意的

t

⃗

→

$\vec{t}$
取负号也会得到同样结果。所以从

$\mathbf{E}$
分解到

$\mathbf{R}$
和

t

⃗

→

$\vec{t}$
时一共存在四个可能的解。

后续为了检查哪个解是正确的时候，可以把任意一点代入四个解中，当该点在两个相机下的深度皆为正的深度时(即解为正数)，即可确认该解是所述问题的正确的解。

剩下的问题

：

如何确认解出的

$\mathbf{E}$
满足内在性质

？

假设对

$\mathbf{E}$
做SVD分解后，奇异值矩阵

Σ

=

d

i

a

g

(

ρ

1

,

ρ

2

,

ρ

3

)

(

)

$\Sigma=diag(\rho_1, \rho_2, \rho_3)$
, 不妨设

ρ

1

≥

ρ

2

≥

ρ

3

≥

3

$\rho_1\geq \rho_2\geq \rho_3$
，构造：

E = U d i a g (ρ 1 + ρ 2 2, ρ 2 + ρ 3 2, 0) V ⊤

$\mathbf{E} = \mathbf{U}diag\left(\frac{\rho_1+\rho_2}{2}, \frac{\rho_2 + \rho_3}{2}, 0\right) \mathbf{V}^\top$

即把求出的矩阵投影到

$\mathbf{E}$
的流形上，即可保证其满足内在性质。更简单的做法是直接将奇异值矩阵取为diag(1, 1, 0)使得

$\mathbf{E}$
具有尺度等价性。

单应矩阵

单应矩阵(Homography)

$\mathbf{H}$
:描述了两个平面之间的映射关系。它描述了处于共同平面上的一些点在两张图像之间的变换关系。

假设图像

I

1

1

$I_1$
和

I

2

2

$I_2$
有匹配好的点

p

1

1

$p_1$
和

p

2

2

$p_2$
，这些特征点落在平面上，平面满足方程：

n ⃗ ⊤ P + d = 0

$\vec{n}^\top\mathbf{P} + d = 0$

整理有：

- n ⃗ ⊤ P d = 1

$-\frac{\vec{n}^\top\mathbf{P}}{d} = 1$

则：

p ⃗ 2 = K (R P + t ⃗) = K (R P + t ⃗ \cdot (- n ⃗ ⊤ P d)) = K (R - t ⃗ n ⃗ ⊤ d) P = K (R - t ⃗ n ⃗ ⊤ d) K - 1 p 1

$\begin{aligned} \vec{p}_2&=\mathbf{K}(\mathbf{RP} + \vec{t})\\ &=\mathbf{K}(\mathbf{RP} + \vec{t}\cdot (-\frac{\vec{n}^\top\mathbf{P}}{d} ) )\\ &=\mathbf{K}(\mathbf{R} - \frac{\vec{t}\vec{n}^\top}{d} )\mathbf{P}\\ &=\mathbf{K}(\mathbf{R} - \frac{\vec{t}\vec{n}^\top}{d} )\mathbf{K}^{-1}p_1 \end{aligned}$

这是一个关于图像坐标

p

1

1

$p_1$
和

p

2

2

$p_2$
的变换，为方便把中间这部记为

$\mathbf{H}$
, 于是：

p ⃗ 2 = H p ⃗ 1

$\vec{p}_2 = \mathbf{H}\vec{p}_1$

为求解

$\mathbf{H}$
，类似于

$\mathbf{E}$
的做法，将上式展开：

⎡ ⎣ ⎢ u 2 v 2 1 ⎤ ⎦ ⎥ = ⎡ ⎣ ⎢ h 1 h 4 h 7 h 2 h 5 h 8 h 3 h 6 h 9 ⎤ ⎦ ⎥ ⎡ ⎣ ⎢ u 1 v 1 1 ⎤ ⎦ ⎥

$\begin{bmatrix} u_2\\v_2\\1 \end{bmatrix} =\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}$

为简化问题，实际上常常乘以一个非0因子使得

h

9

=

1

1

$h_9=1$
,即乘上

1

/

(

h

7

u

1

+

h

8

v

1

+

h

9

)

(

)

$1/(h_7u_1+h_8v_1+h_9)$
，再次展开得到式子：

u 2 = h 1 u 1 + h 2 v 1 + h 3 h 7 u 1 + h 8 v 1 + h 9 v 2 = h 4 u 1 + h 5 v 1 + h 6 h 7 u 1 + h 8 v 1 + h 9

$\begin{aligned} u_2 = \frac{h_1u_1 + h_2v_1 + h_3}{h_7u_1+h_8v_1+h_9}\\ v_2 = \frac{h_4u_1 + h_5v_1 + h_6}{h_7u_1+h_8v_1+h_9} \end{aligned}$

注意到

h

9

=

1

1

$h_9=1$
, 整理后可得到：

u 2 = (h 1 u 1 + h 2 v 1 + h 3) - (h 7 u 1 + h 8 v 1) u 2 v 2 = (h 4 u 1 + h 5 v 1 + h 6) - (h 7 u 1 + h 8 v 1) v 2

$\begin{aligned} u_2 = (h_1u_1 + h_2v_1 + h_3)-(h_7u_1+h_8v_1)u_2\\ v_2 = ({h_4u_1 + h_5v_1 + h_6})-({h_7u_1+h_8v_1})v_2 \end{aligned}$

由此可见一对匹配点可构造两项约束，于是自由度为8的单应矩阵可通过4对匹配点来算出。构造方程如下：

⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ u 11 0 u 21 0 u 31 0 u 41 0 v 11 0 v 21 0 v 31 0 v 41 0 10101010 0 u 11 0 u 21 0 u 31 0 u 41 0 v 11 0 v 21 0 v 31 0 v 41 01010101 - u 11 u 12 - u 11 v 12 - u 21 u 22 - u 21 v 22 - u 31 u 32 - u 31 v 32 - u 41 u 42 - u 41 v 42 v 11 u 12 - v 11 v 12 v 21 u 22 - v 21 v 22 v 31 u 32 - v 31 v 32 v 41 u 42 - v 41 v 42 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ u 12 v 12 u 22 v 22 u 32 v 32 u 42 v 42 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$\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}$

解线性方程可得到

$\mathbf{H}$
.此法称为直接线性变换法(Direct Linear Transform).

同本质矩阵相似，为验证

$\mathbf{H}$
，对

$\mathbf{H}$
做SVD分解后可得到四组旋转矩阵与向量，考虑以下事实：

成像的地图点的深度是否全为正值？若是可排除两组解。
场景中的平面的法向量。若场景平面与相机平面平行，又可排除一组解，其法向量

n

⃗

问题讨论：

尺度不确定性问题：对

t

⃗
初始化的纯旋转问题：若相机发生的是纯旋转，导致

t

⃗
多于八对点的情况：不妨设线性化后的对极约束等式中，左侧的系数矩阵为

$\mathbf{A}$
:

$A e ⃗ = 0 ⃗$
$\mathbf{A}\vec{e} = \vec{0}$

只用八点法的话，

$\mathbf{A}$
的大小为8*9；当多于八对点时，可以通过最小化二次型来求解：

$min e ⃗ ∥ A e ⃗ ∥ 22 = min e ⃗ e ⃗ ⊤ A ⊤ A e ⃗$
$\underset{\vec{e}}{\min} \|\mathbf{A}\vec{e}\|^2_2 = \underset{\vec{e}}{\min} \vec{e}^\top \mathbf{A}^\top\mathbf{A}\vec{e}$

这样就求出了在最小二乘意义下的矩阵。也可以通过

Random Sample Concensus

(随机采样一致性)来求解。

对极约束

八对点求解本质矩阵 E E \mathbf{E}

单应矩阵

问题讨论：

你可能也喜欢

八对点求解本质矩阵

$\mathbf{E}$