【立体视觉（二）】之对极几何与关键矩阵

一、对极几何
二、关键矩阵

此为个人学习笔记，在各处借鉴了不少好图好文（参考文献在文末），主要是对相关知识进行梳理，以期形成自己的体系。文字表述东拼西凑，符号公式手动输入，若有错误烦请指出。

一、对极几何

现希望求两帧图像 $I_1、I_2$ 间的运动（ $R 、 t$ ）,若在两帧图像中检测到了一对同名点 $p_1、p_2$ 如图所示。
暂且忘掉成像原理中的符号意义。
在这里插入图片描述

基线（Baseline）： $O_1O_2$ 连线
极点（Epipoles）： $e_1、e_2$
极线（Epipolar line）： $l_1、l_2$
极平面（Epipolar plane）： $O_1、O_2、P$ 形成的平面

设左相机坐标系坐标的空间点 $P=[X,Y,Z]^T$ ，则由针孔相机模型知两个像素位置为：

$s_1 p_1=KP， s_2 p_2=K(RP+t)$

其中， $K$ 为内参矩阵， $R ， t$ 为两个相机之间的运动关系（具体来说，是 $R_{21}，t_{21}$ ）。 $s_1，s_2$ 是 $P$ 点在两个坐标系的 $z$ 轴坐标，在使用齐次坐标时，一个向量将等于它自身乘上任意的非零常数，这通常用于表达一个投影关系，称为尺度意义下相等（equal up to a scale）。记作 $s p ≃ p$ 。因此有
$p_1≃KP，p_2≃K(RP+t)$

接下来开始推导：

把 $K$ 移到等式左边：
$K^1_1p_1≃P，K^1_2p_2≃RP+t$
在归一化平面上，有 $x_1=K^1_1p_1，x_2=K^1_2p_2$ ，则 $x_1=P，x_2=RP+t$ ，那么
$x_2=Rx_1+t$
等式两边用 $t$ 叉乘（矢量与自己叉乘为0）有 $\times x_2=t \times Rx_1+t \times t$ ，即
$\times x_2=t \times Rx_1$
等式两边同时左乘 $x_2^T$ ，得 $x_2^Tt \times x_2=x_2^T t \times Rx_1$ ，此时等式左边显然为0，那么
$x_2^T t \times Rx_1=0$
由于叉乘等价于反对称矩阵的点乘， $t$ 的反对称矩阵为 $t^{\land}$ （相当于两侧同时与 $t$ 做外积），则
$x_2^T t^{\land} Rx_1=0$

该式描述了两个像素坐标 $x_1、x_2$ 之间的联系，即空间点在两个相机的成像点通过 $t$ 外参矩阵建立了一个等式关系，或者称为一种约束关系，这个约束就叫做对极约束。
注意，以上 $x_1、x_2$ 为归一化坐标，若用 $p_1、p_2$ 回代 $x_1、x_2$ ，有
$p_2^TK^{-T}_2 t^{\land} RK^1_1p_1=0$
这两个公式等价，都称为对极约束，几何意义为 $O_1、O_2、P、p_1、p_2$ 共面。

二、关键矩阵

关键矩阵：本质矩阵、基础矩阵、单应性矩阵。
关键矩阵可以建立两个视图公共点之间的坐标联系，或者完成公共点之间的坐标转换。它们就像立体视觉双视图之间的桥梁，让彼此紧密相连，形成一个整体，才算有了立体视觉系统的概念。

一）本质矩阵与基础矩阵

记 $E=t^{\land} R$ ， $F=K^{-T}_2 t^{\land} RK^1_1$ ，分别称为本质矩阵(Essential Matrix) 和基础矩阵(Fundamental Matrix)，则对极约束可表示为：
$x_2^T Ex_1=0\\ p_2^TFp_1=0$

对极约束简洁第给出了两个匹配点的空间位置关系，于是相机位姿估计问题变为以下两步：

根据配对点的像素位置求出 $E$ 或 $F$ 。
根据 $E$ 或 $F$ 求出 $R 、 t$

可以看到，本质矩阵 $E$ 与基础矩阵 $F$ 的区别：本质矩阵直接与归一化坐标 $x$ 建立关系，但 $x$ 需要由内参矩阵 $K$ 与像素坐标 $p$ 计算，所以使用本质矩阵的前提是已知内参矩阵 $K$ ；而基础矩阵直接与像素坐标 $p$ 建立关系，不需要内参矩阵已知。或者说：

本质矩阵：反映【空间一点P的像点】在【不同视角摄像机】下【摄像机坐标系】中的表示之间的关系。
基础矩阵：反映【空间一点P的像点】在【不同视角摄像机】下【图像坐标系】中的表示之间的关系。

由于 $E$ 和 $F$ 只相差了相机内参(两者的求解方法是一样的)，而内参在SLAM等问题中通常是已知的，所以实践中往往使用形式更简单的 $E$ 。

二）本质矩阵的求解

由于平移和旋转各有 3 个自由度，故 $E=t^{\land} R$ 共有 6 个自由度。由于对极约束是等式为零的约束，所以对 E 乘以任意非零常数后，对极约束依然满足，即 $E$ 在不同尺度下是等价的，故 $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$ ，根据对极约束，有
$\left[ \begin{matrix} u_2 & v_2 & 1 \end{matrix} \right] \left[ \begin{matrix} e_1 & e_2 & e_3\\ e_4 & e_5 & e_6\\ e_7 & e_8 & e_9\\ \end{matrix} \right] \left[ \begin{matrix} u_1\\ v_1 \\ 1 \end{matrix} \right]=0$
将矩阵 $E$ 展开，写成向量形式：
$e=[e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9]^T$
那么，对极约束可以写成与 $e$ 相关的线性形式：
$[u_2u_1,u_2v_1,u_2,v_2u_1,v_2v_1v_2,u_1,v_1,1] \cdot e=0$
同理，对其他点对也有相同的表示。则可得一个线性方程组（ $u^i, v^i$ 表示第 $i$ 个特征点，以此类推）：
$\left[ \begin{matrix} u^1_2u^1_1,u^1_2v^1_1,u^1_2,v^1_2u^1_1,v^1_2v^1_1v^1_2,u^1_1,v^1_1,1\\ u^2_2u^2_1,u^2_2v^2_1,u^2_2,v^2_2u^2_1,v^2_2v^2_1v^2_2,u^2_1,v^2_1,1\\ u^3_2u^3_1,u^3_2v^3_1,u^3_2,v^3_2u^3_1,v^3_2v^3_1v^3_2,u^3_1,v^3_1,1\\ ...\\ u^8_2u^8_1,u^8_2v^8_1,u^8_2,v^8_2u^8_1,v^8_2v^8_1v^8_2,u^8_1,v^8_1,1 \end{matrix} \right] \left[ \begin{matrix} e_1 \\ e_2 \\ e_3\\ e_4 \\ e_5 \\ e_6\\ e_7 \\\ e_8 \\ e_9\\ \end{matrix} \right] =0$
解该线性方程 $A e = 0$ 得到矢量 $e$ ，即可得到本质矩阵 $E$ 。矢量 $e$ 为系数矩阵 $A$ 的零空间，由线性代数的知识可知方程的特解数为 $9 - r ank (A)$ ，当 $r ank (A) = 8$ 时，存在唯一特解，方程所有解均为特解的常数倍，对应 $E$ 的尺度不变性。

三）本质矩阵的分解

本质矩阵 $E=t^{\land} R$ 是一个 $3\times 3$ 矩阵，其奇异值必定为 $[\sigma,\sigma,0]^T$ 的形式，称为本质矩阵的内在性质（继承于反对称矩阵 $t^{\land}$ 的性质）。可以通过 SVD 分解得到 $R$ 和 $t$ ，若 $E$ 的 SVD 分解为 $E=U\Sigma V^T$ ，则 $R$ 和 $t$ 可能的结果有：
$t_1^{\land}=UZU^T, t_2^{\land}=UZ^T U^T,\\R_1=UW U^T, R_2=UW^T U^T$
其中， $W=\left[ \begin{matrix} 0 & -1 & 0\\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{matrix} \right]$ （正交阵）， $Z=\left[ \begin{matrix} 0 & 1 & 0\\ -1 & 0 & 0 \\ 0 & 0 & 1 \end{matrix} \right]$ （反对称矩阵，若矢量z= $W=\left( \begin{matrix} 0 & -1 & 0 \end{matrix} \right)^T$ ，则 $z^{\land}=Z$ ）。

