vpKeyPoint.h

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Key point functionalities.
 */
#ifndef _vpKeyPoint_h_
#define _vpKeyPoint_h_
 
#include <algorithm> // std::transform
#include <float.h>   // DBL_MAX
#include <fstream>   // std::ofstream
#include <limits>
#include <map>      // std::map
#include <numeric>  // std::accumulate
#include <stdlib.h> // srand, rand
#include <time.h>   // time
#include <vector>   // std::vector
 
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/core/vpPlane.h>
#include <visp3/core/vpPoint.h>
#include <visp3/vision/vpBasicKeyPoint.h>
#include <visp3/vision/vpPose.h>
#ifdef VISP_HAVE_MODULE_IO
#include <visp3/io/vpImageIo.h>
#endif
#include <visp3/core/vpConvert.h>
#include <visp3/core/vpCylinder.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPolygon.h>
#include <visp3/vision/vpXmlConfigParserKeyPoint.h>
 
// Require at least OpenCV >= 2.1.1
#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_FEATURES2D)
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/imgproc_c.h>
 
#if defined(VISP_HAVE_OPENCV_XFEATURES2D) // OpenCV >= 3.0.0
#include <opencv2/xfeatures2d.hpp>
#elif defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION >= 0x020400) &&                                   \
    (VISP_HAVE_OPENCV_VERSION < 0x030000)
#include <opencv2/nonfree/nonfree.hpp>
#endif
 
class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint
{
 
public:
  enum vpFilterMatchingType {
    constantFactorDistanceThreshold, 
    stdDistanceThreshold,            
    ratioDistanceThreshold,          
    stdAndRatioDistanceThreshold,    
    noFilterMatching                 
  };
 
  enum vpDetectionMethodType {
    detectionThreshold, 
    detectionScore      
  };
 
  typedef enum {
    jpgImageFormat, 
    pngImageFormat, 
    ppmImageFormat, 
    pgmImageFormat  
  } vpImageFormatType;
 
  enum vpFeatureDetectorType {
#if (VISP_HAVE_OPENCV_VERSION >= 0x020403)
    DETECTOR_FAST,
    DETECTOR_MSER,
    DETECTOR_ORB,
    DETECTOR_BRISK,
    DETECTOR_GFTT,
    DETECTOR_SimpleBlob,
#if (VISP_HAVE_OPENCV_VERSION < 0x030000) || (defined(VISP_HAVE_OPENCV_XFEATURES2D))
    DETECTOR_STAR,
#endif
#if defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D) ||                                      \
    (VISP_HAVE_OPENCV_VERSION >= 0x030411 && CV_MAJOR_VERSION < 4) || (VISP_HAVE_OPENCV_VERSION >= 0x040400)
    DETECTOR_SIFT,
#endif
#if defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D)
    DETECTOR_SURF,
#endif
#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
    DETECTOR_KAZE,
    DETECTOR_AKAZE,
    DETECTOR_AGAST,
#endif
#if (VISP_HAVE_OPENCV_VERSION >= 0x030100) && defined(VISP_HAVE_OPENCV_XFEATURES2D)
    DETECTOR_MSD,
#endif
#endif
    DETECTOR_TYPE_SIZE
  };
 
  enum vpFeatureDescriptorType {
#if (VISP_HAVE_OPENCV_VERSION >= 0x020403)
    DESCRIPTOR_ORB,
    DESCRIPTOR_BRISK,
#if (VISP_HAVE_OPENCV_VERSION < 0x030000) || (defined(VISP_HAVE_OPENCV_XFEATURES2D))
    DESCRIPTOR_FREAK,
    DESCRIPTOR_BRIEF,
#endif
#if defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D) ||                                      \
    (VISP_HAVE_OPENCV_VERSION >= 0x030411 && CV_MAJOR_VERSION < 4) || (VISP_HAVE_OPENCV_VERSION >= 0x040400)
    DESCRIPTOR_SIFT,
#endif
#if defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D)
    DESCRIPTOR_SURF,
#endif
#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
    DESCRIPTOR_KAZE,
    DESCRIPTOR_AKAZE,
#if defined(VISP_HAVE_OPENCV_XFEATURES2D)
    DESCRIPTOR_DAISY,
    DESCRIPTOR_LATCH,
#endif
#endif
#if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && defined(VISP_HAVE_OPENCV_XFEATURES2D)
    DESCRIPTOR_VGG,
    DESCRIPTOR_BoostDesc,
#endif
#endif
    DESCRIPTOR_TYPE_SIZE
  };
 
