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

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

一、什么是对极约束

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

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

图1

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

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

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

 

二、对极约束有什么作用

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

 

三、基本概念

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

图2

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

 

四、Essential矩阵推导

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

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

有了上面的两个概念之后，我们根据 图2 可以得到 结论 1 \color{red}{结论1} 结论1: O 0 P 0 → ⋅ ( O 0 O 1 → × O 1 P 1 → ) = 0 \color{green} \overrightarrow{O_0P_0}\cdot(\overrightarrow{O_0O_1}\times \overrightarrow{O_1P_1})=0 O0​P0​ ​⋅(O0​O1​ ​×O1​P1​ ​)=0因为 O 0 O 1 → × O 1 P 1 → \overrightarrow{O_0O_1}\times \overrightarrow{O_1P_1} O0​O1​ ​×O1​P1​ ​ 得到的向量垂直于平面 O 0 O 1 P O_0O_1P O0​O1​P,   O 1 P 1 → \overrightarrow{O_1P_1} O1​P1​ ​ 属于平面 O 0 O 1 P O_0O_1P O0​O1​P，则 O 1 P 1 → \overrightarrow{O_1P_1} O1​P1​ ​ 垂直于 O 0 O 1 → × O 1 P 1 → \overrightarrow{O_0O_1}\times \overrightarrow{O_1P_1} O0​O1​ ​×O1​P1​ ​ 的结果，又根据点乘的定义，所以上述公式成立。

图2 中的点 p 0 , p 1 p_0,p_1 p0​,p1​ 都是二维点，一般来说向量叉乘是三维的概念,.我们假设一个归一化的图像平面，该平面上焦距f =1,这里把它变成三维的方向向量来考虑。因此我们可以定义在以 O 0 O_0 O0​ 为原点的坐标系下 p 0 = ( x 0 y 0 1 ) \color{green} p_0=\begin{pmatrix} x_0\\ y_0\\ 1\\ \end{pmatrix} p0​=⎝ ⎛​x0​y0​1​⎠ ⎞​ 而在以 O 1 O_1 O1​ 为原点的坐标系下: p 1 = ( x 1 y 1 1 ) \color{green} p_1=\begin{pmatrix} x_1\\ y_1\\ 1\\ \end{pmatrix} p1​=⎝ ⎛​x1​y1​1​⎠ ⎞​显而易见 p 0 , p 1 p_0,p_1 p0​,p1​ 不在同一坐标系中。前面说过， p 0 p_0 p0​ 在以 O 0 O_0 O0​为原点的参考坐标系， p 1 p_1 p1​ 在以 O 1 O_1 O1​ 为原点的参考坐标系，所以我们还是需要转换坐标系。这里我们把所有点的坐标都转换到以 O 0 O_0 O0​为原点的坐标系。前面说过这些向量都是方向向量和向量起始位置无关，所以这里坐标系变换只考虑旋转就可以。我们记 R R R 为从 O 1 O_1 O1​ 坐标系到 O 0 O_0 O0​ 坐标系的旋转矩阵, 之前的等式结论1 ： O 0 P 0 → ⋅ ( O 0 O 1 → × O 1 P 1 → ) = 0 \color{green} \overrightarrow{O_0P_0}\cdot(\overrightarrow{O_0O_1}\times \overrightarrow{O_1P_1})=0 O0​P0​ ​⋅(O0​O1​ ​×O1​P1​ ​)=0最左边的 O 0 P 0 → \overrightarrow{O_0P_0} O0​P0​ ​ 可以使用 p 0 p_0 p0​ 表示，向量 O 0 O 1 → \overrightarrow{O_0O_1} O0​O1​ ​ 就是光心 O 1 O_1 O1​ 相对于 O 0 O_0 O0​ 的平移，记为 t t t, 向量 O 1 P 1 → \overrightarrow{O_1P_1} O1​P1​ ​根据前面的讨论，可以用 R p 1 Rp_1 Rp1​ 来表示，那么结论1可以表示为以下 结论 2 \color{red}{结论2} 结论2: p 0 ⋅ ( t × R p 1 ) = 0 \color{green} p_0 \cdot(t\times Rp_1)=0 p0​⋅(t×Rp1​)=0

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

在2D平面上，一条直线 l 可以用方程 a x + b y + c = 0 ax+by+c=0 ax+by+c=0 来表示，该直线用向量表示的话一般记做 l = ( a , b , c ) T \color{green} l=(a,b,c)^T l=(a,b,c)T,我们知道点 p = ( x , y ) p = (x, y) p=(x,y)在直线 l l l 上的充分必要条件是 a x + b y + c = 0 ax+by+c=0 ax+by+c=0, 如果使用齐次坐标的话，点p的齐次坐标就是 p ′ = ( x , y , 1 ) p'=(x, y, 1) p′=(x,y,1) ，那么 a x + b y + c = 0 ax + by + c = 0 ax+by+c=0 就可以用两个向量的内积（点乘）来表示： a x + b y + c ∗ 1 = ( a , b , c ) T ( x , y , 1 ) = l T ∗ p ′ = 0 \color{green} a x+b y+c * 1=(a, b, c)^{T}(x, y, 1)=l^{T} * p^{\prime}=0 ax+by+c∗1=(a,b,c)T(x,y,1)=lT∗p′=0因此，点 p p p 在直线 l l l 上的充分必要条件就是 直线 l l l 与 p p p 的齐次坐标p’的内积： l T ∗ p ′ = 0 \color{green} l^{T} * p^{\prime}=0 lT∗p′=0 根据上面的知识，结论3我们就可以把 E p 1 Ep_1 Ep1​ 看做是直线的方程， p 0 p_0 p0​ 看做是直线上的点，也就是说 E p 1 Ep_1 Ep1​就是以 O 0 O_0 O0​ 为原点坐标系中的极线了。如下图中红色线条所示，就是极线啦，它的方程是 E ∗ p 1 E*p_1 E∗p1​ 。 通过前面的推导，我们得到了结论 p 0 T E p 1 = 0 p_0^T Ep_1=0 p0T​Ep1​=0 其中 E ∗ p 1 E*p_1 E∗p1​ 为极线方程。总的来说就是 平面 I 0 I0 I0 中的任意一点 p 0 p_0 p0​，一定在平面 I 1 I1 I1 的极线 E p 1 Ep_1 Ep1​上。这里的 E E E 我们称为本质或者本征矩阵(Essential)。  

 

