视觉SLAM14讲笔记04：ch7对极几何(epipolar geometry)

2D-2D:对极几何(epipolar geometry)

对极约束

现在，假设我们从两张图像中，得到了一对配对好的特征点，像图7-7里显示的那样。如果我们有若干对这样的匹配点，就可以通过这些二维图像点的对应关系，恢复出在两帧之间摄像机的运动。

连线 $\overline{O_1{p}_1}$ 和连线 $\overline{O_2{p}_2}$ 在三维空间中会相交于点 $P$。$O_1{O}_{2}P$称为极平面（Epipolar plane）。$e_1,e_2,$ 称为极点（Epipoles）， $O_1{O}_2$ 被称为基线（Baseline）。极平面与两个像平面 $I_1,I_2$ 之间的相交线 $l_1,l_2$ 为极线（Epipolar line）。

现在，我们从代数角度来看一下这里出现的几何关系。在第一监的坐标系下，设 P 的空间位置为:
$$\mathbf{P}=[X,Y,Z{]}^T$$
根据针孔相机模型，我们知道两个像素点 $p_1,p_2$ 的像素位置为:
$$s_1{\mathbf{p}}_1=\mathbf{K}\mathbf{P},s_2{\mathbf{p}}_2=\mathbf{K}(\mathbf{R}\mathbf{P}+\mathbf{t})$$
这里 $K$ 为相机内参矩阵，$R,t$ 为两个坐标系的相机运动。如果使用齐次坐标，我们也可以把上式写成在乘以非零常数下成立的(up to a scale) 等式
$${\mathbf{p}}_1=\mathbf{K}\mathbf{P},{\mathbf{p}}_2=\mathbf{K}(\mathbf{R}\mathbf{P}+\mathbf{t})$$
现在，取:
$${\mathbf{x}}_1={\mathbf{K}}^{-1}{\mathbf{p}}_1,{\mathbf{x}}_2={\mathbf{K}}^{-1}{\mathbf{p}}_2$$
这里的 ${\mathbf{x}}_1,{\mathbf{x}}_2$ 是两个像素点的归一化平面上的坐标。代入上式，得:
$${\mathbf{x}}_2=\mathbf{R}{\mathbf{x}}_1+\mathbf{t}$$
两边同时左乘 ${\mathbf{t}}^{\wedge }\circ$ 回忆 ${}^{\wedge }$ 的定义，这相当于两侧同时与 $\mathbf{t}$ 做外积:
$${\mathbf{t}}^{\wedge }{\mathbf{x}}_2={\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{x}}_1$$
然后，两侧同时左乘 ${\mathbf{x}}_2^T$
$${\mathbf{x}}_2^T{\mathbf{t}}^{\wedge }{\mathbf{x}}_2={\mathbf{x}}_2^T{\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{x}}_1$$
观察等式左侧，$t^{\wedge }{x}_2$ 是一个与 $\mathbf{t}$ 和 ${\mathbf{x}}_2$ 都垂直的向量。把它再和 ${\mathbf{x}}_2$ 做内积时，将得
到
0。因此，我们就得到了一个简洁的式子:
$${\mathbf{x}}_2^T{\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{x}}_1=0$$
重新代入 ${\mathbf{p}}_1,{\mathbf{p}}_2,$ 有:
$${\mathbf{p}}_2^T{\mathbf{K}}^{-T}{\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{K}}^{-1}{\mathbf{p}}_1=0$$
这两个式子都称为对极约束，它以形式简洁著名。它的几何意义是 $O_1,P,O_2$ 三者 共面。对极约束中同时包含了平移和旋转。我们把中间部分记作两个矩阵: 基础矩阵 （Fundamental Matrix）$F$ 和本质矩阵（Essential Matrix） $\mathbf{E}$, 可以进一步简化对极约束:
$$\mathbf{E}={\mathbf{t}}^{\wedge }\mathbf{R},\mathbf{F}={\mathbf{K}}^{-T}\mathbf{E}{\mathbf{K}}^{-1},{\mathbf{x}}_2^T\mathbf{E}{\mathbf{x}}_1={\mathbf{p}}_2^T\mathbf{F}{\mathbf{p}}_1=0$$
对极约束简洁地给出了两个匹配点的空间位置关系。于是，相机位姿估计问题变为以下两步：

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

本质矩阵

根据定义，本质矩阵 $E=t^{\wedge }R$ 。它是一个 $3\times 3$ 的矩阵，内有 9 个未知数。那么，是
不是任意一个 $3\times 3$ 的矩阵都可以被当成本质矩阵呢？从 $E$ 的构造方式上看，有以下值得
注意的地方:

1. 基础矩阵是由对极约束定义的。由于对极约束是等式为零的约束，所以对 $E$ 乘以任 意非零常数后，对极约束依然满足。我们把这件事情称为 $E$ 在不同尺度下是等价的。
2. 根据 $E=t^{\wedge }R,$ 可以证明本质矩阵 $E$ 的奇异值必定是 $[\sigma ,\sigma ,0{]}^T$ 的形式。这称为本质矩阵的内在性质。
3. 另一方面，由于平移和旋转各有三个自由度，故 $t^{\wedge }R$ 共有六个自由度。但由于尺度等价性，$E$ 实际上有五个自由度。