  vpKeyPoint(const vpFeatureDetectorType &detectorType, const vpFeatureDescriptorType &descriptorType,
             const std::string &matcherName, const vpFilterMatchingType &filterType = ratioDistanceThreshold);
  vpKeyPoint(const std::string &detectorName = "ORB", const std::string &extractorName = "ORB",
             const std::string &matcherName = "BruteForce-Hamming",
             const vpFilterMatchingType &filterType = ratioDistanceThreshold);
  vpKeyPoint(const std::vector<std::string> &detectorNames, const std::vector<std::string> &extractorNames,
             const std::string &matcherName = "BruteForce",
             const vpFilterMatchingType &filterType = ratioDistanceThreshold);
 
  unsigned int buildReference(const vpImage<unsigned char> &I);
  unsigned int buildReference(const vpImage<unsigned char> &I, const vpImagePoint &iP, unsigned int height,
                              unsigned int width);
  unsigned int buildReference(const vpImage<unsigned char> &I, const vpRect &rectangle);
 
  unsigned int buildReference(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &trainKeyPoints,
                              std::vector<cv::Point3f> &points3f, bool append = false, int class_id = -1);
  unsigned int buildReference(const vpImage<unsigned char> &I, const std::vector<cv::KeyPoint> &trainKeyPoints,
                              const cv::Mat &trainDescriptors, const std::vector<cv::Point3f> &points3f,
                              bool append = false, int class_id = -1);
 
  unsigned int buildReference(const vpImage<vpRGBa> &I_color);
  unsigned int buildReference(const vpImage<vpRGBa> &I_color, const vpImagePoint &iP, unsigned int height,
                              unsigned int width);
  unsigned int buildReference(const vpImage<vpRGBa> &I_color, const vpRect &rectangle);
 
  unsigned int buildReference(const vpImage<vpRGBa> &I_color, std::vector<cv::KeyPoint> &trainKeyPoints,
                              std::vector<cv::Point3f> &points3f, bool append = false, int class_id = -1);
  unsigned int buildReference(const vpImage<vpRGBa> &I, const std::vector<cv::KeyPoint> &trainKeyPoints,
                              const cv::Mat &trainDescriptors, const std::vector<cv::Point3f> &points3f,
                              bool append = false, int class_id = -1);
 
  static void compute3D(const cv::KeyPoint &candidate, const std::vector<vpPoint> &roi, const vpCameraParameters &cam,
                        const vpHomogeneousMatrix &cMo, cv::Point3f &point);
 
  static void compute3D(const vpImagePoint &candidate, const std::vector<vpPoint> &roi, const vpCameraParameters &cam,
                        const vpHomogeneousMatrix &cMo, vpPoint &point);
 
