2D-2D：对极几何与三角测量

尺度不确定性

如果把相机的运动和场景同时放大两倍，单目相机所看到的像是一样的。同样的，把这个大小乘以任意倍数，我们都将看到一样的景象。这说明，单目SLAM估计的轨迹和地图将与真实的轨迹和地图相差一个因子，也就是所谓的尺度(Scale)。由于单目SLAM无法仅凭图像确定这个真实尺度，所以又称尺度不确定性(Scale Ambiguity)。

对极几何

${\mathrm{C}}_0,{\mathrm{C}}_1$分别是两个位置中相机的光心，也就是针孔相机模型中的针孔
$P$是空间中的一个三维点，${\mathrm{p}}_0,{\mathrm{p}}_1$分别是P点在不同成像平面上对应的像素点
${\mathrm{C}}_0$-${\mathrm{C}}_1$-$\mathrm{P}$-${\mathrm{p}}_0$-${\mathrm{p}}_1$他们都是在同一个平面上的，你可以想象${\mathrm{C}}_0$-${\mathrm{C}}_1$-$\mathrm{P}$组成的平面是一个三角尺，它所在的平面称之为极平面（epipolar plane），它像一把锋利的刀，切割了左右两个成像平面。

在${\mathrm{C}}_0$的相机坐标系${\mathbf{X}}_0{\mathbf{Y}}_0{\mathbf{Z}}_0$中描述这一关系， ${\mathbf{p}}_0$和${\mathbf{p}}_1$都是图像平面上的二维点，要将其转化为三维方向向量。
${\mathbf{x}}_0$为${\mathbf{p}}_0$在相机${\mathrm{C}}_0$的归一化平面上的坐标，${\mathbf{q}}_0$为其齐次像素坐标，则${\mathbf{p}}_0$在${\mathrm{C}}_0$的相机坐标系${\mathbf{X}}_0{\mathbf{Y}}_0{\mathbf{Z}}_0$的归一化向量为x
$$\overrightarrow{{\mathbf{C}}_0{\mathbf{p}}_0}{|}_{{\mathbf{X}}_0{\mathbf{Y}}_0{\mathbf{Z}}_0}={\mathbf{x}}_0=\left(\begin{array}{c}{x}_0\\ y_0\\ 1\end{array}\right)={\mathbf{K}}^{-1}{\mathbf{q}}_0$$
${\mathbf{x}}_1$为${\mathbf{p}}_1$在相机${\mathrm{C}}_1$的归一化平面上的坐标，${\mathbf{q}}_1$为其齐次像素坐标，则${\mathbf{p}}_1$在${\mathrm{C}}_1$的相机坐标系的归一化向量为
$$\overrightarrow{{\mathbf{C}}_1{\mathbf{p}}_1}{|}_{{\mathbf{X}}_1{\mathbf{Y}}_1{\mathbf{Z}}_1}={\mathbf{x}}_1=\left(\begin{array}{c}{x}_1\\ y_1\\ 1\end{array}\right)={\mathbf{K}}^{-1}{\mathbf{q}}_1$$
不考虑偏移，${\mathbf{p}}_1$的归一化向量在${\mathrm{C}}_0$相机坐标系下的描述为
$$\overrightarrow{{\mathbf{C}}_1{\mathbf{p}}_1}{|}_{{\mathbf{X}}_0{\mathbf{Y}}_0{\mathbf{Z}}_0}=\mathbf{R}\cdot \overrightarrow{{\mathbf{C}}_1{\mathbf{p}}_1}{|}_{{\mathbf{X}}_1{\mathbf{Y}}_1{\mathbf{Z}}_1}={\mathbf{Rx}}_1$$
于是在${\mathbf{X}}_0{\mathbf{Y}}_0{\mathbf{Z}}_0$坐标系下：
$$\begin{gathered} \overrightarrow{{\mathbf{C}}_0{\mathbf{p}}_0}\cdot \left(\overrightarrow{{\mathbf{C}}_0{\mathbf{C}}_1}\times \overrightarrow{{\mathbf{C}}_1{\mathbf{p}}_1}\right)=0 \\ ⟹{\mathbf{x}}_0^T{\mathbf{t}}^{\wedge }{\mathbf{Rx}}_1=0 \end{gathered}$$
将归一化坐标与齐次像素坐标的关系式代入得
$$\begin{gathered} ({\mathbf{K}}^{-1}{\mathbf{p}}_0{)}^T{\mathbf{t}}^{\wedge }\mathbf{R}({\mathbf{K}}^{-1}{\mathbf{p}}_1)=0 \\ ⟹{\mathbf{q}}_0^T{\mathbf{K}}^{-T}{\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{K}}^{-1}{\mathbf{q}}_1=0 \end{gathered}$$
把中间部分记作两个矩阵，本质矩阵(Essential Matrix)：
$$\mathbf{E}={\mathbf{t}}^{\wedge }\mathbf{R}$$
基础矩阵(Fundamental Matrix)：
$$\mathbf{F}={\mathbf{K}}^{-T}{\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{K}}^{-1}$$
于是对极约束可以简化为：
$${\mathbf{x}}_0^T{\mathbf{Ex}}_1={\mathbf{q}}_0^T\mathbf{F}{\mathbf{q}}_1=0$$
对极约束简洁地给出了两个匹配点的空间位置关系。于是，相机位姿估计问题变为以下两步：

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

