imported from gitlab

src/feature_descriptor/feature_descriptor.cpp

+#include "feature_descriptor.h"
+
+CFeature_Descriptor::CFeature_Descriptor(const std::string& _type, const CDescriptor_Params& _params)
+{
+    setAllTypes();
+
+	is_init_ = init(_type, _params);
+
+	if (!is_init_)
+	{
+		std::cerr
+				<< "[Feature Descriptor]: Something went wrong during initialization! Feature Descriptor not initialized."
+				<< std::endl;
+	}
+}
+
+CFeature_Descriptor::CFeature_Descriptor()
+{
+    setAllTypes();
+}
+
+CFeature_Descriptor::~CFeature_Descriptor() {
+}
+
+void CFeature_Descriptor::setAllTypes(void)
+{
+    // Define all types
+    std::vector<std::string> types;
+    types += "ORB", "SIFT", "SURF", "KAZE", "AKAZE", "BRISK", "LATCH", "FREAK", "BRIEF", "DAISY", "LUCID";
+    types_.set(types);
+    type_ = "ORB"; // Default value
+}
+
+bool CFeature_Descriptor::init(const std::string& _type, const CDescriptor_Params& _params)
+{
+    if (is_init_)
+    {
+        std::cerr << "[CFeature_Descriptor]: Descriptor already initialized." << std::endl;
+        return false;
+    }
+
+    // TODO: Set parameters for each detector type
+    if (_type.compare(types_(0))==0)
+    {
+        feature_descriptor_ = cv::ORB::create();
+        type_ = types_(0);
+    }
+    else if (_type.compare(types_(1))==0)
+    {
+        feature_descriptor_ = cv::xfeatures2d::SIFT::create();
+        type_ = types_(1);
+    }
+    else if (_type.compare(types_(2))==0)
+    {
+        feature_descriptor_ = cv::xfeatures2d::SURF::create();
+        type_ = types_(2);
+    }
+    else if (_type.compare(types_(3))==0)
+    {
+        feature_descriptor_ = cv::KAZE::create();
+        type_ = types_(3);
+    }
+    else if (_type.compare(types_(4))==0)
+    {
+        feature_descriptor_ = cv::AKAZE::create();
+        type_ = types_(4);
+    }
+    else if (_type.compare(types_(5))==0)
+    {
+        feature_descriptor_ = cv::BRISK::create();
+        type_ = types_(0);
+    }
+    else if (_type.compare(types_(6))==0)
+    {
+        feature_descriptor_ = cv::xfeatures2d::LATCH::create();
+        type_ = types_(6);
+    }
+    else if (_type.compare(types_(7))==0)
+    {
+        feature_descriptor_ = cv::xfeatures2d::FREAK::create();
+        type_ = types_(7);
+    }
+    else if (_type.compare(types_(8))==0)
+    {
+        feature_descriptor_ = cv::xfeatures2d::BriefDescriptorExtractor::create();
+        type_ = types_(8);
+    }
+    else if (_type.compare(types_(9))==0)
+    {
+        feature_descriptor_ = cv::xfeatures2d::DAISY::create();
+        type_ = types_(9);
+            }
+    else if (_type.compare(types_(10))==0)
+    {
+        feature_descriptor_ = cv::xfeatures2d::LUCID::create(1,2);
+        type_ = types_(10);
+            }
+    else
+    {
+        std::cerr << "[Feature Descriptor]: descriptor type " << _type << " doesn't exist !" << std::endl;
+        return false;
+    }
+
+    is_init_ = true;
+    return true;
+}
+
+cv::Mat CFeature_Descriptor::getDescriptor(const cv::Mat& _image, KeyPointVector& _kpts)
+{
+    if (!is_init_)
+        std::cerr << "[CFeature_Descriptor::getDescriptor]: Descriptor non initialized." << std::endl;
+
+    cv::Mat descriptors;
+
+    clock_t tStart = clock();
+    if (!_kpts.empty())
+    	feature_descriptor_->compute(_image, _kpts, descriptors);
+    comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
+
+    return descriptors;
+}