  static void compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
                                           std::vector<cv::KeyPoint> &candidates,
                                           const std::vector<vpPolygon> &polygons,
                                           const std::vector<std::vector<vpPoint> > &roisPt,
                                           std::vector<cv::Point3f> &points, cv::Mat *descriptors = NULL);
 
  static void compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
                                           std::vector<vpImagePoint> &candidates,
                                           const std::vector<vpPolygon> &polygons,
                                           const std::vector<std::vector<vpPoint> > &roisPt,
                                           std::vector<vpPoint> &points, cv::Mat *descriptors = NULL);
 
  static void
  compute3DForPointsOnCylinders(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
                                std::vector<cv::KeyPoint> &candidates, const std::vector<vpCylinder> &cylinders,
                                const std::vector<std::vector<std::vector<vpImagePoint> > > &vectorOfCylinderRois,
                                std::vector<cv::Point3f> &points, cv::Mat *descriptors = NULL);
 
  static void
  compute3DForPointsOnCylinders(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
                                std::vector<vpImagePoint> &candidates, const std::vector<vpCylinder> &cylinders,
                                const std::vector<std::vector<std::vector<vpImagePoint> > > &vectorOfCylinderRois,
                                std::vector<vpPoint> &points, cv::Mat *descriptors = NULL);
 
  bool computePose(const std::vector<cv::Point2f> &imagePoints, const std::vector<cv::Point3f> &objectPoints,
                   const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, std::vector<int> &inlierIndex,
                   double &elapsedTime, bool (*func)(const vpHomogeneousMatrix &) = NULL);
 
  bool computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo, std::vector<vpPoint> &inliers,
                   double &elapsedTime, bool (*func)(const vpHomogeneousMatrix &) = NULL);
 
  bool computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo, std::vector<vpPoint> &inliers,
                   std::vector<unsigned int> &inlierIndex, double &elapsedTime,
                   bool (*func)(const vpHomogeneousMatrix &) = NULL);
 
  void createImageMatching(vpImage<unsigned char> &IRef, vpImage<unsigned char> &ICurrent,
                           vpImage<unsigned char> &IMatching);
  void createImageMatching(vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching);
 
  void createImageMatching(vpImage<unsigned char> &IRef, vpImage<vpRGBa> &ICurrent, vpImage<vpRGBa> &IMatching);
  void createImageMatching(vpImage<vpRGBa> &ICurrent, vpImage<vpRGBa> &IMatching);
 
  void detect(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints,
              const vpRect &rectangle = vpRect());
  void detect(const vpImage<vpRGBa> &I_color, std::vector<cv::KeyPoint> &keyPoints, const vpRect &rectangle = vpRect());
  void detect(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, const cv::Mat &mask = cv::Mat());
  void detect(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
              const vpRect &rectangle = vpRect());
  void detect(const vpImage<vpRGBa> &I_color, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
              const vpRect &rectangle = vpRect());
  void detect(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
              const cv::Mat &mask = cv::Mat());
 
  void detectExtractAffine(const vpImage<unsigned char> &I, std::vector<std::vector<cv::KeyPoint> > &listOfKeypoints,
                           std::vector<cv::Mat> &listOfDescriptors,
                           std::vector<vpImage<unsigned char> > *listOfAffineI = NULL);
 
  void display(const vpImage<unsigned char> &IRef, const vpImage<unsigned char> &ICurrent, unsigned int size = 3);
  void display(const vpImage<unsigned char> &ICurrent, unsigned int size = 3, const vpColor &color = vpColor::green);
  void display(const vpImage<vpRGBa> &IRef, const vpImage<vpRGBa> &ICurrent, unsigned int size = 3);
  void display(const vpImage<vpRGBa> &ICurrent, unsigned int size = 3, const vpColor &color = vpColor::green);
 
  void displayMatching(const vpImage<unsigned char> &IRef, vpImage<unsigned char> &IMatching, unsigned int crossSize,
                       unsigned int lineThickness = 1, const vpColor &color = vpColor::green);
  void displayMatching(const vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching,
                       const std::vector<vpImagePoint> &ransacInliers = std::vector<vpImagePoint>(),
                       unsigned int crossSize = 3, unsigned int lineThickness = 1);
  void displayMatching(const vpImage<unsigned char> &IRef, vpImage<vpRGBa> &IMatching, unsigned int crossSize,
                       unsigned int lineThickness = 1, const vpColor &color = vpColor::green);
  void displayMatching(const vpImage<vpRGBa> &IRef, vpImage<vpRGBa> &IMatching, unsigned int crossSize,
                       unsigned int lineThickness = 1, const vpColor &color = vpColor::green);
  void displayMatching(const vpImage<vpRGBa> &ICurrent, vpImage<vpRGBa> &IMatching,
                       const std::vector<vpImagePoint> &ransacInliers = std::vector<vpImagePoint>(),
                       unsigned int crossSize = 3, unsigned int lineThickness = 1);
 
  void extract(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
               std::vector<cv::Point3f> *trainPoints = NULL);
  void extract(const vpImage<vpRGBa> &I_color, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
               std::vector<cv::Point3f> *trainPoints = NULL);
  void extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
               std::vector<cv::Point3f> *trainPoints = NULL);
  void extract(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
               double &elapsedTime, std::vector<cv::Point3f> *trainPoints = NULL);
  void extract(const vpImage<vpRGBa> &I_color, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
               double &elapsedTime, std::vector<cv::Point3f> *trainPoints = NULL);
  void extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors, double &elapsedTime,
               std::vector<cv::Point3f> *trainPoints = NULL);
 
  inline vpMatrix getCovarianceMatrix() const
  {
    if (!m_computeCovariance) {
      std::cout << "Warning : The covariance matrix has not been computed. "
                   "See setCovarianceComputation() to do it."
                << std::endl;
      return vpMatrix();
    }
 
    if (m_computeCovariance && !m_useRansacVVS) {
      std::cout << "Warning : The covariance matrix can only be computed "
                   "with a Virtual Visual Servoing approach."
                << std::endl
                << "Use setUseRansacVVS(true) to choose to use a pose "
                   "estimation method based on a Virtual Visual Servoing "
                   "approach."
                << std::endl;
      return vpMatrix();
    }
 
    return m_covarianceMatrix;
  }
 
  inline double getDetectionTime() const { return m_detectionTime; }
 
  inline cv::Ptr<cv::FeatureDetector> getDetector(const vpFeatureDetectorType &type) const
  {
    std::map<vpFeatureDetectorType, std::string>::const_iterator it_name = m_mapOfDetectorNames.find(type);
    if (it_name == m_mapOfDetectorNames.end()) {
      std::cerr << "Internal problem with the feature type and the "
                   "corresponding name!"
                << std::endl;
    }
 
    std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator findDetector =
        m_detectors.find(it_name->second);
    if (findDetector != m_detectors.end()) {
      return findDetector->second;
    }
 
    std::cerr << "Cannot find: " << it_name->second << std::endl;
    return cv::Ptr<cv::FeatureDetector>();
  }
 
  inline cv::Ptr<cv::FeatureDetector> getDetector(const std::string &name) const
  {
    std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator findDetector = m_detectors.find(name);
    if (findDetector != m_detectors.end()) {
      return findDetector->second;
    }
 
    std::cerr << "Cannot find: " << name << std::endl;
    return cv::Ptr<cv::FeatureDetector>();
  }
 
  inline std::map<vpFeatureDetectorType, std::string> getDetectorNames() const { return m_mapOfDetectorNames; }
 
  inline double getExtractionTime() const { return m_extractionTime; }
 
  inline cv::Ptr<cv::DescriptorExtractor> getExtractor(const vpFeatureDescriptorType &type) const
  {
    std::map<vpFeatureDescriptorType, std::string>::const_iterator it_name = m_mapOfDescriptorNames.find(type);
    if (it_name == m_mapOfDescriptorNames.end()) {
      std::cerr << "Internal problem with the feature type and the "
                   "corresponding name!"
                << std::endl;
    }
 
    std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator findExtractor =
        m_extractors.find(it_name->second);
    if (findExtractor != m_extractors.end()) {
      return findExtractor->second;
    }
 
    std::cerr << "Cannot find: " << it_name->second << std::endl;
    return cv::Ptr<cv::DescriptorExtractor>();
  }
 
  inline cv::Ptr<cv::DescriptorExtractor> getExtractor(const std::string &name) const
  {
    std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator findExtractor = m_extractors.find(name);
    if (findExtractor != m_extractors.end()) {
      return findExtractor->second;
    }
 
    std::cerr << "Cannot find: " << name << std::endl;
    return cv::Ptr<cv::DescriptorExtractor>();
  }
 
  inline std::map<vpFeatureDescriptorType, std::string> getExtractorNames() const { return m_mapOfDescriptorNames; }
 
  inline vpImageFormatType getImageFormat() const { return m_imageFormat; }
 
  inline double getMatchingTime() const { return m_matchingTime; }
 
  inline cv::Ptr<cv::DescriptorMatcher> getMatcher() const { return m_matcher; }
 
  inline std::vector<cv::DMatch> getMatches() const { return m_filteredMatches; }
 
  inline std::vector<std::pair<cv::KeyPoint, cv::KeyPoint> > getMatchQueryToTrainKeyPoints() const
  {
    std::vector<std::pair<cv::KeyPoint, cv::KeyPoint> > matchQueryToTrainKeyPoints(m_filteredMatches.size());
    for (size_t i = 0; i < m_filteredMatches.size(); i++) {
      matchQueryToTrainKeyPoints.push_back(
          std::pair<cv::KeyPoint, cv::KeyPoint>(m_queryFilteredKeyPoints[(size_t)m_filteredMatches[i].queryIdx],
                                                m_trainKeyPoints[(size_t)m_filteredMatches[i].trainIdx]));
    }
    return matchQueryToTrainKeyPoints;
  }
 
  inline unsigned int getNbImages() const { return static_cast<unsigned int>(m_mapOfImages.size()); }
 
  void getObjectPoints(std::vector<cv::Point3f> &objectPoints) const;
  void getObjectPoints(std::vector<vpPoint> &objectPoints) const;
 
  inline double getPoseTime() const { return m_poseTime; }
 
  inline cv::Mat getQueryDescriptors() const { return m_queryDescriptors; }
 
  void getQueryKeyPoints(std::vector<cv::KeyPoint> &keyPoints, bool matches = true) const;
  void getQueryKeyPoints(std::vector<vpImagePoint> &keyPoints, bool matches = true) const;
 
  inline std::vector<vpImagePoint> getRansacInliers() const { return m_ransacInliers; }
 
  inline std::vector<vpImagePoint> getRansacOutliers() const { return m_ransacOutliers; }
 
  inline cv::Mat getTrainDescriptors() const { return m_trainDescriptors; }
 
  void getTrainKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const;
  void getTrainKeyPoints(std::vector<vpImagePoint> &keyPoints) const;
 
  void getTrainPoints(std::vector<cv::Point3f> &points) const;
  void getTrainPoints(std::vector<vpPoint> &points) const;
 
  void initMatcher(const std::string &matcherName);
 
  void insertImageMatching(const vpImage<unsigned char> &IRef, const vpImage<unsigned char> &ICurrent,
                           vpImage<unsigned char> &IMatching);
  void insertImageMatching(const vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching);
 
  void insertImageMatching(const vpImage<vpRGBa> &IRef, const vpImage<vpRGBa> &ICurrent, vpImage<vpRGBa> &IMatching);
  void insertImageMatching(const vpImage<vpRGBa> &ICurrent, vpImage<vpRGBa> &IMatching);
 
  void loadConfigFile(const std::string &configFile);
 
  void loadLearningData(const std::string &filename, bool binaryMode = false, bool append = false);
 
  void match(const cv::Mat &trainDescriptors, const cv::Mat &queryDescriptors, std::vector<cv::DMatch> &matches,
             double &elapsedTime);
 
  unsigned int matchPoint(const vpImage<unsigned char> &I);
  unsigned int matchPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP, unsigned int height,
                          unsigned int width);
  unsigned int matchPoint(const vpImage<unsigned char> &I, const vpRect &rectangle);
 
  unsigned int matchPoint(const std::vector<cv::KeyPoint> &queryKeyPoints, const cv::Mat &queryDescriptors);
  bool matchPoint(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
                  bool (*func)(const vpHomogeneousMatrix &) = NULL, const vpRect &rectangle = vpRect());
  bool matchPoint(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
                  double &error, double &elapsedTime, bool (*func)(const vpHomogeneousMatrix &) = NULL,
                  const vpRect &rectangle = vpRect());
 
  bool matchPointAndDetect(const vpImage<unsigned char> &I, vpRect &boundingBox, vpImagePoint &centerOfGravity,
                           const bool isPlanarObject = true, std::vector<vpImagePoint> *imPts1 = NULL,
                           std::vector<vpImagePoint> *imPts2 = NULL, double *meanDescriptorDistance = NULL,
                           double *detectionScore = NULL, const vpRect &rectangle = vpRect());
 
  bool matchPointAndDetect(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
                           double &error, double &elapsedTime, vpRect &boundingBox, vpImagePoint &centerOfGravity,
                           bool (*func)(const vpHomogeneousMatrix &) = NULL, const vpRect &rectangle = vpRect());
 
  unsigned int matchPoint(const vpImage<vpRGBa> &I_color);
  unsigned int matchPoint(const vpImage<vpRGBa> &I_color, const vpImagePoint &iP, unsigned int height,
                          unsigned int width);
  unsigned int matchPoint(const vpImage<vpRGBa> &I_color, const vpRect &rectangle);
 
  bool matchPoint(const vpImage<vpRGBa> &I_color, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
                  bool (*func)(const vpHomogeneousMatrix &) = NULL, const vpRect &rectangle = vpRect());
  bool matchPoint(const vpImage<vpRGBa> &I_color, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
                  double &error, double &elapsedTime, bool (*func)(const vpHomogeneousMatrix &) = NULL,
                  const vpRect &rectangle = vpRect());
 
  void reset();
 
  void saveLearningData(const std::string &filename, bool binaryMode = false, bool saveTrainingImages = true);
 
  inline void setCovarianceComputation(const bool &flag)
  {
    m_computeCovariance = flag;
    if (!m_useRansacVVS) {
      std::cout << "Warning : The covariance matrix can only be computed "
                   "with a Virtual Visual Servoing approach."
                << std::endl
                << "Use setUseRansacVVS(true) to choose to use a pose "
                   "estimation method based on a Virtual "
                   "Visual Servoing approach."
                << std::endl;
    }
  }
 
  inline void setDetectionMethod(const vpDetectionMethodType &method) { m_detectionMethod = method; }
 
  inline void setDetector(const vpFeatureDetectorType &detectorType)
  {
    m_detectorNames.clear();
    m_detectorNames.push_back(m_mapOfDetectorNames[detectorType]);
    m_detectors.clear();
    initDetector(m_mapOfDetectorNames[detectorType]);
  }
 
  inline void setDetector(const std::string &detectorName)
  {
    m_detectorNames.clear();
    m_detectorNames.push_back(detectorName);
    m_detectors.clear();
    initDetector(detectorName);
  }
 
