对极约束在单目相机位姿估计中,对齐次坐标、极几何、单应矩阵、基本矩阵是非常重要的概念。
参照《视觉SLAM14讲》以及一些博客和资料,这里想对其概念原理进行总结,对单目精度以及尺度漂移问题做出分析。

齐次坐标系与尺度不变性

在通过二维像素坐标恢复三维坐标的过程中,经常出现这个概念。
这篇博客讲的比较好。

关于齐次坐标系的直观感受

在我们的世界里,两平行线是永远不会相交的,但是在投影空间里,两条平行线在无穷远处是相交的,如这个铁轨一样:

在数学里,我们通过齐次变换来表示这种问题,齐次变换就是将一个原本是n维的向量用一个n+1维向量来表示。
为了制作二维齐次坐标,我们只需在现有坐标中添加一个额外的变量 w。 因此,笛卡尔坐标中的点 (X, Y) 变为齐次坐标中的 (x, y, w),这两个坐标描述的是同一点。 为了将齐次坐标 (x, y, w) 转换为笛卡尔坐标,我们只需将 x 和 y 除以 w:

例如,笛卡尔坐标系下(1,2)的齐次坐标可以表示为(1,2,1),如果点(1,2)移动到无限远处,在笛卡尔坐标下它变为(∞,∞),然后它的齐次坐标表示为(1,2,0),因为(1/0, 2/0) = (∞,∞),我们可以不用”∞”来表示一个无穷远处的点了。

关于其次坐标的尺度不变性

转化齐次坐标到笛卡尔坐标的过程中,我们可以发现:

齐次坐标系点 (1, 2, 3), (2, 4, 6) 和 (4, 8, 12) 对代表欧几里得空间(或笛卡尔空间)中的同一点(1/3, 2/3)。
在SLAM或者其它视觉感知的问题中常常会出现这样子的齐次坐标系构成的等式,如:

其中H或E为一个三乘以三的矩阵,应该有9个变量。这个时候经常会看到一句话,说由于尺度等价性,H或E只有8个自由度,这是什么意思呢?
上式的(u, v, 1)为齐次坐标,其乘以任意常数意义不变(等式依然成立),对应的H或E也应该满足这一点。说得更加具体一点,H或E可以有无数解,但这些解都是倍数关系,例如H或E的解可以为:

这么多解肯定不是我们想要的,所以我们进行一个归一化,规定H或E矩阵的最后一个元素的值为1,这样我们的解就唯一了。
至于为什么叫做“尺度等价性”,因为我们将这个值设置为1对应到真实世界中的长度可能是5cm,也可能是40m,这就需要额外的深度信息介入进行确定,这个以t的长度作为单位的尺度世界只和真实世界之间相差一个“尺度因子”。
所以我们在计算的时候说,由于“尺度不变性”,该三乘以三的矩阵有八个自由度(也就是有8个元素是未知量),还有一个元素作为尺度被设置为1(这个1乘以一个“尺度因子”就是实际的尺度)。

对极约束

我们从两张图像中,得到了一个匹配点,如果我们有几个这种匹配点,我们可以根据它们的对应关系,将相机在两帧画面之间的移动进行还原。

如上图,有如下几个概念:

  • 基本元素:O1、O2为两幅图像的焦点位置,i1、i2平面为两幅图像本身,P为某一3D点,P投影到两幅图像中的位置为p1、p2
  • 极平面:O1、O2、P之间构成的平面,可以看到,对每一个3D点,其都存在一个极平面,这些平面的几何就叫做“对极几何”
  • 极点:O1、O2与成像平面之间有交点e1、e2,e1、e2被称为极点
  • 基线:O1、O2连接成的直线被称为基线
  • 极线:极平面(O1-O2-P)与两幅图像平面i1、i2之间的交线l1、l2为极线

