本文主要参考高翔博士的视觉SLAM十四讲第二版中的7.3章节内容。

1 对极约束

现在，假设我们从两张图像中得到了一对配对好的特征点，如图7.9所示（假如后面我们有若干对这样的匹配点，根据这些点的匹配关系，我们就可以恢复处相机的运动），目的是希望求取两帧图像之间的运动：

假设：

• 第一帧和第二帧的运动为$R，t$
• 两个相机的中心分别为$O_1，O_2$
• $I_1$中有一个特征点$p_1$
• $I_2$中对应特征点$p_2$

如果$p_1,p_2$两个特征点匹配正确，说明它们确实是同一个空间点在两个成像平面上的投影。

那么在以上的假设和匹配正确的基础上，有以下术语来描述它们的关系，连线$\overrightarrow{O_1{p}_1}$和连线$\overrightarrow{O_2{p}_2}$在三维空间中会相交于点$P$：

• 极平面：$O_1，O_2，P$三个点确定的平面
• 基线：$O_1，O_2$的连线
• 极点：$O_1，O_2$的连线与成像平面$I_1，I_2$的交点$e_1，e_2$
• 极线：极平面与两个像平面$I_1，I_2$之间的相交线$l_1，l_2$

从代数角度来分析这里的几何关系，假设在第一帧的坐标系下，设$P$的空间位置为：
$$P=[X,Y,Z{]}^T\text{\hspace{1em}(1)}$$

根据针孔相机模型，可知两个像素点$p_1,p_2$的像素位置为：
$$s_1{p}_1=KP,s_2{p}_2=K(RP+t)\text{\hspace{1em}(2)}$$
其中$K$为相机内参矩阵，$R，t$为两个坐标系的相机运动。

由于齐次坐标下，一个向量将等于它自身乘以任意的非零常数，这通常表达一个投影关系，$s_1{p}_1$和$p_1$成投影关系，它们在齐次坐标的意义下是相等的（尺度意义下的相等），故记作：
$$sp\simeq p$$
那么，公式2的投影关系可写为：
$$p_1\simeq KP,p_2\simeq K(RP+t)\text{\hspace{1em}(3)}$$
现取：
$$x_1=K^{-1}{p}_1,x_2=K^{-1}{p}_2\text{\hspace{1em}(4)}$$
这里$x_1,x_2$是两个像素点的归一化平面上的坐标，代入公式3得：
$$x_2\simeq R{x}_1+t\text{\hspace{1em}(5)}$$
两边同时左乘$t^{\wedge }$得：$t^{\wedge }t=0$
$$t^{\wedge }{x}_2\simeq t^{\wedge }R{x}_1\text{\hspace{1em}(6)}$$
然后两侧再同时左乘$x_2^T$得：
$$x_2^T{t}^{\wedge }{x}_2\simeq x_2^T{t}^{\wedge }R{x}_1\text{\hspace{1em}(7)}$$
观测等式左侧，$t^{\wedge }{x}_2$是一个与$t$和$x_2$都垂直的向量，它再和$x_2$做内积时，结果是$0$，又由于左侧和右侧是齐次坐标下的尺度意义上的相等，$0$乘以任意非零常数的结果也为$0$，于是可以把$\simeq$写出通常的等号，即：
$$x_2^T{t}^{\wedge }R{x}_1=0\text{\hspace{1em}(8)}$$
然后，结合公式4，重新代入$p_1，p_2$得：
$$p_2^T{K}^{-T}{t}^{\wedge }R{K}^{-1}{p}_1=0\text{\hspace{1em}(9)}$$

公式8和公式9都称为对极约束，几何意义是$O_1，P,O_2$三者共面，对极约束中同时包含了平移和旋转，并习惯把中间的部分记作两个矩阵：基础矩阵$F$（Fundamental Matrix） 和 本质矩阵$E$（Essential Matrix）：
$$E=t^{\wedge }R,F=K^{-T}EK-1,x_2^{T}E{x}_1=p_2^{T}F{p}_1=0\text{\hspace{1em}(10)}$$
由于$E$和$F$只相差了相机内参，而内参在SLAM中一般是已知的，所以实践中往往使用更简单的$E$，以下继续从$E=t^{\wedge }R$，来解决相机位姿估计的问题：

1. 根据配对点的像素位置求出$E$
2. 根据$E$求出$R，t$

2 本质矩阵E

本质矩阵$E=t^{\wedge }R$，是一个3*3的矩阵，内有9个未知数，从构造方式来看，本质矩阵有以下特性：

