立体视觉入门指南（2）：关键矩阵（本质矩阵，基础矩阵，单应矩阵）

亲爱的同学们，我们的世界是3D世界，我们的双眼能够观测三维信息，帮助我们感知距离，导航避障，从而翱翔于天地之间。而当今世界是智能化的世界，我们的科学家们探索各种机器智能技术，让机器能够拥有人类的三维感知能力，并希望在速度和精度上超越人类，比如自动驾驶导航中的定位导航，无人机的自动避障，测量仪中的三维扫描等，都是高智机器智能技术在3D视觉上的具体实现。

立体视觉是三维重建领域的重要方向，它模拟人眼结构用双相机模拟双目，以透视投影、三角测量为基础，通过逻辑复杂的同名点搜索算法，恢复场景中的三维信息。它的应用十分之广泛，自动驾驶、导航避障、文物重建、人脸识别等诸多高科技应用都有它关键的身影。

本课程将带大家由浅入深的了解立体视觉的理论与实践知识。我们会从坐标系讲到相机标定，从被动式立体讲到主动式立体，甚至可能从深度恢复讲到网格构建与处理，感兴趣的同学们，来和我一起探索立体视觉的魅力吧！

本课程是电子资源，所以行文并不会有太多条条框框的约束，但会以逻辑清晰、浅显易懂为目标，水平有限，若有不足之处，还请不吝赐教！
个人微信：EthanYs6，加我申请进技术交流群 StereoV3D，一起技术畅聊。
CSDN搜索 ：Ethan Li 李迎松，查看网页版课程。
随课代码，将上传至github上，地址：StereoV3DCode：https://github.com/ethan-li-coding/StereoV3DCode

大家好，上一篇立体视觉入门指南（1）：坐标系与相机参数中，我们对立体视觉的基础坐标系做了详细的介绍，承接上篇，本篇我们继续来讲基础：关键矩阵。

矩阵运算是立体视觉最常见的数学工具了，在前一篇中，相机的内参和外参也都是以矩阵的形式描述，我认为大家都有了一定的基础，线性代数和矩阵论都有涉猎，万一没有，那只能建议你们补补基础课了。

在双目视觉中，两个图像之间会有公共区域，自然地大家会想到这些公共区域的点是否会存在一些联系，更具体地是它们的坐标是否存在一些约束关系，比如关于它们坐标的关系式是否满足某个条件等式，抑或能否直接将某张图像上的点坐标转换成另一个图像上的点坐标。幸运的是，答案都是肯定的。本篇即为大家介绍双目视觉中三个关键的矩阵：本质矩阵、基础矩阵、单应性矩阵，它们可以建立两个视图公共点之间的坐标联系，或者完成公共点之间的坐标转换。它们就像立体视觉双视图之间的桥梁，让彼此紧密相连，形成一个整体，才算有了立体视觉系统的概念。

大家想必会发现，在上一篇中所介绍的坐标系知识，都是针对于单个相机而言，那么双目该如何考虑呢？双目视觉又是如何形成的呢？

首先让我们从人眼出发，我们之所以能够感受周围的立体世界，正是因为人的双眼形成了双目立体，同一个物体在两只眼睛内同时成像，由三角原理可以得到物体的三维空间坐标。

双目立体系统的结构和人眼类似，由两个相机组成，它们的视场相互重叠，重叠区域的场景可以通过寻找在两个相机上各自的成像点，并基于三角测量原理计算空间三维坐标。从硬件上来说，单目到双目只是多了一个相机（广义上来说，同一个相机移动到两个不同的位置也形成双目立体系统，这里以两个相机为角度展开）；从系统上来说，要形成一个系统，就需要双相机之间建立一条让两者紧密相连的纽带。上一课我们描述了单相机的几个重要的坐标系，显然，双相机都会有各自的独立坐标系统，而这条纽带就是建立在各自的独立坐标系统之间，而哪还有比以相机中心为原点的三维相机坐标系更适合这个角色的呢？因此，在双目立体系统中，在双相机的相机坐标系之间建立一个旋转和平移转换关系，也就是熟知的外参矩阵 $R$ 和 $t$ ，直观上来说，它们将空间点在左相机的相机坐标系坐标转换为在右相机的相机坐标系坐标，实质上，它们是描述两个相机的相对位姿关系，让两者紧密相连，互为彼此。如下图为一个典型的双目结构图。

对于$R$和$t$，我们给出纯数学上的描述，假设空间点P在左相机相机坐标系下的坐标为$P$，在右相机相机坐标系下的坐标为$P^{\prime }$，则满足：
$$P^{\prime }=R\ast P+t$$

