[三维重建] 对极几何约束、本质矩阵、基础矩阵、单应矩阵

一、对极几何约束

假设相机从$O_1$经过旋转平移运动，$R，t$，到达新的位置$O_2$ 。$I_1，I_2$分别为相机在两个位置的成像平面，空间中同一点$P$，在两个成像平面上的投影分别为$p_1，p_2$。

极平面：$O_1，P，O_2$三点确定的平面（$\Delta O_{1}P{O}_2$所在的平面）
极点：$O_1，O_2$连线与两个成像平面$I_1，I_2$的交点$e_1，e_2$
基线：两个相机光心的连线$O_1{O}_2$
极线：极平面与两个成像平面之间的相交线$l_1，l_2$

1、本质矩阵

假设在第一帧所在坐标系下，点P的坐标为：
$$P=\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]$$
根据针孔相机模型的投影公式，两个成像平面上像素点$p_1，p_2$的坐标位置为：
$$s_1{p}_1=KP\text{\hspace{1em}(1)}$$
$$s_2{p}_2=K(RP+t)\text{\hspace{1em}(2)}$$
这里，$K$为$3\times 3$的相机内参数矩阵；$R，t$为相机的运动，具体来说，这里应该是$R_{21}，t_{21}$，将$O_1$坐标系下的坐标$P$转换到了$O_2$坐标系下的坐标$(R_{21}P+t_{21})$；$s_1$为深度信息，即空间点$P$到相机光心$O_1$的距离，$s_2$则为空间点$P$到相机光心$O_2$的距离，两者并不相等；$p_1，p_2$为$3\times 1$的齐次坐标形式，$P$为$3\times 1$的欧氏坐标形式。

根据针孔相机模型的投影公式：
$$Z\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=K\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]$$
令等式两边同时乘$\frac{1}{Z}$：
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=K\left[\begin{array}{c}\frac{X}{Z}\\ \\ \frac{Y}{Z}\\ \\ 1\end{array}\right]$$
令等式两边同时左乘$K^{-1}$：
$$K^{-1}\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{c}\frac{X}{Z}\\ \\ \frac{Y}{Z}\\ \\ 1\end{array}\right]$$
可以看出，对像素点二维坐标的齐次坐标左乘$K^{-1}$后，并不能恢复到空间点的三维坐标$\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]$，而是得到这个三维坐标在归一化平面（$Z=1的平面$）上的坐标：$\left[\begin{array}{c}\frac{X}{Z}\\ \\ \frac{Y}{Z}\\ \\ 1\end{array}\right]$

现在，取：
$$x_1=K^{-1}{p}_1\text{\hspace{1em}(3)}$$
$$x_2=K^{-1}{p}_2\text{\hspace{1em}(4)}$$

可知，$x_1，x_2$是两个像素点的归一化平面上的坐标。

联立式（1）（2）（3）（4）：

由$s_2{p}_2=K(RP+t)$等式两边左乘$K^{-1}$：
$$K^{-1}{s}_2{p}_2=RP+t$$
$$s_2{K}^{-1}{p}_2=RP+t$$
$$s_2{x}_2=RP+t\text{\hspace{1em}(5)}$$
由$s_1{p}_1=KP$等式两边左乘$K^{-1}$：
$$K^{-1}{s}_1{p}_1=P$$
$$s_1{K}^{-1}{p}_1=P$$
$$s_1{x}_1=P$$
将$s_1{x}_1=P$代入式（5），得：
$$s_2{x}_2=R{s}_1{x}_1+t$$
在等式两侧同时左乘 $t^{\wedge }$：
$$\begin{gathered} s_2{t}^{\wedge }{x}_2=s_1{t}^{\wedge }R{x}_1+t^{\wedge }t \\ =s_1{t}^{\wedge }R{x}_1 \\ (后面那块t\times t的结果是零向量，所以没有了) \end{gathered}$$
${}^{\wedge }$的定义：在《视觉SLAM十四讲》中，$a^{\wedge }$是与向量$a$一一对应的一个反对称矩阵，$a^{\wedge }b=a\times b$，实际上就是一个叉乘的矩阵表达形式，在某些地方$a^{\wedge }$也可能写成$a_{\times }$，知道是同一个意思就好