解算 $t$ 时，并不需要先解算出 $t^{\land}$ ，以 $t_1^{\land}=UZU^T$ 为例，两边同乘 $t_1$ ，有 $t_1^{\land}t_1=UZU^Tt_1$ ，即 $UZU^Tt_1=0$ ，两边左乘 $U^T$ ，有 $ZU^Tt_1=0$ ，因为 $z^{\land}=Z$ ，且 $z^{\land}z=0$ ，所以有 $U^Tt_1=z$ ，得 $t_1=Uz=U(0,0,1)^T$ ，也就是说， $t_1$ 为矩阵 $U$ 的第三列，即
$t_1=U.col(3)，t_2=-t_1$
一共有四组可能的 $R$ 和 $t$ 的组合：
在这里插入图片描述

但是，只有在上面第一种组合中 $P$ 在两个相机中都具有正的深度。因此，只要将任意观测点带入验证即可得到正确的那组解。

根据线性方程解出的 $E$ ，可能不满足 $E$ 的内在性质：奇异值为 $(σ, σ, 0)$ 。通常的做法是，对八点法求得的 $E$ 进行 SVD 分解后，会得到奇异值矩阵 $Σ = d ia g (σ 1, σ 2, σ 3)$ ，设 $σ 1 ⩾ σ 2 ⩾ σ 3$ ，取：
$E=Udiag(\frac{\sigma_1+\sigma_1}{2},\frac{\sigma_1+\sigma_1}{2},0)$
这相当于是把求出来的矩阵投影到了 $E$ 所在的流形上。更简单的做法是将奇异值矩阵取成 $d ia g (1, 1, 0)$ ，因为 $E$ 具有尺度等价性，所以这样做也是合理的。

应用本质矩阵的前提是已知内参矩阵 $K$ 。而基础矩阵不需要内参，但是未知数多，很难分解得到准确的内外参，我们需要做一些假设，比如具有初始的焦距值，比如像主点在图像中心等。而无论是估计本质矩阵还是估计基础矩阵，分解得到的相机参数都只能作为初值，要得到精确的参数，还需要通过非线性迭代优化求解精确值。

三）单应矩阵及其求解

本质矩阵和基础矩阵并不是两个像素之间的相互转换关系，而是一种坐标之间的内在约束式。那是否存在直接将左视图像素坐标转换到右视图对应点像素坐标的表达式呢？有的，即单应矩阵（Homography matrix）H，当空间中场景是同一个平面时，它们在左右视图的投影点可通过可逆的单应性矩阵一对一相互转换。
单应是射影几何中的概念，又称为射影变换。它把一个射影平面上的点(三维齐次矢量)映射到另一个射影平面上，并且把直线映射为直线。换句话说，单应是关于三维齐次矢量的一种线性变换。

考虑在空间内一平面满足方程： $n^TP+d=0$ ，结合以上内容，有

$p_2≃K(RP+t)\\p_2≃(RP+t(-\frac{n^TP}{d}))\\p_2≃K(R-\frac{n^TP}{d})P\\p_2≃K(R-\frac{n^TP}{d})K^{-1}p_1$
令 $H=K(R-\frac{n^TP}{d})K^{-1}$ ，有
$p_2≃Hp_1\\p_1≃H^{-1}p_2$

单应矩阵描述了两个平面之间的映射关系。若场景中的特征点都落在同一平面上（比如墙、地面等），则可以通过单应性来进行运动估计。它的定义它与旋转、平移及平面的参数有关，是一个 $\times 3$ 矩阵。同样，可以先根据匹配点计算 $H$ ，再分解以计算 $R 、 t$ 。

