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

对极约束

问题描述：求取两帧图像 $I_{1}, I_{2}$ 之间的运动。设第一帧到第二帧的运动为 $R, \vec{t}$ ，其中心分别为 $O_{1}, O_{2}$ ， $I_{1}$ 中有一个点 $p_{1}$ 对应到 $I_{2}$ 的点为 $p_{2}$ . $P$ 为两个摄像机在远处的交点。 $O_{1} O_{2} P$ 称为极平面(Epipolar plane)， $O_{1} O_{2}$ 连线与相机平面交点称为极点(Epipoles), $O_{1} O_{2}$ 称为基线， $l_{1}, l_{2}$ 为极线(Epipolar line).

以第一帧的坐标系作为基准，设 $P$ 坐标为 $P = [X, Y, Z]^{⊤}$ , $p_{1}, p_{2}$ 位置为：

s_{1} p_{1} = K P, s_{2} p_{2} = K (R P + \vec{t})

其中

K

为相机内参,

R, \vec{t}

为坐标系的相机运动。转化为齐次等式：

p_{1} = K P, p_{2} = K (R P + \vec{t})

取

{\vec{x}}_{1} = K^{- 1} {\vec{p}}_{1}

{\vec{x}}_{2} = K^{- 1} {\vec{p}}_{2}

,则有：

{\vec{x}}_{2} = R {\vec{x}}_{1} + \vec{t}

两边左乘

{\vec{t}}^{\land}

，有：

{\vec{t}}^{\land} {\vec{x}}_{2} = {\vec{t}}^{\land} R {\vec{x}}_{1} ({\vec{x}}_{2} \vec{t} = 0)

两边再左乘

{\vec{x}}_{2}^{⊤}

：

{\vec{x}}_{2}^{⊤} {\vec{t}}^{\land} {\vec{x}}_{2} = {\vec{x}}_{2}^{⊤} {\vec{t}}^{\land} R {\vec{x}}_{1}

因为

{\vec{t}}^{\land} {\vec{x}}_{2}

与

{\vec{t}}^{\land}

和

{\vec{x}}_{2}

皆为垂直，所以左侧为0：

{\vec{x}}_{2}^{⊤} {\vec{t}}^{\land} R {\vec{x}}_{1} = 0

再代入

x_{1}, x_{2}

得到：

(K^{- 1} {\vec{p}}_{2})^{⊤} {\vec{t}}^{\land} R K^{- 1} {\vec{p}}_{1} = 0

即：

{\vec{p}}_{2}^{⊤} K^{- ⊤} {\vec{t}}^{\land} R K^{- 1} {\vec{p}}_{1} = 0

此式即为对极约束，几何意义为 $O_{1}, O_{2}, P$ 共面。
将中间拆为基础矩阵和本质矩阵，可简化约束为:

E = {\vec{t}}^{\land} R, R = K^{- T} E K^{- 1}, {\vec{x}}_{2}^{⊤} E {\vec{x}}_{1} = {\vec{p}}_{2}^{⊤} F {\vec{p}}_{1} = 0

上式中，

E

为本质矩阵(Essential Matrix),

F

为基础矩阵(Fundamental Matrix).所以相机位势估计问题变为：

根据配对点的像素位置, 求出 $E$ 或 $F$ ;
根据 $E$ 或 $F$ , 求出 $R, \vec{t}$ .

本质矩阵 $E$ 性质：

尺度等价性： $E$ 在不同尺度下等价
内在性质： $E$ 的奇异值必定是 $[ρ, ρ, 0]$ 的形式
${\vec{t}}^{\land} R$ 有6个*度，而 $E$ 有5个*度

如何求解本质矩阵 $E$ :

方法一：因为 $E$ 有五个*度，说明可以用五对点来求解 $E$ 。但 $E$ 的内在性质是非线性的，用线性的方程求解会带来问题。
方法二：从尺度等价性出发，用八对点来解方程。

八对点求解本质矩阵 $E$

