feature_matching.cpp

Go to the documentation of this file.

//part of the source code of master thesis of Jiri Divis, source code written by Jiri Divis
/*------------------------------------------------------------------------------------------*\
  This file contains RobustFeatureMatcherStats class, which contains heavily
  modyfied code that was taken from the supporting code for chapter 9 of the
  book:  
  Computer Vision Programming using the OpenCV Library. 
  by Robert Laganiere, Packt Publishing, 2011.


  Original license and copyright:
  This program is free software; permission is hereby granted to use, copy, modify, 
  and distribute this source code, or portions thereof, for any purpose, without fee, 
  subject to the restriction that the copyright notice may not be removed 
  or altered from any source or altered source distribution. 
  The software is released on an as-is basis and without any warranties of any kind. 
  In particular, the software is not guaranteed to be fault-tolerant or free from failure. 
  The author disclaims all warranties with regard to this software, any use, 
  and any consequent failure, is purely the responsibility of the user.

  Copyright (C) 2010-2011 Robert Laganiere, www.laganiere.name
\*------------------------------------------------------------------------------------------*/
#include "feature_matching.h"
#include "error_func.h"
#include "emat_estimation.h"
#include <ros/console.h>


using cv::Mat;
using cv::KeyPoint;
using cv::FeatureDetector;
using cv::DMatch;
using std::vector;
using Eigen::Vector2f;

void VirtualFeatureDetector::detectImpl(
        const Mat& image,
        vector<KeyPoint>& keypoints,
        const Mat& mask=Mat()) const {
    typedef pair<int, Vector2d> T;
    BOOST_FOREACH(T pt, points){
        keypoints.push_back(KeyPoint(
            (float)pt.second(0),
            (float)pt.second(1), 
            1.0, //size 
            -1.0, //angle
            0.0, //response
            0, //octave
            pt.first)); //class_id
    }
    removeInvalidPoints(mask, keypoints);
}


/*------------------------------------------------------------------------------------------*/

void VirtualDescriptorExtractor::computeImpl(
        const Mat& image, vector<KeyPoint>& keypoints, Mat& descriptors ) const{
    //XXX not sure if it will work on border pixels
    removeBorderKeypoints(keypoints, image.size(), 2);

    descriptors.create(keypoints.size(), descriptorSize(), descriptorType());
    for(vector<KeyPoint>::size_type i = 0 ; i < keypoints.size(); i++){
        descriptors.at<int>(i, 0) = keypoints[i].class_id;
    }
}

/*------------------------------------------------------------------------------------------*/

cv::Ptr<cv::DescriptorMatcher> VirtualDescriptorMatcher::clone( bool emptyTrainData ) const{
    VirtualDescriptorMatcher * tmp = new VirtualDescriptorMatcher(*this);
    tmp->clear();
    if(!emptyTrainData){
        BOOST_FOREACH(Mat m, trainDescCollection){
            tmp->trainDescCollection.push_back(m.clone());
        }
    }
    return tmp;
}


void VirtualDescriptorMatcher::knnMatchImpl(
        const Mat& queryDescriptors, vector<vector<DMatch> >& matches,
        int k, const vector<Mat>& masks, bool compactResult ){
    for(int i = 0; i < queryDescriptors.size().height; i++){
        vector<DMatch> tmpvec;
        assert(trainDescCollection.size() == 1);
        int z=k-1;
        for(int j = 0; j < trainDescCollection[0].size().height; j++){
            if(trainDescCollection[0].at<int>(j,0) == 
                    std::tr1::round(queryDescriptors.at<int>(i, 0))
                    && isPossibleMatch(masks[0], i, j)){
                boost::uniform_real<> interval(0.0, 1.0);
                double tmp = interval(rng_mt19937);
                if(tmp > inlier_p){
                    int max = std::min(queryDescriptors.size().height, trainDescCollection[0].size().height) -1;
                    tmpvec.push_back(DMatch(std::min(j, max), std::min(i, max), 0));
                } else{
                    tmpvec.push_back(DMatch(i, j, 0));
                }
            }
            if(!tmpvec.empty() && z && isPossibleMatch(masks[0], i, j)){
                tmpvec.push_back(DMatch(i, j, z));
                z--;
            }
        }
        matches.push_back(tmpvec);
    }
}