• 本质矩阵是由对极约束定义的。由于对极约束是等式为零的约束，所以对$\mathbf{E}$乘以任意非零常数，对极约束仍然满足。称为$\mathbf{E}$在不同尺度下是等价的。
• 根据$\mathbf{E}={\mathbf{t}}^{\wedge }\mathbf{R}$，本质矩阵由一个反对称矩阵和正交矩阵相乘，由¹ 知正交矩阵不改变奇异值，由²知反对称矩阵的奇异值为${\left[\begin{array}{ccc}\sigma & \sigma & 0\end{array}\right]}^T$的形式，则本质矩阵$\mathbf{E}$的奇异值也必定为${\left[\begin{array}{ccc}\sigma & \sigma & 0\end{array}\right]}^T$的形式，这称为本质矩阵的内在性质。
• 由于平移和旋转各有3个自由度，且由于尺度等价性，$\mathbf{E}$实际上有6-1=5个自由度。表明我们最少可以用5对点来求解$\mathbf{E}$。但是，E的内在性质是一种非线性性质，在估计时会带来麻烦，因此，也可以只考虑它的尺度等价性，使用8对点来估计$\mathbf{E}$这就是著名的八点法。八点法只利用了$\mathbf{E}$的线性性质，因此可以在线性代数框架下求解。