除此之外，还通常会将双目系统的世界坐标系放在左相机的中心，并和左相机坐标系完全重合，从这个角度来说，$R,t$就相当于是右相机在世界坐标系下的外参，而左相机在世界坐标系的外参则是$I,0$（$I$为单位阵）。

以上便是双目立体系统双相机之间的坐标系联系，但不能忽视的是，这些坐标系似乎并不那么直接，在实际应用中，我们最先接触的可不是虚拟在脑海中的坐标系，而是呈现在眼前亦或存储在计算机内存中的图像！图像像素和像素的像点坐标，才是最直观的输入信息。所以，是否存在像素坐标之间的紧密联系呢？答案是有，也便是本篇的内容：关键矩阵，正是描述像素坐标之间的紧密联系。

本质矩阵和基础矩阵

设空间点P在左相机坐标系坐标的坐标为$P$，则在右相机坐标系中的坐标为$RP+t$，其在左右相机视图中的投影点分别为$p_1,p_2$。基于相机坐标系到影像坐标系的转换公式，得到如下转换式：
$$d_1{p}_1=K_{1}P,d_2{p}_2=K_2(RP+t)$$

左边的 $p$是齐次坐标，如果对空间点也取齐次，即将空间点归一化到$Z=1$的平面，$d$ 就变成了1，上式变成
$$p_1=K_{1}P,p_2=K_2(RP+t)$$

该等式在齐次意义上成立，也就是乘以任意非零常数依旧相等。

咱们把 $K$拿到等式左边来，则公式变成：
$$K_1^{-1}{p}_1=P,K_2^{-1}{p}_2=RP+t$$

设
$$x_1=K_1^{-1}{p}_1,x_2=K_2^{-1}{p}_2$$

可得
$$x_1=P,x_2=RP+t=>x_2=R{x}_1+t$$

两边同时叉乘$t$，可得
$$t\times x_2=t\times R{x}_1+t\times t$$

矢量和自己叉乘等于0，因此 $t\times R{x}_1=t\times x_2$，

再两边同时左乘$x_2^T$，得
$$x_2^{T}t\times R{x}_1=x_2^{T}t\times x_2$$

等式右边显然是等于0的，可得
$$x_2^{T}t\times R{x}_1=0$$

叉乘可以等价于用反对称矩阵来点乘，$t$ 的反对称矩阵表示为 $t$^，上式变成
$$x_2^T{t}^{\wedge }R{x}_1=0$$

该式描述了像平面坐标$x_1,x_2$之间的联系，即空间点在两个像平面的成像点通过外参矩阵$R,t$建立了一个等式关系，或者称为一种约束关系，这个约束就叫做对极约束。

但是不能忽视的一点是，$x_1,x_2$并不是原始像点坐标，而是左乘内参矩阵$K$之后的坐标，那如果是原始坐标应该是怎样的形式呢？我们把$x_1,x_2$还原成$p_1,p_2$，得到
$$p_2^T{K}_2^{-T}{t}^{\wedge }R{K}_1^{-1}{p}_1=0$$

这两个公式是等价的，它们都是对极约束，几何意义是$O_1,P,O_2,p_1,p_2$共面（见下图）。我们把上面两式像点中间夹着的部分记做两个矩阵：本质矩阵E：Essential matrix和基础矩阵F：Fundamental matrix。得到更简洁的表示形式：
$$x_2^{T}E{x}_1=0,p_2^{T}F{p}_1=0$$

其中，本质矩阵和基础矩阵的表达式分别是：
$$E=t^{\wedge }R,F=K_2^{-T}{t}^{\wedge }R{K}_1^{-1}$$

本质矩阵和基础矩阵的区别是：本质矩阵是和$x$建立的关系，而$x$是由内参矩阵$K$和像素坐标$p$计算出来的，所以本质矩阵使用的前提是内参矩阵$K$已知；而基础矩阵直接和像素坐标$p$建立联系，所以不需要已知内参矩阵。

对级约束

本质矩阵的分解

本质矩阵有良好的性质，它的奇异值是$[\sigma ,\sigma ,0]$的形式（继承于反对称矩阵$t^{\wedge }$的性质），可以通过SVD分解得到$R$和$t$。若$E$的SVD分解为$E=U\Sigma V^T$，则$R$和$t$的可能结果如下：
$$t_1^{\wedge }=UZ{U}^T,t_2^{\wedge }=U{Z}^T{U}^T,R_1=UW{V}^T,R_2=U{W}^T{V}^T$$