考虑一对匹配点,它们的归一化坐标为 ${\vec{x}}_{1} = [u_{1}, v_{1}, 1]^{⊤}$ , ${\vec{x}}_{2} = [u_{2}, v_{2}, 1]^{⊤}$ ，根据对极约束有：

[u_{1}, v_{1}, 1]^{⊤} [\begin{matrix} e_{1} & e_{2} & e_{3} \\ e_{4} & e_{5} & e_{6} \\ e_{7} & e_{8} & e_{9} \end{matrix}] [u_{2}, v_{2}, 1]^{⊤}

把

E

展开成向量表示，

\vec{e} = [e_{1}, \dots, e_{9}]^{⊤}

,则线性方程为：

[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 \vec{e} = \vec{0}

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

[\begin{matrix} u_{1}^{1} u_{2}^{1} & u_{1}^{1} v_{2}^{1} & u_{1}^{1} & v_{1}^{1} u_{2}^{1} & v_{1}^{1} v_{2}^{1} & v_{1}^{1} & u_{2}^{1} & v_{2}^{1} & 1 \\ u_{1}^{2} u_{2}^{2} & u_{1}^{2} v_{2}^{2} & u_{1}^{2} & v_{1}^{2} u_{2}^{2} & v_{1}^{2} v_{2}^{2} & v_{1}^{2} & u_{2}^{2} & v_{2}^{2} & 1 \\ u_{1}^{3} u_{2}^{3} & u_{1}^{3} v_{2}^{3} & u_{1}^{3} & v_{1}^{3} u_{2}^{3} & v_{1}^{3} v_{2}^{3} & v_{1}^{3} & u_{2}^{3} & v_{2}^{3} & 1 \\ ⋮ & ⋮ & ⋮ & ⋮ & ⋮ & ⋮ & ⋮ & ⋮ & ⋮ \\ u_{1}^{8} u_{2}^{8} & u_{1}^{8} v_{2}^{8} & u_{1}^{8} & v_{1}^{8} u_{2}^{8} & v_{1}^{8} v_{2}^{8} & v_{1}^{8} & u_{2}^{8} & v_{2}^{8} & 1 \end{matrix}] \vec{e} = \vec{0}

则问题变为：如何根据已估得的本质矩阵 $E$ ，恢复得到 $R$ 和 $\vec{t}$ .

不妨设 $E$ 的SVD分解为:

E = U Σ V^{⊤}

其中

U

和

V

为正交阵，

Σ

为奇异值矩阵，根据之前的推断

Σ = d i a g (ρ, ρ, 0)

,可知对任一

E

存在两个可能的

R

和

\vec{t}

与之对应：

\begin{aligned} {\vec{t}}_{1}^{\land} = U R_{Z} (\frac{π}{2}) Σ U^{⊤}, R_{1} = U R_{Z}^{⊤} (\frac{π}{2}) V^{⊤} \\ {\vec{t}}_{2}^{\land} = U R_{Z} (- \frac{π}{2}) Σ U^{⊤}, R_{2} = U R_{Z}^{⊤} (- \frac{π}{2}) V^{⊤} \end{aligned}

式中

R_{Z} (\frac{π}{2})

表示为沿Z轴旋转90度得到的旋转矩阵。此外，由于-

E

和

E

等价，对任意的

\vec{t}

取负号也会得到同样结果。所以从

E

分解到

R

和

\vec{t}

时一共存在四个可能的解。

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

剩下的问题：如何确认解出的 $E$ 满足内在性质？

假设对 $E$ 做SVD分解后，奇异值矩阵 $Σ = d i a g (ρ_{1}, ρ_{2}, ρ_{3})$ , 不妨设 $ρ_{1} \geq ρ_{2} \geq ρ_{3}$ ，构造：

E = U d i a g (\frac{ρ_{1} + ρ_{2}}{2}, \frac{ρ_{2} + ρ_{3}}{2}, 0) V^{⊤}

即把求出的矩阵投影到

E

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

E

具有尺度等价性。

单应矩阵

单应矩阵(Homography) $H$ :描述了两个平面之间的映射关系。它描述了处于共同平面上的一些点在两张图像之间的变换关系。
假设图像 $I_{1}$ 和 $I_{2}$ 有匹配好的点 $p_{1}$ 和 $p_{2}$ ，这些特征点落在平面上，平面满足方程：

{\vec{n}}^{⊤} P + d = 0

整理有：

- \frac{{\vec{n}}^{⊤} P}{d} = 1

则：

\begin{aligned} {\vec{p}}_{2} & = K (R P + \vec{t}) \\ = K (R P + \vec{t} \cdot (- \frac{{\vec{n}}^{⊤} P}{d})) \\ = K (R - \frac{\vec{t} {\vec{n}}^{⊤}}{d}) P \\ = K (R - \frac{\vec{t} {\vec{n}}^{⊤}}{d}) K^{- 1} p_{1} \end{aligned}

这是一个关于图像坐标

p_{1}

和

p_{2}

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

H

, 于是：

{\vec{p}}_{2} = H {\vec{p}}_{1}

为求解

H

，类似于

E

的做法，将上式展开：

[\begin{matrix} u_{2} \\ v_{2} \\ 1 \end{matrix}] = [\begin{matrix} h_{1} & h_{2} & h_{3} \\ h_{4} & h_{5} & h_{6} \\ h_{7} & h_{8} & h_{9} \end{matrix}] [\begin{matrix} u_{1} \\ v_{1} \\ 1 \end{matrix}]

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

h_{9} = 1

,即乘上

1 / (h_{7} u_{1} + h_{8} v_{1} + h_{9})

，再次展开得到式子：

\begin{aligned} u_{2} = \frac{h_{1} u_{1} + h_{2} v_{1} + h_{3}}{h_{7} u_{1} + h_{8} v_{1} + h_{9}} \\ v_{2} = \frac{h_{4} u_{1} + h_{5} v_{1} + h_{6}}{h_{7} u_{1} + h_{8} v_{1} + h_{9}} \end{aligned}

注意到

h_{9} = 1

, 整理后可得到：

\begin{aligned} 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} \end{aligned}

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