src/feature_descriptor/feature_descriptor.h

+#ifndef _FEATURE_DESCRIPTOR_H
+#define _FEATURE_DESCRIPTOR_H
+
+#include "../common/vu_base.h"
+
+typedef cv::Ptr<cv::DescriptorExtractor> FeatureDescriptorPtr;
+
+/**
+ * \brief Feature descriptor parameters class
+ */
+class CDescriptor_Params: public CParams {
+public:
+    CDescriptor_Params(void) {
+    }
+    ;
+    ~CDescriptor_Params(void) {
+    }
+    ;
+};
+
+class CFeature_Descriptor: public CVu_Base <CDescriptor_Params>{
+
+public:
+
+	CFeature_Descriptor(const std::string& _type, const CDescriptor_Params& _params);
+	CFeature_Descriptor(void);
+
+	~CFeature_Descriptor(void);
+
+	/**
+	 * \brief Get descriptors
+	 */
+	cv::Mat getDescriptor(const cv::Mat& _image,
+			KeyPointVector& _kpts);
+
+private:
+
+	FeatureDescriptorPtr feature_descriptor_; // Feature point descriptor
+
+    /**
+     * \brief Set all types
+     */
+    void setAllTypes(void);
+
+	/**
+	 * \brief Initialize descriptor
+	 */
+	bool init(const std::string &_type, const CDescriptor_Params &_params);
+};
+
+#endif

src/feature_detector/feature_detector.cpp

