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

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

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

基础矩阵表示的是图像中的像点\(p_1\)到另一幅图像对极线\(l_2\)的映射，有如下公式：
\[
l_2 = Fp_1
\]
而和像点\(P_1\)匹配的另一个像点\(p_2\)必定在对极线\(l_2\)上，所以就有：
\[
p_2^Tl_2 = p_2^TFp_1 = 0
\]
这样仅通过匹配的点对，就可以计算出两视图的基础矩阵\(F\)。
基础矩阵\(F\)是一个\(3\times3\)的矩阵，有9个未知元素。然而，上面的公式中使用的齐次坐标，齐次坐标在相差一个常数因子下是相等，\(F\)也就只有8个未知元素，也就是说，只需要8对匹配的点对就可以求解出两视图的基础矩阵\(F\)。下面介绍下8点法 Eight-Point-Algorithm计算基础矩阵的过程。

假设一对匹配的像点\(p_1=[u_1,v_1,1]^T,p_2=[u_2,v_2,1]^T\)，带入式子中，得到：
\[
[u_1,v_1,1]\left[\begin{array}{ccc}f_1&f_2&f_3\\f_4&f_5&f_6\\f_7&f_8&f_9\end{array}\right]
\left[\begin{array}{c}u_2\\v_2\\1\end{array}\right] = 0
\]
把基础矩阵\(F\)的各个元素当作一个向量处理
\[
f = [f_1,f_2,f_3,f_4,f_5,f_6,f_7,f_8,f_9]
\]
那么上面式子可以写为
\[
[u_1u_2,u_1v_2,u_1,v_1u_2,v_1v_2,v_1,u_2,v_2,1]\cdot f = 0
\]
对于其他的点对也使用同样的表示方法。这样将得到的所有方程放到一起，得到一个线性方程组(\((u^i,v^i)\)表示第\(i\)个特征点）
\[
\left[
\begin{array}{ccccccccc}
u_1^1u_2^1&u_1^1v_2^1&u_1^1&v_1^1u_2^1&v_1^1v_2^1&v_1^1&u_2^1&v_2^1&1\u_1^2u_2^2&u_1^2v_2^2&u_1^2&v_1^2u_2^2&v_1^2v_2^2&v_1^2&u_2^2&v_2^2&1\u_1^3u_2^3&u_1^3v_2^3&u_1^3&v_1^3u_2^1&v_1^3v_2^1&v_1^3&u_2^3&v_2^3&1\\cdots&\cdots&\cdots&\cdots&\cdots&\cdots&\cdots&\cdots&\cdots \u_1^8u_2^8&u_1^8v_2^8&u_1^8&v_1^8u_2^8&v_1^8v_2^8&v_1^8&u_2^8&v_2^8&1
\end{array}
\right]
\left[
\begin{array}{c}
f_1\\f_2\\f_3\\f_4\\f_5\\f_6\\f_7\\f_8\\f_9
\end{array}
\right]=0
\]
求解上面的方程组就可以得到基础矩阵各个元素了。当然这只是理想中的情况，由于噪声、数值的舍入误差和错误的匹配点的影响，仅仅求解上面的线性方程组得到的基础矩阵非常的不稳定，因此在8点法的基础上有各种改进方法。

图像坐标归一化 Normalizing transformation

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

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

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

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

  1. 对点进行平移使其形心位于原点。
  2. 对点进行缩放，使它们到原点的平均距离为\(\sqrt{2}\)
  3. 对两幅图像独立进行上述变换


上图左边是原始图像的坐标，右边是归一化后的坐标，\(H\)是归一化的变换矩阵，可记为如下形式：
\[
H = S\left[\begin{array}{ccc}
1&0&-\bar{\mu} \\0&1&-\bar{\nu}\\0&0&\frac{1}{S}
\end{array}\right]
\]
其中，\(\bar{\mu},\bar{\nu}\)是图像像点坐标两个分量的平均值
\[
\bar{\mu} = \frac{1}{N}\sum_{i=1}^{N}\mu_i,\bar{\nu} = \frac{1}{N}\sum_{i=1}^{N}\nu_i
\]
\(S\)表示尺度，其表达式为：
\[
S = \frac{\sqrt{2\cdot N}}{\sqrt{\sum_{i=1}^{N}(\mu_i-\bar{\mu})^2 + (\nu_i-\bar{\nu})^2}}
\]

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

  1. 对图像1进行归一化处理，计算一个只包含平移和缩放的变换\(H_1\)，将图像1中的匹配点集\({p_i^1}\)变换到新的点集\(\hat{p_i^1}\)，新点集的形心位于原点\((0,0)^T\)，并且它们到原点的平均距离是\(\sqrt{2}\)。
  2. 对图像2，计算变换矩阵\(H_2\)进行相同的归一化处理
  3. 使用8点法利用变换后的点集估计基础矩阵\(\hat{F}\)
  4. 根据变换\(F = H_2^T\hat{F}H_1\)

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

最小二乘法

仅仅对图像坐标进行归一化处理，能在一定程度上提高计算的精度，但在实践中还是不够的。8点法中，只使用8对匹配的点对估计基础矩阵，但通常两幅图像的匹配的点对远远多于8对，可以利用更多匹配的点对来求解上面的方程。
由于基础矩阵\(F\)在一个常量因子下是等价的，这样可以给基础矩阵\(F\)的元素组成的向量\(f\)施加一个约束条件
\[
\parallel f \parallel = 1
\]
这样由\(K \ge 8\)个匹配的点对，组合成一个矩阵\(Q_{K\times9}\)，求解上面方程就变成了求解如下问题的最小二乘解
\[
\min_{\parallel f \parallel = 1}\parallel Qf \parallel ^2
\]
其中，矩阵\(Q\)的每一行来自一对匹配点；\(f\)是基础矩阵\(F\)元素构成的待求解的向量，根据2-范数的定义
\[
\parallel Qf \parallel^2 = (Qf)^T(Qf)=f^T(Q^TQ)f
\]
将上式的中间部分提取出来得到矩阵\(M=Q^TQ\),这是一个\(9\times9\)的矩阵。基于拉格朗日-欧拉乘数优化定理，在\(\parallel f \parallel = 1\)约束下，\(Qf=0\)的最小二乘解，为矩阵\(M=Q^TQ\)的最小特征值对应的特征向量。所以可以对矩阵\(Q\)进行奇异值分解（SVD）
\[Q = U\Sigma V^T\]
其中，\(Q\)是\(K\times9\)的矩阵；\(U\)是\(K\times K\)的正交阵；\(\Sigma\)是\(K \times 9\)的对角矩阵，对角线的元素是奇异值；\(V^T\)是\(9 \times 9\)的正交阵，每一个列向量对应着\(\Sigma\)中的奇异值。所以，最小二乘解就是\(V^T\)的第9个列向量，也就是可由向量\(f=V_9\)构造基础矩阵\(F\)。
基础矩阵\(F\)还有一个重要的性质，这里可以作为进一步的约束条件。那就是基础矩阵\(F\)的秩为2，
\[
Rank(F) = 2
\]
上述使用列向量\(V_9\)构造的基础矩阵的秩通常不为2，需要进一步的优化。在估计基础矩阵时，设其最小奇异值为0，对上面方法取得的基础矩阵进行SVD分解
\[
F = SVD = S\left[\begin{array}{ccc}
v_1&0&0\\0&v_2&0\\0&0&v_3
\end{array}\right]D
\]
其最小特征值\(v_3\)被设为0，以使得\(F\)的秩为2.这样得到
\[
F = SVD = S\left[\begin{array}{ccc}
v_1&0&0\\0&v_2&0\\0&0&0
\end{array}\right]D
\]
上述\(F\)就是最终得到的基础矩阵。

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

  1. 对取得两幅图像的匹配点进行归一化处理，转换矩阵分别是\(H_1,H_2\)
  2. 有对应的匹配点(\(K\ge8\))构造系数矩阵\(Q\)
  3. 对矩阵\(Q\)进行SVD分解,\(Q=U\Sigma V^T\)，由向量\(f=V_9\)构造矩阵\(\hat{F}\)
  4. 对得到的矩阵\(\hat{F}\)进行秩为2的约束，即对\(\hat{F}\)进行SVD分解，\(\hat{F}=S\cdot diag(v_1,v_2,v_3) \cdot V^T\)，令\(v_3=0\)得到基础矩阵的估计\(F'=S\cdot diag(v_1,v_2,0) \cdot V^T\)
  5. 对上一步得到的解进行反归一化处理，得到基础矩阵\(F=H_2^TF'H_1\)

随机采样一致性 RANSAC

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

  1. 不精确的测量点位置引起的系统误差，通常服从高斯分布
  2. 错误匹配引起的误差，这些不匹配的点被称为外点，通常不服从高斯分布

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

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

  • 数据集中包含的内点，内点的分布符合一个数学模型；而数据集中的外点不复合该数学模型
  • 能够一组内点（通常很少）集合能够估计出其符合的数据模型

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

  1. 从集合\(P\)中随机选择\(n\)个点
  2. 使用这\(n\)个点估计出一个模型\(M\)
  3. 对\(P\)中剩余的点，计算每个点与模型\(M\)的距离，距离超过阈值则认为是外点；不超过阈值则认为是内点，并记录该模型\(M\)所对应的内点的个数\(m_i\)

将上面步骤重复\(k\)次，选择最大\(m_i\)所对应的模型\(M\)作为最终结果。
在迭代次数\(k\)的选择是一个关键，可以通过理论的方式计算出\(k\)的取值。在选择\(n\)个点估计模型时，要保证选择的\(n\)的个点都是内点的概率足够的高。假设，从数据集中选择一个点为内点的概率为\(\varpi\)，则选择的\(n\)个点都是内点的概率为\(\varpi^n\);则\(1-\varpi^n\)表示选择的\(n\)个点至少有一个是外点，用包含外点估算的模型显然是不正确的，则迭代\(k\)次均得不到正确模型的概率为\((1-\varpi^n)^k\)。设\(p\)表示\(k\)次迭代中至少有一次选择的点都是内点的概率，也就是估计出了正确的模型，则\(1-p\)就表示\(k\)次跌点都得到正确的模型，所以有
\[
1-p = 1-\varpi^n)^k
\]
两边同时取对数，则有
\[
k = \frac{log(1-p)}{log(1-\varpi^n)}
\]
一般要求\(p\)大于95%即可。

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

  • 从匹配的点对中选择8个点，使用8点法估算出基础矩阵\(F_i\)
  • 计算其余的点对到其对应对极线的距离\(d_n\)，如果\(d_n\le d\)则该点为内点，否则为外点。记下符合该条件的内点的个数为\(m_i\)
  • 迭代k次，或者某次得到内点的数目\(m_i\)占有的比例大于等于95%，则停止。选择\(m_i\)最大的基础矩阵作为最终的结果。

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);

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

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

原文地址：https://www.cnblogs.com/wangguchangqing/p/8214032.html

时间： 2024-11-04 15:04:22

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

SLAM入门之视觉里程计(5)：单应矩阵

在之前的博文OpenCV,计算两幅图像的单应矩阵,介绍调用OpenCV中的函数,通过4对对应的点的坐标计算两个图像之间单应矩阵\(H\),然后调用射影变换函数,将一幅图像变换到另一幅图像的视角中.当时只是知道通过单应矩阵,能够将图像1中的像素坐标\((u_1,v_1)\)变换到图像2中对应的位置上\((u_2,v_2)\),而没有深究其中的变换关系. 单应(Homography)是射影几何中的概念,又称为射影变换.它把一个射影平面上的点(三维齐次矢量)映射到另一个射影平面上,并且把直线映射为直线

SLAM入门之视觉里程计(3)：两视图对极约束 基础矩阵

在上篇相机模型中介绍了图像的成像过程,场景中的三维点通过"小孔"映射到二维的图像平面,可以使用下面公式描述: \[ x = MX \]其中,\(c\)是图像中的像点,\(M\)是一个\(3\times4\)的相机矩阵,\(X\)是场景中的三维点. 通过小孔相机模型,可知假如从像点\(x\)向相机的中心\(C\)反投影一条射线\(\overrightarrow{xC}\),则该射线必定经过对应像点的三维空间点\(X\),但显然仅仅通过一个像点无法确定\(X\)的具体位置,因为在射线\(\

SLAM入门之视觉里程计(6)：相机标定 张正友经典标定法详解

想要从二维图像中获取到场景的三维信息,相机的内参数是必须的,在SLAM中,相机通常是提前标定好的.张正友于1998年在论文:"A Flexible New Technique fro Camera Calibration"提出了基于单平面棋盘格的相机标定方法.该方法介于传统的标定方法和自标定方法之间,使用简单实用性强,有以下优点: 不需要额外的器材,一张打印的棋盘格即可. 标定简单,相机和标定板可以任意放置. 标定的精度高. 相机的内参数 设\(P=(X,Y,Z)\)为场景中的一点,在

视觉里程计VO

视觉里程计的主要问题是如何根据图像来估计相机运动,VO的实现方法,按照是否需要提取特征,分为特征点法的前端以及不提取特征的直接法前端.基于特征点法的前端,长久以来被认为是视觉里程计的主流方法,它运行稳定,对光照.动态物体不敏感,是目前较为成熟的解决方案. 计算机视觉邻域的研究者们,设计了许多比角点更加稳定的局部图像特征,比如SIFT,SURF,ORB等.特征点由关键点(Key-point)和描述子(Descriper)两部分组成.SIFT(Scale Invariant Feature Tran

第三篇 视觉里程计（VO）的初始化过程以及openvslam中的相关实现详解

视觉里程计(Visual Odometry, VO),通过使用相机提供的连续帧图像信息(以及局部地图,先不考虑)来估计相邻帧的相机运动,将这些相对运行转换为以第一帧为参考的位姿信息,就得到了相机载体(假设统一的刚体)的里程信息. 初始化实例 在实例化跟踪器的时候会实例化一个初始化实例,有一些比较重要的参数需要注意下,看代码注释以及初始值,参数值也可以在yaml文件中自定义. // src/openvslam/module/initializer.h:83 //! max number of it

关于视觉里程计以及VI融合的相关研究（长期更新）

1. svo 源码:https://github.com/uzh-rpg/rpg_svo 国内对齐文章源码的研究: (1)冯斌: 对其代码重写 https://github.com/yueying/OpenMVO 对原理的一步步分析http://fengbing.net/ (2)白巧克力: 对文章的具体分析:http://blog.csdn.net/heyijia0327/article/details/51083398 2. svo+msf 文章:见我的分享http://pan.baidu.c

视觉里程计 Visual Odometry

首先分析include头文件下的slamBase.h文件 # pragma once // 各种头文件 // C++标准库 #include <fstream> #include <vector> #include <map> using namespace std; // Eigen #include <Eigen/Core> #include <Eigen/Geometry> // OpenCV #include <opencv2/co

视觉里程计07 Qt的一些bug修改记录

无法添加textedit: ui文件更新后在.h文件中还有一段时间,这时可以考虑clean后重新编译,则可以更新 ui指针实例化后用 . 来获取对象,调用方法用 -> 无法解析的外部符号错误: 添加新的空间或方法时要及时更新头文件和链接文件,这里主要是链接文件,即Qt5SerialBusd.lib和Qt5SerialPortd.lib没有添加导致连接失败. 原文地址:https://www.cnblogs.com/RegressionWorldLine/p/8546935.html

点线特征融合的单目视觉里程计

点线特征融合的单目视觉里程计---袁梦1*李艾华1 为了解决地下工程场景下巡逻机器人的定位与建图问题,提出了一种点线特征融合的单目半直接视觉里程计.本算法分为特征提取.状态估计和深度滤波器三个线程.特征提取线程负责图像点线特征的提取,之后状态估计线程利用点.线特征不同的匹配与跟踪策略获得相机的 6 自由度位姿,并通过帧与帧.特征与特征.局部帧之间约束关系进一步优化相机的位姿.深度滤波器线程通过概率分布的方式刻画 3 维路标点相对于相机光心的深度信息,该方式相对于固定深度值的方式能够提高深度估计的