考虑一对匹配点，它们的归一化坐标为${\mathbf{x}}_0={\left[x_0,y_0,1\right]}^T,{\mathbf{x}}_1={\left[x_1,y_1,1\right]}^T$。根据对极约束，有
$$\left(x_0,y_0,1\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}{x}_1\\ y_1\\ 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$$
那么，对极约束可以写成与$\mathbf{e}$有关的线性形式：
$$\left[x_0{x}_1,x_0{y}_1,x_0,y_0{x}_1,y_0{y}_1,y_0,x_1,y_1,1\right]\cdot \mathbf{e}=0$$
同理，对于其他点也有相同的表示。我们把所有点都放到一个方程中，变成线性方程组($u^i,v^i$表示第i个特征点)：
$$\left(\begin{array}{ccccccccc}{x}_0^1{x}_1^1& x_0^1{y}_1^1& x_0^1& y_0^1{x}_1^1& y_0^1{y}_1^1& y_0^1& x_1^1& y_1^1& 1\\ x_0^2{x}_1^2& x_0^2{y}_1^2& x_0^2& y_0^2{x}_1^2& y_0^2{y}_1^2& y_0^2& x_1^2& y_1^2& 1\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ x_0^8{x}_1^8& x_0^8{y}_1^8& x_0^8& y_0^8{x}_1^8& y_0^8{y}_1^8& y_0^8& x_1^8& y_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$$
这8个方程构成了一个线性方程组。它的系数矩阵由特征点位置构成，大小为$8\times 9$。$\mathbf{e}$位于该矩阵的零空间中。如果系数矩阵是满秩的(即秩为8)，那么它的零空间维数为1，也就是$\mathbf{e}$构成一条线。这与$\mathbf{e}$的尺度等价性是一致的。如果8对匹配点组成的矩阵满足秩为8的条件，那么$\mathbf{E}$的各元素就可由上述方程解得。
接下来的问题是如何根据已经估得得本质矩阵$\mathbf{E}$，恢复出相机的运动$\mathbf{R},\mathbf{t}$。这个过程是由奇异值分解(SVD)得到的
$$\begin{gathered} \mathbf{E}=\mathbf{U}\mathbf{\Sigma }{\mathbf{V}}^T=\mathbf{U}\left(\begin{array}{ccc}\sigma & & \\ & \sigma & \\ & & 0\end{array}\right){\mathbf{V}}^T \\ \begin{array}{l}=\mathbf{U}\left(\begin{array}{ccc}0& -\sigma & 0\\ \sigma & 0& 0\\ 0& 0& 0\end{array}\right)\left(\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 1\end{array}\right){\mathbf{V}}^T\\ =\underset{t_1^{\wedge }}{\underbrace{\mathbf{U}\left(\begin{array}{ccc}0& -\sigma & 0\\ \sigma & 0& 0\\ 0& 0& 0\end{array}\right){\mathbf{U}}^T}}\underset{R_1}{\underbrace{\mathbf{U}\left(\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 1\end{array}\right){\mathbf{V}}^T}}\end{array}\begin{array}{l}=\mathbf{U}\left(\begin{array}{ccc}0& \sigma & 0\\ -\sigma & 0& 0\\ 0& 0& 0\end{array}\right)\left(\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right)\\ \underset{t_2^{\wedge }}{\underbrace{=\mathbf{U}\left(\begin{array}{ccc}0& \sigma & 0\\ -\sigma & 0& 0\\ 0& 0& 0\end{array}\right){\mathbf{U}}^T}}\underset{R_2}{\underbrace{\mathbf{U}\left(\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right){\mathbf{V}}^T}}\end{array} \end{gathered}$$
对于任意一个$\mathbf{E}$，存在两个可能的$\mathbf{t},\mathbf{R}$与它对应：
$$\begin{gathered} {\mathbf{t}}_1=\mathbf{U}\left(\begin{array}{c}0\\ 0\\ \sigma \end{array}\right){\mathbf{R}}_1={\mathbf{UR}}_z(-\frac{\pi }{2}){\mathbf{V}}^T \\ {\mathbf{t}}_1=\mathbf{U}\left(\begin{array}{c}0\\ 0\\ -\sigma \end{array}\right){\mathbf{R}}_1={\mathbf{UR}}_z(\frac{\pi }{2}){\mathbf{V}}^T \end{gathered}$$
其中${\mathbf{R}}_z(\frac{\pi }{2})$表示沿Z轴旋转$90^{\circ }$得到的旋转矩阵。同时，由于$-\mathbf{E}$与$\mathbf{E}$等价，所以对任意一个$\mathbf{t}$取负号，也会得到相同的结果。因此，从$\mathbf{E}$分解到$\mathbf{t},\mathbf{R}$时，一共存在4个可能的解。

上图展示了分解本质矩阵得到旋转矩阵得到的4个解。我们已知空间点在相机(蓝色线)上的投影(红色点)，想要求解相机的运动。在保持红色点不变的情况下，可以画出4种可能的情况。不过幸运的是，只有第一种解中P在两个相机中都具有正的深度。因此，只要把任意一点代入4种解，检测该点在两个相机下的深度，就可以确定哪个解是正确的了。
剩下的一个问题是：根据线性方程解出的$\mathbf{E}$，可能不满足$\mathbf{E}$的内在性质–它的奇异值不一定为$\sigma ,\sigma ,0$的形式。这时，我们会刻意地把$\mathbf{\Sigma }$矩阵调整为上面的样子。通常的做法是，对八点法求得的$\mathbf{\Sigma }=\mathrm{diag}({\sigma }_1,{\sigma }_2,{\sigma }_3)$，不妨设${\sigma }_1⩾{\sigma }_2⩾{\sigma }_3$。取：
$$\mathbf{E}=\mathbf{U}\mathrm{diag}(\frac{{\sigma }_1+{\sigma }_2}{2},\frac{{\sigma }_1+{\sigma }_2}{2},0){\mathbf{V}}^T$$
这相当于是把求出来的矩阵投影到了$\mathbf{E}$所在的流形上。当然，更简单的做法是将奇异值矩阵取成$\mathrm{diag}(1,1,0)$，因为$\mathbf{E}$具有尺度等价性，所以这样做也是合理的。

由于$\mathbf{E}$本身具有尺度等价性，它分解得到的$\mathbf{t},\mathbf{R}$也有一个尺度等价性。而$\mathbf{R}\in \mathrm{SO}(3)$自身具有约束，所以我们认为$\mathbf{t}$具有一个尺度。换言之，在分解过程中，对$\mathbf{t}$乘以任意非零常数，分解都是成立的。因此，我们通常把$\mathbf{t}$进行归一化，让它的长度等于1。

尺度不确定性

对$\mathbf{t}$长度的归一化，直接导致了单目视觉的尺度不确定性。因为对$\mathbf{t}$乘以任意比例常数后，对极约束依然成立。换言之，在单目SLAM中，对轨迹和地图同时缩放任意倍数，我们得到的图像依然是一样的。

在单目视觉中，我们对两张图像的$\mathbf{t}$ 归一化相当于固定了尺度。虽然我们不知道它的实际长度是多少，但我们以这时的$\mathbf{t}$为单位1，计算相机运动和特征点的三维位置。这被称为单目SLAM的初始化。在初始化之后，就可以用3D-2D计算相机运动。初始化之后的轨迹和地图的单位，就是初始化时固定的尺度。因此，单目SLAM有一步不可避免的初始化。初始化两张图像必须有一定程度的平移，而后的轨迹和地图都将以此步的位移为单位。

除了对$\mathbf{t}$进行归一化，另一种方法是令初始化时所有的特征点的平均深度为1，也可以固定一个尺度。相比于令$\mathbf{t}$长度为1的做法，把特征点深度归一化可以控制场景的规模大小，使计算在数值上更稳定。

初始化的纯旋转问题

从 $\mathbf{E}$ 分解到 $\mathbf{R},\mathbf{t}$ 的过程中，如果相机发生的是纯旋转，导致$\mathbf{t}$ 为零，那么，得到的$\mathbf{E}$ 也称为零，这将导致我们无从求解$\mathbf{R}$ 。不过，此时我们可以依靠 $,\mathbf{H}$求取旋转，但仅有旋转时，我们无法用三角测量估计特征点的空间位置（这将在下文提到），于是，另一个结论是，单目初始化不能只有纯旋转，必须要有一定程度的平移。如果没有平移，单目将无法初始化。在实践中，如果初始化时平移太小，会使得位姿求解与三角化结果不稳定，从而导致失败。相对地，如果把相机左右移动而不是原地旋转，就容易让单目 SLAM 初始化。因而，有经验的 SLAM 研究人员，在单目 SLAM 情况下经常选择让相机进行左右平移以顺利地进行初始化。

三角测量

在单目SLAM中，仅通过单张图像无法获得像素的深度信息，我们需要通过三角测量(Triangulation)(或三角化)的方法估计地图点的深度。三角测量是指 ，通过在两处观察同一个点的夹角，从而确定该点的距离。

通过对极几何约束估计了相机的运动 $\mathbf{R},\mathbf{t}$，求解两个特征点的深度$s_1,s_2$。从几何上看，可以在射线$O_1{p}_1$ 上寻找3D点，使其投影位置接近${\mathbf{p}}_2$。同理，也可以在$O_2{p}_2$上找，或者在两条线的中间找，不同的策略对应不同的计算方式。
按照对极几何中的定义，设${\mathbf{x}}_1,{\mathbf{x}}_2$为两个特征点的归一化坐标，则满足：
$$s_2{\mathbf{x}}_2=s_1{\mathbf{Rx}}_1+\mathbf{t}$$

最小二乘法

上式可写成$\mathbf{Ax}=\mathbf{b}$的形式
$$\left[\begin{array}{cc}-{\mathbf{Rx}}_1& {\mathbf{x}}_2\end{array}\right]\left[\begin{array}{c}{s}_1\\ s_2\end{array}\right]=\mathbf{t}$$
通过最小化平方误差，求解$\mathbf{x}$，实际上就是求广义逆的过程。
$$\min \Vert \mathbf{Ax}-\mathbf{b}{\Vert }_2^2$$

单应性矩阵

单应矩阵通常描述处于同一平面上的一些点在两张图像之间的变换关系，两张图像可以是同一相机在不同位置拍摄的，也可以来源于不同的相机。

两个相机的观测值的齐次坐标形式
$${\mathbf{q}}_i=\frac{1}{Z_i}{\mathbf{K}}_i{\mathbf{p}}_i{\mathbf{p}}_i=\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\end{array}\right]$$