• 本质矩阵是由对极约束定义的，由于对极约束是等式为零的约束，所以对E乘以任意非零常数后，对极约束依然满足，称为$E$在不同尺度下是等价的，即尺度等价性；
• 根据$E=t^{\wedge }R$，可证，本质矩阵$E$的奇异值必定是$[\sigma ,\sigma ,0{]}^T$的形式，即本质矩阵的内在性质；
• 由于平移和旋转各有3个自由度，但由于尺度等价性，所以$E$实际有5个自由度。

$E$实际有5个自由度，理论上最少可以用5对点来求解$E$，但$E$的内在性质是一种非线性性质，在估计时会带来麻烦，因此，可只考虑它的尺度等价性，使用8对点来估计$E$，即经典的八点法（Eight point algorithm），八点法只利用$E$的线性性质，故可在线性代数下求解：

假设一对匹配点，它们的归一化坐标为$x_1=[u_1,v_1,1{]}^T,x_2=[u_2,v_2,1{]}^T$，根据对极约束中的公式10，可得：

$$\left(\begin{array}{c}{u}_2,v_2,1\end{array}\right)\left(\begin{array}{ccc}{e}_1& e_2& e_3\\ e_4& e_5& e_6\\ e_7& e_8& e_9\end{array}\right)\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)=0\text{\hspace{1em}(11)}$$
其中本质矩阵$E$展开，写成向量的形式：
$$e=[e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9{]}^T$$

那么对极约束公式11可写成与$e$有关的线性形式：
$$[u_2{u}_1,u_2{v}_1,u2,v_2{u}_1,v_2{v}_1,v_2,u_1,v_1,1]\cdot e=0\text{\hspace{1em}(12)}$$

同理，八点法中的其他7个点也有相同的表示，8个点放到一个线性方程中，即：
$$\left(\begin{array}{ccccccccc}{u}_2^1{u}_1^1& u_2^1{v}_1^1& u_2^1& v_2^1{u}_1^1& v_2^1{v}_1^1& v_2^1& u_1^1& v_1^1& 1\\ u_2^2{u}_1^2& u_2^2{v}_1^2& u_2^2& v_2^2{u}_1^2& v_2^2{v}_1^2& v_2^2& u_1^2& v_1^2& 1\\ \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots \\ u_2^8{u}_1^8& u_2^8{v}_1^8& u_2^8& v_2^8{u}_1^8& v_2^8{v}_1^8& v_2^8& u_1^8& v_1^8& 1\end{array}\right)\left(\begin{array}{c}{e}_1\\ e_2\\ e_3\\ e_4\\ e_5\\ e_6\\ e_7\\ e_8\\ e_9\end{array}\right)=0\text{\hspace{1em}(13)}$$

该线性方程组的系数矩阵由特征点位置构成，大小为8*9，$e$位于该矩阵中的零空间中，如果系数矩阵是满秩（秩为8），那么零空间维数为1，即$e$构成一条线，与本质矩阵$e$的尺度等价性是一致的。

假设8对匹配点的矩阵满足秩为8，那么通过公式13即可求得本质矩阵$E$。

那么接下来的问题就是如何根据已经估算的本质矩阵$E$，恢复出相机的运动姿态$R，t$，这个过程用奇异值分解（SVD）求解的，假设$E$的SVD为：
$$E=U\Sigma V^T\text{\hspace{1em}(14)}$$
其中$U,V$为正交阵，$\Sigma$为奇异值矩阵，根据$E$的内在性质，可知$\Sigma =diag(\sigma ,\sigma ,0)$，而在SVD分解中，对于任意一个$E$，存在两个可能的$R,t$与之对应：
$$\begin{array}{c}{t}_1^{\wedge }=U{R}_Z(\frac{\pi }{2})\Sigma U^T,R_1=U{R}_Z^T(\frac{\pi }{2})V^T\\ t_2^{\wedge }=U{R}_Z(-\frac{\pi }{2})\Sigma U^T,R_2=U{R}_Z^T(-\frac{\pi }{2})V^T\end{array}\text{\hspace{1em}(15)}$$
其中，$R_Z(\frac{\pi }{2})$表示沿$Z$轴旋转90°得到的旋转矩阵，同时，由于$-E$和$E$等价，所以对任意一个$t$取负号，也会得到同样的结果。因此，从$E$分解到$R，t$时，一共存在4个可能的解。

图7-10形象地展示了分解本质矩阵得到的4个解。我们已知空间点在相机（蓝色线）上的 投影（红色点），想要求解相机的运动。在保持红色点不变的情况下，可以画出4种可能的情况。 不过幸运的是，只有第一种解中$P$在两个相机中都具有正的深度。因此，只要把任意一点代入4 种解中检测该点在两个相机下的深度，就可以确定哪个解是正确的了。