$\left[ \begin{matrix} u_2\\ v_2 \\ 1 \end{matrix} \right]≃\left[ \begin{matrix} h_1 & h_2 & h_3\\ h_4 & h_5 & h_6\\ h_7 & h_8 & h_9\\ \end{matrix} \right] \left[ \begin{matrix} u_1\\ v_1 \\ 1 \end{matrix} \right]$

不妨让 $h_9=1$ （在它取非零值时），按照齐次公式的常规处理方式，我们会让第1，2行除以第3行以去除尺度因子，可得
(注意式子里的 $u$ 与 $v$ 的区别)

$u_2=\frac{h_1u_1+h_2v_1+h_3}{h_7u_1+h_8v_1+h_9}\\ v_1=\frac{h_4u_1+h_5v_1+h_6}{h_7u_1+h_8v_1+h_9}$

整理得
$h_7u_1+h_8v_1+h_9-h_7u_1u_2-h_8v_1u_2=u_2\\ h_4u_1+h_5v_1+h_6-h_7u_1v_2-h_8v_1v_2=v_2$

这样一组匹配点对就可以构造出两项约束（事实上有三个约束，但因为线性相关，只取前两个），于是自由度为 8 的单应矩阵可以通过 4 对匹配特征点算出（注意，这些特征点不能有三点共线的情况），即求解以下的线性方程组（当 h9 = 0 时，右侧为零）。
$\left[\begin{matrix} u_1^1 & v_1^1 & 1 & 0 & 0 & 0 & -u_1^1u_2^1& -v_1^1u_2^1\\ 0 & 0 & 0 &u_1^1 & v_1^1 & 1 & -u_1^1v_2^1& -v_1^1v_2^1\\ u_1^2 & v_1^2 & 1 & 0 & 0 & 0 & -u_1^2u_2^2& -v_1^2u_2^2\\ 0 & 0 & 0 & u_1^2 & v_1^2 & 1 & -u_1^2v_2^2& -v_1^2v_2^2\\ u_1^3 & v_1^3 & 1 & 0 & 0 & 0 & -u_3^2u_2^3& -v_1^3u_2^3\\ 0 & 0 & 0 & u_1^3 & v_1^3 & 1 & -u_1^3v_2^3& -v_1^3v_2^3\\ u_1^4 & v_1^4 & 1 & 0 & 0 & 0 & -u_3^4u_2^4& -v_1^4u_2^4\\ 0 & 0 & 0 & u_1^4 & v_1^4 & 1 & -u_1^4v_2^4& -v_1^4v_2^4\\ \end{matrix}\right] \left[\begin{matrix}h_1\\h_2\\h_3\\h_4\\h_5\\h_6\\h_7\\h_8\end{matrix}\right] = \left[\begin{matrix}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{matrix}\right]$
这种做法把 $H$ 矩阵看成了向量，通过解该向量的线性方程来恢复 $H$ ，又称**直接线性变换法(Direct Linear Transform，DLT)*。与本质矩阵相似，求出单应矩阵以后需要对其进行分解，才可以得到相应的旋转矩阵 $R$ 和平移向量 $t$ 。分解的方法包括数值法与解析法。单应矩阵的分解同样会返回 4 组旋转矩阵与平移向量，并且同时可以计算出它们分别对应的场景点所在平面的法向量。如果已知成像的地图点的深度全为正值（即在相机前方），则又可以排除两组解。最后仅剩两组解，这时需要通过更多的先验信息进行判断。

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

除了DLT，还可以用RANSAC。

参考文献：

[1] 高翔, 张涛, 视觉SLAM十四讲：从理论到实践（第2版）
[2] 立体视觉入门指南（2）：关键矩阵（本质矩阵，基础矩阵，单应矩阵）
[3] 对极几何和三角测量
[4] 计算机视觉6—对极几何和基础矩阵
[5] 单应矩阵与对极几何

【立体视觉（二）】之对极几何与关键矩阵

【立体视觉（二）】之对极几何与关键矩阵

一、对极几何

二、关键矩阵

一）本质矩阵与基础矩阵

二）本质矩阵的求解

三）本质矩阵的分解

三）单应矩阵及其求解

猜你喜欢