void VirtualDescriptorMatcher::radiusMatchImpl(
        const Mat& queryDescriptors, vector<vector<DMatch> >& matches, float maxDistance, 
        const vector<Mat>& masks, bool compactResult){
    assert(false);
}



/*------------------------------------------------------------------------------------------*/

void RobustFeatureMatcher::nonsymetricalMatch(
        const vector<cv::KeyPoint>& train_keypoints,
        const vector<cv::KeyPoint>& query_keypoints,
        const cv::Mat& train_descriptors,
        const cv::Mat& query_descriptors,
        RobustFeatureMatcherStats& stats,
        vector<cv::DMatch>& matches) {
    assert(matcher);

    // train->query match based on k nearest neighbours (with k=2)
    std::vector<std::vector<cv::DMatch> > matches0;
    matcher->knnMatch(query_descriptors, train_descriptors,
            matches0, // vector of matches (up to 2 per entry) 
            2);           // return 2 nearest neighbours

    stats.qt=matches0.size();
    //ROS_DEBUG_STREAM_NAMED("RobustMatcher", 
    //        "Number of non-symetrical feature matches before ratio filter: " << stats.qt);

    // clean image matches
    ratioTest(matches0, matches);
    stats.ratio_qt=matches.size();
    //ROS_DEBUG_STREAM_NAMED("RobustMatcher", 
    //        "Number of non-symetrical feature matches: " << stats.ratio_qt);
}

void RobustFeatureMatcher::symaetricalMatch(
        const std::vector<cv::KeyPoint>& keypoints_c1,
        const std::vector<cv::KeyPoint>& keypoints_c2,
        const cv::Mat& descriptors_c1,
        const cv::Mat& descriptors_c2,
        RobustFeatureMatcherStats& stats,
        std::vector<cv::DMatch>& sym_matches_c1c2){
    assert(matcher);

    // from image 1 to image 2.
    vector<cv::DMatch> matches_c1c2;
    nonsymetricalMatch(
            keypoints_c1, keypoints_c2, descriptors_c1, descriptors_c2, stats, matches_c1c2);
    stats.tq=stats.qt;
    // from image 2 to image 1.
    vector<cv::DMatch> matches_c2c1;
    nonsymetricalMatch(
            keypoints_c2, keypoints_c1, descriptors_c2, descriptors_c1, stats, matches_c2c1);

    // Remove non-symmetrical matches
    std::vector<cv::DMatch> symMatches;
    symmetryTest(matches_c1c2, matches_c2c1, keypoints_c2.size(), sym_matches_c1c2);
    stats.symetry=sym_matches_c1c2.size();
    //ROS_DEBUG_STREAM_NAMED("RobustMatcher", 
    //        "Number of symetrical feature matches: " << stats.symetry);
}



void RobustFeatureMatcher::ratioTest(
        std::vector<std::vector<cv::DMatch> > & matches0,
        std::vector<cv::DMatch> & matches) {
    assert(matches.size() == 0);
    for (std::vector<std::vector<cv::DMatch> >::iterator matchIterator= matches0.begin();
            matchIterator!= matches0.end(); ++matchIterator) {
        if (matchIterator->size() > 1) { // if 2 NN has been identified
            // check distance ratio
            if ((*matchIterator)[0].distance/(*matchIterator)[1].distance < ratio) {
                matches.push_back((*matchIterator)[0]);
            }
        } else { // does not have 2 neighbours
            ;
        }
    }
}



void RobustFeatureMatcher::symmetryTest(
        const vector<cv::DMatch>& matches_c1c2,
        const vector<cv::DMatch>& matches_c2c1,
        int keypoints2_size,
        vector<cv::DMatch>& symMatches) {
    int BAD_INDEX = -1;
    vector<int> keypoint2i_to_match_c2c1i_map = vector<int>( keypoints2_size, BAD_INDEX);
    for(size_t i=0; i< matches_c2c1.size(); i++){
        keypoint2i_to_match_c2c1i_map[matches_c2c1[i].trainIdx] = i;
    }

    std::vector<cv::DMatch>::const_iterator match_c1c2_it = matches_c1c2.begin();
    while(match_c1c2_it != matches_c1c2.end()){
        int match_c2c1i = keypoint2i_to_match_c2c1i_map[match_c1c2_it->queryIdx];
        if(match_c2c1i!=BAD_INDEX){
            if(match_c1c2_it->trainIdx == matches_c2c1[match_c2c1i].queryIdx){
                assert(matches_c2c1[match_c2c1i].trainIdx==match_c1c2_it->queryIdx);
                symMatches.push_back(*match_c1c2_it);
            }
        }
        ++match_c1c2_it;
    }
}


