史上最简SLAM零基础解读(2) - 对极约束→Essential矩阵、Fundamental矩阵推导

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始
有兴趣的朋友可以加微信 17575010159 相互讨论技术 - 文末公众号也可关注
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、什么是对极约束

首先，我们用通俗的方式来理解什么是 $对极约束$,图像如下所示，

(1)、$O_0,O_1$ 分别是两个位置中相机的光心，也就是针孔相机模型中的针孔。
(2)、$I0,I1$ 分别是两个相机的成像平面。
(3)、$P$ 是空间中的一个三维点，$p_0$, $p_1$ 分别是 $P$ 点在成像平面 $I0,I1$ 上的成像点
其上大家需要注意一点: $p_0$, $p_1$ 的坐标是相对于 $O_0,O_1$ 而言，也就是其为图像坐标，并非$像素坐标$。

图1

 
很明显的可以看到，在直线 $p_0$$P$ 上选取无穷个点，其映射到 平面 $I0$ 上都为同一点 $p_0$, 但是映射到平面 $I1$ 中，其并非是一个点了，而是一条直线。

$结论:$ 成像平面 $I0$ 任意一个像素点，对应三维空间中的无穷个点，该无穷个点都在同一直线线上，且该直线在平面 $I1$ 中的投影(Epipolar Line) 必定经过 $O_0{O}_1$ 与平面$I1$ 的交点(极点Epipole)。

$简而言之:$ 平面 $I0$ 中的任意一个像素点，映射到平面 $I1$ 中，存在无穷个解。其所有的解，都被约束在一条直线上 → 这就是对极约束。

 

二、对极约束有什么作用

从上面来看，我们虽然大致知道了其原理，但是其有什么用呢? 其作用是很大的，再后续我们会经常用到他，比如本人后面的几篇博客，都与对极约束兮兮相关。 这里本人打个比方: 小学的时候，我们学了乘法口诀，其我们可以理解为约束，比如 1x1=2，2x9=18 … ,有了这些约束以后，我们就可以做什么东西，比如两位以上的乘法，或者两位以上的除法，都是再在法口诀的约束下完成的。简单的来说，我们可以把约束理解为定理，再这个定理的基础上，我们可以做很多的推导。

 

三、基本概念

通过前面的描述，我们大致明白了对极约束的原理，那么我们下面就来看看其推导过程，再讲解之前，我们再来补充几个概念，来看看如下图像：

图2

 
(1)、基线(base line): 两个相机光心的连线$O_0{O}_1$称为基线。
(2)、对极点(epipolar): $e_0{e}_1$是对极点，是基线与两个成像平面的交点，也就是两个相机在另一个成像平面上的像点。
(3)、对极平面(epipolar plane):过基线的平面都称之为对极平面，其中两个相机的中心$O_0$和$O_1$,三维点$P$，以及三维点在两个相机成像点 $p_0$ ,$p_1$ 这五点必定在同一对极平面上，当三维点X变化时，对极平面绕着基线旋转，形成对极平面束。
(4)、对极线(epipolar line)：是对极平面和成像平面的交线，所有的对极线都相交于极点。
(5)、焦距: $O_0$ 与平面 $I_0$, 或者 $O_1$ 与平面 $I_1$ 的垂直距离。

 

四、Essential矩阵推导

以下不懂的概念可以百度以下。

$向量叉乘：$两个向量的叉乘结果是一个同时垂直于这两个向量的向量，其方向通过右手定则决定。其结果为向量(矢量)
$向量点乘：$ $\vec{a}\cdot \vec{b}=|a|\ast |b|\ast cos(\theta )$, 因此如果是两个相互垂直的向量点乘, $cos(\theta )=0$,那么向量点乘结果也为零
$方向向量：$只考虑它的方向，而不考虑它的起点或终点的向量。

有了上面的两个概念之后，我们根据 图2 可以得到 $结论1$: $$\overrightarrow{O_0{P}_0}\cdot (\overrightarrow{O_0{O}_1}\times \overrightarrow{O_1{P}_1})=0$$因为 $\overrightarrow{O_0{O}_1}\times \overrightarrow{O_1{P}_1}$ 得到的向量垂直于平面 $O_0{O}_{1}P$,  $\overrightarrow{O_1{P}_1}$ 属于平面 $O_0{O}_{1}P$，则 $\overrightarrow{O_1{P}_1}$ 垂直于 $\overrightarrow{O_0{O}_1}\times \overrightarrow{O_1{P}_1}$ 的结果，又根据点乘的定义，所以上述公式成立。

图2 中的点 $p_0,p_1$ 都是二维点，一般来说向量叉乘是三维的概念,.我们假设一个归一化的图像平面，该平面上焦距f =1,这里把它变成三维的方向向量来考虑。因此我们可以定义在以 $O_0$ 为原点的坐标系下$$p_0=\left(\begin{array}{c}{x}_0\\ y_0\\ 1\end{array}\right)$$
而在以 $O_1$ 为原点的坐标系下:
$$p_1=\left(\begin{array}{c}{x}_1\\ y_1\\ 1\end{array}\right)$$显而易见 $p_0,p_1$ 不在同一坐标系中。前面说过，$p_0$ 在以 $O_0$为原点的参考坐标系，$p_1$ 在以 $O_1$ 为原点的参考坐标系，所以我们还是需要转换坐标系。这里我们把所有点的坐标都转换到以$O_0$为原点的坐标系。前面说过这些向量都是方向向量和向量起始位置无关，所以这里坐标系变换只考虑旋转就可以。我们记 $R$ 为从 $O_1$ 坐标系到$O_0$ 坐标系的旋转矩阵, 之前的等式结论1 ：$$\overrightarrow{O_0{P}_0}\cdot (\overrightarrow{O_0{O}_1}\times \overrightarrow{O_1{P}_1})=0$$最左边的 $\overrightarrow{O_0{P}_0}$ 可以使用 $p_0$ 表示，向量 $\overrightarrow{O_0{O}_1}$ 就是光心 $O_1$ 相对于 $O_0$ 的平移，记为 $t$, 向量$\overrightarrow{O_1{P}_1}$根据前面的讨论，可以用 $R{p}_1$ 来表示，那么结论1可以表示为以下$结论2$:$$p_0\cdot (t\times R{p}_1)=0$$