从第一帧图像看起,在像素平面i1中有一点p1,其对应的3D点的位置被约束在射线O1-p1上,或者说像素点p1为射线O1-p1上所有3D点在像素平面i1上的投影位置。
而在第二帧图像中,像素点p1所对应的所有3D点,在像素平面i2中的投影位置,被约束在直线e2-p2上,或者说直线e2-p2为射线O1-p1上所有3D点在图像平面i2上的投影位置的集合。
综上:所谓对极约束就是说同一个点在两像素平面上的投影,若已知左图映射点p1,那么右图映射点p2一定在相对于p1的极线上。
在此基础上,若知道p1与p2的对应关系,即已知投影到像素平面i1上p1点的3D点投影到像素平面i2上的p2点的像素坐标,则可以得到两像素平面i1、i2坐标系之间的位姿关系。


通常我们已知相机内参矩阵K,然后通过特征点匹配得知p1与p2为同一3D点在两像素平面上的投影位置,p1和p2在各自的像素平面坐标系上的像素坐标也已知,这里p1与p2分别代表一个3*1矩阵大小的齐次坐标。

这里直接将图像看做是像素平面,不考虑畸变(或者说图像已经是经过畸变矫正了的)。
设3D点P的空间位置为(X, Y, Z),像素平面i2坐标系到像素平面i1坐标系的欧式变换[R|t]表示为T21,则点P在像素平面i2对应的相机坐标系下的坐标P(i2)与点P在像素平面i1对应的相机坐标系下的坐标P(i1)关系为:

根据针孔相机模型,s1、s2为尺度信息(常数,就是3D点的Z坐标)在像素平面i1的像素坐标系下:

在像素平面i2的像素坐标系下:

令:

综上,有:

两侧同时与t做外积(也就是向量叉乘,表示为t”),再左乘 x_{2}的转置,得到:

x_{2}与t”做外积后的向量与t、x_{2}都垂直,再左乘x_{2}的转置相当于再与x_{2}做内积,得到的结果为0,也就说等式左边等于0。
故,有:

带入p1、p2得

以上两个等式就是对极约束所对应的数学内容,我们的目标是通过同一3D点在两不同像素平面上的坐标,得到两像素平面坐标系之间的转换
需要注意,以上过程中的欧式变换[R|t]表示为从坐标系2到坐标系1的,也就是T21
p1与p2分别代表一个3*1矩阵大小的齐次坐标

基本矩阵与本质矩阵

矩阵求解

我们把上一节的对极约束的两个公式的中间两部分记作两个矩阵:

E被称为本质矩阵(Essential Matrix),F被称为基础矩阵(Fundamental Matrix),则有:

本质矩阵E和基础矩阵F计算起来的思路其实是一样的,因为齐次坐标点x_{1}在乘以一个内参矩阵后依然是一个齐次坐标点:

根据齐次坐标的尺度等价性,E和F都有一个尺度因子、8个自由度,通过8个不同的点对可以得到,也就是我们常说的“八点法”。
当然,对于本质矩阵E,它有更多的约束,根据E = t”R(前面说过这个t”代表做外积的意思),E的形式应该是[r1 r2 0](在这个矩阵基础上在转置一下)的一个矩阵,也就是未知元素只有r1和r2(它们各自为一个三乘以一的矩阵),所以E只有6个自由度;再因为尺度等价性,E的自由度变为5。也就是可以通过“五点法”求本质矩阵。
一般情况下内参矩阵K是已知的,我们通常首先求得基本矩阵,然后通过E21 = K.transpose() _ F21 _ K关系求得本质矩阵E,然后对E进行分解得到两像素平面坐标系之间旋转矩阵与位移矩阵的具体内容。
在opencv中提供了FindFundamental函数来求解基本矩阵:

CV_EXPORTS_W Mat findFundamentalMat( InputArray points1, InputArray points2,
                                     int method = FM_RANSAC,
                                     double param1 = 3., double param2 = 0.99,
                                     OutputArray mask = noArray() );