#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
  template <typename T1, typename T2, typename T3>
  inline void setDetectorParameter(const T1 detectorName, const T2 parameterName, const T3 value)
  {
    if (m_detectors.find(detectorName) != m_detectors.end()) {
      m_detectors[detectorName]->set(parameterName, value);
    }
  }
#endif
 
  inline void setDetectors(const std::vector<std::string> &detectorNames)
  {
    m_detectorNames.clear();
    m_detectors.clear();
    m_detectorNames = detectorNames;
    initDetectors(m_detectorNames);
  }
 
  inline void setExtractor(const vpFeatureDescriptorType &extractorType)
  {
    m_extractorNames.clear();
    m_extractorNames.push_back(m_mapOfDescriptorNames[extractorType]);
    m_extractors.clear();
    initExtractor(m_mapOfDescriptorNames[extractorType]);
  }
 
  inline void setExtractor(const std::string &extractorName)
  {
    m_extractorNames.clear();
    m_extractorNames.push_back(extractorName);
    m_extractors.clear();
    initExtractor(extractorName);
  }
 
#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
  template <typename T1, typename T2, typename T3>
  inline void setExtractorParameter(const T1 extractorName, const T2 parameterName, const T3 value)
  {
    if (m_extractors.find(extractorName) != m_extractors.end()) {
      m_extractors[extractorName]->set(parameterName, value);
    }
  }
