Subversion Repositories seema-scanner

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

// Works like cv::windowedMatchingMask in conjunction with cv::BFMatcher::match().

// F is the fundamental matrix.

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

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

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

        return cv::Mat();

    int n1 = (int)keypoints1.size(), n2 = (int)keypoints2.size();

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

    // Point to line distance

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

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

//        // Epipolar line defined by p1

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

//        l /= sqrt(l(0)*l(0) + l(1)*l(1));

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

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

//            // Signed distance to line

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

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

//        }

//    }

    // Symmetric epipolar distance

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

    cvtools::keypointsToPoints(keypoints1, q1);

    cvtools::keypointsToPoints(keypoints2, q2);

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

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

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

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

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

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

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

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

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

            float d = d12*d12 + d21*d21;

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

        }

    }

//    // Sampson Error (H&Z, p. 287) (expensive...)

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

//    cvtools::keypointsToPoints(keypoints1, q1);

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

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

//    cv::convertPointsToHomogeneous(q1, p1);

//    cv::convertPointsToHomogeneous(q2, p2);

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

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

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

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

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

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

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

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

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

//        }

//    }

    return mask;

}

// Remove correspondences which have a distance metric above thresh.

void matchingThreshold(const std::vector<cv::DMatch> &matchesIn, std::vector<cv::DMatch> &matchesOut, float thresh){

    int nMatches = matchesIn.size();

    matchesOut.clear();

    matchesOut.reserve(nMatches);

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

        if(matchesIn[i].distance < thresh)

            matchesOut.push_back(matchesIn[i]);

    }

}