points1:第一张图像的N个点;points2: 第二张图像的点;method:求解方法,FM_7POINT(七点法)、FM_8POINT(八点法)、FM_LMEDS(基于最小中值的鲁棒算法)、FM_RANSAC(基于RANSAC的鲁棒算法);param1: 该参数用于RANSAC算法(随机采样过程一致性),它是从点到对极线的最大距离(以像素为单位),超过该距离,该点被视为异常值,并且不用于计算最终的基础矩阵,取值范围1-3;param2: 该参数仅仅在RANSAC算法以及LMedS算法中, 它指定了估计矩阵正确的期望置信度(概率)。

SVD分解

在通过五点法或八点法求得本质矩阵E后。我们需要从中提取出从像素平面2到像素平面1的欧式变换[R|t],这个过程通过SVD奇异值分解得到。
SVD叫做奇异值分解,其实意思就是主成分分解,求解奇异值。参考知乎大佬的文章,如下所示,就是将矩阵A分解为这个样子:

关于通过SVD分解的值来得到R、t的原理我也看不太明白。。。总之在SVD分解中,对任意E,均存在两个可能的R、t与之对应:


虽然有四个解,但只有第一种情况才符合我们的实际情况(P点在两个相机中都具有正的深度)。因此,只要把任意一点代入四种解中,检测该点在两个相机下的深度,就可以确定哪个解是正确的了。
SVD分解可以使用opencv的函数cv::SVD::compute:

这里有两点需要注意:SVD分解得到的位移矩阵t是归一化后的值,若位移矩阵t为0则基础矩阵也为0就无法分解得到旋转矩阵R

单应矩阵

单应矩阵就比本质矩阵直接得多,同样是3D空间上一点P在两相素平面上的投影点p1、p2,其表示方式为:
p1 = H_p2
其中p1、p2为三行一列的齐次坐标,H为三行三列的矩阵。

根据尺度不变性,H有八个自由度,即8个未知量。
边同时叉乘p2(也就是对p2做外积)有:

展开、写成齐次方程后整理,有:

引入尺度不变性后,h9 = 1。所以需要4对点提供8个约束方程就可以求解,这种把H矩阵看成了向量,并构建包含该向量的等式方程来求解H的方法,被称做直接线性变换法DLT(Direct Linear Transform)。
_*单应矩阵等同于透视变换中使用的矩阵,描述了两个平面之间的映射关系__,具体为什么可以看单应矩阵的理解与推导
由于点是共面的,在平面不移动的情况下,通常直接令平面坐标系上的点z=0,再进行求解,张正友标定法就是这么干的。
opencv提供findHomography函数计算单应矩阵:

findHomography(srcPoints, dstPoints, method=None, 
               ransacReprojThreshold=None, mask=None, maxIters=None, confidence=None)

srcPoints:源平面中点的坐标矩阵;dstPoints:目标平面中点的坐标矩阵;method:求解方法,0(利用所有点的常规方法),RANSAC(基于RANSAC的鲁棒算法),LMEDS(最小中值鲁棒算法),RHO(基于PROSAC的鲁棒算法);ransacReprojThreshold:将点对视为内点的最大允许重投影错误阈值(仅用于RANSAC和RHO方法),参数通常设置在1到10的范围内(单位像素);mask:可选输出掩码矩阵,通常由鲁棒算法(RANSAC或LMEDS)设置。 请注意,输入掩码矩阵是不需要设置的;maxIters:RANSAC算法的最大迭代次数,默认值为2000;confidence:置信度,取值范围为0到1。
分解单应矩阵的方法与本质矩阵相同,使用SVD分解。

在ORB_SLAM算法里,作者在src/TwoViewReconstruction.cc里纯手写了基本矩阵和单应矩阵的求解(FindFundamental、FindHomography)、分解(ReconstructF、ReconstructH),有兴趣可以去研究研究

结尾玄月~