#endif
 
  inline void setExtractors(const std::vector<std::string> &extractorNames)
  {
    m_extractorNames.clear();
    m_extractorNames = extractorNames;
    m_extractors.clear();
    initExtractors(m_extractorNames);
  }
 
  inline void setImageFormat(const vpImageFormatType &imageFormat) { m_imageFormat = imageFormat; }
 
  inline void setMatcher(const std::string &matcherName)
  {
    m_matcherName = matcherName;
    initMatcher(m_matcherName);
  }
 
  void setMaxFeatures(int maxFeatures) { m_maxFeatures = maxFeatures; }
 
  inline void setFilterMatchingType(const vpFilterMatchingType &filterType)
  {
    m_filterType = filterType;
 
    // Use k-nearest neighbors (knn) to retrieve the two best matches for a
    // keypoint  So this is useful only for ratioDistanceThreshold method
    if (filterType == ratioDistanceThreshold || filterType == stdAndRatioDistanceThreshold) {
      m_useKnn = true;
 
#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
      if (m_matcher != NULL && m_matcherName == "BruteForce") {
        // if a matcher is already initialized, disable the crossCheck
        // because it will not work with knnMatch
        m_matcher->set("crossCheck", false);
      }
#endif
    } else {
      m_useKnn = false;
 
#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
      if (m_matcher != NULL && m_matcherName == "BruteForce") {
        // if a matcher is already initialized, set the crossCheck mode if
        // necessary
        m_matcher->set("crossCheck", m_useBruteForceCrossCheck);
      }
#endif
    }
  }
 
  inline void setMatchingFactorThreshold(const double factor)
  {
    if (factor > 0.0) {
      m_matchingFactorThreshold = factor;
    } else {
      throw vpException(vpException::badValue, "The factor must be positive.");
    }
  }
 
  inline void setMatchingRatioThreshold(double ratio)
  {
    if (ratio > 0.0 && (ratio < 1.0 || std::fabs(ratio - 1.0) < std::numeric_limits<double>::epsilon())) {
      m_matchingRatioThreshold = ratio;
    } else {
      throw vpException(vpException::badValue, "The ratio must be in the interval ]0 ; 1].");
    }
  }
 
  inline void setRansacConsensusPercentage(double percentage)
  {
    if (percentage > 0.0 &&
        (percentage < 100.0 || std::fabs(percentage - 100.0) < std::numeric_limits<double>::epsilon())) {
      m_ransacConsensusPercentage = percentage;
    } else {
      throw vpException(vpException::badValue, "The percentage must be in the interval ]0 ; 100].");
    }
  }
 
  inline void setRansacFilterFlag(const vpPose::RANSAC_FILTER_FLAGS &flag) { m_ransacFilterFlag = flag; }
 
  inline void setRansacIteration(int nbIter)
  {
    if (nbIter > 0) {
      m_nbRansacIterations = nbIter;
    } else {
      throw vpException(vpException::badValue, "The number of iterations must be greater than zero.");
    }
  }
 
  inline void setRansacParallel(bool parallel) { m_ransacParallel = parallel; }
 
  inline void setRansacParallelNbThreads(unsigned int nthreads) { m_ransacParallelNbThreads = nthreads; }
 
  inline void setRansacReprojectionError(double reprojectionError)
  {
    if (reprojectionError > 0.0) {
      m_ransacReprojectionError = reprojectionError;
    } else {
      throw vpException(vpException::badValue, "The Ransac reprojection "
                                               "threshold must be positive "
                                               "as we deal with distance.");
    }
  }
 
  inline void setRansacMinInlierCount(int minCount)
  {
    if (minCount > 0) {
      m_nbRansacMinInlierCount = minCount;
    } else {
      throw vpException(vpException::badValue, "The minimum number of inliers must be greater than zero.");
    }
  }
 
  inline void setRansacThreshold(double threshold)
  {
    if (threshold > 0.0) {
      m_ransacThreshold = threshold;
    } else {
      throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
    }
  }
 
  inline void setUseAffineDetection(bool useAffine) { m_useAffineDetection = useAffine; }
 