剩下的一个问题是： 根据线性方程解出的$E$, 可能不满足$E$的内在性质，它的奇异值不 一定为$[\sigma ,\sigma ,0]$的形式。这时，我们会刻意地把$\Sigma$矩阵调整成上面的样子。通常的做法是：对八点 法求得的$E$进行SVD，会得到奇异值矩阵：$E=diag(\sigma 1,\sigma 2,\sigma 3)$， 不妨设$\sigma 1>\sigma 2>\sigma 3$。取：
$$E=Udiag(\frac{\sigma 1+\sigma 2}{2},\frac{\sigma 1+\sigma 2}{2},0)V^T\text{\hspace{1em}(16)}$$
这相当于是把求出来的矩阵投影到了$E$所在的流形上。当然， 更简单的做法是将奇异值矩阵取成$diag(l,1,0)$，因为$E$具有尺度等价性，所以这样做也是合理的。

3 单应矩阵

除了基本矩阵和本质矩阵，二视图中还有一种常见的矩阵：单应矩阵$H$（Humography），它描述了两个平面之间的映射关系。

单应矩阵通常描述处于共同平面上的一些点在两张图像之间的变换关系，设图像$I_1$ 和 $I_2$ 有一对匹配好的特征点$p_1$和$p_2$，这个特征点落在平面$P$上，设这个平面满足方程：

$$n^{T}P+d=0\text{\hspace{1em}(17)}$$

也即：
$$\frac{-n^{T}P}{d}=1\text{\hspace{1em}(18)}$$

然后，结合公式2和公式3，得：
$$\begin{array}{c}{p}_2\simeq K(RP+t)\\ \simeq K(RP+t)\cdot \frac{-n^{T}P}{d}\\ \simeq K(R-\frac{t{n}^T}{d})P\\ \simeq K(R-\frac{t{n}^T}{d})K^{-1}{p}_1\end{array}$$
于是，可得到一个直接描述图像坐标$p_1$和$p_2$之间的变换，把中间这部分定义为单应矩阵$H$，即：
$$p_2\simeq H{p}_1\text{\hspace{1em}(19)}$$
它的定义与旋转、平移及平面的参数有关。与基础矩阵$F$类似，单应矩阵$H$也是一个3*3的矩阵，求解时的思路和F类似， 同样可以先根据匹配点计算H，然后将它分解以计算旋转和平移。把上式展开，即：
$$\left(\begin{array}{c}{u}_2,v_2,1\end{array}\right)\simeq \left(\begin{array}{ccc}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right)\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)\text{\hspace{1em}(20)}$$
这里仍旧是尺度意义下的相等，所以H矩阵也可以乘以任意非零常数，实际处理可令h_9=1（在它取非零值时），然后根据第3行，去掉这个非零因子，可得：
$$\begin{array}{c}{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{array}$$
整理可得：
$$\begin{array}{c}{u}_2=h_1{u}_1+h_2{v}_1+h_3-h_7{u}_1{u}_2-h_8{v}_1{u}_2\\ v_2=h_4{u}_1+h_5{v}_1+h_6-h_7{u}_1{v}_2-h_8{v}_1{v}_2\end{array}\text{\hspace{1em}(21)}$$

这样，一组匹配点对就可以构造出两项约束（事实上有三个约束，但是因为线性相关， 只取前两个），于是自由度为8的单应矩阵可以通过4对匹配特征点算出（在非退化的情况下， 即这些特征点不能有三点共线的情况），即求解以下的线性方程组（当$h_9＝0$时， 右侧为零）：
$$\left(\begin{array}{cccccccc}{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{array}\right)\left(\begin{array}{c}{h}_1\\ h_2\\ h_3\\ h_4\\ h_5\\ h_6\\ h_7\\ h_8\end{array}\right)=\left(\begin{array}{c}{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{array}\right)\text{\hspace{1em}(22)}$$
这种做法把$H$矩阵看成了向量，通过解该向擞的线性方程来恢复$H$,又称直接线性变换法（Direct Linear Transform, DLT）。与本质矩阵相似，求出单应矩阵后需要对其进行分解，才可以得到相应的$R，t$，分解的方法包括数值法和解析法。与本质矩阵相似的分解类似，单应矩阵的分解同样会返回4组解，同时，可计算出它们分别对应的场景点所在平面的法向量。如果已知成像的地图点的深度全为正值（即在相机前方），则又可排除两组解，最后仅剩两组解，这时需要通过更多的先验信息进行判断。通常，可通过假设已知场景平面的法向量来解决，如场景平面与相机平面平行，那法向量$n$的理论值为$1^T$。

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

Reference:
-

须知少时凌云志，曾许人间第一流。

⭐️👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍🌔