SLAM入门之视觉里程计(4)：基础矩阵的估计

在上篇文章中，介绍了三位场景中的同一个三维点在不同视角下的像点存在着一种约束关系：对极约束，基础矩阵是这种约束关系的代数表示，并且这种约束关系独立与场景的结构，只依赖与相机的内参和外参（相对位姿）。这样可以通过通过匹配的像点对计算出两幅图像的基础矩阵，然后分解基础矩阵得到相机的相对位姿。

通过匹配点对估算基础矩阵

基础矩阵表示的是图像中的像点p1到另一幅图像对极线l2的映射，有如下公式：

l2=Fp1

而和像点P1匹配的另一个像点p2必定在对极线l2上，所以就有：

pT2l2=pT2Fp1=0

这样仅通过匹配的点对，就可以计算出两视图的基础矩阵F。

基础矩阵F是一个3×3的矩阵，有9个未知元素。然而，上面的公式中使用的齐次坐标，齐次坐标在相差一个常数因子下是相等，F也就只有8个未知元素，也就是说，只需要8对匹配的点对就可以求解出两视图的基础矩阵F。下面介绍下8点法 Eight-Point-Algorithm计算基础矩阵的过程。

假设一对匹配的像点p1=[u1,v1,1]T,p2=[u2,v2,1]T，带入式子中，得到：

[u1,v1,1]⎡⎣⎢f1f4f7f2f5f8f3f6f9⎤⎦⎥⎡⎣⎢u2v21⎤⎦⎥=0

把基础矩阵F的各个元素当作一个向量处理

f=[f1,f2,f3,f4,f5,f6,f7,f8,f9]

那么上面式子可以写为

[u1u2,u1v2,u1,v1u2,v1v2,v1,u2,v2,1]⋅f=0

对于其他的点对也使用同样的表示方法。这样将得到的所有方程放到一起，得到一个线性方程组((ui,vi)表示第i个特征点）

⎡⎣⎢⎢⎢⎢⎢⎢⎢u11u12u21u22u31u32⋯u81u82u11v12u21v22u31v32⋯u81v82u11u21u31⋯u81v11u12v21u22v31u12⋯v81u82v11v12v21v22v31v12⋯v81v82v11v21v31⋯v81u12u22u32⋯u82v12v22v32⋯v82111⋯1⎤⎦⎥⎥⎥⎥⎥⎥⎥⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢f1f2f3f4f5f6f7f8f9⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥=0

求解上面的方程组就可以得到基础矩阵各个元素了。当然这只是理想中的情况，由于噪声、数值的舍入误差和错误的匹配点的影响，仅仅求解上面的线性方程组得到的基础矩阵非常的不稳定，因此在8点法的基础上有各种改进方法。

图像坐标归一化 Normalizing transformation

将上面公式中由匹配的点对坐标组成的矩阵记为系数矩阵A

Af=0

系数矩阵A是利用8点法求基础矩阵的关键，所以Hartey就认为，利用8点法求基础矩阵不稳定的一个主要原因就是原始的图像像点坐标组成的系数矩阵A不好造成的，而造成A不好的原因是像点的齐次坐标各个分量的数量级相差太大。基于这个原因，Hartey提出一种改进的8点法，在应用8点法求基础矩阵之前，先对像点坐标进行归一化处理，即对原始的图像坐标做同向性变换，这样就可以减少噪声的干扰，大大的提高8点法的精度。

预先对图像坐标进行归一化有以下好处：

能够提高运算结果的精度

利用归一化处理后的图像坐标，对任何尺度缩放和原点的选择是不变的。归一化步骤预先为图像坐标选择了一个标准的坐标系中，消除了坐标变换对结果的影响。

归一化操作分两步进行，首先对每幅图像中的坐标进行平移（每幅图像的平移不同）使图像中匹配的点组成的点集的形心（Centroid）移动到原点；接着对坐标系进行缩放是的点p=(x,y,w)T中的各个分量总体上有一样的平均值，各个坐标轴的缩放相同的，最后选择合适的缩放因子使点p到原点的平均距离是2√。 概括起来变换过程如下：

对点进行平移使其形心位于原点。

对点进行缩放，使它们到原点的平均距离为2√

对两幅图像独立进行上述变换



上图左边是原始图像的坐标，右边是归一化后的坐标，H是归一化的变换矩阵，可记为如下形式：

H=S⎡⎣⎢⎢⎢100010−μ¯−ν¯1S⎤⎦⎥⎥⎥

其中，μ¯,ν¯是图像像点坐标两个分量的平均值

μ¯=1N∑i=1Nμi,ν¯=1N∑i=1Nνi

S表示尺度，其表达式为：

S=2⋅N−−−−√∑Ni=1(μi−μ¯)2+(νi−ν¯)2−−−−−−−−−−−−−−−−−−−−−√

这样，首先对原始的图像坐标进行归一化处理，再利用8点法求解基础矩阵，最后将求得的结果解除归一化，得到基础矩阵F，总结如下：

对图像1进行归一化处理，计算一个只包含平移和缩放的变换H1，将图像1中的匹配点集p1i变换到新的点集p1i^，新点集的形心位于原点(0,0)T，并且它们到原点的平均距离是2√。

对图像2，计算变换矩阵H2进行相同的归一化处理

使用8点法利用变换后的点集估计基础矩阵F^

根据变换F=HT2F^H1

使用归一化的坐标虽然能够在一定程度上消除噪声、错误匹配带来的影响，但还是不够的。

最小二乘法

仅仅对图像坐标进行归一化处理，能在一定程度上提高计算的精度，但在实践中还是不够的。8点法中，只使用8对匹配的点对估计基础矩阵，但通常两幅图像的匹配的点对远远多于8对，可以利用更多匹配的点对来求解上面的方程。

由于基础矩阵F在一个常量因子下是等价的，这样可以给基础矩阵F的元素组成的向量f施加一个约束条件

∥f∥=1

这样由K≥8个匹配的点对，组合成一个矩阵QK×9，求解上面方程就变成了求解如下问题的最小二乘解

min∥f∥=1∥Qf∥2

其中，矩阵Q的每一行来自一对匹配点；f是基础矩阵F元素构成的待求解的向量，根据2-范数的定义

∥Qf∥2=(Qf)T(Qf)=fT(QTQ)f

将上式的中间部分提取出来得到矩阵M=QTQ,这是一个9×9的矩阵。基于拉格朗日-欧拉乘数优化定理，在∥f∥=1约束下，Qf=0的最小二乘解，为矩阵M=QTQ的最小特征值对应的特征向量。所以可以对矩阵Q进行奇异值分解（SVD）

Q=UΣVT

其中，Q是K×9的矩阵；U是K×K的正交阵；Σ是K×9的对角矩阵，对角线的元素是奇异值；VT是9×9的正交阵，每一个列向量对应着Σ中的奇异值。所以，最小二乘解就是VT的第9个列向量，也就是可由向量f=V9构造基础矩阵F。

基础矩阵F还有一个重要的性质，这里可以作为进一步的约束条件。那就是基础矩阵F的秩为2，

Rank(F)=2

上述使用列向量V9构造的基础矩阵的秩通常不为2，需要进一步的优化。在估计基础矩阵时，设其最小奇异值为0，对上面方法取得的基础矩阵进行SVD分解

F=SVD=S⎡⎣⎢v1000v2000v3⎤⎦⎥D

其最小特征值v3被设为0，以使得F的秩为2.这样得到

F=SVD=S⎡⎣⎢v1000v20000⎤⎦⎥D

上述F就是最终得到的基础矩阵。

对上面进行总结，使用最小二乘法估算基础矩阵的步骤如下：

对取得两幅图像的匹配点进行归一化处理，转换矩阵分别是H1,H2

有对应的匹配点(K≥8)构造系数矩阵Q

对矩阵Q进行SVD分解,Q=UΣVT，由向量f=V9构造矩阵F^

对得到的矩阵F^进行秩为2的约束，即对F^进行SVD分解，F^=S⋅diag(v1,v2,v3)⋅VT，令v3=0得到基础矩阵的估计F′=S⋅diag(v1,v2,0)⋅VT

对上一步得到的解进行反归一化处理，得到基础矩阵F=HT2F′H1

随机采样一致性 RANSAC

基于匹配点对估算两视图的基础矩阵，唯一的已知条件就是匹配的点对坐标。在实践中，点对的匹配肯定是存在误差的，主要有两种类型的误差：

不精确的测量点位置引起的系统误差，通常服从高斯分布

错误匹配引起的误差，这些不匹配的点被称为外点，通常不服从高斯分布

对于基础矩阵的估算，不匹配的点能够造成很大的误差，即使是只有一对错误的匹配都能使估算值极大的偏离真实值。因此，需要找到一种方法，从包含错误点（外点）的匹配点对集合中，筛选出正确的匹配点（内点）。

RANSAC(Random Sample Consensus)随机采样一致性从一组含有外点的数据集中，通过迭代的方式估计出符合该数据集的数学模型的参数。因此，它也可以用来检测出数据集中的外点。

RANSAC有两个基本的假设：

数据集中包含的内点，内点的分布符合一个数学模型；而数据集中的外点不复合该数学模型

能够一组内点（通常很少）集合能够估计出其符合的数据模型

RANSAC的具体思想是：给定N个数据点组成的集合P，假设集合中大多数的点都是可以通过一个模型来产生的，且最少通过n个点可以拟合出模型的参数，则可以通过以下的迭代方式拟合该参数：

从集合P中随机选择n个点

使用这n个点估计出一个模型M

对P中剩余的点，计算每个点与模型M的距离，距离超过阈值则认为是外点；不超过阈值则认为是内点，并记录该模型M所对应的内点的个数mi

将上面步骤重复k次，选择最大mi所对应的模型M作为最终结果。

在迭代次数k的选择是一个关键，可以通过理论的方式计算出k的取值。在选择n个点估计模型时，要保证选择的n的个点都是内点的概率足够的高。假设，从数据集中选择一个点为内点的概率为ϖ，则选择的n个点都是内点的概率为ϖn;则1−ϖn表示选择的n个点至少有一个是外点，用包含外点估算的模型显然是不正确的，则迭代k次均得不到正确模型的概率为(1−ϖn)k。设p表示k次迭代中至少有一次选择的点都是内点的概率，也就是估计出了正确的模型，则1−p就表示k次跌点都得到正确的模型，所以有

1−p=1−ϖn)k

两边同时取对数，则有

k=log(1−p)log(1−ϖn)

一般要求p大于95%即可。

使用RANSAC估算基础矩阵时，首先需要确定判断点是内点还是外点的依据。通过上一篇的两视图的对极几何可知，像点总是在对极线，因此可以选择像点到对极线的距离作为判断该点是内点还是外点的依据，设点到对极的距离的阈值为d。则使用RANSAC的方法估算基础矩阵的步骤：

从匹配的点对中选择8个点，使用8点法估算出基础矩阵Fi

计算其余的点对到其对应对极线的距离dn，如果dn≤d则该点为内点，否则为外点。记下符合该条件的内点的个数为mi

迭代k次，或者某次得到内点的数目mi占有的比例大于等于95%，则停止。选择mi最大的基础矩阵作为最终的结果。

RANSAC能够不依赖于任何额外信息的情况下，将数据划分为内点和外点。但也有其相应的缺点，RANSAC并不能保证得到正确的结果，需要提高迭代的次数；另一个是，内点外点的判断需要设置阈值。

OpenCV 计算基础矩阵

上面写了那么多基础矩阵的计算方法，在OpenCV中也就是一个函数的封装

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

其中，

points1

,

points2

是匹配点对的像素坐标，并且能够一一对应。

method

表是使用那种方法，默认的是

FM_RANSAC

也就是RANSAC的方法估算基础矩阵。

param1

表示RANSAC迭代过程中，判断点是内点还是外点的阈值（到对极线的像素距离）；

param2

表示内点占的比例，以此来判断估计出的基础矩阵是否正确。

//////////////////////////////////////////////////////
//
// 利用已经匹配的点对，使用RANSAC的方法，估计两视图的基础矩阵
//
//////////////////////////////////////////////////////

// 1. 对齐匹配的点对
vector<KeyPoint> alignedKps1, alignedKps2;
for (auto i = 0; i < good_matches.size(); i++)
{
alignedKps1.push_back(keypoints1[good_matches[i].queryIdx]);
alignedKps2.push_back(keypoints2[good_matches[i].trainIdx]);
}
// 2. 取得特征点的像素坐标
vector<Point2f> ps1, ps2;
for (auto i = 0; i < alignedKps1.size(); i++)
{
ps1.push_back(alignedKps1[i].pt);
ps2.push_back(alignedKps2[i].pt);
}
// 3. RANSAC 计算基础矩阵F
Mat F;
F = findFundamentalMat(ps1, ps2, FM_RANSAC);
cout << "基础矩阵F:" << endl << F << endl;

首先需要将匹配的点对放在两个数组中，并且一一对应（匹配的点在两个数组中的下标是一样的），然后再取得匹配点的像素坐标，最后调用

findFundamentalMat

使用RANSAC方法估计基础矩阵。

通过两视图的对极几何可知，所有的对极线相交于对极点，可以以此来验证估计的基础矩阵是否正确。在OpenCV中也封装了计算对极线的方法

computeCorrespondEpilines

，

vector<Vec3f> linesl;
computeCorrespondEpilines(ps1, 1, F, linesl);

for (auto it = linesl.begin(); it != linesl.end(); it++)
{
line(img1, Point(0, -(*it)[2] / (*it)[1]), Point(img1.cols, -((*it)[2] + (*it)[0] * img1.cols) / (*it)[1]), Scalar(255, 255, 255));
}
imshow("第一幅图像的对极线", img1);

vector<Vec3f> lines2;
computeCorrespondEpilines(ps2, 2, F, lines2);
for (auto it = lines2.begin(); it != lines2.end(); it++)
{
line(img2, Point(0, -(*it)[2] / (*it)[1]), Point(img2.cols, -((*it)[2] + (*it)[0] * img2.cols) / (*it)[1]), Scalar(255, 255, 255));
}
imshow("第二幅图像的对极线", img2);

上面代码执行的结果如下：



可以看出所有的对极线都相交于一点，该点就是对极点。