视觉SLAM十四讲学习笔记-第七讲-视觉里程计-对极几何和对极约束、本质矩阵、基础矩阵

专栏系列文章如下：

 专栏汇总

视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-开发环境搭建_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转矩阵和Eigen库_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转向量、欧拉角、四元数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-相似、仿射、射影变换和eigen程序、可视化演示_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记---前三讲学习笔记总结之SLAM的作用、变换和位姿表示_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-李代数求导与扰动模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-Sophus实践、相似变换群与李代数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-相机模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-图像和实践_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲---第五讲学习笔记总结---李群和李代数、相机_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲学习笔记总结（1）---非线性优化原理_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲学习笔记总结（2）---非线性优化应用_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第七讲-视觉里程计-特征点发和特征提取和匹配实践_goldqiu的博客-CSDN博客

7.3 2D−2D: 对极几何

7.3.1对极约束

假设从两张图像中得到了一对配对好的特征点,如图:

如果有若干对这样的匹配点,就可以通过这些二维图像点的对应关系,恢复出在两帧之间摄像机的运动。求取两帧图像I1, I2之间的运动,设第一帧到第二帧的运动为R,t。两个相机中心分别为O1,O2。如果I1中有一个特征点p1 ,它在I2中对应着特征点p2，两者是通过特征匹配得到的。如果匹配正确,说明它们是同一个空间点在两个成像平面上的投影。首先,连线O1p1和连线O2p2在三维空间中会相交于点P。这时候点O1, O2 ,P三个点可以确定一个平面,称为极平面(Epipolar plane) 。O1O2连线与像平面I1, I2 的交点分别为 e1, e2 。e1, e2 称为极点(Epipoles) ,O1O2被称为基线(Baseline) 。称极平面与两个像平面 I1, I2之间的相交线 L1, L2极线(Epipolar line) 。

直观讲,从第一帧的角度看,射线O1p1是某个像素可能出现的空间位置——因为该射线上的所有点都会投影到同一个像素点。同时,如果不知道P的位置,那么当在第二幅图像上看时,连线e2p2(也就是第二幅图像中的极线)就是P可能出现的投影的位置,也就是射线O1p1在第二个相机中的投影。由于通过特征点匹配确定了p2的像素位置,所以能够推断P的空间位置,以及相机的运动。如果没有特征匹配，就没法确定p2到底在极线的哪个位置了。

从代数角度来看一下这里的几何关系。在第一帧的坐标系下,设P的空间位置为P = [X, Y, Z]T.根据针孔相机模型知道两个像素点 p1, p2的像素位置为：

这里K为相机内参矩阵,R, t 为两个坐标系的相机运动。具体来说,这里计算的是 R21和 t21 ,因为是把第一个坐标系下的坐标转换到第二个坐标系下。在使用齐次坐标时,一个向量等于它自身乘上任意的非零常数。这通常用于表达一个投影关系。例如 s1p1 和 p1 成投影关系,它们在齐次坐标的意义下是相等的。称这种相等关系为尺度意义下相等(equal up to a scale),记作:sp ≃ p.那么,上述两个投影关系可写为:p1 ≃ KP ,p2 ≃ K(RP + t) .现在取:

这里的x1,x2是两个像素点的归一化平面上的坐标。代入上式,得:x2 ≃ Rx1 + t.

两边同时左乘 t∧ 。根据∧的定义,这相当于两侧同时与t做外积:

然后,两侧同时左乘

得：

观察等式左侧

是一个与 t 和x2都垂直的向量。把它再和x2做内积时,将得到0。由于等式左侧严格为零,那么乘以任意非零常数之后也为零,于是可以把 ≃ 写成通常的等号。因此,就得到了:

重新代入 p1, p2 ,有:

这两个式子都称为对极约束。它的几何意义是O1, P, O2三者共面。对极约束中同时包含了平移和旋转。把中间部分记作两个矩阵:基础矩阵(Fundamental Matrix)F和本质矩阵(Essential Matrix)E,于是可以进一步简化对极约束:

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