#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
  inline void setUseBruteForceCrossCheck(bool useCrossCheck)
  {
    // Only available with BruteForce and with k=1 (i.e not used with a
    // ratioDistanceThreshold method)
    if (m_matcher != NULL && !m_useKnn && m_matcherName == "BruteForce") {
      m_matcher->set("crossCheck", useCrossCheck);
    } else if (m_matcher != NULL && m_useKnn && m_matcherName == "BruteForce") {
      std::cout << "Warning, you try to set the crossCheck parameter with a "
                   "BruteForce matcher but knn is enabled";
      std::cout << " (the filtering method uses a ratio constraint)" << std::endl;
    }
  }
#endif
 
  inline void setUseMatchTrainToQuery(bool useMatchTrainToQuery) { m_useMatchTrainToQuery = useMatchTrainToQuery; }
 
  inline void setUseRansacConsensusPercentage(bool usePercentage) { m_useConsensusPercentage = usePercentage; }
 
  inline void setUseRansacVVS(bool ransacVVS) { m_useRansacVVS = ransacVVS; }
 
  inline void setUseSingleMatchFilter(bool singleMatchFilter) { m_useSingleMatchFilter = singleMatchFilter; }
 
private:
  bool m_computeCovariance;
  vpMatrix m_covarianceMatrix;
  int m_currentImageId;
  vpDetectionMethodType m_detectionMethod;
  double m_detectionScore;
  double m_detectionThreshold;
  double m_detectionTime;
  std::vector<std::string> m_detectorNames;
  // with a key based upon the detector name.
  std::map<std::string, cv::Ptr<cv::FeatureDetector> > m_detectors;
  double m_extractionTime;
  std::vector<std::string> m_extractorNames;
  // with a key based upon the extractor name.
  std::map<std::string, cv::Ptr<cv::DescriptorExtractor> > m_extractors;
  std::vector<cv::DMatch> m_filteredMatches;
  vpFilterMatchingType m_filterType;
  vpImageFormatType m_imageFormat;
  std::vector<std::vector<cv::DMatch> > m_knnMatches;
  std::map<vpFeatureDescriptorType, std::string> m_mapOfDescriptorNames;
  std::map<vpFeatureDetectorType, std::string> m_mapOfDetectorNames;
  std::map<int, int> m_mapOfImageId;
  std::map<int, vpImage<unsigned char> > m_mapOfImages;
  cv::Ptr<cv::DescriptorMatcher> m_matcher;
  std::string m_matcherName;
  std::vector<cv::DMatch> m_matches;
  double m_matchingFactorThreshold;
  double m_matchingRatioThreshold;
  double m_matchingTime;
  std::vector<std::pair<cv::KeyPoint, cv::Point3f> > m_matchRansacKeyPointsToPoints;
  int m_nbRansacIterations;
  int m_nbRansacMinInlierCount;
  std::vector<cv::Point3f> m_objectFilteredPoints;
  double m_poseTime;
  cv::Mat m_queryDescriptors;
  std::vector<cv::KeyPoint> m_queryFilteredKeyPoints;
  std::vector<cv::KeyPoint> m_queryKeyPoints;
  double m_ransacConsensusPercentage;
  vpPose::RANSAC_FILTER_FLAGS m_ransacFilterFlag;
  std::vector<vpImagePoint> m_ransacInliers;
  std::vector<vpImagePoint> m_ransacOutliers;
  bool m_ransacParallel;
  unsigned int m_ransacParallelNbThreads;
  double m_ransacReprojectionError;
  double m_ransacThreshold;
  // detected in the train images).
  cv::Mat m_trainDescriptors;
  std::vector<cv::KeyPoint> m_trainKeyPoints;
  std::vector<cv::Point3f> m_trainPoints;
  std::vector<vpPoint> m_trainVpPoints;
  bool m_useAffineDetection;