其中，$W=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$（正交阵），$Z=\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]$（反对称矩阵，若矢量$z=(0,0,1{)}^T$，则$z^{\wedge }=Z$）。

（证明见《Multiple View Geometry in Computer Vision(Second Edition)》chapter 9.6.2, page 258）

在解算$t$时，并不需要先计算出$t^{\wedge }$，拿$t_1$举例，实际上因为$t_1^{\wedge }=UZ{U}^T$，而$t_1^{\wedge }{t}_1=0$，因此$UZ{U}^T{t}_1=0$，等式两边左乘$U^T$，可得$Z(U^T{t}_1)=0$，由于$Z=z^{\wedge }$且$z^{\wedge }z=0$，因此$U^T{t}_1=z$，得$t_1=Uz=U(0,0,1{)}^T$，即$t_1$为矩阵$U$的第三列，即
$$t_1=U.col(3),t_2=-t_1$$

一共存在四组可能的$R,t$组合，如下图所示：

图来源：《Multiple View Geometry in Computer Vision (Second Edition)》

只有一种情况下，三角化后的点P均在两个相机前方，即正解（图中(a)即为正解）。将观测点带入验证即可得到正确的那组解。

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

单应性矩阵

想必有的同学注意到，本质矩阵和基础矩阵并不是两个像素之间的相互转换关系，而是一种坐标之间的内在约束式。那是否存在直接将左视图像素坐标转换到右视图对应点像素坐标的表达式呢？确实是有的，即单应性矩阵Homography matrix，但有特定条件：当空间中场景是同一个平面时，它们在左右视图的投影点可通过可逆的单应性矩阵一对一相互转换，表达式为
$$p_2=H{p}_1,p_1=H^{-1}{p}_2$$

实际上，单应性矩阵不只是描述同一平面的像素点之间的关系，而是同一个平面在任意坐标系之间都可以建立单应性变换关系，比如影像坐标系与影像坐标系之间，世界坐标系和影像坐标系之间，如下图所示，

在双目立体视觉内，单应性变换是Zhang式相机标定法¹的理论基础，纯平的标定板平面和影像平面存在单应性变换关系，同时它们存在世界坐标系到影像坐标系之间的投影变换关系，两个关系对等即可解出相机的内外参数，详细我们后面在讲。

单应性变换的另一个典型应用是图像拼接，将像素点假象成一个局部小平面，并求解局部的单应性矩阵，即可建立两个视图之间像素的一一映射，从而完成拼接。

从单应性矩阵的角度，我们来完成基础矩阵的另一个推导，假设左右视图中的一对同名点$p_1,p_2$，它们之间的单应性变换矩阵为$H$，则有
$$p_2=H{p}_1$$

设右视图上的极点为$e_2$，则$e_2$和$p_2$共同构成极线 $l$，则有
$$l=e_2\times p_2=e_2^{\wedge }H{p}_1$$

由于$p_2$在极线上，则有$p_2^{T}l=0$，推得
$$p_2^T{e}_2^{\wedge }H{p}_1=0$$

参照前面的推导：$p_2^{T}F{p}_1=0$，可得
$$F=e_2^{\wedge }H$$

即基础矩阵等于右视图极点的反对称矩阵和像素之间单应性矩阵的乘积 。

矩阵自由度分析

矩阵求解

本质矩阵和单应性矩阵的求解是相对更常见的需求，本篇重点介绍这两个矩阵的求解。

本质矩阵求解

本质矩阵$E$的自由度是5，一对点可以列1个方程，这表明最少要用5对点来求解；但是由于这5个自由度是非线性相关的，如果要用5对来求解，就涉及到非线性解算，比较麻烦，所以更常见的方法是不按照$E$的真实自由度，而是只考虑尺度等价性，把$E$当做8个自由度来求解，最少用8对点，称之为8点法。

假设一对匹配点在左右视图的像素坐标分别为$p_1=[u_1,v_1,1{]}^T,p_2=[u_2,v_2,1{]}^T$，左乘内参矩阵$K$之后的坐标为$x_1=[x_1,y_1,1{]}^T,x_2=[x_2,y_2,1{]}^T$，则根据对极约束$x_2^{T}E{x}_1=0$，有
$$\left[\begin{array}{ccc}{x}_2& y_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}{x}_1\\ y_1\\ 1\end{array}\right]=0$$

其中，未知数$e_1,e_2...e_9$是本质矩阵的9个元素，若记一维矢量$e=[e_1,e_2,...,e_9{]}^T$，把上式展开，可得
$$[x_2{x}_1,x_2{y}_1,x_2,y_2{x}_1,y_2{y}_1,y_2,x_1,y_1,1]e=0$$