• 其中${\mathbf{p}}_i$是对应$i=a,b$两个相机坐标系中点P的坐标。假设我们已知点$P$所在的平面方程，那么在这两个相机坐标系下表示的平面方程可以记作：
  { n i , d i } \{\boldsymbol{n}_i,d_i\} {ni​,di​}
• ${\mathbf{q}}_i=\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]$是齐次像素坐标
• 其中$d_i$代表相机$i$到平面的距离，${\mathbf{n}}_i$代表参考系$i$下的平面法向量。
• ${\mathbf{K}}_i$为相机内参矩阵：${\mathbf{K}}_i=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$
  因为$P$在平面上，所以有如下关系
  $${\mathbf{n}}_i^T{\mathbf{p}}_i+d_i=0$$

由像素系到相机系

$${\mathbf{p}}_i=Z_i{\mathbf{K}}_i^{-1}{\mathbf{q}}_i$$
由于存在未知的深度$Z_i$，因此无法由像素系坐标计算出相机系坐标。
由上式和平面方程得
$$\begin{gathered} Z_i{\mathbf{n}}_i^T{\mathbf{K}}_i^{-1}{\mathbf{q}}_i+d_i=0 \\ ⟹Z_i=-\frac{d_i}{{\mathbf{n}}_i^T{\mathbf{K}}_i^{-1}{\mathbf{q}}_i} \end{gathered}$$