这就是对极约束最直观的解释。又三维列向量下面 $\vec{a}\times \vec{b}={\vec{a}}^{\wedge }\vec{b}$ 等式恒成立(参考博客SLAM练习题（七）—— 对极约束)。
其中等式左边 X 表示叉乘，等式右边上三角符号表示反对称矩阵。所以一般把中间的部分拿出来存在 $t\times R=t^{\wedge }R$，像下面这样，记为本质矩阵或本征矩阵（Essential Matrix）。
$$E=t^{\wedge }R$$然后我们可以得到如下$结论3$$$p_0\cdot E{p}_1=p_0^{T}E{p}_1=0$$在前面我们提到，平面 $I_0$ 中的任意一点，在平面 $I_1$ 中都有对应的一条对应过极点的直线与之对应。另外我们再来看看什么是齐次坐标.

在2D平面上，一条直线 l 可以用方程 $ax+by+c=0$ 来表示，该直线用向量表示的话一般记做$$l=(a,b,c{)}^T$$,我们知道点$p=(x,y)$在直线 $l$ 上的充分必要条件是 $ax+by+c=0$, 如果使用齐次坐标的话，点p的齐次坐标就是 $p^{\prime }=(x,y,1)$ ，那么 $ax+by+c=0$ 就可以用两个向量的内积（点乘）来表示：$$ax+by+c\ast 1=(a,b,c{)}^T(x,y,1)=l^T\ast p^{\prime }=0$$因此，点 $p$ 在直线 $l$ 上的充分必要条件就是 直线 $l$ 与 $p$ 的齐次坐标p’的内积：$$l^T\ast p^{\prime }=0$$ 根据上面的知识，结论3我们就可以把 $E{p}_1$ 看做是直线的方程，$p_0$ 看做是直线上的点，也就是说$E{p}_1$就是以 $O_0$ 为原点坐标系中的极线了。如下图中红色线条所示，就是极线啦，它的方程是 $E\ast p_1$ 。

通过前面的推导，我们得到了结论 $p_0^{T}E{p}_1=0$ 其中 $E\ast p_1$ 为极线方程。总的来说就是 平面 $I0$ 中的任意一点$p_0$，一定在平面 $I1$ 的极线 $E{p}_1$上。这里的 $E$ 我们称为本质或者本征矩阵(Essential)。
 

 

五、Fundamental矩阵推导

前面我们假设一个归一化的图像平面，该平面上焦距f =1。$p_0$, $p_1$ 的坐标是相对于 $O_0,O_1$ 而言，也就是其为图像坐标，并非$像素坐标$。
$$p_0=\left(\begin{array}{c}{x}_0\\ y_0\\ 1\end{array}\right)p_1=\left(\begin{array}{c}{x}_1\\ y_1\\ 1\end{array}\right)\text{\hspace{1em}(1)}$$
前面推导的公式 $p_0\cdot E{p}_1=0$，现在我们在这个公式的结果继续进行推导，假如我们知道相机 $O_0$, $O_1$,对应的内参分别为 $K_0,K_1$。那么我们可以把 $p_0,p_1$ 转化为像素坐标 $v_0,v1$。$$v_0=K_0{p}_0{v}_1=K{p}_1\text{\hspace{1em}(2)}$$
至于为什么可以这样转换，大家可以去看一下针孔相机的成像过程，或者百度下世界坐标如何转行为像素坐标。那么进一步我们可以得到
$$p_0=v_o{K}_0^{-1}{p}_1=v_1{K}_1^{-1}\text{\hspace{1em}(3)}$$

我们把（3）式带入 $p_0\cdot E{p}_1=p_0^{T}E{p}_1=0$ 我们可以得到如下结论：$$(v_0{K}_0^{-1}{)}^{T}E(v_1{K}_1^{-1})=K_0^{-T}{v}_0^{T}E{v}_1{K}_1^{-1}=v_0^T(K_0^{-T}E{K}_1^{-1})v_1=0$$然后我们令$$F=(K_0^{-T}E{K}_1^{-1})$$那么我们可以得到：$$v_o^{T}F{v}_1=0$$这样，我相信大家就比较理解了，这里的 $F$ 我们称为 Fundamental 矩阵，也就是基本矩阵。

 

六、结语

这样我们就推理出来了如下两个结论，Essential 矩阵：
$$E=t^{\wedge }R{p}_0^{T}E{p}_1=0$$与 Fundamental 矩阵: $$F=(K_0^{-T}E{K}_1^{-1})v_o^{T}F{v}_1=0$$其上的 $p_0,p_1$ 是图像坐标，$v_0,v_1$ 是像素坐标。这样我们就使用 对极约束 推理出来了 Essential矩阵以及Fundamental矩阵。并且可以得到，只要已知两个相机内参，我们把他们对应的 Essential 矩转换为 Fundamental矩阵。