OpenCV求解基础矩阵函数说明

从两张图像的对应点计算基础矩阵

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:   第二张图像的点；
param1:   该参数用于RANSAC算法（随机采样过程一致性），它是从点到对极线的最大距离（以像素为单位），超过该距离，该点被视为异常值，并且不用于计算最终的基础矩阵。 可以将其设置为1-3，具体取决于点定位的精度，图像分辨率和图像噪声。
param2:   该参数仅仅在RANSAC算法以及LMedS算法中， 它指定了估计矩阵正确的期望置信度（概率）。

计算基础矩阵的方法：

• 7点法

• 8点法 矩阵E中有九个参数，根据尺度不变性，可以通过八对点估计基础矩阵，也就是所谓的八点法

• RANSAC算法

• LMedS算法

该函数使用上面列出的四种方法之一计算基础矩阵，然后返回找到的基础矩阵。 通常只找到一个矩阵。 但是在使用7点算法的情况下，该函数最多可以返回3个解，该矩阵顺序存储所有3个矩阵）。

所计算的基础矩阵可以进一步传递给
computeCorrespondEpilines，后者找到与指定点相对应的极线。 也可以将其传递给stereoRectifyUncalibrated来计算整流变换。

代码实现：

enum { FM_7POINT = 1, //!< 7-point algorithm
       FM_8POINT = 2, //!< 8-point algorithm
       FM_LMEDS  = 4, //!< least-median algorithm.
       FM_RANSAC = 8  //!< RANSAC algorithm. 
     };

example.cpp

 int point_count = 100;
    vector<Point2f> points1(point_count);
    vector<Point2f> points2(point_count);

    // initialize the points here ...
    for( int i = 0; i < point_count; i++ )
    {
        points1[i] = ...;
        points2[i] = ...;
    }

    Mat fundamental_matrix =
     findFundamentalMat(points1, points2, FM_RANSAC, 3, 0.99);

如何从两帧图像恢复相机的运动（即可到从一张图片到另一张图片的变换矩阵）

• 特征匹配得到关键点（必须依靠正确的匹配）
• 计算基础矩阵E(findFundamentalMat)或者本质矩阵
• 从E中通过SVD分解得到旋转矩阵R和平移矩阵t['decomposeE’函数]

2D->2D点： 对极几何

基础矩阵（Fundamental matrix）

本质矩阵（Essential matix）

手写基础矩阵恢复R，t

void InitialEXRotation::decomposeE(cv::Mat E,
                                 cv::Mat_<double> &R1, cv::Mat_<double> &R2,
                                 cv::Mat_<double> &t1, cv::Mat_<double> &t2)
{
    cv::SVD svd(E, cv::SVD::MODIFY_A);
    cv::Matx33d W(0, -1, 0,
                  1, 0, 0,
                  0, 0, 1);
    cv::Matx33d Wt(0, 1, 0,
                   -1, 0, 0,
                   0, 0, 1);
    R1 = svd.u * cv::Mat(W) * svd.vt;
    R2 = svd.u * cv::Mat(Wt) * svd.vt;
    t1 = svd.u.col(2);//opencv函数 工程trick
    cv::Mat_<double> t3;
    cv::Mat_<double> ut;
    cv::transpose(svd.u, ut);
    cv::Matx33d t(1, 0, 0, 0, 1, 0, 0, 0, 0);
    std::cout << " Wt: " <<t.col(0)<< std::endl;
    std::cout << "svd.u " << svd.u << std::endl;
    std::cout << "svd.vt" << svd.vt << std::endl;
    t3 = svd.u * cv::Mat(W) *cv::Mat(t)*ut;// 视觉SLAM十四讲公式
    std::cout << " t1: " << t1 << std::endl; //验证t1与t3是否相等
    std::cout << " t3: " << t3<< std::endl;
    t2 = -svd.u.col(2);
}

R1,R2,t1,t2四种组合得到四组可能的解

一种新型的本质矩阵分解算法

从所得到的深度大于0得到正确的旋转矩阵R

double InitialEXRotation::testTriangulation(const vector<cv::Point2f> &l,
                                          const vector<cv::Point2f> &r,
                                          cv::Mat_<double> R, cv::Mat_<double> t)
{
    cv::Mat pointcloud;
    cv::Matx34f P = cv::Matx34f(1, 0, 0, 0,
                                0, 1, 0, 0,
                                0, 0, 1, 0);
    cv::Matx34f P1 = cv::Matx34f(R(0, 0), R(0, 1), R(0, 2), t(0),
                                 R(1, 0), R(1, 1), R(1, 2), t(1),
                                 R(2, 0), R(2, 1), R(2, 2), t(2));
    cv::triangulatePoints(P, P1, l, r, pointcloud);
    // std::cout << "pointcloud: " << pointcloud.size()<< std::endl;//150*4
    int front_count = 0;
    for (int i = 0; i < pointcloud.cols; i++)
    {
        double normal_factor = pointcloud.col(i).at<float>(3);

        cv::Mat_<double> p_3d_l = cv::Mat(P) * (pointcloud.col(i) / normal_factor);
        // std::cout << "p_3d_l:" << p_3d_l << std::endl;//3*1
        // std::cout << "p_3d_l(2)" << p_3d_l(2) << std::endl;
        cv::Mat_<double> p_3d_r = cv::Mat(P1) * (pointcloud.col(i) / normal_factor);
        if (p_3d_l(2) > 0 && p_3d_r(2) > 0)
            front_count++;
    }
    std::cout << "front_count" << front_count << std::endl;
    // ROS_DEBUG("MotionEstimator: %f", 1.0 * front_count / pointcloud.cols);
    return 1.0 * front_count / pointcloud.cols;
}

 double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2));
        double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2));
        cv::Mat_<double> ans_R_cv = ratio1 > ratio2 ? R1 : R2;
       

CV_EXPORTS_W void triangulatePoints( InputArray projMatr1, InputArray projMatr2,
                                     InputArray projPoints1, InputArray projPoints2,
                                     OutputArray points4D );

第一个参数：projMatr1是第一帧图像的变换矩阵（3×4）一般作为参考帧作为单位矩阵

第二个参数：第一帧图像到第二帧图像的变换矩阵（3×4）

第三个参数：第一帧图像中所有的特征点（2×N）

第四个参数：第二帧图像中所有的特征点（2×N）

第五个参数：在齐次坐标中重建点

该函数功能通过使用双目相机的观察来重建3维点（在齐次坐标中）。 可以从stereoRectify获得投影矩阵。