1. 根据配对点的像素位置求出 E 或者 F 。
2. 根据E或者F求出 R, t。由于 E 和 F 只相差了相机内参,而内参在SLAM中通常是已知的,所以实践当中往往使用形式更简单的E。

7.3.2本质矩阵

根据定义,本质矩阵 E = t∧R。它是一个3 × 3的矩阵,内有 9 个未知数。从E的构造方式上看,有以下值得注意的地方:

• 本质矩阵是由对极约束定义的。由于对极约束是等式为零的约束,所以对E乘以任意非零常数后,对极约束依然满足。这称为E在不同尺度下是等价的。

• 根据 E = t∧R,可以证明,本质矩阵E的奇异值必定是 [σ, σ, 0]T的形式。这称为本质矩阵的内在性质。

• 另一方面,由于平移和旋转各有 3 个自由度,故 t∧R 共有 6 个自由度。但由于尺度等价性,故 E 实际上有 5 个自由度。表明最少可以用5对点来求解E。但是,E的内在性质是一种非线性性质,在估计时会带来麻烦,因此,也可以只考虑它的尺度等价性,使用8对点来估计E——这就是经典的八点法(Eight-point-algorithm) 。八点法只利用了E的线性性质,因此可以在线性代数框架下求解。

考虑一对匹配点,它们的归一化坐标为 x1= [u1 , v1 , 1] T , x2 = [u2 , v2 , 1] T 。根据对极约束,有:

把矩阵 E 展开,写成向量的形式:e = [e1 , e2 , e3 , e4 , e5 , e6 , e7 , e8 , e9 ] T ,那么对极约束可以写成与e有关的线性形式:[u1u2 , u1v2 , u1, v1u2, v1v2, v1, u2, v2, 1] ·e = 0.同理,对于其他点对也有相同的表示。把所有点都放到一个方程中,变成线性方程组(ui , vi 表示第 i 个特征点，以此类推）：

这八个方程构成了一个线性方程组。它的系数矩阵由特征点位置构成，大小为 8 × 9。 e 位于该矩阵的零空间中。如果系数矩阵是满秩的（即秩为 8），那么它的零空间维数为 1， 也就是 e 构成一条线。这与 e 的尺度等价性是一致的。如果八对匹配点组成的矩阵满足秩为 8 的条件，那么E的各元素就可由上述方程解得。

接下来的问题是如何根据已经估得的本质矩阵E，恢复出相机的运动 R, t。这个过程是由奇异值分解（SVD）得到的。设 E 的 SVD 分解为：

其中 U,V 为正交阵，Σ 为奇异值矩阵。根据 E 的内在性质，可以知道 Σ = diag(σ, σ, 0)。 在 SVD 分解中，对于任意一个 E，存在两个可能的 t, R 与它对应：

其中

表示沿 Z 轴旋转 90 度得到的旋转矩阵。同时，由于 −E 和 E 等价，所以对任意一个 t 取负号，也会得到同样的结果。因此，从 E 分解到 t, R 时，一共存在四个可能的解。只有第一种解中，P 在两个相机中都具有正的深度。因此，只要把任意一点代入四种解中，检测该点在两个相机下的深度，就可以确定哪个解是正确的了。

如果利用 E 的内在性质，那么它只有五个自由度。所以最小可以通过五对点来求解相机运动。然而这种做法形式复杂，从工程实现角度考虑，由于平时通常会有几十对乃至上百对的匹配点，从八对减至五对意义并不明显。剩下的问题还有一个：根据线性方程解出的 E，可能不满足 E 的内在性质——它的奇异值不一定为 σ, σ, 0 的形式。这时，在做 SVD 时会刻意地把 Σ 矩阵调整成上面的样子。通常的做法是，对八点法求得的 E 进行 SVD 分解后，会得到奇异值矩阵 Σ = diag(σ1, σ2, σ3)，不妨设 σ1 ≥ σ2 ≥ σ3。取：

这相当于是把求出来的矩阵投影到了 E 所在的流形上。更简单的做法是将奇异值矩阵取成 diag(1, 1, 0)，因为 E 具有尺度等价性，这样做也是合理的。