vpKeyPoint.h

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 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
 * ("GPL") version 2 as published by the Free Software Foundation.
 * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
 *
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * 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.
 *
 * Authors:
 * Souriya Trinh
 *
 *****************************************************************************/
#ifndef __vpKeyPoint_h__
#define __vpKeyPoint_h__

#include <algorithm>    // std::transform
#include <vector>       // std::vector
#include <stdlib.h>     // srand, rand
#include <time.h>       // time
#include <fstream>      // std::ofstream
#include <numeric>      // std::accumulate
#include <float.h>      // DBL_MAX
#include <map>          // std::map
#include <limits>

#include <visp/vpConfig.h>
#include <visp/vpBasicKeyPoint.h>
#include <visp/vpImageConvert.h>
#include <visp/vpPoint.h>
#include <visp/vpDisplay.h>
#include <visp/vpPlane.h>
#include <visp/vpPixelMeterConversion.h>
#include <visp/vpMbEdgeTracker.h>
#include <visp/vpIoTools.h>
#include <visp/vpPose.h>
#include <visp/vpImageIo.h>
#include <visp/vpPolygon.h>
#include <visp/vpXmlConfigParserKeyPoint.h>
#include <visp/vpConvert.h>

// Require at least OpenCV >= 2.1.1
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)

#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#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

#ifdef VISP_HAVE_XML2
#  include <libxml/xmlwriter.h>
#endif

class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint {

public:

  typedef enum {
    constantFactorDistanceThreshold,  
    stdDistanceThreshold,             
    ratioDistanceThreshold,           
    stdAndRatioDistanceThreshold,     
    noFilterMatching                  
  } vpFilterMatchingType;

  typedef enum {
    detectionThreshold,  
    detectionScore       
  } vpDetectionMethodType;


  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, const unsigned int height, const unsigned int width);
  unsigned int buildReference(const vpImage<unsigned char> &I, const vpRect& rectangle);

  void buildReference(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &trainKeyPoint,
                      std::vector<cv::Point3f> &points3f, bool append=false);
  void buildReference(const vpImage<unsigned char> &I, const std::vector<cv::KeyPoint> &trainKeyPoint,
                      const cv::Mat &trainDescriptors, const std::vector<cv::Point3f> &points3f, bool append=false);

  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> &candidate, std::vector<vpPolygon> &polygons, 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> &candidate, std::vector<vpPolygon> &polygons, std::vector<std::vector<vpPoint> > &roisPt,
      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)(vpHomogeneousMatrix *)=NULL);

  bool computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo,
               std::vector<vpPoint> &inliers, double &elapsedTime, bool (*func)(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 detect(const vpImage<unsigned char> &I, 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 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 extract(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors, double &elapsedTime);
  void extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors, double &elapsedTime);

  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 double getExtractionTime() const {
    return m_extractionTime;
  }

  inline double getMatchingTime() const {
    return m_matchingTime;
  }

  inline std::vector<cv::DMatch> getMatches() const {
    return m_filteredMatches;
  }

  inline std::vector<std::pair<cv::KeyPoint, cv::KeyPoint> > getMatchQueryToTrainKeyPoints() const {
    return m_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) const;
  void getQueryKeyPoints(std::vector<vpImagePoint> &keyPoints) 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);

#ifdef VISP_HAVE_XML2
  void loadConfigFile(const std::string &configFile);
#endif

  void loadLearningData(const std::string &filename, const bool binaryMode=false, const 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,
                          const unsigned int height, const unsigned int width);
  unsigned int matchPoint(const vpImage<unsigned char> &I, const vpRect& rectangle);

  bool matchPoint(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
                  double &error, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)=NULL);

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

  bool matchPointAndDetect(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
                  double &error, double &elapsedTime, vpRect &boundingBox, vpImagePoint &centerOfGravity,
                  bool (*func)(vpHomogeneousMatrix *)=NULL);

  void saveLearningData(const std::string &filename, const bool binaryMode=false, const 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 std::string &detectorName) {
    m_detectorNames.clear();
    m_detectorNames.push_back(detectorName);
    m_detectors.clear();
    initDetector(detectorName);
  }

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

  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 std::string &extractorName) {
    m_extractorNames.clear();
    m_extractorNames.push_back(extractorName);
    m_extractors.clear();
    initExtractor(extractorName);
  }

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

  inline void setExtractors(const std::vector<std::string> &extractorNames) {
    m_extractorNames.clear();
    m_extractorNames = extractorNames;
    m_extractors.clear();
    initExtractors(m_extractorNames);
  }

  inline void setMatcher(const std::string &matcherName) {
    m_matcherName = matcherName;
    initMatcher(m_matcherName);
  }

  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;
    } else {
      m_useKnn = false;
    }
  }

  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(const 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(const 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 setRansacIteration(const int nbIter) {
    if(nbIter > 0) {
      m_nbRansacIterations = nbIter;
    } else {
      throw vpException(vpException::badValue, "The number of iterations must be greater than zero.");
    }
  }

  inline void setRansacReprojectionError(const 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(const 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(const 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(const bool useAffine) {
    m_useAffineDetection = useAffine;
  }

#if (VISP_HAVE_OPENCV_VERSION >= 0x020400)

  inline void setUseBruteForceCrossCheck(const 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);
    }
  }
#endif

  inline void setUseRansacConsensusPercentage(const bool usePercentage) {
    m_useConsensusPercentage = usePercentage;
  }

  inline void setUseRansacVVS(const bool ransacVVS) {
    m_useRansacVVS = ransacVVS;
  }

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;
  std::vector<std::vector<cv::DMatch> > m_knnMatches;
  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::KeyPoint> > m_matchQueryToTrainKeyPoints;
  std::vector<std::pair<cv::KeyPoint, cv::Point3f> > m_matchRansacKeyPointsToPoints;
  std::vector<std::pair<cv::KeyPoint, cv::KeyPoint> > m_matchRansacQueryToTrainKeyPoints;
  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;
  std::vector<vpImagePoint> m_ransacInliers;
  std::vector<vpImagePoint> m_ransacOutliers;
  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;
  bool m_useBruteForceCrossCheck;
  bool m_useConsensusPercentage;
  bool m_useKnn;
  bool m_useRansacVVS;


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

//TODO: Try to implement a pyramidal feature detection
//#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
//  void pyramidFeatureDetection(cv::Ptr<cv::FeatureDetector> &detector, const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask,
//      const int maxLevel=2);
//  void runByPixelsMask(std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask);
//#endif
};

#endif
#endif