上式中，再同时左乘$x_2^T$：
$$s_2{x}_2^T{t}^{\wedge }{x}_2=s_1{x}_2^T{t}^{\wedge }R{x}_1$$
观察上述等式的左侧，$t^{\wedge }{x}_2$的结果是一个与$t，x_2$均垂直的向量，左乘$x_2^T$相当于$x_2$点乘向量$t^{\wedge }{x}_2$。由于向量$t^{\wedge }{x}_2$必定与$x_2$垂直，因此$x_2$点乘向量$t^{\wedge }{x}_2$的结果自然是0。
$$\frac{s_2}{s_1}{x}_2^T{t}^{\wedge }{x}_2=x_2^T{t}^{\wedge }R{x}_1$$
由于$x_2^T{t}^{\wedge }{x}_2=0$，$s_1，s_2$是常数，最终得到结果：
$$x_2^T{t}^{\wedge }R{x}_1=0\text{\hspace{1em}(6)}$$
将式（6）的中间部分$t^{\wedge }R$记作 $E$ ，即本质矩阵：
$$x_2^{T}E{x}_1=0$$

从式子中可以看到，本质矩阵约束了空间中同一个三维点，在两个归一化平面坐标之间的联系

2、基础矩阵

再将归一化坐标与像素点坐标的关系$x_1=K^{-1}{p}_1$，$x_2=K^{-1}{p}_2$ 代入式（6），得：
$$(K^{-1}{p}_2{)}^T{t}^{\wedge }R{K}^{-1}{p}_1=0$$
$$p_2^T{K}^{-T}{t}^{\wedge }R{K}^{-1}{p}_1=0\text{\hspace{1em}(7)}$$
将式（7）的中间部分$K^{-T}{t}^{\wedge }R{K}^{-1}$记作 $F$ ，即基础矩阵：
$$p_2^{T}F{p}_1=0$$
从式子中可以看到，基础矩阵约束了空间中同一个三维点，在两个成像平面上，像素坐标之间的联系，记住这里的$p_1，p_2$ 是 $3\times 1$ 的齐次坐标形式。

可以看出，基础矩阵 $F$ 与本质矩阵 $E$ 之间只相差相机内参数矩阵 $K$：
$$F=K^{-T}E{K}^{-1}$$

3、单应矩阵

（1）前置知识：

平面的表示形式：
$$n^{T}P+d=0$$
其中，n是平面的单位法向量，P是平面上一点，d是该平面距离坐标原点的有向距离，如果平面面向原点，则d为正，如果平面背向原点，则d为负。

推导：来自于平面的一般式
$$Ax+By+Cz+D=0$$
一般式中，（A,B,C）为平面的法向量，（x,y,z）为平面上任意一点。而D实际是平面距原点的有向距离。

（0,0,0）到平面的距离:
$$d=\frac{|A\times 0+B\times 0+C\times 0+D|}{\sqrt{A^2+B^2+C^2}}=\frac{|D|}{1}=|D|$$
在这里，d是无向距离，D是有向距离，有正负的。

所以，由一般式：
$$Ax+By+Cz+有向距离=0$$
$$n^{T}P+有向距离=0$$

(2)单应矩阵：

单应矩阵用于描述处于共同平面上的一些点（三维空间中）在两张图像（二维图像）之间的对应关系。

如上图所示，空间中存在一平面Π，平面的单位法向量为n，平面上一点P在两台摄像机所拍摄到的图像上的投影为 $p_1，p_2$

设平面Π满足方程：
$$n^{T}P+d=0$$
$$-\frac{n^{T}P}{d}=1$$
由于P在图像$I_2$上的投影为$p_2$：
$$s_2{p}_2=K(RP+t)$$
$$=K(RP+t\cdot (-\frac{n^{T}P}{d}))$$
将P提公因式：
$$=K(R-\frac{t{n}^T}{d})P$$
由于：
$$s_1{p}_1=KP$$
$$s_1{K}^{-1}{p}_1=P$$
有：
$$s_2{p}_2=K(R-\frac{t{n}^T}{d})s_1{K}^{-1}{p}_1$$
$$\frac{s_2}{s_1}{p}_2=K(R-\frac{t{n}^T}{d})K^{-1}{p}_1$$

$s_1，s_2$ 为尺度，在尺度等价下，有：

$$p_2\cong K(R-\frac{t{n}^T}{d})K^{-1}{p}_1$$

令
$$K(R-\frac{t{n}^T}{d})K^{-1}=H$$
即单应矩阵

$$p_2\cong H{p}_1$$

观察 $H$ 的形式能发现，单应矩阵的定义中包含了摄像机的旋转、平移以及平面的相关参数。

4、总结

本质矩阵