这是一个线性方程，而8对匹配点可以得到线性方程组：
$$\left[\begin{array}{ccccccccc}{x}_2^1{x}_1^1& x_2^1{y}_1^1& x_2^1& y_2^1{x}_1^1& y_2^1{y}_1^1& y_2^1& x_1^1& y_1^1& 1\\ x_2^2{x}_1^2& x_2^2{y}_1^2& x_2^2& y_2^2{x}_1^2& y_2^2{y}_1^2& y_2^2& x_1^2& y_1^2& 1\\ .& .& .& .& .& .& .& .& \\ .& .& .& .& .& .& .& .& \\ .& .& .& .& .& .& .& .& \\ x_2^8{x}_1^8& x_2^8{y}_1^8& x_2^8& y_2^8{x}_1^8& y_2^8{y}_1^8& y_2^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$$

解该线性方程$Ae=0$得到矢量$e$，即可得到本质矩阵$E$。矢量$e$为系数矩阵$A$的零空间，由线性代数的知识可知方程的特解数为$9-rank(A)$，当$rank(A)=8$时，存在唯一特解，方程所有解均为特解的常数倍，正对应着$E$尺度不变性的性质。

一般情况下，我们基于大量观测值求最小二乘解：矢量$e$是方阵$A^{T}A$最小特征值对应的特征向量（证略）。同时，奇异值分解也一种解法。

需要注意的是，由于噪声的存在，通过线性方程解出来的本质矩阵，大概率不满足内在性质，即奇异值不是$[\sigma ,\sigma ,0]$的形式，通常的做法是将奇异值强制调整成$[\sigma ,\sigma ,0]$的形式，若对求解得到的E进行SVD分解，得到奇异值矩阵$\Sigma =diag({\sigma }_1,{\sigma }_2,{\sigma }_3)$，不妨设${\sigma }_1,{\sigma }_2>{\sigma }_3$，则更新$E$为:
$$E=U\left[\begin{array}{ccc}\frac{{\sigma }_1+{\sigma }_2}{2}& & \\ & \frac{{\sigma }_1+{\sigma }_2}{2}& \\ & & 0\end{array}\right]V^T$$

实际上就是把最小值去掉（最小值大概率是接近0的），让新的$\sigma$取两个大值的平均值。另一种更简单做法是直接让$\sigma =1$，因为$E$的尺度等价性质。

单应性矩阵求解

单应性矩阵的自由度是8，一对点可以列2个方程，这表明最少要用4对点来求解。假设一对匹配点在左右视图的像素坐标分别为$p_1=[u_1,v_1,1{]}^T,p_2=[u_2,v_2,1{]}^T$，则根据对极约束$p_2=H{p}_1$，有
$$\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=\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}_2\\ v_2\\ 1\end{array}\right]$$

上式具有尺度不变性，即$H$可以乘以任意非零常数，那我们不妨让$h_9=1$（即$H$除以$h_9$），按照齐次公式的常规处理方式，我们会让第1，2行除以第3行以去除尺度因子，可得
$$\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}$$

代入$h_9=1$，并将未知数$h_1,h_2...h_8$整理到等式左边，得到
$$\begin{gathered} u_1{h}_1+v_1{h}_2+h_3-u_1{u}_2{h}_7-v_1{u}_2{h}_8=u_2 \\ {u}_1{h}_4+v_1{h}_5+h_6-u_1{v}_2{h}_7-v_1{v}_2{h}_8=v_2 \end{gathered}$$

这样一对匹配点可以列出两个方程，求解8个未知数需要至少4对点，前提是这4对点不存在三点一线的退化情况。同样列出4对点情况下的线性方程组：
$$\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^4{u}_2^4& -v_1^4{u}_2^4\\ 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\\ h_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]$$

解该线性方程组即可得到单应性矩阵$H$。

作业

这里为大家准备了一些练习题，可以通过实践加深理解：

1 提供同名像点对的像素坐标集合，以及内参矩阵$K$，求解本质矩阵$E$，并分解得到$R$和$t$。
2 提供同名像点对的像素坐标集合，求解单应性矩阵$H$。
(假设以上像素坐标都是畸变纠正后的坐标。)

参考答案地址：https://github.com/ethan-li-coding/StereoV3DCode [代码持续更新，大家感兴趣可以star和watch]

参考文献

- [1] Zhang Z . A Flexible New Technique for Camera Calibration[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2000, 22(11):1330-1334. -

- [2] Hartley R , Zisserman A . Multiple view geometry in computer vision. With foreword by Olivier Faugeras. 2nd edition. 2003.