$E$ 具有五个自由度的事实，表明我们最少可以用五对点来求解 $E$。但是，E 的内在性质是一种非线性性质，在求解线性方程时会带来麻烦，因此，也可以只考虑它的尺度等价性，使用八对点来估计 $E$，这就是经典的八点法(Eight-point-algorithm)。

考虑一对匹配点，它们的归一化坐标为: $x_1={\left[u_1,v_1,1\right]}^T,{\mathbf{x}}_2={\left[u_2,v_2,1\right]}^T$ 。根据对
极约束，有:
$$\left(\begin{array}{l}{u}_1,v_1,1\end{array}\right)\left(\begin{array}{lll}{e}_1& e_2& e_3\\ e_4& e_5& e_6\\ e_7& e_8& e_9\end{array}\right)\left(\begin{array}{l}{u}_2\\ v_2\\ 1\end{array}\right)=0$$
我们把矩阵 $\mathbf{E}$ 展开，写成向量的形式:
$$\mathbf{e}={\left[e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9\right]}^T$$
那么对极约束可以写成与 e 有关的线性形式:
$$\left[u_1{u}_2,u_1{v}_2,u_1,v_1{u}_2,v_1{v}_2,u_2,v_2,1\right]\cdot \mathbf{e}=0$$
这八个方程构成了一个线性方程组。它的系数矩阵由特征点位置构成，大小为 $8\times 9$。

接下来的问题是如何根据已经估得的本质矩阵 $\mathbf{E},$ 恢复出相机的运动 $\mathbf{R},\mathbf{t}$ 。这个过 程是由奇异值分解 (SVD) 得到的。设 $E$ 的 SVD 分解为
$$E=U\Sigma V^T$$
其中 $U,V$ 为正交阵， $\Sigma$ 为奇异值矩阵。根据 $E$ 的内在性质，我们知道 $\Sigma =\mathrm{diag}(\sigma ,\sigma ,0)$在SVD分解中，对于任意一个 $E$, 存在两个可能的$t,R$与它对应：
$$\begin{array}{l}{t}_1^{\wedge }=U{R}_Z\left(\frac{\pi }{2}\right)\Sigma U^T,R_1=U{R}_Z^T\left(\frac{\pi }{2}\right)V^T\\ t_2^{\wedge }=U{R}_Z\left(-\frac{\pi }{2}\right)\Sigma U^T,R_2=U{R}_Z^T\left(-\frac{\pi }{2}\right)V^T\end{array}$$
其中 $R_Z\left(\frac{\pi }{2}\right)$ 表示沿 $Z$ 轴旋转 90 度得到的旋转矩阵。同时，由于 $-E$ 和 $E$ 等价，所 以对任意一个 $t$ 取负号，也会得到同样的结果。因此，从 $E$ 分解到 $t,R$ 时，一共存在四个可能的解。幸运的是，只有一种解中，$P$ 在两个相机中都具有正的深度。因此，只要把任意一点代入四种解中，检测该点在两个相机下的深度，就可以确定哪个解是正确的了。

剩下的问题还有一个: 根据线性方程解出的 $E,$ 可能不满足 $E$ 的内在性质一它 的奇异值不一定为 $\sigma ,\sigma ,0$ 的形式。这时，在做 SVD 时，我们会刻意地把 \Sigma 矩阵调整成上面的样子。通常的做法是，对八点法求得的 E 进行 SVD 分解后，会得到奇异值矩阵
$\Sigma =\mathrm{diag}\left({\sigma }_1,{\sigma }_2,{\sigma }_3\right),$ 不妨设 ${\sigma }_1\ge {\sigma }_2\ge {\sigma }_3$。 取:
$$E=U\mathrm{diag}\left(\frac{{\sigma }_1+{\sigma }_2}{2},\frac{{\sigma }_1+{\sigma }_2}{2},0\right)V^T$$

当给定的点数多于八对时，我们可以计算一个最小二乘解。
$$\underset{e}{\min }\Vert Ae{\Vert }_2^2=\underset{e}{\min }{e}^T{A}^{T}Ae$$

不过，当可能存在误匹配的情况时，倾向于使用随机采样一致性(Random Sample Concensus, RANSAC)来求，而不是最小二乘。

单应矩阵

除了基本矩阵和本质矩阵，我们还有一种称为单应矩阵(Homography) $H$ 的东西, 它描述了两个平面之间的映射关系。若场景中的特征点都落在同一平面上(比如墙，地面等)，则可以通过单应性来进行运动估计。这种情况在无人机携带的俯视相机，或扫地机携带的顶视相机中比较常见。

单应性在SLAM中具重要意义。当特征点共面，或者相机发生纯旋转的时候，基础矩阵的自由度下降，这就出现了所谓的退化(degenerate)。现实中的数据总包含一些噪声，这时候如果我们继续使用八点法求解基础矩阵，基础矩阵多余出来的自由度将会主要由噪声决定。为了能够避免退化现象造成的影响，通常我们会同时估计基础矩阵 $F$ 和单应矩阵 $H$ , 选择重投影误差比较小的那个作为最终的运动估计矩阵。