#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
  bool m_useBruteForceCrossCheck;
#endif
  bool m_useConsensusPercentage;
  bool m_useKnn;
  bool m_useMatchTrainToQuery;
  bool m_useRansacVVS;
  bool m_useSingleMatchFilter;
  vpImage<unsigned char> m_I;
  int m_maxFeatures;
 
  void affineSkew(double tilt, double phi, cv::Mat &img, cv::Mat &mask, cv::Mat &Ai);
 
  double computePoseEstimationError(const std::vector<std::pair<cv::KeyPoint, cv::Point3f> > &matchKeyPoints,
                                    const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo_est);
 
  void filterMatches();
 
  void init();
  void initDetector(const std::string &detectorNames);
  void initDetectors(const std::vector<std::string> &detectorNames);
 
  void initExtractor(const std::string &extractorName);
  void initExtractors(const std::vector<std::string> &extractorNames);
 
  void initFeatureNames();
 
  inline size_t myKeypointHash(const cv::KeyPoint &kp)
  {
    size_t _Val = 2166136261U, scale = 16777619U;
    Cv32suf u;
    u.f = kp.pt.x;
    _Val = (scale * _Val) ^ u.u;
    u.f = kp.pt.y;
    _Val = (scale * _Val) ^ u.u;
    u.f = kp.size;
    _Val = (scale * _Val) ^ u.u;
    // As the keypoint angle can be computed for certain type of keypoint only
    // when extracting  the corresponding descriptor, the angle field is not
    // taking into account for the hash
    //    u.f = kp.angle; _Val = (scale * _Val) ^ u.u;
    u.f = kp.response;
    _Val = (scale * _Val) ^ u.u;
    _Val = (scale * _Val) ^ ((size_t)kp.octave);
    _Val = (scale * _Val) ^ ((size_t)kp.class_id);
    return _Val;
  }
 
