Mastering Opencv ch4：SFM详解（二）

前文分析了如何进行特征检测匹配，接下来分析如何求解摄像机矩阵。

I：求解基础矩阵

首先介绍两个代码段，关于关键点和Point2f相互转化的函数

void KeyPointsToPoints(const vector& kps, vector& ps) {
    ps.clear();
    for (unsigned int i=0; ivoid PointsToKeyPoints(const vector& ps, vector& kps) {
    kps.clear();
    for (unsigned int i=0; i1.0f));
}

这两个函数在求解基础矩阵的时候用的特别多。这里提前放着。

下面是求解每两幅图像的基础矩阵

void MultiCameraPnP::PruneMatchesBasedOnF() {
    //prune the match between <_i> and all views using the Fundamental matrix to prune
//#pragma omp parallel for
//求解每两幅图像的基础矩阵
    for (int _i=0; _i < imgs.size() - 1; _i++)
    {
        for (unsigned int _j=_i+1; _j < imgs.size(); _j++) {
            int older_view = _i, working_view = _j;

            GetFundamentalMat( imgpts[older_view], 
                imgpts[working_view], 
                imgpts_good[older_view],
                imgpts_good[working_view], 
                matches_matrix[std::make_pair(older_view,working_view)]
#ifdef __SFM__DEBUG__
                ,imgs_orig[older_view],imgs_orig[working_view]
#endif
            );
            //update flip matches as well
#pragma omp critical
            matches_matrix[std::make_pair(working_view,older_view)] = FlipMatches(matches_matrix[std::make_pair(older_view,working_view)]);
        }
    }
}

核心函数GetFundamentalMat的实现：
计算出两幅图像的基础矩阵，并优化匹配组，基本去除错误匹配

Mat GetFundamentalMat(const vector& imgpts1,
                       const vector& imgpts2,
                       vector& imgpts1_good,
                       vector& imgpts2_good,
                       vector& matches
#ifdef __SFM__DEBUG__
                      ,const Mat& img_1,
                      const Mat& img_2
#endif
                      ) 
{
    //Try to eliminate keypoints based on the fundamental matrix
    //(although this is not the proper way to do this)
    vector status(imgpts1.size());

#ifdef __SFM__DEBUG__
    std::vector< DMatch > good_matches_;
    std::vector keypoints_1, keypoints_2;
#endif      
    //  undistortPoints(imgpts1, imgpts1, cam_matrix, distortion_coeff);
    //  undistortPoints(imgpts2, imgpts2, cam_matrix, distortion_coeff);
    //
    imgpts1_good.clear(); imgpts2_good.clear();

    vector imgpts1_tmp;
    vector imgpts2_tmp;
    if (matches.size() <= 0) { 
        //points already aligned...
        imgpts1_tmp = imgpts1;
        imgpts2_tmp = imgpts2;
    } else {
        GetAlignedPointsFromMatch(imgpts1, imgpts2, matches, imgpts1_tmp, imgpts2_tmp);//选取特征点集中匹配的特征点
    }

    Mat F;
    {
        vector pts1,pts2;
        KeyPointsToPoints(imgpts1_tmp, pts1);//特征点转化成Point2f点
        KeyPointsToPoints(imgpts2_tmp, pts2);
#ifdef __SFM__DEBUG__
        cout << "pts1 " << pts1.size() << " (orig pts " << imgpts1_tmp.size() << ")" << endl;
        cout << "pts2 " << pts2.size() << " (orig pts " << imgpts2_tmp.size() << ")" << endl;
#endif
        double minVal,maxVal;
        cv::minMaxIdx(pts1,&minVal,&maxVal);
        F = findFundamentalMat(pts1, pts2, FM_RANSAC, 0.006 * maxVal, 0.99, status); //threshold from [Snavely07 4.1]//核心函数，计算F，并得到优化的匹配组模板status
    }

    vector new_matches;
    cout << "F keeping " << countNonZero(status) << " / " << status.size() << endl; 
    for (unsigned int i=0; iif (status[i]) //根据模板，重新优化匹配组及特征点，找到最优的匹配组，基本就没什么错误匹配了。
        {
            imgpts1_good.push_back(imgpts1_tmp[i]);
            imgpts2_good.push_back(imgpts2_tmp[i]);

            if (matches.size() <= 0) { //points already aligned...
                new_matches.push_back(DMatch(matches[i].queryIdx,matches[i].trainIdx,matches[i].distance));
            } else {
                new_matches.push_back(matches[i]);
            }

#ifdef __SFM__DEBUG__
            good_matches_.push_back(DMatch(imgpts1_good.size()-1,imgpts1_good.size()-1,1.0));
            keypoints_1.push_back(imgpts1_tmp[i]);
            keypoints_2.push_back(imgpts2_tmp[i]);
#endif
        }
    }   

    cout << matches.size() << " matches before, " << new_matches.size() << " new matches after Fundamental Matrix\n";
    matches = new_matches; //keep only those points who survived the fundamental matrix

#if 0
    //-- Draw only "good" matches
#ifdef __SFM__DEBUG__
    if(!img_1.empty() && !img_2.empty()) {      
        vector i_pts,j_pts;
        Mat img_orig_matches;
        { //draw original features in red绘制原始特征点用红色
            vector vstatus(imgpts1_tmp.size(),1);
            vector<float> verror(imgpts1_tmp.size(),1.0);
            img_1.copyTo(img_orig_matches);
            KeyPointsToPoints(imgpts1_tmp, i_pts);
            KeyPointsToPoints(imgpts2_tmp, j_pts);
            drawArrows(img_orig_matches, i_pts, j_pts, vstatus, verror, Scalar(0,0,255));
        }
        { //superimpose filtered features in green用绿色绘制经过此次优化的特征点
            vector vstatus(imgpts1_good.size(),1);
            vector<float> verror(imgpts1_good.size(),1.0);
            i_pts.resize(imgpts1_good.size());
            j_pts.resize(imgpts2_good.size());
            KeyPointsToPoints(imgpts1_good, i_pts);
            KeyPointsToPoints(imgpts2_good, j_pts);
            drawArrows(img_orig_matches, i_pts, j_pts, vstatus, verror, Scalar(0,255,0));
            imshow( "Filtered Matches", img_orig_matches );
        }
        int c = waitKey(0);
        if (c=='s') {
            imwrite("fundamental_mat_matches.png", img_orig_matches);
        }
        destroyWindow("Filtered Matches");
    }
#endif      
#endif

    return F;
}

其中从每幅图像的所有的特征集点中选取匹配的特征点函数

void GetAlignedPointsFromMatch(const std::vector& imgpts1,
                               const std::vector& imgpts2,
                               const std::vector& matches,
                               std::vector& pt_set1,
                               std::vector& pt_set2) 
{
    for (unsigned int i=0; i//      cout << "matches[i].queryIdx " << matches[i].queryIdx << " matches[i].trainIdx " << matches[i].trainIdx << endl;
        assert(matches[i].queryIdx < imgpts1.size());
        pt_set1.push_back(imgpts1[matches[i].queryIdx]);
        assert(matches[i].trainIdx < imgpts2.size());
        pt_set2.push_back(imgpts2[matches[i].trainIdx]);
    }   
}

以上就求出了每两幅图像最佳匹配组，接下来就是对每两幅图像的最佳匹配进行处理。

2：计算两幅图像的单应内点
找到在两幅图像中的内点数，通过RANSAC筛选，得到单应矩阵收敛时符合单应变换的内点

//Following Snavely07 4.2 - find how many inliers are in the Homography between 2 views
//找到在两幅图像中的内点数，通过RANSAC筛选
int MultiCameraPnP::FindHomographyInliers2Views(int vi, int vj) 
{
    vector ikpts,jkpts; vector ipts,jpts;
    GetAlignedPointsFromMatch(imgpts[vi],imgpts[vj],matches_matrix[make_pair(vi,vj)],ikpts,jkpts);
    KeyPointsToPoints(ikpts,ipts); KeyPointsToPoints(jkpts,jpts);

    double minVal,maxVal; cv::minMaxIdx(ipts,&minVal,&maxVal); //TODO flatten point2d?? or it takes max of width and height

    vector status;
    cv::Mat H = cv::findHomography(ipts,jpts,status,CV_RANSAC, 0.004 * maxVal); //threshold from Snavely07
    return cv::countNonZero(status); //number of inliers
}

使用map容器，按内点概率从高到底依次排列
list

//sort pairwise matches to find the lowest Homography inliers [Snavely07 4.2]
    cout << "Find highest match...";
    listint,pair<int,int> > > matches_sizes;
    //TODO: parallelize!
    for(std::map<std::pair<int,int> ,std::vector >::iterator i = matches_matrix.begin(); i != matches_matrix.end(); ++i) {
        if((*i).second.size() < 100)
            matches_sizes.push_back(make_pair(100,(*i).first));
        else {
            int Hinliers = FindHomographyInliers2Views((*i).first.first,(*i).first.second);
            int percent = (int)(((double)Hinliers) / ((double)(*i).second.size()) * 100.0);
            cout << "[" << (*i).first.first << "," << (*i).first.second << " = "<"] ";
            matches_sizes.push_back(make_pair((int)percent,(*i).first));
        }
    }
    cout << endl;
    matches_sizes.sort(sort_by_first);//按内点概率从大到小排列匹配组

3：根据上面排好序的匹配组，依次计算摄像机矩阵

bool goodF = false;
    int highest_pair = 0;
    m_first_view = m_second_view = 0;
    //reverse iterate by number of matches
    for(listint,pair<int,int> > >::iterator highest_pair = matches_sizes.begin(); 
        highest_pair != matches_sizes.end() && !goodF; 
        ++highest_pair) 
    {
        m_second_view = (*highest_pair).second.second;
        m_first_view  = (*highest_pair).second.first;

        std::cout << " -------- " << imgs_names[m_first_view] << " and " << imgs_names[m_second_view] << " -------- " <<std::endl;
        //what if reconstrcution of first two views is bad? fallback to another pair
        //See if the Fundamental Matrix between these two views is good
        goodF = FindCameraMatrices(K, Kinv, distortion_coeff,
            imgpts[m_first_view], 
            imgpts[m_second_view], 
            imgpts_good[m_first_view],
            imgpts_good[m_second_view], 
            P, 
            P1,
            matches_matrix[std::make_pair(m_first_view,m_second_view)],
            tmp_pcloud
#ifdef __SFM__DEBUG__
            ,imgs[m_first_view],imgs[m_second_view]
#endif
        );

这里预设

    cv::Matx34d P(1,0,0,0,
                  0,1,0,0,
                  0,0,1,0),
                P1(1,0,0,0,
                   0,1,0,0,
                   0,0,1,0);

4：计算基础矩阵，本质矩阵

Mat F = GetFundamentalMat(imgpts1,imgpts2,imgpts1_good,imgpts2_good,matches
#ifdef __SFM__DEBUG__
                                  ,img_1,img_2
#endif
                                  );
        if(matches.size() < 100) { // || ((double)imgpts1_good.size() / (double)imgpts1.size()) < 0.25
            cerr << "not enough inliers after F matrix" << endl;
            return false;
        }

        //Essential matrix: compute then extract cameras [R|t]
        Mat_<double> E = K.t() * F * K; //according to HZ (9.12)

        //according to http://en.wikipedia.org/wiki/Essential_matrix#Properties_of_the_essential_matrix
        if(fabsf(determinant(E)) > 1e-07) {
            cout << "det(E) != 0 : " << determinant(E) << "\n";
            P1 = 0;
            return false;
        }

通过计算出的基础矩阵计算本质矩阵E。

5：接下来，就是把本质矩阵进行SVD奇异值分解，得到旋转部分和平移部分。首先若fabsf(determinant(E)) > 1e-07，则E无法分解。若得出的determinant(R1)+1.0 < 1e-09，就把E取负值重新进行分解。若fabsf(determinant(R))-1.0 > 1e-07，那么分解得到的R1是错误的。

bool DecomposeEtoRandT(
    Mat_<double>& E,
    Mat_<double>& R1,
    Mat_<double>& R2,
    Mat_<double>& t1,
    Mat_<double>& t2) 
{
#ifdef DECOMPOSE_SVD
    //Using HZ E decomposition
    Mat svd_u, svd_vt, svd_w;
    TakeSVDOfE(E,svd_u,svd_vt,svd_w);//进行SVD分解

    //check if first and second singular values are the same (as they should be)检查第一个和第二个奇异值是不是相同，应该相同的。
    double singular_values_ratio = fabsf(svd_w.at<double>(0) / svd_w.at<double>(1));
    if(singular_values_ratio>1.0) singular_values_ratio = 1.0/singular_values_ratio; // flip ratio to keep it [0,1]
    if (singular_values_ratio < 0.7) {
        cout << "singular values are too far apart\n";
        return false;
    }

    Matx33d W(0,-1,0,   //HZ 9.13
        1,0,0,
        0,0,1);
    Matx33d Wt(0,1,0,
        -1,0,0,
        0,0,1);
    R1 = svd_u * Mat(W) * svd_vt; //HZ 9.19
    R2 = svd_u * Mat(Wt) * svd_vt; //HZ 9.19
    t1 = svd_u.col(2); //u3
    t2 = -svd_u.col(2); //u3
#else
    //Using Horn E decomposition，使用另一种方法分解
    DecomposeEssentialUsingHorn90(E[0],R1[0],R2[0],t1[0],t2[0]);
#endif
    return true;
}

其中TakeSVDOfE函数实现了E的SVD分解，得到u，vt，w。

void TakeSVDOfE(Mat_<double>& E, Mat& svd_u, Mat& svd_vt, Mat& svd_w) {
#if 1
    //Using OpenCV's SVD
    SVD svd(E,SVD::MODIFY_A);
    svd_u = svd.u;
    svd_vt = svd.vt;
    svd_w = svd.w;
#else
    //Using Eigen's SVD
    cout << "Eigen3 SVD..\n";
    Eigen::Matrix3f  e = Eigen::Mapdouble,3,3,Eigen::RowMajor> >((double*)E.data).cast<float>();
    Eigen::JacobiSVD svd(e, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::MatrixXf Esvd_u = svd.matrixU();
    Eigen::MatrixXf Esvd_v = svd.matrixV();
    svd_u = (Mat_<double>(3,3) << Esvd_u(0,0), Esvd_u(0,1), Esvd_u(0,2),
                          Esvd_u(1,0), Esvd_u(1,1), Esvd_u(1,2), 
                          Esvd_u(2,0), Esvd_u(2,1), Esvd_u(2,2)); 
    Mat_<double> svd_v = (Mat_<double>(3,3) << Esvd_v(0,0), Esvd_v(0,1), Esvd_v(0,2),
                          Esvd_v(1,0), Esvd_v(1,1), Esvd_v(1,2), 
                          Esvd_v(2,0), Esvd_v(2,1), Esvd_v(2,2));
    svd_vt = svd_v.t();
    svd_w = (Mat_<double>(1,3) << svd.singularValues()[0] , svd.singularValues()[1] , svd.singularValues()[2]);
#endif

    cout << "----------------------- SVD ------------------------\n";
    cout << "U:\n"<"\nW:\n"<"\nVt:\n"<cout << "----------------------------------------------------\n";
}

下面是另一种方法进行分解的E，得到R1，R2，T1，T2。

void DecomposeEssentialUsingHorn90(double _E[9], double _R1[9], double _R2[9], double _t1[3], double _t2[3]) {
    //from : http://people.csail.mit.edu/bkph/articles/Essential.pdf
#ifdef USE_EIGEN
    using namespace Eigen;

    Matrix3d E = Map3,3,RowMajor> >(_E);
    Matrix3d EEt = E * E.transpose();
    Vector3d e0e1 = E.col(0).cross(E.col(1)),e1e2 = E.col(1).cross(E.col(2)),e2e0 = E.col(2).cross(E.col(2));
    Vector3d b1,b2;

#if 1
    //Method 1
    Matrix3d bbt = 0.5 * EEt.trace() * Matrix3d::Identity() - EEt; //Horn90 (12)
    Vector3d bbt_diag = bbt.diagonal();
    if (bbt_diag(0) > bbt_diag(1) && bbt_diag(0) > bbt_diag(2)) {
        b1 = bbt.row(0) / sqrt(bbt_diag(0));
        b2 = -b1;
    } else if (bbt_diag(1) > bbt_diag(0) && bbt_diag(1) > bbt_diag(2)) {
        b1 = bbt.row(1) / sqrt(bbt_diag(1));
        b2 = -b1;
    } else {
        b1 = bbt.row(2) / sqrt(bbt_diag(2));
        b2 = -b1;
    }
#else
    //Method 2
    if (e0e1.norm() > e1e2.norm() && e0e1.norm() > e2e0.norm()) {
        b1 = e0e1.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
        b2 = -b1;
    } else if (e1e2.norm() > e0e1.norm() && e1e2.norm() > e2e0.norm()) {
        b1 = e1e2.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
        b2 = -b1;
    } else {
        b1 = e2e0.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
        b2 = -b1;
    }
#endif

    //Horn90 (19)
    Matrix3d cofactors; cofactors.col(0) = e1e2; cofactors.col(1) = e2e0; cofactors.col(2) = e0e1;
    cofactors.transposeInPlace();

    //B = [b]_x , see Horn90 (6) and http://en.wikipedia.org/wiki/Cross_product#Conversion_to_matrix_multiplication
    Matrix3d B1; B1 <<  0,-b1(2),b1(1),
                        b1(2),0,-b1(0),
                        -b1(1),b1(0),0;
    Matrix3d B2; B2 <<  0,-b2(2),b2(1),
                        b2(2),0,-b2(0),
                        -b2(1),b2(0),0;

    Map3,3,RowMajor> > R1(_R1),R2(_R2);

    //Horn90 (24)
    R1 = (cofactors.transpose() - B1*E) / b1.dot(b1);
    R2 = (cofactors.transpose() - B2*E) / b2.dot(b2);
    Map t1(_t1),t2(_t2); 
    t1 = b1; t2 = b2;

    cout << "Horn90 provided " << endl << R1 << endl << "and" << endl << R2 << endl;
#endif
}

到这里就已经分解出R1,R2,t1,t2了，P0就是预设的单位矩阵，P1可以由这四个值组合成四种。应该取哪一个，下一章再分析