+#include "feature_detector.h"
+
+CFeature_Detector::CFeature_Detector(const std::string& _type, const CDetector_Params& _params) :
+    is_line_(false), is_limited_(false), keypoints_limit_(-1)
+{
+    setAllTypes();
+
+    is_init_ = init(_type, _params);
+
+    if (!is_init_)
+    {
+        std::cerr << "[Feature Detector]: Something went wrong during initialization! Feature Detector not initialized."
+                << std::endl;
+    }
+}
+
+CFeature_Detector::CFeature_Detector(void) :
+     is_line_(false), is_limited_(false), keypoints_limit_(-1)
+{
+    setAllTypes();
+}
+
+CFeature_Detector::~CFeature_Detector()
+{
+}
+
+void CFeature_Detector::setAllTypes(void)
+{
+    // Define all types
+    std::vector<std::string> types;
+    types += "FAST", "SIFT", "SURF", "ORB", "BRISK", "MSER", "GFTT", "HARRIS", "SBD", "KAZE", "AKAZE", "AGAST", "LSD", "ED";
+    types_.set(types);
+    type_ = "ORB"; // Default value
+}
+
+bool CFeature_Detector::init(const std::string& _type, const CDetector_Params& _params)
+{
+	if (is_init_)
+    {
+        std::cerr << "[CFeature_Detector::init]: Detector already initialized." << std::endl;
+        return false;
+    }
+
+    // TODO: Set parameters for each detector type
+    if (_type.compare(types_(0))==0)
+    {
+    	feature_detector_ = cv::FastFeatureDetector::create();
+    	type_ = types_(0);
+    }
+    else if (_type.compare(types_(1))==0)
+    {
+    	feature_detector_ = cv::xfeatures2d::SIFT::create();
+    	type_ = types_(1);
+    }
+    else if (_type.compare(types_(2))==0)
+    {
+    	feature_detector_ = cv::xfeatures2d::SURF::create();
+    	type_ = types_(2);
+    }
+    else if (_type.compare(types_(3))==0)
+    {
+    	feature_detector_ = cv::ORB::create();
+    	type_ = types_(3);
+    }
+    else if (_type.compare(types_(4))==0)
+    {
+    	feature_detector_ = cv::BRISK::create();
+    	type_ = types_(4);
+    }
+    else if (_type.compare(types_(5))==0)
+    {
+    	feature_detector_ = cv::MSER::create();
+    	type_ = types_(0);
+    }
+    else if (_type.compare(types_(6))==0)
+    {
+    	feature_detector_ = cv::GFTTDetector::create(1000, 0.01, 1.0, 3, false, 0.04);
+    	type_ = types_(6);
+    }
+    else if (_type.compare(types_(7))==0)
+    {
+    	feature_detector_ = cv::GFTTDetector::create(1000, 0.01, 1.0, 3, true, 0.04);
+    	type_ = types_(7);
+    }
+    else if (_type.compare(types_(8))==0)
+    {
+    	feature_detector_ = cv::SimpleBlobDetector::create();
+    	type_ = types_(8);
+    }
+    else if (_type.compare(types_(9))==0)
+    {
+    	feature_detector_ = cv::KAZE::create();
+    	type_ = types_(9);
+    	    }
+    else if (_type.compare(types_(10))==0)
+    {
+    	feature_detector_ = cv::AKAZE::create();
+    	type_ = types_(10);
+    	    }
+    else if (_type.compare(types_(11))==0)
+    {
+    	feature_detector_ = cv::AgastFeatureDetector::create();
+    	type_ = types_(11);
+    	    }
+    else if (_type.compare(types_(12))==0)
+    {
+        lsd_detector_ = cv::line_descriptor::LSDDetector::createLSDDetector();
+        type_ = types_(12);
+            }
+    else if (_type.compare(types_(13))==0)
+    {
+    	ed_detector_ = cv::line_descriptor::BinaryDescriptor::createBinaryDescriptor();
+    	type_ = types_(13);
+    	    }
+    else
+    {
+    	std::cerr << "[Feature Detector]: detector_type " << _type << " doesn't exist !" << std::endl;
+    	return false;
+    }
+
+    is_init_ = true;
+    return true;
+}
+
+bool CFeature_Detector::isLine(void)
+{
+    return is_line_;
+}
+
+bool CFeature_Detector::isLimited(void)
+{
+    return is_limited_;
+}
+
+int CFeature_Detector::getKeyPointsLimit(void)
+{
+    return keypoints_limit_;
+}
+
+KeyPointVector CFeature_Detector::detectKeyPoints(const cv::Mat& _image)
+{
+	cv::Mat mask = cv::Mat::ones(_image.size(), CV_8U);
+	return detectKeyPoints(_image, mask);
+}
+
+KeyPointVector CFeature_Detector::detectKeyPoints(const cv::Mat& _image, const cv::Mat& _mask)
+{
+    if (!is_init_)
+        std::cerr << "[CFeature_Detector::detectKeyPoints]: Detector non initialized." << std::endl;
+
+    KeyPointVector kpts;
+
+    clock_t tStart = clock();
+    feature_detector_->detect(_image, kpts, _mask);
+    comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
+
+    if (isLimited())
+    {
+        cv::KeyPointsFilter filter;
+        filter.retainBest(kpts, this->getKeyPointsLimit());
+    }
+
+    return kpts;
+}
+
+KeyLineVector CFeature_Detector::detectKeyLines(const cv::Mat& _image)
+{
+	cv::Mat mask = cv::Mat::ones(_image.size(), CV_8U);
+	return detectKeyLines(_image, mask);
+}
+
+KeyLineVector CFeature_Detector::detectKeyLines(const cv::Mat& _image, const cv::Mat& _mask)
+{
+    if (!is_init_)
+        std::cerr << "[CFeature_Detector::detectKeyLines]: Detector non initialized." << std::endl;
+
+    KeyLineVector kls;
+    KeyLineVector kls2;
+
+    clock_t tStart = clock();
+    if (type_.compare("LSD")==0)
+        lsd_detector_->detect(_image, kls, SCALE_FACTOR_LINE_DETECTOR, NUM_OCTAVE_LINE_DETECTOR, _mask);
+    else
+        ed_detector_->detect(_image, kls2, _mask);
+    comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
+    kls.insert(kls.end(), kls2.begin(), kls2.end());
+
+    return kls;
+}
+
+bool CFeature_Detector::setLimitKeypts(unsigned int _nFeatures)
+{
+    if (_nFeatures > 0)
+    {
+        keypoints_limit_ = _nFeatures;
+        return is_limited_ = true;
+    }
+
+    return false;
+}
+
+

