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

对极约束

问题描述：求取两帧图像 $I_1, I_2$ 之间的运动。设第一帧到第二帧的运动为 $\mathbf{R}, \vec{t}$ ，其中心分别为 $O_1, O_2$ ， $I_1$ 中有一个点 $p_1$ 对应到 $I_2$ 的点为 $p_2$ . $P$ 为两个摄像机在远处的交点。 $O_1O_2P$ 称为极平面(Epipolar plane)， $O_1O_2$ 连线与相机平面交点称为极点(Epipoles), $O_1O_2$ 称为基线， $l_1,l_2$ 为极线(Epipolar line).

以第一帧的坐标系作为基准，设 $P$ 坐标为 $\mathbf{P}=[X, Y, Z]^\top$ , $p_1, p_2$ 位置为：

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

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

K

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

R, \vec{t}

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

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

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

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

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

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

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

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

$\vec{x}_2 = \mathbf{R}\vec{x}_1 + \vec{t}$
两边左乘

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

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

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

$\vec{t}^\wedge\vec{x}_2 = \vec{t}^\wedge \mathbf{R}\vec{x}_1~~(\vec{x}_2 \vec{t}=0)$
两边再左乘

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

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

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

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

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

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

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

$\vec{t}^\wedge$ 和

{\vec{x}}_{2}

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

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

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

x_{1}, x_{2}

$x_1, x_2$ 得到：

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

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

{\vec{p}}_{2}^{⊤} K^{- ⊤} {\vec{t}}^{\land} R K^{- 1} {\vec{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$ 共面。
将中间拆为基础矩阵和本质矩阵，可简化约束为:

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

$\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$
上式中，

E

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

F

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

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

本质矩阵 $\mathbf{E}$ 性质：
- 尺度等价性： $\mathbf{E}$ 在不同尺度下等价
- 内在性质： $\mathbf{E}$ 的奇异值必定是 $[\rho, \rho, 0]$ 的形式
- $\vec{t}^\wedge\mathbf{R}$ 有6个自由度，而 $\mathbf{E}$ 有5个自由度

如何求解本质矩阵 $\mathbf{E}$ :
- 方法一：因为 $\mathbf{E}$ 有五个自由度，说明可以用五对点来求解 $\mathbf{E}$ 。但 $\mathbf{E}$ 的内在性质是非线性的，用线性的方程求解会带来问题。
- 方法二：从尺度等价性出发，用八对点来解方程。

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

考虑一对匹配点,它们的归一化坐标为 $\vec{x}_1 = [u_1, v_1, 1]^\top$ , $\vec{x}_2 = [u_2, v_2, 1]^\top$ ，根据对极约束有：

[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]^{⊤}

$[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$
把

E

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

\vec{e} = [e_{1}, \dots, 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 \vec{e} = \vec{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个点对的方程放在一起可组成一个线性方程：

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

$\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}$ 和 $\vec{t}$ .

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

E = U Σ V^{⊤}

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

U

$\mathbf{U}$ 和

V

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

Σ

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

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

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

E

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

R

$\mathbf{R}$ 和

\vec{t}

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

$\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} (\frac{π}{2})

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

E

$\mathbf{E}$ 和

E

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

\vec{t}

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

E

$\mathbf{E}$ 分解到

R

$\mathbf{R}$ 和

\vec{t}

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

后续为了检查哪个解是正确的时候，可以把任意一点代入四个解中，当该点在两个相机下的深度皆为正的深度时(即解为正数)，即可确认该解是所述问题的正确的解。
剩下的问题：如何确认解出的 $\mathbf{E}$ 满足内在性质？
假设对 $\mathbf{E}$ 做SVD分解后，奇异值矩阵 $\Sigma=diag(\rho_1, \rho_2, \rho_3)$ , 不妨设 $\rho_1\geq \rho_2\geq \rho_3$ ，构造：

E = U d i a g (\frac{ρ_{1} + ρ_{2}}{2}, \frac{ρ_{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$
即把求出的矩阵投影到

E

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

E

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

单应矩阵

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

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

$\vec{n}^\top\mathbf{P} + d = 0$
整理有：

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

$-\frac{\vec{n}^\top\mathbf{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}

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

$p_1$ 和

p_{2}

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

H

$\mathbf{H}$ , 于是：

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

$\vec{p}_2 = \mathbf{H}\vec{p}_1$
为求解

H

$\mathbf{H}$ ，类似于

E

$\mathbf{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}]

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

$h_9=1$ ,即乘上

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

$1/(h_7u_1+h_8v_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}

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

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

$\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对匹配点来算出。构造方程如下：

[\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}]

$\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}$
解线性方程可得到

H

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

同本质矩阵相似，为验证 $\mathbf{H}$ ，对 $\mathbf{H}$ 做SVD分解后可得到四组旋转矩阵与向量，考虑以下事实：
1) 成像的地图点的深度是否全为正值？若是可排除两组解。
2) 场景中的平面的法向量。若场景平面与相机平面平行，又可排除一组解，其法向量 $\vec{n}$ 的理论值应为 $\vec{1}$ .
可以看到最后的解是通过场景事实筛选而获得的。

问题讨论：

1) 尺度不确定性问题：对 $\vec{t}$ 归一化时，会导致单目视觉的尺度不确定性(Scale Ambiguity). 当对两张图的 $\vec{t}$ 归一化时，相当于固定了尺度，即 $\vec{t}$ 的单位为1,称之为单目SLAM的初始化。要求初始化的两张图像须有一定程度的平移。
2) 初始化的纯旋转问题：若相机发生的是纯旋转，导致 $\vec{t}$ 为零，会导致无法求解 $\mathbf{R}$ . 因而要求：单目初始化不能只有纯旋转，必须要有一定程度的平移。
3) 多于八对点的情况：不妨设线性化后的对极约束等式中，左侧的系数矩阵为 $\mathbf{A}$ :

A \vec{e} = \vec{0}

$\mathbf{A}\vec{e} = \vec{0}$
只用八点法的话，

A

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

min_{\vec{e}} ‖ A \vec{e} ‖_{2}^{2} = min_{\vec{e}} {\vec{e}}^{⊤} A^{⊤} A \vec{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(随机采样一致性)来求解。