摘要

在图像特征匹配中我们提到，有了特征匹配，就可以估计相机的位姿变换，这个过程被称为对极几何。

对极几何

假设相机由如图的位姿变换。$P$为空间中的某一点，在两幅图像中的像素坐标分别为$p_1$和$p_2$，在相机坐标系下的坐标为$c_{1}$和$c_{2}$，根据针孔相机中的坐标变换公式可知：
$z_{1}p_1=Kc_{1}$
$z_{2}p_2=Kc_{2}$
其中$z_{1}$是$c_{1}$在$Z$轴上的坐标，$z_{2}$是$c_{2}$在$Z$轴上的坐标，$K$是相机的内参矩阵。

由于$C_0,C_1,P$三点共面，所以有：
$\overrightarrow{C_0 P} \cdot (\overrightarrow{C_0 C_1}\times \overrightarrow{C_1 P}) = 0 \Rightarrow c_{1} \cdot (t\times Rc_{2}) = 0 $
其中$R$为旋转矩阵，$t$为平移向量。

又因为两个向量的叉乘可以表示为第一个向量的反对称矩阵乘以另一个向量，因此上式可以转变为：
$ c_{1}^T [t]_{\times} R c_{2} = 0 $
其中$[t]_{\times}$为反对称矩阵，具体形式为：
$[t]_{\times}=\begin{bmatrix}
0 & -a_3 & a_2 \\
a_3 & 0 & -a_1 \\
-a_2 & a_1 & 0
\end{bmatrix}$

代入$p_1$和$p_2$可得：
$ p_{1}^T K^{-T} [t]_{\times} R K^{-1} p_{2} = 0 $

一般用$E=[t]_{\times} R$，称为本质矩阵(Essential Marix)。
一般用$F=K^{-T} [t]_{\times} R K^{-1}$，称为基础矩阵(Fundamental Matrix)。
因此上式可以转变为：
$c_{1}^T E c_{2} = 0$
$p_{1}^T F p_{2} = 0$
这两个等式称为对极约束。

但是在2D-2D问题中，我们只有像素坐标$p_{1}$和$p_{2}$，$c_{1}$和$c_{2}$都是未知的，那该怎么办？
此时可以引入一个归一化坐标的概念。即令：
$
x_1 = \dfrac{c_1}{z_1} = K^{-1}p_1 \\
x_2 = \dfrac{c_2}{z_2} = K^{-1}p_2
$

因此有：
$
x_1Ex_2=0
$
其中$x_1$和$x_2$可以从像素坐标$p_1$和$p_2$中求得。$E$和$F$之间存在转换关系：$F=K^{-T}EK^{-1}$

对极约束简洁地给出了两幅图像之间的空间位姿关系，于是相机位姿估计问题变为以下两步：
1.根据两幅图像的像素坐标，求出$E$或者$F$。
2.根据$E$或者$F$，求出$R$，$t$。

本质矩阵求解

本质矩阵相对于基础矩阵来说，相差一个相机的内参矩阵，在SLAM问题中，这个矩阵往往是已知的。因此在估计相机位姿变化时，通常求解形式相对简单的本质矩阵。

考虑一对匹配点的像素坐标$p_1$为$[u_1, v_1, 1]$，$p_2$为$[u_2, v_2, 1]$，则根据对极约束有:

$(u_1, v_1, 1)\begin{pmatrix}
e_1 & e_2 & e_3 \\
e_4 & e_5& e_6 \\
e_7 & e_8& e_9
\end{pmatrix}(u_2, v_2, 1)=0$

另$e=[e_1, e_2,…,e_9]^T$，则上式可转化为：
$[u_1u_2, u_1v_2, u_1, v_1u_2, v_1v_2, v_1 , u_2, v_2, 1] \cdot e = 0$
对于这个线性方程，存在尺度等价性，即对$e$乘以任何常数，等式依旧成立。因此即使$e$有9个未知变量，只需要8个方程，构成线性方程组即可对$e$进行求解。这就是求解本质矩阵最经典的八点法。

旋转矩阵是一个正交矩阵，因此从本质矩阵恢复t和R通常对本质矩阵进行奇异值分解：
$E=U \Sigma V^T$
则：
$R=UR_Z^T(\pm \frac{\pi}{2})V^T$
$[T]_{\times}=U R_Z^T(\pm \frac{\pi}{2}) \Sigma U^T$

其中$R_Z^T(\pm \frac{\pi}{2})$表示绕Z轴旋转90度得到的旋转矩阵。
因此$R$,$T$共有4种组合情况。如图所示。

只有第一种解正确的，因为P点在两个相机中的深度都是为正的。因此只要把任意一点代入四种解中，检测该点在两个相机下的深度，就可以确定哪个解是正确的。