因此
$${\mathbf{p}}_i=-\frac{d_i{\mathbf{K}}_i^{-1}{\mathbf{q}}_i}{{\mathbf{n}}_i^T{\mathbf{K}}_i^{-1}{\mathbf{q}}_i}$$

可知，加入平面信息 { n i , d i } \{\boldsymbol{n}_i,d_i\} {ni​,di​}后，可完全由像素坐标还原出相机坐标系。

由像素系a到像素系b

a相机系到b相机系的外参为$({}^b{\mathbf{R}}_a,{}^b{\mathbf{t}}_a)$
$${\mathbf{p}}_b={}^b{\mathbf{R}}_a{\mathbf{p}}_a+{}^b{\mathbf{t}}_a$$
代入齐次像素坐标与相机系坐标的转换关系：
$$Z_b{\mathbf{K}}_b^{-1}{\mathbf{q}}_b=Z_a{}^b{\mathbf{R}}_a{\mathbf{K}}_a^{-1}{\mathbf{q}}_a+{}^b{\mathbf{t}}_a$$
化简得：
$$\begin{array}{l}{\mathbf{q}}_b=\frac{Z_a}{Z_b}{\mathbf{K}}_b\left({}^b{\mathbf{R}}_a{\mathbf{K}}_a^{-1}{\mathbf{q}}_a+\frac{1}{Z_a}{}^b{\mathbf{t}}_a\right)\\ \begin{array}{l}=\frac{Z_a}{Z_b}{\mathbf{K}}_b\left({}^b{\mathbf{R}}_a{\mathbf{K}}_a^{-1}{\mathbf{q}}_a-\frac{{}^b{\mathbf{t}}_a{\mathbf{n}}_a^T{\mathbf{K}}_a^{-1}{\mathbf{q}}_a}{d_a}\right)\\ =\frac{Z_a}{Z_b}{\mathbf{K}}_b\left({}^b{\mathbf{R}}_a-\frac{{}^b{\mathbf{t}}_a{\mathbf{n}}_a^T}{d_a}\right){\mathbf{K}}_a^{-1}{\mathbf{q}}_a\\ =\frac{Z_a}{Z_b}{\mathbf{K}}_b{}^b{\mathbf{R}}_a\left(\mathbf{I}+\frac{{}^a{\mathbf{t}}_b{\mathbf{n}}_a^T}{d_a}\right){\mathbf{K}}_a^{-1}{\mathbf{q}}_a\end{array}\end{array}$$
最后一步利用了${}^b{\mathbf{t}}_a=-{}^b{\mathbf{R}}_a{}^a{\mathbf{t}}_b$。