[\begin{matrix} 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} \end{matrix}] [\begin{matrix} h_{1} \\ h_{2} \\ h_{3} \\ h_{4} \\ h_{5} \\ h_{6} \\ h_{7} \\ h_{8} \end{matrix}] = [\begin{matrix} 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{matrix}]

解线性方程可得到

H

.此法称为直接线性变换法(Direct Linear Transform).

同本质矩阵相似，为验证 $H$ ，对 $H$ 做SVD分解后可得到四组旋转矩阵与向量，考虑以下事实：

成像的地图点的深度是否全为正值？若是可排除两组解。
场景中的平面的法向量。若场景平面与相机平面平行，又可排除一组解，其法向量 $\vec{n}$ 的理论值应为 $\vec{1}$ .
可以看到最后的解是通过场景事实筛选而获得的。

问题讨论：

尺度不确定性问题：对 $\vec{t}$ 归一化时，会导致单目视觉的尺度不确定性(Scale Ambiguity). 当对两张图的 $\vec{t}$ 归一化时，相当于固定了尺度，即 $\vec{t}$ 的单位为1,称之为单目SLAM的初始化。要求初始化的两张图像须有一定程度的平移。
初始化的纯旋转问题：若相机发生的是纯旋转，导致 $\vec{t}$ 为零，会导致无法求解 $R$ . 因而要求：单目初始化不能只有纯旋转，必须要有一定程度的平移。
多于八对点的情况：不妨设线性化后的对极约束等式中，左侧的系数矩阵为 $A$ :
$A \vec{e} = \vec{0}$
只用八点法的话， $A$ 的大小为8*9；当多于八对点时，可以通过最小化二次型来求解：
$min_{\vec{e}} ‖ A \vec{e} ‖_{2}^{2} = min_{\vec{e}} {\vec{e}}^{⊤} A^{⊤} A \vec{e}$
这样就求出了在最小二乘意义下的矩阵。也可以通过Random Sample Concensus(随机采样一致性)来求解。

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

对极约束

八对点求解本质矩阵EE

单应矩阵

问题讨论：

相关推荐

八对点求解本质矩阵 $E$