Subversion Repositories seema-scanner

// Create a mask for feature matching which disallows matches not satisfying the epipolar constraint.

// Removes matches not satisfying the epipolar constraint.

// maxD is the maximum point to line distance permissible.

// Works like cv::correctMatches(), except it removes matches with an error distance greater than maxD.

cv::Mat epipolarMatchingMask(const cv::vector<cv::KeyPoint> &keypoints1, const cv::vector<cv::KeyPoint> &keypoints2, cv::Matx33f F, float maxD){

void removeIncorrectMatches(const cv::Mat F, const std::vector<cv::Point2f> &q0, const std::vector<cv::Point2f> &q1, const float maxD,

    if(keypoints1.empty() || keypoints2.empty())

    int n0 = (int)q0.size(), n1 = (int)q1.size();

        return cv::Mat();

    q0Correct.reserve(n0);

    cv::Mat mask(n1, n2, CV_8UC1);

    q1Correct.reserve(n1);

//        cv::Vec3f p1 = cv::Vec3f(keypoints1[i].pt.x, keypoints1[i].pt.y, 1.0);

//        cv::Vec3f p0 = cv::Vec3f(q0[i].x, q0[i].y, 1.0);

//        // Epipolar line defined by p1

//        // Epipolar line defined by p0

//        cv::Vec3f l = F*p1;

//        cv::Vec3f l = F*p0;

//            cv::Vec3f p2 = cv::Vec3f(keypoints2[j].pt.x, keypoints2[j].pt.y, 1.0);

//            cv::Vec3f p1 = cv::Vec3f(q1[i].x, q1[i].y, 1.0);

//            float d = l.dot(p2);

//            float d = l.dot(p1);

//            mask.at<uchar>(i, j) = fabs(d) < maxD;

//                q1Correct.push_back(q1[i]);

    std::vector<cv::Point3f> l1, l2;

    std::vector<cv::Point3f> l0, l1;

    cv::computeCorrespondEpilines(q1, 1, F, l1);

    cv::computeCorrespondEpilines(q0, 1, F, l0);

    cv::computeCorrespondEpilines(q2, 2, F, l2);

    cv::computeCorrespondEpilines(q1, 2, F, l1);

    for( int i = 0; i < n1; i++ ){

    for(int i = 0; i < n0; i++){

        cv::Vec3f p1 = cv::Vec3f(q1[i].x, q1[i].y, 1.0);

        cv::Vec3f p0 = cv::Vec3f(q0[i].x, q0[i].y, 1.0);

        for( int j = 0; j < n2; j++ ){

        for(int j = 0; j < n1; j++){

            cv::Vec3f p2 = cv::Vec3f(q2[j].x, q2[j].y, 1.0);

            cv::Vec3f p1 = cv::Vec3f(q1[j].x, q1[j].y, 1.0);

            float d12 = l1[i].dot(p2);

            float d01 = l0[i].dot(p1);

            float d21 = l2[j].dot(p1);

            float d10 = l1[j].dot(p0);

            float d = d12*d12 + d21*d21;

            float d = d01*d01 + d10*d10;

            mask.at<uchar>(i, j) = d < maxD;

            if(d < maxD){

//    std::vector<cv::Point2f> q1, q2;

//    std::vector<cv::Point3f> p0, p1;

//    cvtools::keypointsToPoints(keypoints2, q2);

//    cv::convertPointsToHomogeneous(q0, p0);

//    cv::Mat Fp1Mat = cv::Mat(F)*cv::Mat(p1).reshape(1).t();

//    cv::Mat Fp0Mat = cv::Mat(F)*cv::Mat(p0).reshape(1).t();

//    cv::Mat FTp2Mat = cv::Mat(F.t())*cv::Mat(p2).reshape(1).t();

//    cv::Mat FTp1Mat = cv::Mat(F.t())*cv::Mat(p1).reshape(1).t();

//        cv::Vec3f Fp1 = Fp1Mat.col(i);

//        cv::Vec3f Fp0 = Fp0Mat.col(i);

//            cv::Vec3f FTp2 = FTp2Mat.col(j);

//            cv::Vec3f FTp1 = FTp1Mat.col(j);

//            cv::Matx<float,1,1> p2TFp1 = cv::Matx31f(p2[j]).t()*F*cv::Matx31f(p1[i]);

//            cv::Matx<float,1,1> p1TFp0 = cv::Matx31f(p1[j]).t()*F*cv::Matx31f(p0[i]);

//            float d = p2TFp1(0)*p2TFp1(0) / (Fp1(0)*Fp1(0) + Fp1(1)*Fp1(1) + FTp2(0)*FTp2(0) + FTp2(1)*FTp2(1));

//            float d = p1TFp0(0)*p1TFp0(0) / (Fp0(0)*Fp0(0) + Fp0(1)*Fp0(1) + FTp1(0)*FTp1(0) + FTp1(1)*FTp1(1));

//            mask.at<uchar>(i, j) = d < maxD;

//            if(d < maxD){

    return mask;

    return;