src/feature_detector/feature_detector.h

+#ifndef _FEATURE_DETECTOR_H
+#define _FEATURE_DETECTOR_H
+
+#include "../common/vu_base.h"
+
+#define SCALE_FACTOR_LINE_DETECTOR 2
+#define NUM_OCTAVE_LINE_DETECTOR 1
+
+typedef cv::Ptr<cv::FeatureDetector> FeatureDetectorPtr;
+typedef cv::Ptr<cv::line_descriptor::LSDDetector> LSDDetector;
+typedef cv::Ptr<cv::line_descriptor::BinaryDescriptor> EDDetector;
+
+/**
+ * \brief Feature detector parameters class
+ */
+class CDetector_Params: public CParams {
+public:
+	CDetector_Params(void) {
+	}
+	;
+	~CDetector_Params(void) {
+	}
+	;
+};
+
+/**
+ * \brief Main feature detector class
+ */
+class CFeature_Detector: public CVu_Base <CDetector_Params> {
+public:
+
+	/**
+	 * \brief Constructor
+	 */
+	CFeature_Detector(const std::string& _type, const CDetector_Params& _params);
+	CFeature_Detector(void);
+
+	/**
+	 * \brief Destructor
+	 */
+	~CFeature_Detector(void);
+
+	/**
+	 * \brief Inform if the detector searches points or lines
+	 */
+	bool isLine(void);
+
+	/**
+	 * \brief Inform if there is a maximum number of features to be detected
+	 */
+	bool isLimited(void);
+
+	/**
+	 * \brief Get number of maximum keypoints allowed
+	 */
+	int getKeyPointsLimit(void);
+
+	/**
+	 * \brief Set number of maximum keypoints allowed
+	 */
+	bool setLimitKeypts(unsigned int _nFeatures);
+
+	/**
+	 * \brief Detect features (points or lines)
+	 */
+	KeyPointVector detectKeyPoints(const cv::Mat& _image);
+	KeyPointVector detectKeyPoints(const cv::Mat& _image, const cv::Mat& _mask);
+	KeyLineVector detectKeyLines(const cv::Mat& _image);
+	KeyLineVector detectKeyLines(const cv::Mat& _image, const cv::Mat& _mask);
+
+private:
+
+	// Flags
+	bool is_line_;
+	bool is_limited_;
+
+	unsigned int keypoints_limit_; // Max. number of keypoints (saturation)
+
+	FeatureDetectorPtr feature_detector_; // Feature point detectors
+	LSDDetector lsd_detector_; // Feature line detector
+	EDDetector ed_detector_; // Feature line detector
+
+    /**
+     * \brief Set all types
+     */
+	void setAllTypes(void);
+
+	/**
+	 * \brief Initialize detector
+	 */
+	bool init(const std::string &_type, const CDetector_Params &_params);
+};
+
+#endif

src/feature_matcher/feature_matcher.cpp