本质矩阵有几个非常重要的特性：
1）基础矩阵是由对极约束定义的。由于对极约束是等式为零的约束，所以对$E$乘以任意非零常数后，对极约束依然满足。因此$E$具有不同尺度下的尺度等价性。
2）根据$E=[t]_{\times}R$可以证明，本质矩阵E的奇异值必定是$[\sigma ,\sigma ,0]^T$的形式，这称为本质矩阵的内在性质。
3）由于平移和旋转各有三个自由度，故$[t]_{\times}$共有6个自由度。但由于E具有尺度等价性，故$E$实际上有五个自由度。

因为一些误差的存在，通过8点法计算得到的本质矩阵$E$可能不会严格满足上述的特性2，因此通常会对奇异值矩阵做调整。通常的做法是，对八点法求得的E进行SVD分解后，会得到奇异值矩阵$\Sigma=\textbf{diag}(\sigma_1, \sigma_2, \sigma_3)$。不妨设$\sigma_1 \geq \sigma_2 \geq \sigma_3$，取：
$E = U \textbf{diag}(\dfrac{\sigma_1 + \sigma_2}{2},\dfrac{\sigma_1 + \sigma_2}{2},0)$
由于$E$具有尺度等价性，因此更简单的做法是将奇异值矩阵取为$\textbf{diag}(1,1,0)$。

单应矩阵

除了基本矩阵和本质矩阵，还有一种单应矩阵(Homography)$H$，它描述了两个平面之间的映射关系。若场景中的特征点都落同一个平面上(比如墙，地面等)，则可以通过单应性来进行运动估计。

假设P在三维空间中的某个平面上，这个平面满足：
$n^TP+d=0$

则有：
$p_2 = K(R-\dfrac{tn^T}{d})K^{-1}p_1$

中间部分记为$H$，称为单应矩阵。于是
$p_2 = Hp_1$

单应矩阵的定义与旋转、平移以及平面的参数有关。对上式展开可得：

$\begin{pmatrix}
u_2 \\
v_2 \\
1
\end{pmatrix}=\begin{pmatrix}
h_1 & h_2 & h_3 \\
h_4 & h_5& h_6 \\
h_7 & h_8& h_9
\end{pmatrix}\begin{pmatrix}
u_1 \\
v_1 \\
1
\end{pmatrix}$

简单转换可得：
$u_2 = \dfrac{h_1 v_1+h_2 v_1+h_3}{h_7 v_1+h_8 v_1+h_9}$

$v_2 = \dfrac{h_4 v_1+h_5 v_1+h_6}{h_7 v_1+h_8 v_1+h_9}$

这样一组匹配点就可以构造2个方程。因为像素坐标为齐次坐标，所以单应矩阵乘以任意常数项，等式仍然成立。因此即使单应矩阵的未知变量为9个，但其自由度为8。因此需要4组匹配点，构造8个方程，来对单应矩阵进行求解。与本质矩阵相似，求出单应矩阵后需要对其进行分解，才可以得到相应的旋转矩阵R和平移向量t。

单应性在SLAM中具有重要意义。当特征点共面时或相机发生纯旋转时，基础矩阵的自由度下降，这就出现了所谓的退化。这时候继续用8点法求解基础矩阵会导致受到噪声的影响增加。为了避免这种情况，通常会同时估计基础矩阵F和单应矩阵H，选择重投影误差比较小的那个作为最终的运动估计矩阵。

讨论

首先由于$E$具有尺度等价性，因此它分解得到的$t$和$R$也有一个尺度等价性。而$R\in SO(3)$自身具有约束，因此可以认为$t$也具有尺度等价性。$t$的尺度等价性直接导致$t$的大小与现实世界中平移的大小并不是对应的。也就是说在单目SLAM中，每次都要确定$t$的尺度，通常的做法是对$t$进行归一化。这称为单目SLAM的初始化。初始化之后就可以用3D-2D来计算相机运动，初始化之后的轨迹和地图的单位，就是初始化时固定的尺度。
其次即使可以用单应矩阵来处理纯旋转的情况，但是由于纯旋转缺少位移，因此无法对空间点进行三角化。也就是说对于单目SLAM来说，初始化必须要有位移。
最后我们提到用8点法求解本质矩阵。但是实际中往往匹配点会远远多于8对，此时有两种求解思路。
当匹配正确率比较高的时候，可以构造最小二乘。
当匹配错误率比较高的时候，可以用随机采样一致性$\textrm{(RANSAC)}$求解。

参考文献

《视觉SLAM十四讲》第七讲
Monocular slam 的理论基础(1)