#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
  /*
   * Adapts a detector to detect points over multiple levels of a Gaussian
   * pyramid. Useful for detectors that are not inherently scaled.
   * From OpenCV 2.4.11 source code.
   */
  class PyramidAdaptedFeatureDetector : public cv::FeatureDetector
  {
  public:
    // maxLevel - The 0-based index of the last pyramid layer
    PyramidAdaptedFeatureDetector(const cv::Ptr<cv::FeatureDetector> &detector, int maxLevel = 2);
 
    // TODO implement read/write
    virtual bool empty() const;
 
  protected:
    virtual void detect(cv::InputArray image, CV_OUT std::vector<cv::KeyPoint> &keypoints,
                        cv::InputArray mask = cv::noArray());
    virtual void detectImpl(const cv::Mat &image, std::vector<cv::KeyPoint> &keypoints,
                            const cv::Mat &mask = cv::Mat()) const;
 
    cv::Ptr<cv::FeatureDetector> detector;
    int maxLevel;
  };
 
  /*
   * A class filters a vector of keypoints.
   * Because now it is difficult to provide a convenient interface for all
   * usage scenarios of the keypoints filter class, it has only several needed
   * by now static methods.
   */
  class KeyPointsFilter
  {
  public:
    KeyPointsFilter() {}
 
    /*
     * Remove keypoints within borderPixels of an image edge.
     */
    static void runByImageBorder(std::vector<cv::KeyPoint> &keypoints, cv::Size imageSize, int borderSize);
    /*
     * Remove keypoints of sizes out of range.
     */
    static void runByKeypointSize(std::vector<cv::KeyPoint> &keypoints, float minSize, float maxSize = FLT_MAX);
    /*
     * Remove keypoints from some image by mask for pixels of this image.
     */
    static void runByPixelsMask(std::vector<cv::KeyPoint> &keypoints, const cv::Mat &mask);
    /*
     * Remove duplicated keypoints.
     */
    static void removeDuplicated(std::vector<cv::KeyPoint> &keypoints);
 
    /*
     * Retain the specified number of the best keypoints (according to the
     * response)
     */
    static void retainBest(std::vector<cv::KeyPoint> &keypoints, int npoints);
  };
 
#endif
};
 
#endif
#endif