+#include "feature_matcher.h"
+
+CFeature_Matcher::CFeature_Matcher(const std::string& _type, const std::string& _match_out_type, const CMatcher_Params& _params)
+{
+    setAllTypes();
+
+    is_init_ = init(_type, _params);
+
+    if (!is_init_)
+    {
+        std::cerr << "[Feature Matcher]: Something went wrong during initialization! Feature Matcher not initialized."
+                << std::endl;
+    }
+}
+
+CFeature_Matcher::CFeature_Matcher(void)
+{
+    setAllTypes();
+}
+
+CFeature_Matcher::~CFeature_Matcher(void)
+{
+}
+
+void CFeature_Matcher::setAllTypes(void)
+{
+    // Define all types
+    std::vector<std::string> types;
+    types += "FlannBased", "BruteForce", "BruteForce-L1", "BruteForce-Hamming", "BruteForce-Hamming(2)";
+    types_.set(types);
+    type_ = "BruteForce"; // Default value
+
+    // Define all match output variants
+    std::vector<std::string> out_types;
+    out_types += "Match", "knnMatch", "radiusMatch";
+    match_search_types_.set(out_types);
+    match_search_type_ = "knnMatch"; // Default value
+}
+
+std::vector<std::string> CFeature_Matcher::listSearchTypes(void)
+{
+	return match_search_types_.list();
+}
+
+void CFeature_Matcher::getSearchType(int& _type)
+{
+	_type = match_search_types_(match_search_type_);
+}
+
+void CFeature_Matcher::getSearchType(std::string& _type)
+{
+	_type = match_search_type_;
+}
+
+bool CFeature_Matcher::setSearchType(const std::string& _type, const CMatcher_Params& _params)
+{
+    // TODO: Set parameters for each matcher type
+    for (unsigned int ii = 0; ii < match_search_types_.size(); ++ii)
+    {
+    	if (_type.compare(match_search_types_(ii))==0)
+    	{
+    	    	match_search_type_ = _type;
+    	    	match_search_params_ = _params;
+    	    	return true;
+    	}
+    }
+
+    std::cerr << "[Feature Matcher]: matcher output type " << _type << " doesn't exist !" << std::endl;
+    return false;
+}
+
+
+bool CFeature_Matcher::init(const std::string& _type, const CMatcher_Params& _params)
+{
+    if (is_init_)
+    {
+        std::cerr
+                << "[CFeature_Matcher::init]: Matcher already initialized."
+                << std::endl;
+        return false;
+    }
+
+    std::cout << _type << std::endl;
+
+
+    // TODO: Set parameters for each matcher type
+    if (_type.compare(types_(0))==0)
+    {
+        feature_matcher_ = cv::DescriptorMatcher::create(_type);
+        type_ = types_(0);
+    }
+    else if (_type.compare(types_(1))==0)
+    {
+        feature_matcher_ = cv::DescriptorMatcher::create(_type);
+        type_ = types_(1);
+    }
+    else if (_type.compare(types_(2))==0)
+    {
+        feature_matcher_ = cv::DescriptorMatcher::create(_type);
+        type_ = types_(2);
+    }
+    else if (_type.compare(types_(3))==0)
+    {
+        feature_matcher_ = cv::DescriptorMatcher::create(_type);
+        type_ = types_(3);
+    }
+    else if (_type.compare(types_(4))==0)
+    {
+        feature_matcher_ = cv::DescriptorMatcher::create(_type);
+        type_ = types_(4);
+    }
+    else
+    {
+        std::cerr << "[Feature Matcher]: mathcer_type " << _type << " doesn't exist !" << std::endl;
+        return false;
+    }
+
+    is_init_ = true;
+    return true;
+}
+
+// TODO: Match features and process matches (e.g. https://github.com/kipr/opencv/blob/master/samples/cpp/detector_descriptor_matcher_evaluation.cpp)
+void CFeature_Matcher::match(const cv::Mat& _desc1, const cv::Mat& _desc2, std::vector<cv::DMatch>& matches)
+{
+	std::cout << __LINE__ << std::endl;
+    if (!is_init_)
+        std::cerr << "[CFeature_Matcher::match]: Matcher non initialized." << std::endl;
+
+    if (match_search_type_.compare(match_search_types_(0))==0)
+    {
+    	clock_t tStart = clock();
+    	// TODO: use parameters related with the search type (match_search_params_)
+    	std::cout << __LINE__ << " " << _desc1.size() << " " << _desc2.size() << std::endl;
+    	if (!_desc1.empty() && !_desc2.empty())
+    		feature_matcher_->match( _desc1, _desc2, matches);
+    	comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
+    }
+    else
+    	std::cerr << "[CFeature_Matcher::match]: The selected matcher output is different than your object." << std::endl;
+}
+
+// TODO: Match features and process matches (e.g. https://github.com/kipr/opencv/blob/master/samples/cpp/detector_descriptor_matcher_evaluation.cpp)
+void CFeature_Matcher::match(const cv::Mat& _desc1, const cv::Mat& _desc2, std::vector< std::vector<cv::DMatch> >& matches)
+{
+    if (!is_init_)
+        std::cerr << "[CFeature_Matcher::match]: Matcher non initialized." << std::endl;
+
+    if (match_search_type_.compare(match_search_types_(1))==0)// knn match
+    {
+    	clock_t tStart = clock();
+    	// TODO: use parameters related with the search type (match_search_params_)
+    	if (!_desc1.empty() && !_desc2.empty())
+    		feature_matcher_->knnMatch(_desc1, _desc2, matches, 2);
+    	comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
+    }
+    else if (match_search_type_.compare(match_search_types_(2))==0) // radius match
+	{
+    	clock_t tStart = clock();
+    	// TODO: use parameters related with the search type (match_search_params_)
+    	if (!_desc1.empty() && !_desc2.empty())
+    		feature_matcher_->radiusMatch(_desc1, _desc2, matches, 2);
+    	comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
+	}
+    else
+    	std::cerr << "[CFeature_Matcher::match]: The selected matcher output is different than your object." << std::endl;
+}
+
+void CFeature_Matcher::filterMatches(const KeyPointVector& _kpts1,const KeyPointVector& _kpts2, const std::vector< std::vector<cv::DMatch> >& _dirty, const int& _img_width, const int& _img_height, std::vector<cv::DMatch>& _filtered_matches, KeyPointVector& _filtered_kpts)
+{
+	//only 25% of maximum of possible distance
+	double tresholdDist = 0.25 * sqrt(double(_img_height*_img_height + _img_width*_img_width));
+
+	std::vector< cv::DMatch > good_matches2;
+	_filtered_matches.reserve(_dirty.size());
+	for (size_t ii = 0; ii < _dirty.size(); ++ii)
+	{
+	    for (unsigned int jj = 0; jj < _dirty[ii].size(); jj++)
+	    {
+	        cv::Point2f from = _kpts1[_dirty[ii][jj].queryIdx].pt;
+	        cv::Point2f to = _kpts2[_dirty[ii][jj].trainIdx].pt;
+
+	        //calculate local distance for each possible match
+	        double dist = std::sqrt((from.x - to.x) * (from.x - to.x) + (from.y - to.y) * (from.y - to.y));
+
+	        //save as best match if local distance is in specified area and on same height
+	        if (dist < tresholdDist && std::abs(from.y-to.y)<5)
+	        {
+	        	_filtered_matches.push_back(_dirty[ii][jj]);
+	        	cv::KeyPoint kpt = cv::KeyPoint(to,1);
+	        	_filtered_kpts.push_back(kpt);
+	            jj = _dirty[ii].size();
+	        }
+	    }
+	}
+}

