feature_matching.h

Go to the documentation of this file.

//part of the source code of master thesis of Jiri Divis, source code written by Jiri Divis
#ifndef VSLAM_FEATURE_MATCHING_H_INCLUDED
#define VSLAM_FEATURE_MATCHING_H_INCLUDED

#include "utils.h"
#include <vector>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <memory>
#include "model_estimation.h"
#include "sfm_utils.h"

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


class CV_EXPORTS VirtualFeatureDetector : public cv::FeatureDetector {
    protected:
        IdxPoint2dVec points;

        virtual void detectImpl( const cv::Mat& image, vector<cv::KeyPoint>& keypoints,
                const cv::Mat& mask) const;
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
        VirtualFeatureDetector(const IdxPoint2dVec & _points) : points(_points) {}
};



class CV_EXPORTS NoisyVirtualFeatureDetector : public VirtualFeatureDetector {
    protected:
        double sigma;
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
        NoisyVirtualFeatureDetector(const IdxPoint2dVec & _points, double _sigma = 1.0) : 
                VirtualFeatureDetector(_points), sigma(_sigma) {
            boost::normal_distribution<double> nd(0.0, sigma);
            boost::variate_generator<boost::mt19937&, 
                   boost::normal_distribution<> > var_nd(rng_mt19937, nd);
            for(vector<cv::KeyPoint>::size_type i = 0; i < points.size(); i++){
                points[i].second(0) += var_nd(); 
                points[i].second(1) += var_nd(); 
            }
        }
};


class VirtualDescriptorExtractor : public cv::DescriptorExtractor{
    public:
        virtual int descriptorSize() const { return 1; };
        virtual int descriptorType() const { return CV_32SC1; };
    protected:
        virtual void computeImpl(
                const cv::Mat& image, vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors ) const;
};


class VirtualDescriptorMatcher : public cv::DescriptorMatcher {
    private:
        double inlier_p;
    public:
        VirtualDescriptorMatcher(double _inlier_p=1.0){
            inlier_p=_inlier_p;
        }

        virtual bool isMaskSupported() const {return false; };

        virtual cv::Ptr<cv::DescriptorMatcher> clone( bool emptyTrainData=false ) const;

    protected:
        // In fact the matching is implemented only by the following two methods. These methods suppose
        // that the class object has been trained already. Public match methods call these methods
        // after calling train().
        virtual void knnMatchImpl( const cv::Mat& queryDescriptors,
                vector<vector<cv::DMatch> >& matches, int k,
                const vector<cv::Mat>& masks=vector<cv::Mat>(), bool compactResult=false);

        virtual void radiusMatchImpl( const cv::Mat& queryDescriptors, 
                vector<vector<cv::DMatch> >& matches, float maxDistance, 
                const vector<cv::Mat>& masks=vector<cv::Mat>(), bool compactResult=false);
};



struct RobustFeatureMatcherStats{
    int tq;
    int qt;
    int ratio_tq;
    int ratio_qt;
    int symetry;
    int ransac;
    int filter;

    bool success;
    operator bool(){
        return success;
    }
};



class RobustFeatureMatcher {
    private:
        float ratio; 
        int ransac_iterations; 
        double ransac_model_min_inliers_ratio;
        double max_outlier_removal_error;
        shared_ptr<cv::DescriptorMatcher> matcher; 
        shared_ptr<EMatrixModelBuilder> mbuilder;
        shared_ptr<EMatrixModelBuilder> maximal_data_mbuilder;

    public:
        typedef shared_ptr<EMatrixModelBuilder> MBuilderPtr;

        RobustFeatureMatcher(
                shared_ptr<cv::DescriptorMatcher> _matcher,
                MBuilderPtr _mbuilder = MBuilderPtr(),
                MBuilderPtr _maximal_data_mbuilder = MBuilderPtr()) :
                ratio(0.75f), ransac_iterations(100), ransac_model_min_inliers_ratio(0.10), 
                max_outlier_removal_error((M_PI / 180.0)*1.0),
                matcher(_matcher), mbuilder(_mbuilder),
                maximal_data_mbuilder(_maximal_data_mbuilder) {}

        void setRanSaCModelMinInliersRatio(int ratio) {
            ransac_model_min_inliers_ratio = ratio;
        }

        void setRansacIterationsNum(int num) {
            ransac_iterations = num;
        }

        void setRatio(float r) {
            ratio= r;
        }

        void setMaxOutlierRemovalError(double _e){
            max_outlier_removal_error=_e;
        }


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

        void 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);

        bool ransacTest(
                const ImageMatchVec& matches_c1c2,
                const vector<cv::DMatch>& dmatches_c1c2,
                RobustFeatureMatcherStats& stats,
                vector<cv::DMatch>& filtered_matches_c1c2, 
                SE3& T_c1c2);
        
        void outlierRemoval(
                const SE3& T_c1c2,
                const ImageMatchVec& matches_c1c2,
                const vector<cv::DMatch>& dmatches_c1c2,
                RobustFeatureMatcherStats& stats,
                vector<cv::DMatch>& filtered_matches_c1c2); 
    protected:
        void ratioTest(
                std::vector<std::vector<cv::DMatch> > & matches0,
                std::vector<cv::DMatch> & matches);



        void symmetryTest(
                const vector<cv::DMatch>& matches_c1c2,
                const vector<cv::DMatch>& matches_c2c1,
                int keypoints2_size,
                vector<cv::DMatch>& symMatches);
};
#endif