bool RobustFeatureMatcher::ransacTest(
        const ImageMatchVec& matches_c1c2,
        const vector<cv::DMatch>& dmatches_c1c2,
        RobustFeatureMatcherStats& stats,
        vector<cv::DMatch>& filtered_matches_c1c2, 
        SE3& T_c1c2) {

    unsigned int min_inliers = std::max(
            (double)(mbuilder->getSize() + 1), 
            std::ceil(dmatches_c1c2.size() * ransac_model_min_inliers_ratio));
    if(min_inliers < mbuilder->getSize()){
        ROS_WARN_NAMED("RobustMatcher", "Aborting. min_inliers < model_size");
        return false;
    }
    //shared_ptr<EMatrixModel> model = ranSaC(mbuilder, matches_c1c2, ransac_iterations,
    //        min_inliers);
    typedef RealErrorMaxErrorAccumulator<AngleError> ErrorAccumT; 
    typedef EMatrixModel ModelT;
    shared_ptr<DataSelector<ModelT::DataVec> > selector(
            new RandomDataSelector<ModelT::DataVec>(
            matches_c1c2, mbuilder->getSize(), ransac_iterations));
    shared_ptr<ModelT> model = ranSaC<ErrorAccumT>(mbuilder, selector, matches_c1c2);
    //@todo check min inliers XXX.

    if(!model){
        ROS_WARN_NAMED("RobustMatcher", "Aborting. RanSaC could not find a fitting model.");
        return false;
    }

    MaskVec::const_iterator itIn= model->getInlierMask().begin();
    std::vector<cv::DMatch>::const_iterator itM= dmatches_c1c2.begin();
    while(itIn!= model->getInlierMask().end()) {
        if (*itIn) { // it is a valid match
            filtered_matches_c1c2.push_back(*itM);
        }
        ++itIn, ++itM;
    }
    stats.ransac=filtered_matches_c1c2.size();

    //TODO XXX
    /*if (maximal_data_mbuilder) {
        // The F matrix will be recomputed with all accepted matches
        // Compute 8-point F from all accepted matches
    }*/

    if(extractTransformFromE(model->getModelData(), model->getModel(), T_c1c2)){
        //ROS_DEBUG_STREAM_NAMED("RobustMatcher", 
        //        "Number of features that passed ransac test: " << stats.ransac);
        return true;
    } else{
        ROS_WARN_NAMED("RobustMatcher", "Aborting. Cannot extract transform.");
        return false;
    }
}

void RobustFeatureMatcher::outlierRemoval(
        const SE3& T_c1c2,
        const ImageMatchVec& matches_c1c2,
        const vector<cv::DMatch>& dmatches_c1c2,
        RobustFeatureMatcherStats& stats,
        vector<cv::DMatch>& filtered_matches_c1c2){ 
    Eigen::Matrix3d E = SO3::hat(T_c1c2.translation()) * T_c1c2.rotation_matrix();
    ALIGNED<Matrix3d>::vector Ematrices;
    Ematrices.push_back(E);
    shared_ptr<EMatrixModel> model = 
            DummyModelBuilderStd(Ematrices, max_outlier_removal_error)(ImageMatchVec());
    model->compute(matches_c1c2);
    MaskVec::const_iterator itIn= model->getInlierMask().begin();
    std::vector<cv::DMatch>::const_iterator itM= dmatches_c1c2.begin();
    while(itIn!= model->getInlierMask().end()) {
        if (*itIn) { // it is a valid match
            filtered_matches_c1c2.push_back(*itM);
        }
        ++itIn, ++itM;
    }
    stats.filter=filtered_matches_c1c2.size();
}