由于使用的是齐次像素坐标，不受缩放因子的影响，所以参数$\frac{z_a}{z_b}$相当于是${\mathbf{q}}_b$的缩放因子，可以去掉
$${\mathbf{q}}_b={\mathbf{K}}_b{}^b{\mathbf{R}}_a\left(\mathbf{I}+\frac{{}^a{\mathbf{t}}_b{\mathbf{n}}_a^T}{d_a}\right){\mathbf{K}}_a^{-1}{\mathbf{q}}_a={}^b{\mathbf{H}}_a{\mathbf{q}}_a$$
单应性矩阵包含了相机内参矩阵${\mathbf{K}}_a,{\mathbf{K}}_b$、旋转${}^b{\mathbf{R}}_a$、平移${}^a{\mathbf{t}}_b$和平面参数 { n a , d a } \{\boldsymbol{n}_a,d_a\} {na​,da​}信息。引入单应矩阵后，可以直接通过 a 系像素得到 b 系像素。
$${}^b{\mathbf{H}}_a={\mathbf{K}}_b{}^b{\mathbf{R}}_a\left(\mathbf{I}+\frac{{}^a{\mathbf{t}}_b{\mathbf{n}}_a^T}{d_a}\right){\mathbf{K}}_a^{-1}$$

求解单应性矩阵

单应矩阵的定义是通过旋转平移信息计算的，现实中有时不知道旋转平移信息，而知道两张图像中的匹配点，可以由匹配点计算出单应矩阵。

对于图片上的一对匹配点有如下关系：
$${\mathbf{q}}_2\propto {\mathbf{Hq}}_1$$
展开得
$$\left(\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right)\propto \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)$$
因为使用的是齐次坐标，所以单应矩阵$\mathbf{H}$与尺度无关，可以乘以任意非零因子，所以可以令$h_9=1$(在它取非零值时)，然后根据第三行，去掉非零因子，于是有
$$\begin{gathered} 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{gathered}$$