$$x_2^{T}E{x}_1=0$$
$$E=t^{\wedge }R$$
本质矩阵约束了空间中同一个三维点，在两个归一化平面坐标之间的联系。（归一化平面即Z=1的平面）

基础矩阵

$$p_2^{T}F{p}_1=0$$
$$F=K^{-T}{t}^{\wedge }R{K}^{-1}$$
$$F=K^{-T}E{K}^{-1}$$
基础矩阵约束了空间中同一个三维点，在两个成像平面上的像素坐标之间的联系。

且基础矩阵 $F$ 与本质矩阵 $E$ 之间只相差相机内参数矩阵 $K$。

单应矩阵

$$p_2\cong H{p}_1$$
$$H=K(R-\frac{t{n}^T}{d})K^{-1}$$
单应矩阵约束了空间中处于某一已知平面上的同一个三维点，在两个成像平面上的像素坐标之间的联系

二、本质矩阵求解

本质矩阵的自由度：$E=t^{\wedge }R$，是一个$3\times 3$的矩阵，内有9个未知数，因此存在9个自由度。

1.八点法

由于对极约束 $x_2^{T}E{x}_1=0$ 是等式为零的约束，意思是中间的本质矩阵 $E$ 即使经过任意常数 $k$ 倍的缩放，等式仍然成立：$x_2^{T}kE{x}_1=0$
意味着本质矩阵 $E$ 与它的 $k$ 倍缩放 $kE$ 是等价效果的，即 $E$ 在不同尺度下是等价的：
$$E\simeq kE$$
由于这种性质，可以将 $E$ 中所有元素均除以矩阵内任意一个非零元素，如$e_1$：
$$E=\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]\simeq \left[\begin{array}{ccc}\frac{e_1}{e_1}& \frac{e_2}{e_1}& \frac{e_3}{e_1}\\ \frac{e_4}{e_1}& \frac{e_5}{e_1}& \frac{e_6}{e_1}\\ \frac{e_7}{e_1}& \frac{e_8}{e_1}& \frac{e_9}{e_1}\end{array}\right]\simeq \left[\begin{array}{ccc}1& \frac{e_2}{e_1}& \frac{e_3}{e_1}\\ \frac{e_4}{e_1}& \frac{e_5}{e_1}& \frac{e_6}{e_1}\\ \frac{e_7}{e_1}& \frac{e_8}{e_1}& \frac{e_9}{e_1}\end{array}\right]$$
使其中某个元素处的值为1，从而减少一个自由度

在这种情况下，9个自由度的本质矩阵 $E$ 可变成8个自由度

考虑一对匹配点，它们的归一化坐标为$x_1=\left[\begin{array}{c}{U}_1\\ V_1\\ 1\end{array}\right]$，$x_2=\left[\begin{array}{c}{U}_2\\ V_2\\ 1\end{array}\right]$，根据本质矩阵约束有：
$$x_2^{T}E{x}_1=\left[\begin{array}{ccc}{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$$
$$\left[\begin{array}{ccccccccc}{U}_2{U}_1& U_2{V}_1& U_2& V_2{U}_1& V_2{V}_1& V_2& U_1& V_1& 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$$
可见，一对匹配点能得到一条方程，求解一个未知数，针对本质矩阵 $E$ 的八个自由度，需要求解八个未知数，即需要八对匹配点，组成一个齐次线性方程组：
$${\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\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ 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]}_{8\times 9}\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$$
解上述齐次线性方程组，即可得到本质矩阵 $E$ 。

2.五点法

本质矩阵 $E=t^{\wedge }R$，根据本质矩阵的定义，平移有3个自由度，旋转有3个自由度，因此本质矩阵可以缩小到6个自由度，再根据八点法中提到的尺度等价约束，可以再缩小一个自由度，最终本质矩阵实际上可以是5个自由度，即由5对匹配点就可以进行求解。

但由于这种做法形式复杂，而从工程实际角度考虑，实际上两幅图片大概率会有非常多对匹配点，甚至多达上百对，从八点法降低到五点法的意义并不明显。

参考：
1.对极几何 Epipolar Geometry
https://zhuanlan.zhihu.com/p/79845576
2.《视觉SLAM十四讲》——高翔、张涛
3.视觉SLAM中，本质矩阵、基础矩阵、单应性矩阵自由度和秩分析。
https://blog.csdn.net/Walking_roll/article/details/119343924
4.SLAM基础知识补充：多视图几何
https://note.youdao.com/ynoteshare1/index.html?id=5e98f487c40ef22f90e1177f29271be5&type=note