src/feature_matcher/feature_matcher.h

+#ifndef _FEATURE_MATCHER_H
+#define _FEATURE_MATCHER_H
+
+#include "../common/vu_base.h"
+
+typedef cv::Ptr<cv::DescriptorMatcher> FeatureMatcherPtr;
+
+/**
+ * \brief Feature descriptor parameters class
+ */
+class CMatcher_Params: public CParams {
+public:
+    CMatcher_Params(void) {
+    }
+    ;
+    ~CMatcher_Params(void) {
+    }
+    ;
+};
+
+class CFeature_Matcher: public CVu_Base<CMatcher_Params>
+{
+    public:
+
+        CFeature_Matcher(const std::string& _type, const std::string& _match_out_type, const CMatcher_Params& _params);
+        CFeature_Matcher(void);
+
+        ~CFeature_Matcher(void);
+
+        /**
+         * \brief list all possible match output types (All, K nearest, in a radius search, ...)
+         */
+        std::vector<std::string> listSearchTypes(void);
+
+        /**
+         * \brief Set matcher search method
+         */
+        bool setSearchType(const std::string& _type, const CMatcher_Params& _params);
+
+        /**
+         * \brief Get matcher search method
+         */
+        void getSearchType(int& _type);
+        void getSearchType(std::string& _type);
+        /**
+         * \brief Find Matches
+         */
+        void match(const cv::Mat& _desc1, const cv::Mat& _desc2, std::vector<cv::DMatch>& matches);
+
+        /**
+         * \brief Find K best matches or matches in a radius (depending on selected match_search_type_)
+         */
+        void match(const cv::Mat& _desc1, const cv::Mat& _desc2, std::vector< std::vector<cv::DMatch> >& matches);
+
+        /**
+         * \brief 	Look whether the match is inside a defined area of the image
+         *
+         * 			From https://stackoverflow.com/questions/17967950/improve-matching-of-feature-points-with-opencv
+         */
+        void filterMatches(const KeyPointVector& _kpts1,const KeyPointVector& _kpts2, const std::vector< std::vector<cv::DMatch> >& _dirty, const int& _img_width, const int& _img_height, std::vector<cv::DMatch>& _filtered_matches, KeyPointVector& _filtered_kpts);
+
+    private:
+
+        FeatureMatcherPtr feature_matcher_; // Feature matcher
+
+        CTypes match_search_types_; // Match type (match with all, knn and radius search).
+        CMatcher_Params match_search_params_; // Search type parameters.
+        std::string match_search_type_; // Match type (match with all, knn and radius search).
+
+        /**
+         * \brief Set all types
+         */
+        void setAllTypes(void);
+
+        /**
+         * \brief Initialize matcher
+         */
+        bool init(const std::string &_type, const CMatcher_Params &_params);
+};
+
+#endif