五、Fundamental矩阵推导

前面我们假设一个归一化的图像平面，该平面上焦距f =1。 p 0 p_0 p0​, p 1 p_1 p1​ 的坐标是相对于 O 0 , O 1 O_0, O_1 O0​,O1​ 而言，也就是其为图像坐标，并非 像素坐标 \color{red}{像素坐标} 像素坐标。 p 0 = ( x 0 y 0 1 )                p 1 = ( x 1 y 1 1 ) (1) \tag {1} \color{green} p_0=\begin{pmatrix} x_0\\ y_0\\ 1\\ \end{pmatrix} ~~~~~~~~~~~~~~p_1=\begin{pmatrix} x_1\\ y_1\\ 1\\ \end{pmatrix} p0​=⎝ ⎛​x0​y0​1​⎠ ⎞​              p1​=⎝ ⎛​x1​y1​1​⎠ ⎞​(1) 前面推导的公式 p 0 ⋅ E p 1 = 0 p_0 \cdot Ep_1=0 p0​⋅Ep1​=0，现在我们在这个公式的结果继续进行推导，假如我们知道相机 O 0 O_0 O0​, O 1 O_1 O1​,对应的内参分别为 K 0 , K 1 K_0, K_1 K0​,K1​。那么我们可以把 p 0 , p 1 p_0,p_1 p0​,p1​ 转化为像素坐标 v 0 , v 1 v_0,v1 v0​,v1。 v 0 = K 0 p 0                 v 1 = K p 1 (2) \tag {2} \color{green} v_0=K_0p_0 ~~~~~~~~~~~~~~~v_1=Kp_1 v0​=K0​p0​               v1​=Kp1​(2) 至于为什么可以这样转换，大家可以去看一下针孔相机的成像过程，或者百度下世界坐标如何转行为像素坐标。那么进一步我们可以得到 p 0 = v o K 0 − 1                 p 1 = v 1 K 1 − 1 (3) \tag {3} \color{green} p_0=v_oK_0^{-1} ~~~~~~~~~~~~~~~p_1=v_1K_1^{-1} p0​=vo​K0−1​               p1​=v1​K1−1​(3)

我们把（3）式带入 p 0 ⋅ E p 1 = p 0 T E p 1 = 0 p_0 \cdot Ep_1=p_0^TEp_1=0 p0​⋅Ep1​=p0T​Ep1​=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 \color{green} (v_0K_0^{-1})^TE(v_1K_1^{-1})= K_0^{-T}v_0^T Ev_1K_1^{-1}=v_0^T(K_0^{-T} EK_1^{-1})v_1=0 (v0​K0−1​)TE(v1​K1−1​)=K0−T​v0T​Ev1​K1−1​=v0T​(K0−T​EK1−1​)v1​=0然后我们令 F = ( K 0 − T E K 1 − 1 ) \color{green} F=(K_0^{-T} EK_1^{-1}) F=(K0−T​EK1−1​)那么我们可以得到： v o T F v 1 = 0 \color{green} v_o^TFv_1=0 voT​Fv1​=0这样，我相信大家就比较理解了，这里的 F F F 我们称为 Fundamental 矩阵，也就是基本矩阵。

 

六、结语

这样我们就推理出来了如下两个结论，Essential 矩阵： E = t ∧ R                      p 0 T E p 1 = 0 \color{green} E=t ^{\wedge} R~~~~~~~~~~~~~~~~~~~~\color{green} p_0^T Ep_1=0 E=t∧R                    p0T​Ep1​=0与 Fundamental 矩阵: F = ( K 0 − T E K 1 − 1 )            v o T F v 1 = 0 \color{green} F=(K_0^{-T} EK_1^{-1}) ~~~~~~~~~~ \color{green} v_o^TFv_1=0 F=(K0−T​EK1−1​)          voT​Fv1​=0其上的 p 0 , p 1 p_0,p_1 p0​,p1​ 是图像坐标， v 0 , v 1 v_0,v_1 v0​,v1​ 是像素坐标。这样我们就使用 对极约束 推理出来了 Essential矩阵以及Fundamental矩阵。并且可以得到，只要已知两个相机内参，我们把他们对应的 Essential 矩转换为 Fundamental矩阵。