视觉SLAM14讲——视觉里程计1（2D-2D对极几何）

1、对极几何

1、对极约束

从上一讲中，我们知道了如何匹配特征点，一个特征点由关键点和描述子两部分组成，描述子的距离表示两个特征之间的相似程度。假设我们目前已经从两张图像中得到了一对配对好的特征点，如果有若干对这样的匹配点，我们就可以通过二维图像间的对应关系，恢复出两帧之间相机的运动。

如图所示，我们希望知道两帧图像I1,I2之间的运动，设第一帧到第二帧的运动为R,t，两个相机中心分别为O1,O2,现在I1中有一个特征点p1，它对应在图像I2特征点为p2。两者是通过上一讲的特征匹配得到的，如果匹配正确，那么说明他们是同一空间点在不同平面上的投影。

接下来我们介绍一下它们之间的几何关系。首先，连线O1p1和连线O2p2会在空间中交于点P，此时P,O1,O2组成了一个平面，我们称之为极平面，O1O2连线会与I1，I2平面出现交点，记作e1，e2，我们称之为极点，O1O2连线称为基线，极平面与两个像平面I1,I2（大写）之间的交线l1，l2（小写）称为极线。

当我们从第一帧的角度上看，射线O1p1是某个像素可能出现的空间位置，也就是说空间点P一定会在射线O1p1上，从第二帧图像上来看，第一帧的射线O1p1在第二帧图像上的投影就是极线l2。

接下来分析它们之间的几何关系，在第一帧坐标系下，点P的空间位置为：

 根据之前讲过的针孔相机模型，两个像素点p1，p2的像素位置为：

 我们用齐次坐标表示像素点，在齐次坐标时，一个向量等于它自身乘上任意的非零常数。这通常用于表达投影关系。s1p1和p1成投影关系，所以可写为：

 带入上式得：

 现在我们用归一化坐标表示，归一化坐标左乘内参可以得到像素坐标：

 此处的x1，x2是归一化的坐标。加上旋转矩阵和平移后：

 等式两边同时和t做外积

 再变形：

 将x1，x2与p1，p2再次替换：

 以上两个式子都成为对极约束。几何意义为：O1,P,O2三者共面。对极约束中同时包括了平移和旋转。我们将中间部分记作：基础矩阵F和本质矩阵E，所以可以简化为：

 上述问题就变为：

根据配对点的像素位置或者归一化坐标求出E或者F。

再根据E或者F求出R和t。E和F相差了一个相机内参，所以实践中更多的使用E。

2、本质矩阵

 本质矩阵的定义：E=t^R，是一个3x3的矩阵，有9个未知数，本质矩阵有如下需要注意的：

 实践中可以用八点法来求解E

假设有一对匹配点，他们的归一化坐标为：

 将其带入对极约束的定义中：

 将E展开写为向量的形式：

 上面的对极约束可以写为：

 因为八点法是使用8对点来估计E，所以把其他点也放在方程中，变为线性方程组为：

 上述方程组的系数矩阵为特征点的位置，按照线性代数的知识，如果系数矩阵满秩（秩为8）则E有解。

当我们求解出E后，接下来的任务是如何根据E来恢复相机运动R，t。

本质矩阵E恢复相机运动的过程是由奇异值分解得到的

 在奇异值分解中，对于任意一个E，存在两个可能的t，R与之对应。

 分解本质矩阵会得到4个解，只需要把任意一点带入4种解中，检测该点在两个相机下的深度，P在两个相机中都有正深度就是正确的解。

2、三角测量

前面我们知道了如何从不同特征点中，通过八点法求出本质矩阵E，然后再通过对本质矩阵E进行奇异值分解，最终获得相机运动R和t。

在单目SLAM中，仅通过单张图像无法获得像素的深度信息，需要用三角测量来估计深度。

 设x1，x2是两个特征点的归一化坐标：

 现在已经知道了R，t想要计算出s1和s2：

 由于R，t，x1，x2都为已知，所以上述式子就是一个关于s1的方程，求解出s1就可以求解出s2