一组匹配点可以构造两个约束方程，于是自由度为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^2{u}_2^2& -v_1^2{u}_2^2\\ 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\\ n_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)$$

这种做法把$\mathbf{H}$矩阵看成了向量，通过解该向量的线性方程来恢复$\mathbf{H}$，又称直接线性变换法（Direct Linear Transform）。与本质矩阵相似，求出单应矩阵以后需要对其进行分解，才可以得相应的旋转矩阵$\mathbf{R}$和平移向量$\mathbf{t}$ 。分解的方法包括数值解法与解析解法 。与本质矩阵的分解类似，单应矩阵的分解同样会返回4组旋转矩阵与平移向量，并且同时可以计算出她它们对应的场景点所对在平面的法向量。如果已知成像的地图点的深度为正值（即在相机前方），则又可以排除两组解。
最后仅剩两组解，这时需要通过更多的先验信息进行判断。通常我们可以通过假设已知场景平面的法向量来解决，如场景平面与相机平面平行，那么法向量$\mathbf{n}$的理论为$1$ 。

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

---

1. 酉相抵有相同的奇异值
   设$A,B\in {\mathbb{C}}^{m\times n},\exists m$阶酉矩阵$U$和$n$阶酉矩阵$V$,使$U^{H}AV=B$,则称$A$与$B$酉相抵
   证:
   $$U^{H}AV=B,B^{H}B=(U^{H}AV{)}^H(U^{H}AV)=V^H{A}^{H}AV$$
   说明$B^{H}B$与$A^{H}A$酉相似,所以他们有相同的特征值
   故A,B有相同的奇异值 ↩︎

2. 反对称矩阵的奇异值分解
   $$\begin{gathered} v^{\wedge 2}=v{v}^T-v^{T}v{I}_{3\times 3}=v{v}^T-{\Vert v\Vert }_2^2{I}_{3\times 3} \\ {v}^{\wedge 3}=\underset{v\times v}{\underbrace{v^{\wedge }v}}{v}^T-{\Vert v\Vert }_2^2{v}^{\wedge }=-{\Vert v\Vert }_2^2{v}^{\wedge } \\ \left(v^{\wedge 2}+{\Vert v\Vert }_2^{2}I\right)v^{\wedge }=0 \end{gathered}$$
   可得$v^{\wedge }$的零化多项式(${\lambda }^2+\Vert v{\Vert }_2^2)\lambda =0$
   若$v\ne 0$ 则$\mathrm{rank}(v^{\wedge })=2,v^{\wedge }$必有非零特征值
   因此可得$({\lambda }^2+\Vert v{\Vert }_2^2)\lambda =0$也是最小多项式
   因此$v^{\wedge }$的三个特征值是${\lambda }_1=0,{\lambda }_{2,3}=\pm i\Vert v{\Vert }_2$
   $\begin{array}{l}U{v}^{\wedge }{U}^H=\Lambda =\left[\begin{array}{ccc}{\lambda }_2& & \\ & {\lambda }_3& \\ & & {\lambda }_1\end{array}\right]\\ (U{v}^{\wedge }{U}^H{)}^H(U{v}^{\wedge }{U}^H)=U{v}^{\wedge H}{v}^{\wedge }{U}^H=-U{v}^{\wedge 2}{U}^H\\ ={\Lambda }^H\Lambda =\left[\begin{array}{ccc}\Vert v{\Vert }_2^2& & \\ & \Vert v{\Vert }_2^2& \\ & & 0\end{array}\right]\\ ⟹U{v}^{\wedge H}{v}^{\wedge }{U}^T=\left[\begin{array}{ccc}\Vert v{\Vert }_2^2& & \\ & \Vert v{\Vert }_2^2& \\ & & 0\end{array}\right]\end{array}$
   $v^{\wedge }$的奇异值为$\Vert v{\Vert }_2,\Vert v{\Vert }_2,0$ ↩︎