src/vision_utils.cpp

+#include "vision_utils.h"
+
+CVision_Utils::CVision_Utils() {
+}
+
+CVision_Utils::~CVision_Utils() {
+}
+
+bool LessPoints(const cv::Point2f& lhs, const cv::Point2f& rhs)
+{
+    return (lhs.x < rhs.x) || ((lhs.x == rhs.x) && (lhs.y < rhs.y));
+}
+
+std::vector<cv::Point2f> vecIntersec(std::vector<cv::Point2f> v1, std::vector<cv::Point2f> v2)
+{
+  std::vector<cv::Point2f> v3;
+  // Sort vectors
+  std::sort(v1.begin(), v1.end(), LessPoints);
+  std::sort(v2.begin(), v2.end(), LessPoints);
+  // Intersect
+  std::set_intersection(v1.begin(), v1.end(), v2.begin(), v2.end(), std::back_inserter(v3), LessPoints);
+  return v3;
+}
+
+std::vector<cv::Point2f> vecUnion(std::vector<cv::Point2f> v1, std::vector<cv::Point2f> v2)
+{
+  std::vector<cv::Point2f> v3;
+  // Sort vectors
+  std::sort(v1.begin(), v1.end(), LessPoints);
+  std::sort(v2.begin(), v2.end(), LessPoints);
+  // Intersect
+  std::set_union(v1.begin(), v1.end(), v2.begin(), v2.end(), std::back_inserter(v3), LessPoints);
+  return v3;
+}

src/vision_utils.h

+#ifndef _VISION_UTILS_H
+#define _VISION_UTILS_H
+
+#include "cam_utils/cam_utils.h"
+#include "feature_detector/feature_detector.h"
+#include "feature_descriptor/feature_descriptor.h"
+#include "feature_matcher/feature_matcher.h"
+
+#include <functional>
+#include <set>
+
+class CVision_Utils {
+public:
+	CVision_Utils();
+	~CVision_Utils();
+};
+
+template <typename T>
+T readFromUser(const T& def_num)
+{
+	T read = def_num;
+	std::string input;
+	std::getline( std::cin, input );
+	if ( !input.empty() )
+	{
+	   std::istringstream stream( input );
+	   stream >> read;
+	}
+
+	return read;
+}
+
+bool LessPoints(const cv::Point2f& lhs, const cv::Point2f& rhs);
+std::vector<cv::Point2f> vecIntersec(std::vector<cv::Point2f> v1, std::vector<cv::Point2f> v2);
+std::vector<cv::Point2f> vecUnion(std::vector<cv::Point2f> v1, std::vector<cv::Point2f> v2);
+
+
+#endif