vpKeyPoint.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2015 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://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.
 *
 * Authors:
 * Souriya Trinh
 *
 *****************************************************************************/

#include <limits>
#include <iomanip>
#include <stdint.h> //uint32_t ; works also with >= VS2010 / _MSC_VER >= 1600

#include <visp3/vision/vpKeyPoint.h>
#include <visp3/core/vpIoTools.h>

#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)

#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
#  include <opencv2/calib3d/calib3d.hpp>
#endif


//TODO: test saveLearningData with little / big endian platform
//Specific Type transformation functions
//   Convert a list of cv::DMatch to a cv::DMatch (extract the first cv::DMatch, the nearest neighbor).
//
//   \param knnMatches : List of cv::DMatch.
//   \return The nearest neighbor.
// */
inline cv::DMatch knnToDMatch(const std::vector<cv::DMatch> &knnMatches) {
  if(knnMatches.size() > 0) {
    return knnMatches[0];
  }

  return cv::DMatch();
}

//   Convert a cv::DMatch to an index (extract the train index).
//
//   \param match : Point to convert in ViSP type.
//   \return The train index.
// */
inline vpImagePoint matchRansacToVpImage(const std::pair<cv::KeyPoint, cv::Point3f> &pair) {
  return vpImagePoint(pair.first.pt.y, pair.first.pt.x);
}

bool isBigEndian() {
  union {
    uint32_t i;
    char c[4];
  } bint = { 0x01020304 };

  return bint.c[0] == 1;
}

uint16_t reverse16bits(const uint16_t n) {
  unsigned char *np = (unsigned char *) &n;

  return ((uint16_t) np[0] << 8) | (uint16_t) np[1];
}

uint32_t reverse32bits(const uint32_t n) {
  unsigned char *np = (unsigned char *) &n;

  return ((uint32_t) np[0] << 24) | ((uint32_t) np[1] << 16)
       | ((uint32_t) np[2] << 8)  | (uint32_t) np[3];
}

float reverseFloat(const float f) {
  union {
    float f;
    unsigned char b[4];
  } dat1, dat2;

  dat1.f = f;
  dat2.b[0] = dat1.b[3];
  dat2.b[1] = dat1.b[2];
  dat2.b[2] = dat1.b[1];
  dat2.b[3] = dat1.b[0];
  return dat2.f;
}

double reverseDouble(const double d) {
  union {
    double d;
    unsigned char b[8];
  } dat1, dat2;

  dat1.d = d;
  dat2.b[0] = dat1.b[7];
  dat2.b[1] = dat1.b[6];
  dat2.b[2] = dat1.b[5];
  dat2.b[3] = dat1.b[4];
  dat2.b[4] = dat1.b[3];
  dat2.b[5] = dat1.b[2];
  dat2.b[6] = dat1.b[1];
  dat2.b[7] = dat1.b[0];
  return dat2.d;
}

void writeBinaryUShortLE(std::ofstream &file, const unsigned short ushort_value) {
  if(isBigEndian()) {
    //Reverse bytes order to little endian
    uint16_t reverse_ushort = reverse16bits(ushort_value);
    file.write((char *)(&reverse_ushort), sizeof(reverse_ushort));
  } else {
    file.write((char *)(&ushort_value), sizeof(ushort_value));
  }
}

void writeBinaryShortLE(std::ofstream &file, const short short_value) {
  if(isBigEndian()) {
    //Reverse bytes order to little endian
    uint16_t reverse_short = reverse16bits((uint16_t) short_value);
    file.write((char *)(&reverse_short), sizeof(reverse_short));
  } else {
    file.write((char *)(&short_value), sizeof(short_value));
  }
}

void writeBinaryUIntLE(std::ofstream &file, const unsigned int uint_value) {
  if(isBigEndian()) {
    //Reverse bytes order to little endian
    if(sizeof(uint_value) == 4) {
      uint32_t reverse_uint = reverse32bits(uint_value);
      file.write((char *)(&reverse_uint), sizeof(reverse_uint));
    } else {
      uint16_t reverse_uint = reverse16bits(uint_value);
      file.write((char *)(&reverse_uint), sizeof(reverse_uint));
    }
  } else {
    file.write((char *)(&uint_value), sizeof(uint_value));
  }
}

void writeBinaryIntLE(std::ofstream &file, const int int_value) {
  if(isBigEndian()) {
    //Reverse bytes order to little endian
    if(sizeof(int_value) == 4) {
      uint32_t reverse_int = reverse32bits((uint32_t) int_value);
      file.write((char *)(&reverse_int), sizeof(reverse_int));
    } else {
      uint16_t reverse_int = reverse16bits((uint16_t) int_value);
      file.write((char *)(&reverse_int), sizeof(reverse_int));
    }
  } else {
    file.write((char *)(&int_value), sizeof(int_value));
  }
}

void writeBinaryFloatLE(std::ofstream &file, const float float_value) {
  if(isBigEndian()) {
    //Reverse bytes order to little endian
    float reverse_float = reverseFloat(float_value);
    file.write((char *)(&reverse_float), sizeof(reverse_float));
  } else {
    file.write((char *)(&float_value), sizeof(float_value));
  }
}

void writeBinaryDoubleLE(std::ofstream &file, const double double_value) {
  if(isBigEndian()) {
    //Reverse bytes order to little endian
    double reverse_double = reverseDouble(double_value);
    file.write((char *)(&reverse_double), sizeof(reverse_double));
  } else {
    file.write((char *)(&double_value), sizeof(double_value));
  }
}

vpKeyPoint::vpKeyPoint(const std::string &detectorName, const std::string &extractorName,
                       const std::string &matcherName, const vpFilterMatchingType &filterType)
  : m_computeCovariance(false), m_covarianceMatrix(), m_currentImageId(0), m_detectionMethod(detectionScore),
    m_detectionScore(0.15), m_detectionThreshold(100.0), m_detectionTime(0.), m_detectorNames(),
    m_detectors(), m_extractionTime(0.), m_extractorNames(), m_extractors(), m_filteredMatches(), m_filterType(filterType),
    m_imageFormat(jpgImageFormat), m_knnMatches(), m_mapOfImageId(), m_mapOfImages(),
    m_matcher(), m_matcherName(matcherName),
    m_matches(), m_matchingFactorThreshold(2.0), m_matchingRatioThreshold(0.85), m_matchingTime(0.),
    m_matchRansacKeyPointsToPoints(), m_nbRansacIterations(200), m_nbRansacMinInlierCount(100), m_objectFilteredPoints(),
    m_poseTime(0.), m_queryDescriptors(), m_queryFilteredKeyPoints(), m_queryKeyPoints(),
    m_ransacConsensusPercentage(20.0), m_ransacInliers(), m_ransacOutliers(), m_ransacReprojectionError(6.0),
    m_ransacThreshold(0.01), m_trainDescriptors(), m_trainKeyPoints(), m_trainPoints(),
    m_trainVpPoints(), m_useAffineDetection(false),
    #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
    m_useBruteForceCrossCheck(true),
    #endif
    m_useConsensusPercentage(false),
    m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true), m_useSingleMatchFilter(true)
{
  //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;
  }

  m_detectorNames.push_back(detectorName);
  m_extractorNames.push_back(extractorName);

  init();
}

vpKeyPoint::vpKeyPoint(const std::vector<std::string> &detectorNames, const std::vector<std::string> &extractorNames,
                       const std::string &matcherName, const vpFilterMatchingType &filterType)
  : m_computeCovariance(false), m_covarianceMatrix(), m_currentImageId(0), m_detectionMethod(detectionScore),
    m_detectionScore(0.15), m_detectionThreshold(100.0), m_detectionTime(0.), m_detectorNames(detectorNames),
    m_detectors(), m_extractionTime(0.), m_extractorNames(extractorNames), m_extractors(), m_filteredMatches(),
    m_filterType(filterType), m_imageFormat(jpgImageFormat), m_knnMatches(), m_mapOfImageId(), m_mapOfImages(),
    m_matcher(),
    m_matcherName(matcherName), m_matches(), m_matchingFactorThreshold(2.0), m_matchingRatioThreshold(0.85), m_matchingTime(0.),
    m_matchRansacKeyPointsToPoints(), m_nbRansacIterations(200), m_nbRansacMinInlierCount(100), m_objectFilteredPoints(),
    m_poseTime(0.), m_queryDescriptors(), m_queryFilteredKeyPoints(), m_queryKeyPoints(),
    m_ransacConsensusPercentage(20.0), m_ransacInliers(), m_ransacOutliers(), m_ransacReprojectionError(6.0),
    m_ransacThreshold(0.01), m_trainDescriptors(), m_trainKeyPoints(), m_trainPoints(),
    m_trainVpPoints(), m_useAffineDetection(false),
    #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
    m_useBruteForceCrossCheck(true),
    #endif
    m_useConsensusPercentage(false),
    m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true), m_useSingleMatchFilter(true)
{
  //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;
  }

  init();
}

void vpKeyPoint::affineSkew(double tilt, double phi, cv::Mat& img,
    cv::Mat& mask, cv::Mat& Ai) {
  int h = img.rows;
  int w = img.cols;

  mask = cv::Mat(h, w, CV_8UC1, cv::Scalar(255));

  cv::Mat A = cv::Mat::eye(2, 3, CV_32F);

  //if (phi != 0.0) {
  if (std::fabs(phi) > std::numeric_limits<double>::epsilon()) {
    phi *= M_PI / 180.;
    double s = sin(phi);
    double c = cos(phi);

    A = (cv::Mat_<float>(2, 2) << c, -s, s, c);

    cv::Mat corners = (cv::Mat_<float>(4, 2) << 0, 0, w, 0, w, h, 0, h);
    cv::Mat tcorners = corners * A.t();
    cv::Mat tcorners_x, tcorners_y;
    tcorners.col(0).copyTo(tcorners_x);
    tcorners.col(1).copyTo(tcorners_y);
    std::vector<cv::Mat> channels;
    channels.push_back(tcorners_x);
    channels.push_back(tcorners_y);
    cv::merge(channels, tcorners);

    cv::Rect rect = cv::boundingRect(tcorners);
    A = (cv::Mat_<float>(2, 3) << c, -s, -rect.x, s, c, -rect.y);

    cv::warpAffine(img, img, A, cv::Size(rect.width, rect.height),
        cv::INTER_LINEAR, cv::BORDER_REPLICATE);
  }
  //if (tilt != 1.0) {
  if (std::fabs(tilt - 1.0) > std::numeric_limits<double>::epsilon()) {
    double s = 0.8 * sqrt(tilt * tilt - 1);
    cv::GaussianBlur(img, img, cv::Size(0, 0), s, 0.01);
    cv::resize(img, img, cv::Size(0, 0), 1.0 / tilt, 1.0, cv::INTER_NEAREST);
    A.row(0) = A.row(0) / tilt;
  }
  //if (tilt != 1.0 || phi != 0.0) {
  if (std::fabs(tilt - 1.0) > std::numeric_limits<double>::epsilon() || std::fabs(phi) > std::numeric_limits<double>::epsilon() ) {
    h = img.rows;
    w = img.cols;
    cv::warpAffine(mask, mask, A, cv::Size(w, h), cv::INTER_NEAREST);
  }
  cv::invertAffineTransform(A, Ai);
}

unsigned int vpKeyPoint::buildReference(const vpImage<unsigned char> &I) {
  return buildReference(I, vpRect());
}

unsigned int vpKeyPoint::buildReference(const vpImage<unsigned char> &I,
                                        const vpImagePoint &iP,
                                        const unsigned int height, const unsigned int width) {

  return buildReference(I, vpRect(iP, width, height));
}

unsigned int vpKeyPoint::buildReference(const vpImage<unsigned char> &I,
                                        const vpRect &rectangle) {
  //Reset variables used when dealing with 3D models
  //So as no 3D point list is passed, we dont need this variables
  m_trainPoints.clear();
  m_mapOfImageId.clear();
  m_mapOfImages.clear();
  m_currentImageId = 1;

  if(m_useAffineDetection) {
    std::vector<std::vector<cv::KeyPoint> > listOfTrainKeyPoints;
    std::vector<cv::Mat> listOfTrainDescriptors;

    //Detect keypoints and extract descriptors on multiple images
    detectExtractAffine(I, listOfTrainKeyPoints, listOfTrainDescriptors);

    //Flatten the different train lists
    m_trainKeyPoints.clear();
    for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfTrainKeyPoints.begin();
        it != listOfTrainKeyPoints.end(); ++it) {
      m_trainKeyPoints.insert(m_trainKeyPoints.end(), it->begin(), it->end());
    }

    bool first = true;
    for(std::vector<cv::Mat>::const_iterator it = listOfTrainDescriptors.begin(); it != listOfTrainDescriptors.end(); ++it) {
      if(first) {
        first = false;
        it->copyTo(m_trainDescriptors);
      } else {
        m_trainDescriptors.push_back(*it);
      }
    }
  } else {
    detect(I, m_trainKeyPoints, m_detectionTime, rectangle);
    extract(I, m_trainKeyPoints, m_trainDescriptors, m_extractionTime);
  }

  //Save the correspondence keypoint class_id with the training image_id in a map
  //Used to display the matching with all the training images
  for(std::vector<cv::KeyPoint>::const_iterator it = m_trainKeyPoints.begin(); it != m_trainKeyPoints.end(); ++it) {
    m_mapOfImageId[it->class_id] = m_currentImageId;
  }

  //Save the image in a map at a specific image_id
  m_mapOfImages[m_currentImageId] = I;

  //Convert OpenCV type to ViSP type for compatibility
  vpConvert::convertFromOpenCV(m_trainKeyPoints, referenceImagePointsList);

  _reference_computed = true;

  //Add train descriptors in matcher object
  m_matcher->clear();
  m_matcher->add(std::vector<cv::Mat>(1, m_trainDescriptors));

  return static_cast<unsigned int>(m_trainKeyPoints.size());
}

void vpKeyPoint::buildReference(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &trainKeyPoints,
                                std::vector<cv::Point3f> &points3f, const bool append, const int class_id) {
  cv::Mat trainDescriptors;
  //Copy the input list of keypoints
  std::vector<cv::KeyPoint> trainKeyPoints_tmp = trainKeyPoints;

  extract(I, trainKeyPoints, trainDescriptors, m_extractionTime, &points3f);

  if(trainKeyPoints.size() != trainKeyPoints_tmp.size()) {
    //Keypoints have been removed
    //Store the hash of a keypoint as the key and the index of the keypoint as the value
    std::map<size_t, size_t> mapOfKeypointHashes;
    size_t cpt = 0;
    for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); ++it, cpt++) {
      mapOfKeypointHashes[myKeypointHash(*it)] = cpt;
    }

    std::vector<cv::Point3f> trainPoints_tmp;
    for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
      if(mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
        trainPoints_tmp.push_back(points3f[mapOfKeypointHashes[myKeypointHash(*it)]]);
      }
    }

    //Copy trainPoints_tmp to points3f
    points3f = trainPoints_tmp;
  }

  buildReference(I, trainKeyPoints, trainDescriptors, points3f, append, class_id);
}

void vpKeyPoint::buildReference(const vpImage<unsigned char> &I, const std::vector<cv::KeyPoint> &trainKeyPoints,
                                const cv::Mat &trainDescriptors, const std::vector<cv::Point3f> &points3f,
                                const bool append, const int class_id) {
  if(!append) {
    m_currentImageId = 0;
    m_mapOfImageId.clear();
    m_mapOfImages.clear();
    this->m_trainKeyPoints.clear();
    this->m_trainPoints.clear();
  }

  m_currentImageId++;

  std::vector<cv::KeyPoint> trainKeyPoints_tmp = trainKeyPoints;
  //Set class_id if != -1
  if(class_id != -1) {
    for(std::vector<cv::KeyPoint>::iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); ++it) {
      it->class_id = class_id;
    }
  }

  //Save the correspondence keypoint class_id with the training image_id in a map
  //Used to display the matching with all the training images
  for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); ++it) {
    m_mapOfImageId[it->class_id] = m_currentImageId;
  }

  //Save the image in a map at a specific image_id
  m_mapOfImages[m_currentImageId] = I;

  //Append reference lists
  this->m_trainKeyPoints.insert(this->m_trainKeyPoints.end(), trainKeyPoints_tmp.begin(), trainKeyPoints_tmp.end());
  if(!append) {
    trainDescriptors.copyTo(this->m_trainDescriptors);
  } else {
    this->m_trainDescriptors.push_back(trainDescriptors);
  }
  this->m_trainPoints.insert(this->m_trainPoints.end(), points3f.begin(), points3f.end());


  //Convert OpenCV type to ViSP type for compatibility
  vpConvert::convertFromOpenCV(this->m_trainKeyPoints, referenceImagePointsList);
  vpConvert::convertFromOpenCV(this->m_trainPoints, m_trainVpPoints);

  //Add train descriptors in matcher object
  m_matcher->clear();
  m_matcher->add(std::vector<cv::Mat>(1, m_trainDescriptors));

  _reference_computed = true;
}

void vpKeyPoint::compute3D(const cv::KeyPoint &candidate, const std::vector<vpPoint> &roi,
    const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, cv::Point3f &point) {
  /* compute plane equation */
  std::vector<vpPoint>::const_iterator it_roi = roi.begin();
  vpPoint pts[3];
  pts[0] = *it_roi;
  ++it_roi;
  pts[1] = *it_roi;
  ++it_roi;
  pts[2] = *it_roi;
  vpPlane Po(pts[0], pts[1], pts[2]);
  double xc = 0.0, yc = 0.0;
  vpPixelMeterConversion::convertPoint(cam, candidate.pt.x, candidate.pt.y, xc, yc);
  double Z = -Po.getD() / (Po.getA() * xc + Po.getB() * yc + Po.getC());
  double X = xc * Z;
  double Y = yc * Z;
  vpColVector point_cam(4);
  point_cam[0] = X;
  point_cam[1] = Y;
  point_cam[2] = Z;
  point_cam[3] = 1;
  vpColVector point_obj(4);
  point_obj = cMo.inverse() * point_cam;
  point = cv::Point3f((float) point_obj[0], (float) point_obj[1], (float) point_obj[2]);
}

void vpKeyPoint::compute3D(const vpImagePoint &candidate, const std::vector<vpPoint> &roi,
    const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, vpPoint &point) {
  /* compute plane equation */
  std::vector<vpPoint>::const_iterator it_roi = roi.begin();
  vpPoint pts[3];
  pts[0] = *it_roi;
  ++it_roi;
  pts[1] = *it_roi;
  ++it_roi;
  pts[2] = *it_roi;
  vpPlane Po(pts[0], pts[1], pts[2]);
  double xc = 0.0, yc = 0.0;
  vpPixelMeterConversion::convertPoint(cam, candidate, xc, yc);
  double Z = -Po.getD() / (Po.getA() * xc + Po.getB() * yc + Po.getC());
  double X = xc * Z;
  double Y = yc * Z;
  vpColVector point_cam(4);
  point_cam[0] = X;
  point_cam[1] = Y;
  point_cam[2] = Z;
  point_cam[3] = 1;
  vpColVector point_obj(4);
  point_obj = cMo.inverse() * point_cam;
  point.setWorldCoordinates(point_obj);
}

void vpKeyPoint::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) {

  std::vector<cv::KeyPoint> candidatesToCheck = candidates;
  candidates.clear();
  points.clear();
  vpImagePoint imPt;
  cv::Point3f pt;
  cv::Mat desc;

  std::vector<std::pair<cv::KeyPoint, size_t> > pairOfCandidatesToCheck(candidatesToCheck.size());
  for(size_t i = 0; i < candidatesToCheck.size(); i++) {
    pairOfCandidatesToCheck[i] = std::pair<cv::KeyPoint, size_t>(candidatesToCheck[i], i);
  }

  size_t cpt1 = 0;
  std::vector<vpPolygon> polygons_tmp = polygons;
  for (std::vector<vpPolygon>::iterator it1 = polygons_tmp.begin(); it1 != polygons_tmp.end(); ++it1, cpt1++) {
    std::vector<std::pair<cv::KeyPoint, size_t> >::iterator it2 = pairOfCandidatesToCheck.begin();

    while(it2 != pairOfCandidatesToCheck.end()) {
      imPt.set_ij(it2->first.pt.y, it2->first.pt.x);
      if (it1->isInside(imPt)) {
        candidates.push_back(it2->first);
        vpKeyPoint::compute3D(it2->first, roisPt[cpt1], cam, cMo, pt);
        points.push_back(pt);

        if(descriptors != NULL) {
          desc.push_back(descriptors->row((int) it2->second));
        }

        //Remove candidate keypoint which is located on the current polygon
        it2 = pairOfCandidatesToCheck.erase(it2);
      } else {
        ++it2;
      }
    }
  }

  if(descriptors != NULL) {
    desc.copyTo(*descriptors);
  }
}

void vpKeyPoint::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) {

  std::vector<vpImagePoint> candidatesToCheck = candidates;
  candidates.clear();
  points.clear();
  vpPoint pt;
  cv::Mat desc;

  std::vector<std::pair<vpImagePoint, size_t> > pairOfCandidatesToCheck(candidatesToCheck.size());
  for(size_t i = 0; i < candidatesToCheck.size(); i++) {
    pairOfCandidatesToCheck[i] = std::pair<vpImagePoint, size_t>(candidatesToCheck[i], i);
  }

  size_t cpt1 = 0;
  std::vector<vpPolygon> polygons_tmp = polygons;
  for (std::vector<vpPolygon>::iterator it1 = polygons_tmp.begin(); it1 != polygons_tmp.end(); ++it1, cpt1++) {
    std::vector<std::pair<vpImagePoint, size_t> >::iterator it2 = pairOfCandidatesToCheck.begin();

    while(it2 != pairOfCandidatesToCheck.end()) {
      if (it1->isInside(it2->first)) {
        candidates.push_back(it2->first);
        vpKeyPoint::compute3D(it2->first, roisPt[cpt1], cam, cMo, pt);
        points.push_back(pt);

        if(descriptors != NULL) {
          desc.push_back(descriptors->row((int) it2->second));
        }

        //Remove candidate keypoint which is located on the current polygon
        it2 = pairOfCandidatesToCheck.erase(it2);
      } else {
        ++it2;
      }
    }
  }
}

bool vpKeyPoint::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 *)) {
  double t = vpTime::measureTimeMs();

  if(imagePoints.size() < 4 || objectPoints.size() < 4 || imagePoints.size() != objectPoints.size()) {
    elapsedTime = (vpTime::measureTimeMs() - t);
    std::cerr << "Not enough points to compute the pose (at least 4 points are needed)." << std::endl;

    return false;
  }

  cv::Mat cameraMatrix =
      (cv::Mat_<double>(3, 3) << cam.get_px(), 0, cam.get_u0(), 0, cam.get_py(), cam.get_v0(), 0, 0, 1);
  cv::Mat rvec, tvec;


  //Bug with OpenCV < 2.4.0 when zero distorsion is provided by an empty array.
  //http://code.opencv.org/issues/1700 ; http://code.opencv.org/issues/1718
  //what(): Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8 or 8x1 floating-point vector in function cvProjectPoints2
  //Fixed in OpenCV 2.4.0 (r7558)
//  cv::Mat distCoeffs;
  cv::Mat distCoeffs = cv::Mat::zeros(1, 5, CV_64F);

  try {
#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
    //OpenCV 3.0.0 (2014/12/12)
    cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs,
        rvec, tvec, false, m_nbRansacIterations, (float) m_ransacReprojectionError,
        0.99,//confidence=0.99 (default) – The probability that the algorithm produces a useful result.
        inlierIndex,
        cv::SOLVEPNP_ITERATIVE);
    //SOLVEPNP_ITERATIVE (default): Iterative method is based on Levenberg-Marquardt optimization.
    //In this case the function finds such a pose that minimizes reprojection error, that is the sum of squared distances
    //between the observed projections imagePoints and the projected (using projectPoints() ) objectPoints .
    //SOLVEPNP_P3P: Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang “Complete Solution Classification
    //for the Perspective-Three-Point Problem”. In this case the function requires exactly four object and image points.
    //SOLVEPNP_EPNP: Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the paper “EPnP: Efficient
    //Perspective-n-Point Camera Pose Estimation”.
    //SOLVEPNP_DLS: Method is based on the paper of Joel A. Hesch and Stergios I. Roumeliotis. “A Direct Least-Squares (DLS)
    //Method for PnP”.
    //SOLVEPNP_UPNP Method is based on the paper of A.Penate-Sanchez, J.Andrade-Cetto, F.Moreno-Noguer.
    //“Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation”. In this case the function also
    //estimates the parameters f_x and f_y assuming that both have the same value. Then the cameraMatrix is updated with the
    //estimated focal length.
#else
    int nbInlierToReachConsensus = m_nbRansacMinInlierCount;
    if(m_useConsensusPercentage) {
      nbInlierToReachConsensus = (int) (m_ransacConsensusPercentage / 100.0 * (double) m_queryFilteredKeyPoints.size());
    }

    cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs,
        rvec, tvec, false, m_nbRansacIterations,
        (float) m_ransacReprojectionError, nbInlierToReachConsensus,
        inlierIndex);
#endif
  } catch (cv::Exception &e) {
    std::cerr << e.what() << std::endl;
    elapsedTime = (vpTime::measureTimeMs() - t);
    return false;
  }
  vpTranslationVector translationVec(tvec.at<double>(0),
                                     tvec.at<double>(1), tvec.at<double>(2));
  vpThetaUVector thetaUVector(rvec.at<double>(0), rvec.at<double>(1),
                              rvec.at<double>(2));
  cMo = vpHomogeneousMatrix(translationVec, thetaUVector);

  if(func != NULL) {
    //Check the final pose returned by solvePnPRansac to discard
    //solutions which do not respect the pose criterion.
    if(!func(&cMo)) {
      elapsedTime = (vpTime::measureTimeMs() - t);
      return false;
    }
  }

  elapsedTime = (vpTime::measureTimeMs() - t);
  return true;
}

bool vpKeyPoint::computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo,
                         std::vector<vpPoint> &inliers, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) {
  std::vector<unsigned int> inlierIndex;
  return computePose(objectVpPoints, cMo, inliers, inlierIndex, elapsedTime, func);
}

bool vpKeyPoint::computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo,
                         std::vector<vpPoint> &inliers, std::vector<unsigned int> &inlierIndex,
                         double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) {
  double t = vpTime::measureTimeMs();

  if(objectVpPoints.size() < 4) {
    elapsedTime = (vpTime::measureTimeMs() - t);
//    std::cerr << "Not enough points to compute the pose (at least 4 points are needed)." << std::endl;

    return false;
  }

  vpPose pose;

  for(std::vector<vpPoint>::const_iterator it = objectVpPoints.begin(); it != objectVpPoints.end(); ++it) {
    pose.addPoint(*it);
  }

  unsigned int nbInlierToReachConsensus = (unsigned int) m_nbRansacMinInlierCount;
  if(m_useConsensusPercentage) {
    nbInlierToReachConsensus = (unsigned int) (m_ransacConsensusPercentage / 100.0 *
                               (double) m_queryFilteredKeyPoints.size());
  }

  pose.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
  pose.setRansacThreshold(m_ransacThreshold);
  pose.setRansacMaxTrials(m_nbRansacIterations);

  bool isRansacPoseEstimationOk = false;
  try {
    pose.setCovarianceComputation(m_computeCovariance);
    isRansacPoseEstimationOk = pose.computePose(vpPose::RANSAC, cMo, func);
    inliers = pose.getRansacInliers();
    inlierIndex = pose.getRansacInlierIndex();

    if(m_computeCovariance) {
      m_covarianceMatrix = pose.getCovarianceMatrix();
    }
  } catch(vpException &e) {
    std::cerr << "e=" << e.what() << std::endl;
    elapsedTime = (vpTime::measureTimeMs() - t);
    return false;
  }

//  if(func != NULL && isRansacPoseEstimationOk) {
//    //Check the final pose returned by the Ransac VVS pose estimation as in rare some cases
//    //we can converge toward a final cMo that does not respect the pose criterion even
//    //if the 4 minimal points picked to respect the pose criterion.
//    if(!func(&cMo)) {
//      elapsedTime = (vpTime::measureTimeMs() - t);
//      return false;
//    }
//  }

  elapsedTime = (vpTime::measureTimeMs() - t);
  return isRansacPoseEstimationOk;
}

double vpKeyPoint::computePoseEstimationError(const std::vector<std::pair<cv::KeyPoint, cv::Point3f> > &matchKeyPoints,
                                              const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo_est) {
  if(matchKeyPoints.size() == 0) {
    //return std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
    return DBL_MAX;
  }

  std::vector<double> errors(matchKeyPoints.size());
  size_t cpt = 0;
  vpPoint pt;
  for(std::vector<std::pair<cv::KeyPoint, cv::Point3f> >::const_iterator it = matchKeyPoints.begin();
      it != matchKeyPoints.end(); ++it, cpt++) {
    pt.set_oX(it->second.x);
    pt.set_oY(it->second.y);
    pt.set_oZ(it->second.z);
    pt.project(cMo_est);
    double u = 0.0, v = 0.0;
    vpMeterPixelConversion::convertPoint(cam, pt.get_x(), pt.get_y(), u, v);
    errors[cpt] = std::sqrt((u-it->first.pt.x)*(u-it->first.pt.x) + (v-it->first.pt.y)*(v-it->first.pt.y));
  }

  return std::accumulate(errors.begin(), errors.end(), 0.0) / errors.size();
}

void vpKeyPoint::createImageMatching(vpImage<unsigned char> &IRef, vpImage<unsigned char> &ICurrent,
                                     vpImage<unsigned char> &IMatching) {
  //Image matching side by side
  unsigned int width = IRef.getWidth() + ICurrent.getWidth();
  unsigned int height = (std::max)(IRef.getHeight(), ICurrent.getHeight());

  IMatching = vpImage<unsigned char>(height, width);
}

void vpKeyPoint::createImageMatching(vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching) {
  //Nb images in the training database + the current image we want to detect the object
  unsigned int nbImg = (unsigned int) (m_mapOfImages.size() + 1);

  if(m_mapOfImages.empty()) {
    std::cerr << "There is no training image loaded !" << std::endl;
    return;
  }

  if(nbImg == 2) {
    //Only one training image, so we display them side by side
    createImageMatching(m_mapOfImages.begin()->second, ICurrent, IMatching);
  } else {
    //Multiple training images, display them as a mosaic image
    //(unsigned int) std::floor(std::sqrt((double) nbImg) + 0.5);
    unsigned int nbImgSqrt = (unsigned int) vpMath::round(std::sqrt((double) nbImg));

    //Number of columns in the mosaic grid
    unsigned int nbWidth = nbImgSqrt;
    //Number of rows in the mosaic grid
    unsigned int nbHeight = nbImgSqrt;

    //Deals with non square mosaic grid and the total number of images
    if(nbImgSqrt * nbImgSqrt < nbImg) {
      nbWidth++;
    }

    unsigned int maxW = ICurrent.getWidth();
    unsigned int maxH = ICurrent.getHeight();
    for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
      if(maxW < it->second.getWidth()) {
        maxW = it->second.getWidth();
      }

      if(maxH < it->second.getHeight()) {
        maxH = it->second.getHeight();
      }
    }

    IMatching = vpImage<unsigned char>(maxH * nbHeight, maxW * nbWidth);
  }
}

void vpKeyPoint::detect(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
                        const vpRect &rectangle) {
  cv::Mat matImg;
  vpImageConvert::convert(I, matImg, false);
  cv::Mat mask = cv::Mat::zeros(matImg.rows, matImg.cols, CV_8U);

  if(rectangle.getWidth() > 0 && rectangle.getHeight() > 0) {
    cv::Point leftTop((int) rectangle.getLeft(), (int) rectangle.getTop()), rightBottom((int) rectangle.getRight(),
                      (int) rectangle.getBottom());
    cv::rectangle(mask, leftTop, rightBottom, cv::Scalar(255), CV_FILLED);
  } else {
    mask = cv::Mat::ones(matImg.rows, matImg.cols, CV_8U) * 255;
  }

  detect(matImg, keyPoints, elapsedTime, mask);
}

void vpKeyPoint::detect(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
                        const cv::Mat &mask) {
  double t = vpTime::measureTimeMs();
  keyPoints.clear();

  for(std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin(); it != m_detectors.end(); ++it) {
    std::vector<cv::KeyPoint> kp;
    it->second->detect(matImg, kp, mask);
    keyPoints.insert(keyPoints.end(), kp.begin(), kp.end());
  }

  elapsedTime = vpTime::measureTimeMs() - t;
}

void vpKeyPoint::display(const vpImage<unsigned char> &IRef,
                         const vpImage<unsigned char> &ICurrent, unsigned int size) {
  std::vector<vpImagePoint> vpQueryImageKeyPoints;
  getQueryKeyPoints(vpQueryImageKeyPoints);
  std::vector<vpImagePoint> vpTrainImageKeyPoints;
  getTrainKeyPoints(vpTrainImageKeyPoints);

  for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
    vpDisplay::displayCross(IRef, vpTrainImageKeyPoints[(size_t)(it->trainIdx)], size, vpColor::red);
    vpDisplay::displayCross(ICurrent, vpQueryImageKeyPoints[(size_t)(it->queryIdx)], size, vpColor::green);
  }
}

void vpKeyPoint::display(const vpImage<unsigned char> &ICurrent, unsigned int size, const vpColor &color) {
  std::vector<vpImagePoint> vpQueryImageKeyPoints;
  getQueryKeyPoints(vpQueryImageKeyPoints);

  for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
    vpDisplay::displayCross (ICurrent, vpQueryImageKeyPoints[(size_t)(it->queryIdx)], size, color);
  }
}

void vpKeyPoint::displayMatching(const vpImage<unsigned char> &IRef, vpImage<unsigned char> &IMatching,
                                 unsigned int crossSize, unsigned int lineThickness, const vpColor &color) {
  bool randomColor = (color == vpColor::none);
  srand((unsigned int) time(NULL));
  vpColor currentColor = color;

  std::vector<vpImagePoint> queryImageKeyPoints;
  getQueryKeyPoints(queryImageKeyPoints);
  std::vector<vpImagePoint> trainImageKeyPoints;
  getTrainKeyPoints(trainImageKeyPoints);

  vpImagePoint leftPt, rightPt;
  for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
    if(randomColor) {
      currentColor = vpColor((rand() % 256), (rand() % 256), (rand() % 256));
    }

    leftPt = trainImageKeyPoints[(size_t)(it->trainIdx)];
    rightPt = vpImagePoint(queryImageKeyPoints[(size_t)(it->queryIdx)].get_i(),
        queryImageKeyPoints[(size_t)it->queryIdx].get_j() + IRef.getWidth());
    vpDisplay::displayCross(IMatching, leftPt, crossSize, currentColor);
    vpDisplay::displayCross(IMatching, rightPt, crossSize, currentColor);
    vpDisplay::displayLine(IMatching, leftPt, rightPt, currentColor, lineThickness);
  }
}

void vpKeyPoint::displayMatching(const vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching,
                                 const std::vector<vpImagePoint> &ransacInliers, unsigned int crossSize, unsigned int lineThickness) {
  if(m_mapOfImages.empty() || m_mapOfImageId.empty()) {
    //No training images so return
    std::cerr << "There is no training image loaded !" << std::endl;
    return;
  }

  //Nb images in the training database + the current image we want to detect the object
  int nbImg = (int) (m_mapOfImages.size() + 1);

  if(nbImg == 2) {
    //Only one training image, so we display the matching result side-by-side
    displayMatching(m_mapOfImages.begin()->second, IMatching, crossSize);
  } else {
    //Multiple training images, display them as a mosaic image
    int nbImgSqrt = vpMath::round(std::sqrt((double) nbImg)); //(int) std::floor(std::sqrt((double) nbImg) + 0.5);
    int nbWidth = nbImgSqrt;
    int nbHeight = nbImgSqrt;

    if(nbImgSqrt * nbImgSqrt < nbImg) {
      nbWidth++;
    }

    std::map<int, int> mapOfImageIdIndex;
    int cpt = 0;
    unsigned int maxW = ICurrent.getWidth(), maxH = ICurrent.getHeight();
    for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
      mapOfImageIdIndex[it->first] = cpt;

      if(maxW < it->second.getWidth()) {
        maxW = it->second.getWidth();
      }

      if(maxH < it->second.getHeight()) {
        maxH = it->second.getHeight();
      }
    }

    //Indexes of the current image in the grid made in order to the image is in the center square in the mosaic grid
    int medianI = nbHeight / 2;
    int medianJ = nbWidth / 2;
    int medianIndex = medianI * nbWidth + medianJ;
    for(std::vector<cv::KeyPoint>::const_iterator it = m_trainKeyPoints.begin(); it != m_trainKeyPoints.end(); ++it) {
      vpImagePoint topLeftCorner;
      int current_class_id_index = 0;
      if(mapOfImageIdIndex[m_mapOfImageId[it->class_id]] < medianIndex) {
        current_class_id_index = mapOfImageIdIndex[m_mapOfImageId[it->class_id]];
      } else {
        //Shift of one unity the index of the training images which are after the current image
        current_class_id_index = mapOfImageIdIndex[m_mapOfImageId[it->class_id]] + 1;
      }

      int indexI = current_class_id_index / nbWidth;
      int indexJ = current_class_id_index - (indexI * nbWidth);
      topLeftCorner.set_ij((int)maxH*indexI, (int)maxW*indexJ);

      //Display cross for keypoints in the learning database
      vpDisplay::displayCross(IMatching, (int) (it->pt.y + topLeftCorner.get_i()), (int) (it->pt.x + topLeftCorner.get_j()),
                              crossSize, vpColor::red);
    }

    vpImagePoint topLeftCorner((int)maxH*medianI, (int)maxW*medianJ);
    for(std::vector<cv::KeyPoint>::const_iterator it = m_queryKeyPoints.begin(); it != m_queryKeyPoints.end(); ++it) {
      //Display cross for keypoints detected in the current image
      vpDisplay::displayCross(IMatching, (int) (it->pt.y + topLeftCorner.get_i()), (int) (it->pt.x + topLeftCorner.get_j()),
                              crossSize, vpColor::red);
    }
    for(std::vector<vpImagePoint>::const_iterator it = ransacInliers.begin();
        it != ransacInliers.end(); ++it) {
      //Display green circle for RANSAC inliers
      vpDisplay::displayCircle(IMatching, (int) (it->get_v() + topLeftCorner.get_i()), (int) (it->get_u() +
                               topLeftCorner.get_j()), 4, vpColor::green);
    }
    for(std::vector<vpImagePoint>::const_iterator it = m_ransacOutliers.begin(); it != m_ransacOutliers.end(); ++it) {
      //Display red circle for RANSAC outliers
      vpDisplay::displayCircle(IMatching, (int) (it->get_i() + topLeftCorner.get_i()), (int) (it->get_j() +
                               topLeftCorner.get_j()), 4, vpColor::red);
    }

    for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
      int current_class_id = 0;
      if(mapOfImageIdIndex[m_mapOfImageId[m_trainKeyPoints[(size_t) it->trainIdx].class_id]] < medianIndex) {
        current_class_id = mapOfImageIdIndex[m_mapOfImageId[m_trainKeyPoints[(size_t) it->trainIdx].class_id]];
      } else {
        //Shift of one unity the index of the training images which are after the current image
        current_class_id = mapOfImageIdIndex[m_mapOfImageId[m_trainKeyPoints[(size_t) it->trainIdx].class_id]] + 1;
      }

      int indexI = current_class_id / nbWidth;
      int indexJ = current_class_id - (indexI * nbWidth);

      vpImagePoint end((int)maxH*indexI + m_trainKeyPoints[(size_t) it->trainIdx].pt.y,
          (int)maxW*indexJ + m_trainKeyPoints[(size_t) it->trainIdx].pt.x);
      vpImagePoint start((int)maxH*medianI + m_queryFilteredKeyPoints[(size_t) it->queryIdx].pt.y,
          (int)maxW*medianJ + m_queryFilteredKeyPoints[(size_t) it->queryIdx].pt.x);

      //Draw line for matching keypoints detected in the current image and those detected
      //in the training images
      vpDisplay::displayLine(IMatching, start, end, vpColor::green, lineThickness);
    }
  }
}

void vpKeyPoint::extract(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
                         double &elapsedTime, std::vector<cv::Point3f> *trainPoints) {
  cv::Mat matImg;
  vpImageConvert::convert(I, matImg, false);
  extract(matImg, keyPoints, descriptors, elapsedTime, trainPoints);
}

void vpKeyPoint::extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
                         double &elapsedTime, std::vector<cv::Point3f> *trainPoints) {
  double t = vpTime::measureTimeMs();
  bool first = true;

  for(std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator itd = m_extractors.begin();
      itd != m_extractors.end(); ++itd) {
    if(first) {
      first = false;
      //Check if we have 3D object points information
      if(trainPoints != NULL && !trainPoints->empty()) {
        //Copy the input list of keypoints, keypoints that cannot be computed are removed in the function compute
        std::vector<cv::KeyPoint> keyPoints_tmp = keyPoints;

        //Extract descriptors for the given list of keypoints
        itd->second->compute(matImg, keyPoints, descriptors);

        if(keyPoints.size() != keyPoints_tmp.size()) {
          //Keypoints have been removed
          //Store the hash of a keypoint as the key and the index of the keypoint as the value
          std::map<size_t, size_t> mapOfKeypointHashes;
          size_t cpt = 0;
          for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints_tmp.begin(); it != keyPoints_tmp.end(); ++it, cpt++) {
            mapOfKeypointHashes[myKeypointHash(*it)] = cpt;
          }

          std::vector<cv::Point3f> trainPoints_tmp;
          for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints.begin(); it != keyPoints.end(); ++it) {
            if(mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
              trainPoints_tmp.push_back((*trainPoints)[mapOfKeypointHashes[myKeypointHash(*it)]]);
            }
          }

          //Copy trainPoints_tmp to m_trainPoints
          *trainPoints = trainPoints_tmp;
        }
      } else {
        //Extract descriptors for the given list of keypoints
        itd->second->compute(matImg, keyPoints, descriptors);
      }
    } else {
      //Copy the input list of keypoints, keypoints that cannot be computed are removed in the function compute
      std::vector<cv::KeyPoint> keyPoints_tmp = keyPoints;

      cv::Mat desc;
      //Extract descriptors for the given list of keypoints
      itd->second->compute(matImg, keyPoints, desc);

      if(keyPoints.size() != keyPoints_tmp.size()) {
        //Keypoints have been removed
        //Store the hash of a keypoint as the key and the index of the keypoint as the value
        std::map<size_t, size_t> mapOfKeypointHashes;
        size_t cpt = 0;
        for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints_tmp.begin(); it != keyPoints_tmp.end(); ++it, cpt++) {
          mapOfKeypointHashes[myKeypointHash(*it)] = cpt;
        }

        std::vector<cv::Point3f> trainPoints_tmp;
        cv::Mat descriptors_tmp;
        for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints.begin(); it != keyPoints.end(); ++it) {
          if(mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
            if(trainPoints != NULL && !trainPoints->empty()) {
              trainPoints_tmp.push_back((*trainPoints)[mapOfKeypointHashes[myKeypointHash(*it)]]);
            }

            if(!descriptors.empty()) {
              descriptors_tmp.push_back(descriptors.row((int) mapOfKeypointHashes[myKeypointHash(*it)]));
            }
          }
        }

        if(trainPoints != NULL) {
          //Copy trainPoints_tmp to m_trainPoints
          *trainPoints = trainPoints_tmp;
        }
        //Copy descriptors_tmp to descriptors
        descriptors_tmp.copyTo(descriptors);
      }

      //Merge descriptors horizontally
      if(descriptors.empty()) {
        desc.copyTo(descriptors);
      } else {
        cv::hconcat(descriptors, desc, descriptors);
      }
    }
  }

  if(keyPoints.size() != (size_t) descriptors.rows) {
    std::cerr << "keyPoints.size() != (size_t) descriptors.rows" << std::endl;
  }
  elapsedTime = vpTime::measureTimeMs() - t;
}

void vpKeyPoint::filterMatches() {
  std::vector<cv::KeyPoint> queryKpts;
  std::vector<cv::Point3f> trainPts;
  std::vector<cv::DMatch> m;

  if(m_useKnn) {
    double max_dist = 0;
    //double min_dist = std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
    double min_dist = DBL_MAX;
    double mean = 0.0;
    std::vector<double> distance_vec(m_knnMatches.size());

    if(m_filterType == stdAndRatioDistanceThreshold) {
      for(size_t i = 0; i < m_knnMatches.size(); i++) {
        double dist = m_knnMatches[i][0].distance;
        mean += dist;
        distance_vec[i] = dist;

        if (dist < min_dist) {
          min_dist = dist;
        }
        if (dist > max_dist) {
          max_dist = dist;
        }
      }
      mean /= m_queryDescriptors.rows;
    }

    double sq_sum = std::inner_product(distance_vec.begin(), distance_vec.end(), distance_vec.begin(), 0.0);
    double stdev = std::sqrt(sq_sum / distance_vec.size() - mean * mean);
    double threshold = min_dist + stdev;

    for(size_t i = 0; i < m_knnMatches.size(); i++) {
      if(m_knnMatches[i].size() >= 2) {
        //Calculate ratio of the descriptor distance between the two nearest neighbors of the keypoint
        float ratio = m_knnMatches[i][0].distance / m_knnMatches[i][1].distance;
//        float ratio = std::sqrt((vecMatches[i][0].distance * vecMatches[i][0].distance)
//            / (vecMatches[i][1].distance * vecMatches[i][1].distance));
        double dist = m_knnMatches[i][0].distance;

        if(ratio < m_matchingRatioThreshold || (m_filterType == stdAndRatioDistanceThreshold && dist < threshold)) {
          m.push_back(cv::DMatch((int) queryKpts.size(), m_knnMatches[i][0].trainIdx, m_knnMatches[i][0].distance));

          if(!m_trainPoints.empty()) {
            trainPts.push_back(m_trainPoints[(size_t)m_knnMatches[i][0].trainIdx]);
          }
          queryKpts.push_back(m_queryKeyPoints[(size_t)m_knnMatches[i][0].queryIdx]);
        }
      }
    }
  } else {
    double max_dist = 0;
    // create an error under Windows. To fix it we have to add #undef max
    //double min_dist = std::numeric_limits<double>::max();
    double min_dist = DBL_MAX;
    double mean = 0.0;
    std::vector<double> distance_vec(m_matches.size());
    for(size_t i = 0; i < m_matches.size(); i++) {
      double dist = m_matches[i].distance;
      mean += dist;
      distance_vec[i] = dist;

      if (dist < min_dist) {
        min_dist = dist;
      }
      if (dist > max_dist) {
        max_dist = dist;
      }
    }
    mean /= m_queryDescriptors.rows;

    double sq_sum = std::inner_product(distance_vec.begin(), distance_vec.end(), distance_vec.begin(), 0.0);
    double stdev = std::sqrt(sq_sum / distance_vec.size() - mean * mean);

    //Define a threshold where we keep all keypoints whose the descriptor distance falls below a factor of the
    //minimum descriptor distance (for all the query keypoints)
    //or below the minimum descriptor distance + the standard deviation (calculated on all the query descriptor distances)
    double threshold = m_filterType == constantFactorDistanceThreshold ? m_matchingFactorThreshold * min_dist : min_dist + stdev;

    for (size_t i = 0; i < m_matches.size(); i++) {
      if(m_matches[i].distance <= threshold) {
        m.push_back(cv::DMatch((int) queryKpts.size(), m_matches[i].trainIdx, m_matches[i].distance));

        if(!m_trainPoints.empty()) {
          trainPts.push_back(m_trainPoints[(size_t)m_matches[i].trainIdx]);
        }
        queryKpts.push_back(m_queryKeyPoints[(size_t)m_matches[i].queryIdx]);
      }
    }
  }

  if(m_useSingleMatchFilter) {
    //Eliminate matches where multiple query keypoints are matched to the same train keypoint
    std::vector<cv::DMatch> mTmp;
    std::vector<cv::Point3f> trainPtsTmp;
    std::vector<cv::KeyPoint> queryKptsTmp;

    std::map<int, int> mapOfTrainIdx;
    //Count the number of query points matched to the same train point
    for(std::vector<cv::DMatch>::const_iterator it = m.begin(); it != m.end(); ++it) {
      mapOfTrainIdx[it->trainIdx]++;
    }

    //Keep matches with only one correspondence
    for(std::vector<cv::DMatch>::const_iterator it = m.begin(); it != m.end(); ++it) {
      if(mapOfTrainIdx[it->trainIdx] == 1) {
        mTmp.push_back(cv::DMatch((int) queryKptsTmp.size(), it->trainIdx, it->distance));

        if(!m_trainPoints.empty()) {
          trainPtsTmp.push_back(m_trainPoints[(size_t) it->trainIdx]);
        }
        queryKptsTmp.push_back(queryKpts[(size_t) it->queryIdx]);
      }
    }

    m_filteredMatches = mTmp;
    m_objectFilteredPoints = trainPtsTmp;
    m_queryFilteredKeyPoints = queryKptsTmp;
  } else {
    m_filteredMatches = m;
    m_objectFilteredPoints = trainPts;
    m_queryFilteredKeyPoints = queryKpts;
  }
}

void vpKeyPoint::getObjectPoints(std::vector<cv::Point3f> &objectPoints) const {
  objectPoints = m_objectFilteredPoints;
}

void vpKeyPoint::getObjectPoints(std::vector<vpPoint> &objectPoints) const {
  vpConvert::convertFromOpenCV(m_objectFilteredPoints, objectPoints);
}

void vpKeyPoint::getQueryKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const {
  keyPoints = m_queryFilteredKeyPoints;
}

void vpKeyPoint::getQueryKeyPoints(std::vector<vpImagePoint> &keyPoints) const {
  keyPoints = currentImagePointsList;
}

void vpKeyPoint::getTrainKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const {
  keyPoints = m_trainKeyPoints;
}

void vpKeyPoint::getTrainKeyPoints(std::vector<vpImagePoint> &keyPoints) const {
  keyPoints = referenceImagePointsList;
}

void vpKeyPoint::getTrainPoints(std::vector<cv::Point3f> &points) const {
  points = m_trainPoints;
}

void vpKeyPoint::getTrainPoints(std::vector<vpPoint> &points) const {
  points = m_trainVpPoints;
}

void vpKeyPoint::init() {
  // Require 2.4.0 <= opencv < 3.0.0
#if defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION >= 0x020400) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
  //The following line must be called in order to use SIFT or SURF
  if (!cv::initModule_nonfree()) {
    std::cerr << "Cannot init module non free, SIFT or SURF cannot be used."
        << std::endl;
  }
#endif

  initDetectors(m_detectorNames);
  initExtractors(m_extractorNames);
  initMatcher(m_matcherName);
}

void vpKeyPoint::initDetector(const std::string &detectorName) {
#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
  m_detectors[detectorName] = cv::FeatureDetector::create(detectorName);

  if(m_detectors[detectorName] == NULL) {
    std::stringstream ss_msg;
    ss_msg << "Fail to initialize the detector: " << detectorName << " or it is not available in OpenCV version: "
        << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
    throw vpException(vpException::fatalError, ss_msg.str());
  }
#else
  std::string detectorNameTmp = detectorName;
  std::string pyramid = "Pyramid";
  std::size_t pos = detectorName.find(pyramid);
  bool usePyramid = false;
  if(pos != std::string::npos) {
    detectorNameTmp = detectorName.substr(pos + pyramid.size());
    usePyramid = true;
  }

  if(detectorNameTmp == "SIFT") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
    cv::Ptr<cv::FeatureDetector> siftDetector = cv::xfeatures2d::SIFT::create();
    if(!usePyramid) {
      m_detectors[detectorNameTmp] = siftDetector;
    } else {
      std::cerr << "Kind of non sense to use SIFT with Pyramid !" << std::endl;
      m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(siftDetector);
    }
#else
    std::stringstream ss_msg;
    ss_msg << "Fail to initialize the detector: SIFT. OpenCV version  "
        << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
    throw vpException(vpException::fatalError, ss_msg.str());
#endif
  } else if(detectorNameTmp == "SURF") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
    cv::Ptr<cv::FeatureDetector> surfDetector = cv::xfeatures2d::SURF::create();
    if(!usePyramid) {
      m_detectors[detectorNameTmp] = surfDetector;
    } else {
      std::cerr << "Kind of non sense to use SURF with Pyramid !" << std::endl;
      m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(surfDetector);
    }
#else
    std::stringstream ss_msg;
    ss_msg << "Fail to initialize the detector: SURF. OpenCV version  "
        << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
    throw vpException(vpException::fatalError, ss_msg.str());
#endif
  } else if(detectorNameTmp == "FAST") {
    cv::Ptr<cv::FeatureDetector> fastDetector = cv::FastFeatureDetector::create();
    if(!usePyramid) {
      m_detectors[detectorNameTmp] = fastDetector;
    } else {
//      m_detectors[detectorName] = new PyramidAdaptedFeatureDetector(cv::FastFeatureDetector::create());
      m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(fastDetector);
    }
  } else if(detectorNameTmp == "MSER") {
    cv::Ptr<cv::FeatureDetector> fastDetector = cv::MSER::create();
    if(!usePyramid) {
      m_detectors[detectorNameTmp] = fastDetector;
    } else {
//      m_detectors[detectorName] = new PyramidAdaptedFeatureDetector(cv::MSER::create());
      m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(fastDetector);
    }
  } else if(detectorNameTmp == "ORB") {
    cv::Ptr<cv::FeatureDetector> orbDetector = cv::ORB::create();
    if(!usePyramid) {
      m_detectors[detectorNameTmp] = orbDetector;
    } else {
      std::cerr << "Kind of non sense to use ORB with Pyramid !" << std::endl;
      m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(orbDetector);
    }
  } else if(detectorNameTmp == "BRISK") {
    cv::Ptr<cv::FeatureDetector> briskDetector = cv::BRISK::create();
    if(!usePyramid) {
      m_detectors[detectorNameTmp] = briskDetector;
    } else {
      std::cerr << "Kind of non sense to use BRISK with Pyramid !" << std::endl;
      m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(briskDetector);
    }
  } else if(detectorNameTmp == "KAZE") {
    cv::Ptr<cv::FeatureDetector> kazeDetector = cv::KAZE::create();
    if(!usePyramid) {
      m_detectors[detectorNameTmp] = kazeDetector;
    } else {
      std::cerr << "Kind of non sense to use KAZE with Pyramid !" << std::endl;
      m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(kazeDetector);
    }
  } else if(detectorNameTmp == "AKAZE") {
    cv::Ptr<cv::FeatureDetector> akazeDetector = cv::AKAZE::create();
    if(!usePyramid) {
      m_detectors[detectorNameTmp] = akazeDetector;
    } else {
      std::cerr << "Kind of non sense to use AKAZE with Pyramid !" << std::endl;
      m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(akazeDetector);
    }
  } else if(detectorNameTmp == "GFTT") {
    cv::Ptr<cv::FeatureDetector> gfttDetector = cv::GFTTDetector::create();
    if(!usePyramid) {
      m_detectors[detectorNameTmp] = gfttDetector;
    } else {
//      m_detectors[detectorName] = new PyramidAdaptedFeatureDetector(cv::GFTTDetector::create());
      m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(gfttDetector);
    }
  } else if(detectorNameTmp == "SimpleBlob") {
    cv::Ptr<cv::FeatureDetector> simpleBlobDetector = cv::SimpleBlobDetector::create();
    if(!usePyramid) {
      m_detectors[detectorNameTmp] = simpleBlobDetector;
    } else {
//      m_detectors[detectorName] = new PyramidAdaptedFeatureDetector(cv::SimpleBlobDetector::create());
      m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(simpleBlobDetector);
    }
  } else if(detectorNameTmp == "STAR") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
    cv::Ptr<cv::FeatureDetector> starDetector = cv::xfeatures2d::StarDetector::create();
    if(!usePyramid) {
      m_detectors[detectorNameTmp] = starDetector;
    } else {
//      m_detectors[detectorName] = new PyramidAdaptedFeatureDetector(cv::xfeatures2d::StarDetector::create());
      m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(starDetector);
    }
#else
    std::stringstream ss_msg;
    ss_msg << "Fail to initialize the detector: STAR. OpenCV version  "
        << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
    throw vpException(vpException::fatalError, ss_msg.str());
#endif
  } else if(detectorNameTmp == "AGAST") {
    cv::Ptr<cv::FeatureDetector> agastDetector = cv::AgastFeatureDetector::create();
    if(!usePyramid) {
      m_detectors[detectorNameTmp] = agastDetector;
    } else {
      m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(agastDetector);
    }
  } else {
    std::cerr << "The detector:" << detectorNameTmp << " is not available." << std::endl;
  }

  bool detectorInitialized = false;
  if(!usePyramid) {
    detectorInitialized = (m_detectors[detectorNameTmp] != NULL);
  } else {
    detectorInitialized = (m_detectors[detectorName] != NULL);
  }

  if(!detectorInitialized) {
    std::stringstream ss_msg;
    ss_msg << "Fail to initialize the detector: " << detectorNameTmp << " or it is not available in OpenCV version: "
        << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
    throw vpException(vpException::fatalError, ss_msg.str());
  }

#endif
}

void vpKeyPoint::initDetectors(const std::vector<std::string> &detectorNames) {
  for(std::vector<std::string>::const_iterator it = detectorNames.begin(); it != detectorNames.end(); ++it) {
    initDetector(*it);
  }
}

void vpKeyPoint::initExtractor(const std::string &extractorName) {
#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
  m_extractors[extractorName] = cv::DescriptorExtractor::create(extractorName);
#else
  if(extractorName == "SIFT") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
    m_extractors[extractorName] = cv::xfeatures2d::SIFT::create();
#else
    std::stringstream ss_msg;
    ss_msg << "Fail to initialize the extractor: SIFT. OpenCV version  "
        << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
    throw vpException(vpException::fatalError, ss_msg.str());
#endif
  } else if(extractorName == "SURF") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
    //Use extended set of SURF descriptors (128 instead of 64)
    m_extractors[extractorName] = cv::xfeatures2d::SURF::create(100, 4, 3, true);
#else
    std::stringstream ss_msg;
    ss_msg << "Fail to initialize the extractor: SURF. OpenCV version  "
        << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
    throw vpException(vpException::fatalError, ss_msg.str());
#endif
  } else if(extractorName == "ORB") {
    m_extractors[extractorName] = cv::ORB::create();
  } else if(extractorName == "BRISK") {
    m_extractors[extractorName] = cv::BRISK::create();
  } else if(extractorName == "FREAK") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
    m_extractors[extractorName] = cv::xfeatures2d::FREAK::create();
#else
    std::stringstream ss_msg;
    ss_msg << "Fail to initialize the extractor: FREAK. OpenCV version  "
        << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
    throw vpException(vpException::fatalError, ss_msg.str());
#endif
  } else if(extractorName == "BRIEF") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
    m_extractors[extractorName] = cv::xfeatures2d::BriefDescriptorExtractor::create();
#else
    std::stringstream ss_msg;
    ss_msg << "Fail to initialize the extractor: BRIEF. OpenCV version  "
        << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
    throw vpException(vpException::fatalError, ss_msg.str());
#endif
  } else if(extractorName == "KAZE") {
    m_extractors[extractorName] = cv::KAZE::create();
  } else if(extractorName == "AKAZE") {
    m_extractors[extractorName] = cv::AKAZE::create();
  } else if(extractorName == "DAISY") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
    m_extractors[extractorName] = cv::xfeatures2d::DAISY::create();
#else
    std::stringstream ss_msg;
    ss_msg << "Fail to initialize the extractor: DAISY. OpenCV version  "
        << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
    throw vpException(vpException::fatalError, ss_msg.str());
#endif
  } else if(extractorName == "LATCH") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
    m_extractors[extractorName] = cv::xfeatures2d::LATCH::create();
#else
    std::stringstream ss_msg;
    ss_msg << "Fail to initialize the extractor: LATCH. OpenCV version  "
        << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
    throw vpException(vpException::fatalError, ss_msg.str());
#endif
  } else if(extractorName == "LUCID") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
    m_extractors[extractorName] = cv::xfeatures2d::LUCID::create(1, 2);
#else
    std::stringstream ss_msg;
    ss_msg << "Fail to initialize the extractor: LUCID. OpenCV version  "
        << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
    throw vpException(vpException::fatalError, ss_msg.str());
#endif
  } else {
    std::cerr << "The extractor:" << extractorName << " is not available." << std::endl;
  }
#endif

  if(m_extractors[extractorName] == NULL) {
    std::stringstream ss_msg;
    ss_msg << "Fail to initialize the extractor: " << extractorName << " or it is not available in OpenCV version: "
        << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
    throw vpException(vpException::fatalError, ss_msg.str());
  }

#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
  if(extractorName == "SURF") {
    //Use extended set of SURF descriptors (128 instead of 64)
    m_extractors[extractorName]->set("extended", 1);
  }
#endif
}

void vpKeyPoint::initExtractors(const std::vector<std::string> &extractorNames) {
  for(std::vector<std::string>::const_iterator it = extractorNames.begin(); it != extractorNames.end(); ++it) {
    initExtractor(*it);
  }

  int descriptorType = CV_32F;
  bool firstIteration = true;
  for(std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator it = m_extractors.begin();
      it != m_extractors.end(); ++it) {
    if(firstIteration) {
      firstIteration = false;
      descriptorType = it->second->descriptorType();
    } else {
      if(descriptorType != it->second->descriptorType()) {
        throw vpException(vpException::fatalError, "All the descriptors must have the same type !");
      }
    }
  }
}

void vpKeyPoint::initMatcher(const std::string &matcherName) {
  int descriptorType = CV_32F;
  bool firstIteration = true;
  for(std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator it = m_extractors.begin();
      it != m_extractors.end(); ++it) {
    if(firstIteration) {
      firstIteration = false;
      descriptorType = it->second->descriptorType();
    } else {
      if(descriptorType != it->second->descriptorType()) {
        throw vpException(vpException::fatalError, "All the descriptors must have the same type !");
      }
    }
  }

  if(matcherName == "FlannBased") {
    if(m_extractors.empty()) {
      std::cout << "Warning: No extractor initialized, by default use floating values (CV_32F) "
          "for descriptor type !" << std::endl;
    }

    if(descriptorType == CV_8U) {
#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
      m_matcher = cv::makePtr<cv::FlannBasedMatcher>(cv::makePtr<cv::flann::LshIndexParams>(12, 20, 2));
#else
      m_matcher = new cv::FlannBasedMatcher(new cv::flann::LshIndexParams(12, 20, 2));
#endif
    } else {
#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
      m_matcher = cv::makePtr<cv::FlannBasedMatcher>(cv::makePtr<cv::flann::KDTreeIndexParams>());
#else
      m_matcher = new cv::FlannBasedMatcher(new cv::flann::KDTreeIndexParams());
#endif
    }
  } else {
    m_matcher = cv::DescriptorMatcher::create(matcherName);
  }

#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
  if(m_matcher != NULL && !m_useKnn && matcherName == "BruteForce") {
    m_matcher->set("crossCheck", m_useBruteForceCrossCheck);
  }
#endif

  if(m_matcher == NULL) {
    std::stringstream ss_msg;
    ss_msg << "Fail to initialize the matcher: " << matcherName << " or it is not available in OpenCV version: "
        << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
    throw vpException(vpException::fatalError, ss_msg.str());
  }
}

void vpKeyPoint::insertImageMatching(const vpImage<unsigned char> &IRef, const vpImage<unsigned char> &ICurrent,
                                     vpImage<unsigned char> &IMatching) {
  vpImagePoint topLeftCorner(0, 0);
  IMatching.insert(IRef, topLeftCorner);
  topLeftCorner = vpImagePoint(0, IRef.getWidth());
  IMatching.insert(ICurrent, topLeftCorner);
}

void vpKeyPoint::insertImageMatching(const vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching) {
  //Nb images in the training database + the current image we want to detect the object
  int nbImg = (int) (m_mapOfImages.size() + 1);

  if(m_mapOfImages.empty()) {
    std::cerr << "There is no training image loaded !" << std::endl;
    return;
  }

  if(nbImg == 2) {
    //Only one training image, so we display them side by side
    insertImageMatching(m_mapOfImages.begin()->second, ICurrent, IMatching);
  } else {
    //Multiple training images, display them as a mosaic image
    int nbImgSqrt = vpMath::round(std::sqrt((double) nbImg)); //(int) std::floor(std::sqrt((double) nbImg) + 0.5);
    int nbWidth = nbImgSqrt;
    int nbHeight = nbImgSqrt;

    if(nbImgSqrt * nbImgSqrt < nbImg) {
      nbWidth++;
    }

    unsigned int maxW = ICurrent.getWidth(), maxH = ICurrent.getHeight();
    for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
      if(maxW < it->second.getWidth()) {
        maxW = it->second.getWidth();
      }

      if(maxH < it->second.getHeight()) {
        maxH = it->second.getHeight();
      }
    }

    //Indexes of the current image in the grid made in order to the image is in the center square in the mosaic grid
    int medianI = nbHeight / 2;
    int medianJ = nbWidth / 2;
    int medianIndex = medianI * nbWidth + medianJ;

    int cpt = 0;
    for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
      int local_cpt = cpt;
      if(cpt >= medianIndex) {
        //Shift of one unity the index of the training images which are after the current image
        local_cpt++;
      }
      int indexI = local_cpt / nbWidth;
      int indexJ = local_cpt - (indexI * nbWidth);
      vpImagePoint topLeftCorner((int)maxH*indexI, (int)maxW*indexJ);

      IMatching.insert(it->second, topLeftCorner);
    }

    vpImagePoint topLeftCorner((int)maxH*medianI, (int)maxW*medianJ);
    IMatching.insert(ICurrent, topLeftCorner);
  }
}

#ifdef VISP_HAVE_XML2

void vpKeyPoint::loadConfigFile(const std::string &configFile) {
  vpXmlConfigParserKeyPoint xmlp;

  try {
    //Reset detector and extractor
    m_detectorNames.clear();
    m_extractorNames.clear();
    m_detectors.clear();
    m_extractors.clear();

    std::cout << " *********** Parsing XML for configuration for vpKeyPoint ************ " << std::endl;
    xmlp.parse(configFile);

    m_detectorNames.push_back(xmlp.getDetectorName());
    m_extractorNames.push_back(xmlp.getExtractorName());
    m_matcherName = xmlp.getMatcherName();

    switch(xmlp.getMatchingMethod()) {
    case vpXmlConfigParserKeyPoint::constantFactorDistanceThreshold:
      m_filterType = constantFactorDistanceThreshold;
      break;

    case vpXmlConfigParserKeyPoint::stdDistanceThreshold:
      m_filterType = stdDistanceThreshold;
      break;

    case vpXmlConfigParserKeyPoint::ratioDistanceThreshold:
      m_filterType = ratioDistanceThreshold;
      break;

    case vpXmlConfigParserKeyPoint::stdAndRatioDistanceThreshold:
      m_filterType = stdAndRatioDistanceThreshold;
      break;

    case vpXmlConfigParserKeyPoint::noFilterMatching:
      m_filterType = noFilterMatching;
      break;

    default:
      break;
    }

    m_matchingFactorThreshold = xmlp.getMatchingFactorThreshold();
    m_matchingRatioThreshold = xmlp.getMatchingRatioThreshold();

    m_useRansacVVS = xmlp.getUseRansacVVSPoseEstimation();
    m_useConsensusPercentage = xmlp.getUseRansacConsensusPercentage();
    m_nbRansacIterations = xmlp.getNbRansacIterations();
    m_ransacReprojectionError = xmlp.getRansacReprojectionError();
    m_nbRansacMinInlierCount = xmlp.getNbRansacMinInlierCount();
    m_ransacThreshold = xmlp.getRansacThreshold();
    m_ransacConsensusPercentage = xmlp.getRansacConsensusPercentage();

    if(m_filterType == ratioDistanceThreshold || m_filterType == stdAndRatioDistanceThreshold) {
      m_useKnn = true;
    } else {
      m_useKnn = false;
    }

    init();
  }
  catch(...) {
    throw vpException(vpException::ioError, "Can't open XML file \"%s\"\n ", configFile.c_str());
  }
}
#endif

void vpKeyPoint::loadLearningData(const std::string &filename, const bool binaryMode, const bool append) {
  int startClassId = 0;
  int startImageId = 0;
  if(!append) {
    m_trainKeyPoints.clear();
    m_trainPoints.clear();
    m_mapOfImageId.clear();
    m_mapOfImages.clear();
  } else {
    //In append case, find the max index of keypoint class Id
    for(std::map<int, int>::const_iterator it = m_mapOfImageId.begin(); it != m_mapOfImageId.end(); ++it) {
      if(startClassId < it->first) {
        startClassId = it->first;
      }
    }

    //In append case, find the max index of images Id
    for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
      if(startImageId < it->first) {
        startImageId = it->first;
      }
    }
  }

  //Get parent directory
  std::string parent = vpIoTools::getParent(filename);
  if(!parent.empty()) {
    parent += "/";
  }

  if(binaryMode) {
    std::ifstream file(filename.c_str(), std::ifstream::binary);
    if(!file.is_open()){
      throw vpException(vpException::ioError, "Cannot open the file.");
    }

    //Read info about training images
    int nbImgs = 0;
    file.read((char *)(&nbImgs), sizeof(nbImgs));

#if !defined(VISP_HAVE_MODULE_IO)
    if(nbImgs > 0) {
      std::cout << "Warning: The learning file contains image data that will not be loaded as visp_io module "
          "is not available !" << std::endl;
    }
#endif

    for(int i = 0; i < nbImgs; i++) {
      //Read image_id
      int id = 0;
      file.read((char *)(&id), sizeof(id));

      int length = 0;
      file.read((char *)(&length), sizeof(length));
      //Will contain the path to the training images
      char* path = new char[length + 1];//char path[length + 1];

      for(int cpt = 0; cpt < length; cpt++) {
        char c;
        file.read((char *)(&c), sizeof(c));
        path[cpt] = c;
      }
      path[length] = '\0';

      vpImage<unsigned char> I;
#ifdef VISP_HAVE_MODULE_IO
      if(vpIoTools::isAbsolutePathname(std::string(path))) {
        vpImageIo::read(I, path);
      } else {
        vpImageIo::read(I, parent + path);
      }

      //Add the image previously loaded only if VISP_HAVE_MODULE_IO
      m_mapOfImages[id + startImageId] = I;
#endif

      //Delete path
      delete[] path;
    }

    //Read if 3D point information are saved or not
    int have3DInfoInt = 0;
    file.read((char *)(&have3DInfoInt), sizeof(have3DInfoInt));
    bool have3DInfo = have3DInfoInt != 0;

    //Read the number of descriptors
    int nRows = 0;
    file.read((char *)(&nRows), sizeof(nRows));

    //Read the size of the descriptor
    int nCols = 0;
    file.read((char *)(&nCols), sizeof(nCols));

    //Read the type of the descriptor
    int descriptorType = 5; //CV_32F
    file.read((char *)(&descriptorType), sizeof(descriptorType));

    cv::Mat trainDescriptorsTmp = cv::Mat(nRows, nCols, descriptorType);
    for(int i = 0; i < nRows; i++) {
      //Read information about keyPoint
      float u, v, size, angle, response;
      int octave, class_id, image_id;
      file.read((char *)(&u), sizeof(u));
      file.read((char *)(&v), sizeof(v));
      file.read((char *)(&size), sizeof(size));
      file.read((char *)(&angle), sizeof(angle));
      file.read((char *)(&response), sizeof(response));
      file.read((char *)(&octave), sizeof(octave));
      file.read((char *)(&class_id), sizeof(class_id));
      file.read((char *)(&image_id), sizeof(image_id));
      cv::KeyPoint keyPoint(cv::Point2f(u, v), size, angle, response, octave, (class_id + startClassId));
      m_trainKeyPoints.push_back(keyPoint);

      if(image_id != -1) {
#ifdef VISP_HAVE_MODULE_IO
        //No training images if image_id == -1
        m_mapOfImageId[class_id] = image_id + startImageId;
#endif
      }

      if(have3DInfo) {
        //Read oX, oY, oZ
        float oX, oY, oZ;
        file.read((char *)(&oX), sizeof(oX));
        file.read((char *)(&oY), sizeof(oY));
        file.read((char *)(&oZ), sizeof(oZ));
        m_trainPoints.push_back(cv::Point3f(oX, oY, oZ));
      }

      for(int j = 0; j < nCols; j++) {
        //Read the value of the descriptor
        switch(descriptorType) {
          case CV_8U:
          {
            unsigned char value;
            file.read((char *)(&value), sizeof(value));
            trainDescriptorsTmp.at<unsigned char>(i, j) = value;
          }
          break;

          case CV_8S:
          {
            char value;
            file.read((char *)(&value), sizeof(value));
            trainDescriptorsTmp.at<char>(i, j) = value;
          }
          break;

          case CV_16U:
          {
            unsigned short int value;
            file.read((char *)(&value), sizeof(value));
            trainDescriptorsTmp.at<unsigned short int>(i, j) = value;
          }
          break;

          case CV_16S:
          {
            short int value;
            file.read((char *)(&value), sizeof(value));
            trainDescriptorsTmp.at<short int>(i, j) = value;
          }
          break;

          case CV_32S:
          {
            int value;
            file.read((char *)(&value), sizeof(value));
            trainDescriptorsTmp.at<int>(i, j) = value;
          }
          break;

          case CV_32F:
          {
            float value;
            file.read((char *)(&value), sizeof(value));
            trainDescriptorsTmp.at<float>(i, j) = value;
          }
          break;

          case CV_64F:
          {
            double value;
            file.read((char *)(&value), sizeof(value));
            trainDescriptorsTmp.at<double>(i, j) = value;
          }
          break;

          default:
          {
            float value;
            file.read((char *)(&value), sizeof(value));
            trainDescriptorsTmp.at<float>(i, j) = value;
          }
          break;
        }
      }
    }

    if(!append || m_trainDescriptors.empty()) {
      trainDescriptorsTmp.copyTo(m_trainDescriptors);
    } else {
      cv::vconcat(m_trainDescriptors, trainDescriptorsTmp, m_trainDescriptors);
    }

    file.close();
  } else {
#ifdef VISP_HAVE_XML2
    xmlDocPtr doc = NULL;
    xmlNodePtr root_element = NULL;

    /*
     * this initialize the library and check potential ABI mismatches
     * between the version it was compiled for and the actual shared
     * library used.
     */
    LIBXML_TEST_VERSION

    /*parse the file and get the DOM */
    doc = xmlReadFile(filename.c_str(), NULL, 0);

    if (doc == NULL) {
      throw vpException(vpException::ioError, "Error with file " + filename);
    }

    root_element = xmlDocGetRootElement(doc);

    xmlNodePtr first_level_node = NULL;
    char *pEnd = NULL;

    int descriptorType = CV_32F; //float
    int nRows = 0, nCols = 0;
    int i = 0, j = 0;

    cv::Mat trainDescriptorsTmp;

    for (first_level_node = root_element->children; first_level_node;
         first_level_node = first_level_node->next) {

      std::string name((char *) first_level_node->name);
      if (first_level_node->type == XML_ELEMENT_NODE && name == "TrainingImageInfo") {
        xmlNodePtr image_info_node = NULL;

        for (image_info_node = first_level_node->children; image_info_node; image_info_node =
             image_info_node->next) {
          name = std::string ((char *) image_info_node->name);

          if(name == "trainImg") {
            //Read image_id
            xmlChar *image_id_property = xmlGetProp(image_info_node, BAD_CAST "image_id");
            int id = 0;
            if(image_id_property) {
              id = std::atoi((char *) image_id_property);
            }
            xmlFree(image_id_property);

            vpImage<unsigned char> I;
#ifdef VISP_HAVE_MODULE_IO
            std::string path((char *) image_info_node->children->content);
            //Read path to the training images
            if(vpIoTools::isAbsolutePathname(std::string(path))) {
              vpImageIo::read(I, path);
            } else {
              vpImageIo::read(I, parent + path);
            }

            //Add the image previously loaded only if VISP_HAVE_MODULE_IO
            m_mapOfImages[id + startImageId] = I;
#endif
          }
        }
      } else if(first_level_node->type == XML_ELEMENT_NODE && name == "DescriptorsInfo") {
        xmlNodePtr descriptors_info_node = NULL;
        for (descriptors_info_node = first_level_node->children; descriptors_info_node; descriptors_info_node =
            descriptors_info_node->next) {
          if (descriptors_info_node->type == XML_ELEMENT_NODE) {
            name = std::string ((char *) descriptors_info_node->name);

            if(name == "nrows") {
              nRows = std::atoi((char *) descriptors_info_node->children->content);
            } else if(name == "ncols") {
              nCols = std::atoi((char *) descriptors_info_node->children->content);
            } else if(name == "type") {
              descriptorType = std::atoi((char *) descriptors_info_node->children->content);
            }
          }
        }

        trainDescriptorsTmp = cv::Mat(nRows, nCols, descriptorType);
      } else if (first_level_node->type == XML_ELEMENT_NODE && name == "DescriptorInfo") {
        xmlNodePtr point_node = NULL;
        double u = 0.0, v = 0.0, size = 0.0, angle = 0.0, response = 0.0;
        int octave = 0, class_id = 0, image_id = 0;
        double oX = 0.0, oY = 0.0, oZ = 0.0;

        std::stringstream ss;

        for (point_node = first_level_node->children; point_node; point_node =
             point_node->next) {
          if (point_node->type == XML_ELEMENT_NODE) {
            name = std::string ((char *) point_node->name);

            //Read information about keypoints
            if(name == "u") {
              u = std::strtod((char *) point_node->children->content, &pEnd);
            } else if(name == "v") {
              v = std::strtod((char *) point_node->children->content, &pEnd);
            } else if(name == "size") {
              size = std::strtod((char *) point_node->children->content, &pEnd);
            } else if(name == "angle") {
              angle = std::strtod((char *) point_node->children->content, &pEnd);
            } else if(name == "response") {
              response = std::strtod((char *) point_node->children->content, &pEnd);
            } else if(name == "octave") {
              octave = std::atoi((char *) point_node->children->content);
            } else if(name == "class_id") {
              class_id = std::atoi((char *) point_node->children->content);
              cv::KeyPoint keyPoint(cv::Point2f((float) u, (float) v), (float) size,
                                    (float) angle, (float) response, octave, (class_id + startClassId));
              m_trainKeyPoints.push_back(keyPoint);
            } else if(name == "image_id") {
              image_id = std::atoi((char *) point_node->children->content);
              if(image_id != -1) {
#ifdef VISP_HAVE_MODULE_IO
                //No training images if image_id == -1
                m_mapOfImageId[m_trainKeyPoints.back().class_id] = image_id + startImageId;
#endif
              }
            } else if (name == "oX") {
              oX = std::atof((char *) point_node->children->content);
            } else if (name == "oY") {
              oY = std::atof((char *) point_node->children->content);
            } else if (name == "oZ") {
              oZ = std::atof((char *) point_node->children->content);
              m_trainPoints.push_back(cv::Point3f((float) oX, (float) oY, (float) oZ));
            } else if (name == "desc") {
              xmlNodePtr descriptor_value_node = NULL;
              j = 0;

              for (descriptor_value_node = point_node->children;
                   descriptor_value_node; descriptor_value_node =
                   descriptor_value_node->next) {

                if (descriptor_value_node->type == XML_ELEMENT_NODE) {
                  //Read descriptors values
                  std::string parseStr((char *) descriptor_value_node->children->content);
                  ss.clear();
                  ss.str(parseStr);

                  if(!ss.fail()) {
                    switch(descriptorType) {
                      case CV_8U:
                      {
                        //Parse the numeric value [0 ; 255] to an int
                        int parseValue;
                        ss >> parseValue;
                        trainDescriptorsTmp.at<unsigned char>(i, j) = (unsigned char) parseValue;
                      }
                      break;

                      case CV_8S:
                        //Parse the numeric value [-128 ; 127] to an int
                        int parseValue;
                        ss >> parseValue;
                        trainDescriptorsTmp.at<char>(i, j) = (char) parseValue;
                      break;

                      case CV_16U:
                        ss >> trainDescriptorsTmp.at<unsigned short int>(i, j);
                      break;

                      case CV_16S:
                        ss >> trainDescriptorsTmp.at<short int>(i, j);
                      break;

                      case CV_32S:
                        ss >> trainDescriptorsTmp.at<int>(i, j);
                      break;

                      case CV_32F:
                        ss >> trainDescriptorsTmp.at<float>(i, j);
                      break;

                      case CV_64F:
                        ss >> trainDescriptorsTmp.at<double>(i, j);
                      break;

                      default:
                        ss >> trainDescriptorsTmp.at<float>(i, j);
                      break;
                    }
                  } else {
                    std::cerr << "Error when converting:" << ss.str() << std::endl;
                  }

                  j++;
                }
              }
            }
          }
        }
        i++;
      }
    }

    if(!append || m_trainDescriptors.empty()) {
      trainDescriptorsTmp.copyTo(m_trainDescriptors);
    } else {
      cv::vconcat(m_trainDescriptors, trainDescriptorsTmp, m_trainDescriptors);
    }

    /*free the document */
    xmlFreeDoc(doc);

    /*
     *Free the global variables that may
     *have been allocated by the parser.
     */
    xmlCleanupParser();
#else
    std::cout << "Error: libxml2 is required !" << std::endl;
#endif
  }

  //Convert OpenCV type to ViSP type for compatibility
  vpConvert::convertFromOpenCV(m_trainKeyPoints, referenceImagePointsList);
  vpConvert::convertFromOpenCV(this->m_trainPoints, m_trainVpPoints);

  //Add train descriptors in matcher object
  m_matcher->clear();
  m_matcher->add(std::vector<cv::Mat>(1, m_trainDescriptors));

  //Set _reference_computed to true as we load learning file
  _reference_computed = true;
}

void vpKeyPoint::match(const cv::Mat &trainDescriptors, const cv::Mat &queryDescriptors,
                       std::vector<cv::DMatch> &matches, double &elapsedTime) {
  double t = vpTime::measureTimeMs();

  if(m_useKnn) {
    m_knnMatches.clear();

    if(m_useMatchTrainToQuery) {
      std::vector<std::vector<cv::DMatch> > knnMatchesTmp;

      //Match train descriptors to query descriptors
      cv::Ptr<cv::DescriptorMatcher> matcherTmp = m_matcher->clone(true);
      matcherTmp->knnMatch(trainDescriptors, queryDescriptors, knnMatchesTmp, 2);

      for(std::vector<std::vector<cv::DMatch> >::const_iterator it1 = knnMatchesTmp.begin(); it1 != knnMatchesTmp.end(); ++it1) {
        std::vector<cv::DMatch> tmp;
        for(std::vector<cv::DMatch>::const_iterator it2 = it1->begin(); it2 != it1->end(); ++it2) {
          tmp.push_back(cv::DMatch(it2->trainIdx, it2->queryIdx, it2->distance));
        }
        m_knnMatches.push_back(tmp);
      }

      matches.resize(m_knnMatches.size());
      std::transform(m_knnMatches.begin(), m_knnMatches.end(), matches.begin(), knnToDMatch);
    } else {
      //Match query descriptors to train descriptors
      m_matcher->knnMatch(queryDescriptors, m_knnMatches, 2);
      matches.resize(m_knnMatches.size());
      std::transform(m_knnMatches.begin(), m_knnMatches.end(), matches.begin(), knnToDMatch);
    }
  } else {
    matches.clear();

    if(m_useMatchTrainToQuery) {
      std::vector<cv::DMatch> matchesTmp;
      //Match train descriptors to query descriptors
      cv::Ptr<cv::DescriptorMatcher> matcherTmp = m_matcher->clone(true);
      matcherTmp->match(trainDescriptors, queryDescriptors, matchesTmp);

      for(std::vector<cv::DMatch>::const_iterator it = matchesTmp.begin(); it != matchesTmp.end(); ++it) {
        matches.push_back(cv::DMatch(it->trainIdx, it->queryIdx, it->distance));
      }
    } else {
      //Match query descriptors to train descriptors
      m_matcher->match(queryDescriptors, matches);
    }
  }
  elapsedTime = vpTime::measureTimeMs() - t;
}

unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I) {
  return matchPoint(I, vpRect());
}

unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I,
                                    const vpImagePoint &iP,
                                    const unsigned int height, const unsigned int width) {
  return matchPoint(I, vpRect(iP, width, height));
}

unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I,
                                    const vpRect& rectangle) {
  if(m_trainDescriptors.empty()) {
    std::cerr << "Reference is empty." << std::endl;
    if(!_reference_computed) {
      std::cerr << "Reference is not computed." << std::endl;
    }
    std::cerr << "Matching is not possible." << std::endl;

    return 0;
  }

  if(m_useAffineDetection) {
    std::vector<std::vector<cv::KeyPoint> > listOfQueryKeyPoints;
    std::vector<cv::Mat> listOfQueryDescriptors;

    //Detect keypoints and extract descriptors on multiple images
    detectExtractAffine(I, listOfQueryKeyPoints, listOfQueryDescriptors);

    //Flatten the different train lists
    m_queryKeyPoints.clear();
    for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfQueryKeyPoints.begin();
        it != listOfQueryKeyPoints.end(); ++it) {
      m_queryKeyPoints.insert(m_queryKeyPoints.end(), it->begin(), it->end());
    }

    bool first = true;
    for(std::vector<cv::Mat>::const_iterator it = listOfQueryDescriptors.begin(); it != listOfQueryDescriptors.end(); ++it) {
      if(first) {
        first = false;
        it->copyTo(m_queryDescriptors);
      } else {
        m_queryDescriptors.push_back(*it);
      }
    }
  } else {
    detect(I, m_queryKeyPoints, m_detectionTime, rectangle);
    extract(I, m_queryKeyPoints, m_queryDescriptors, m_extractionTime);
  }

  match(m_trainDescriptors, m_queryDescriptors, m_matches, m_matchingTime);

  if(m_filterType != noFilterMatching) {
    m_queryFilteredKeyPoints.clear();
    m_objectFilteredPoints.clear();
    m_filteredMatches.clear();

    filterMatches();
  } else {
    if(m_useMatchTrainToQuery) {
      //Add only query keypoints matched with a train keypoints
      m_queryFilteredKeyPoints.clear();
      m_filteredMatches.clear();
      for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
        m_filteredMatches.push_back(cv::DMatch((int) m_queryFilteredKeyPoints.size(), it->trainIdx, it->distance));
        m_queryFilteredKeyPoints.push_back(m_queryKeyPoints[(size_t) it->queryIdx]);
      }
    } else {
      m_queryFilteredKeyPoints = m_queryKeyPoints;
      m_filteredMatches = m_matches;
    }

    if(!m_trainPoints.empty()) {
      m_objectFilteredPoints.clear();
      //Add 3D object points such as the same index in m_queryFilteredKeyPoints and in m_objectFilteredPoints
      // matches to the same train object
      for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
        //m_matches is normally ordered following the queryDescriptor index
        m_objectFilteredPoints.push_back(m_trainPoints[(size_t) it->trainIdx]);
      }
    }
  }

  //Convert OpenCV type to ViSP type for compatibility
  vpConvert::convertFromOpenCV(m_queryFilteredKeyPoints, currentImagePointsList);
  vpConvert::convertFromOpenCV(m_filteredMatches, matchedReferencePoints);

  return static_cast<unsigned int>(m_filteredMatches.size());
}

bool vpKeyPoint::matchPoint(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
                            double &error, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *), const vpRect& rectangle) {
  //Check if we have training descriptors
  if(m_trainDescriptors.empty()) {
    std::cerr << "Reference is empty." << std::endl;
    if(!_reference_computed) {
      std::cerr << "Reference is not computed." << std::endl;
    }
    std::cerr << "Matching is not possible." << std::endl;

    return false;
  }

  if(m_useAffineDetection) {
    std::vector<std::vector<cv::KeyPoint> > listOfQueryKeyPoints;
    std::vector<cv::Mat> listOfQueryDescriptors;

    //Detect keypoints and extract descriptors on multiple images
    detectExtractAffine(I, listOfQueryKeyPoints, listOfQueryDescriptors);

    //Flatten the different train lists
    m_queryKeyPoints.clear();
    for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfQueryKeyPoints.begin();
        it != listOfQueryKeyPoints.end(); ++it) {
      m_queryKeyPoints.insert(m_queryKeyPoints.end(), it->begin(), it->end());
    }

    bool first = true;
    for(std::vector<cv::Mat>::const_iterator it = listOfQueryDescriptors.begin(); it != listOfQueryDescriptors.end(); ++it) {
      if(first) {
        first = false;
        it->copyTo(m_queryDescriptors);
      } else {
        m_queryDescriptors.push_back(*it);
      }
    }
  } else {
    detect(I, m_queryKeyPoints, m_detectionTime, rectangle);
    extract(I, m_queryKeyPoints, m_queryDescriptors, m_extractionTime);
  }

  match(m_trainDescriptors, m_queryDescriptors, m_matches, m_matchingTime);

  elapsedTime = m_detectionTime + m_extractionTime + m_matchingTime;

  if(m_filterType != noFilterMatching) {
    m_queryFilteredKeyPoints.clear();
    m_objectFilteredPoints.clear();
    m_filteredMatches.clear();

    filterMatches();
  } else {
    if(m_useMatchTrainToQuery) {
      //Add only query keypoints matched with a train keypoints
      m_queryFilteredKeyPoints.clear();
      m_filteredMatches.clear();
      for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
        m_filteredMatches.push_back(cv::DMatch((int) m_queryFilteredKeyPoints.size(), it->trainIdx, it->distance));
        m_queryFilteredKeyPoints.push_back(m_queryKeyPoints[(size_t) it->queryIdx]);
      }
    } else {
      m_queryFilteredKeyPoints = m_queryKeyPoints;
      m_filteredMatches = m_matches;
    }

    if(!m_trainPoints.empty()) {
      m_objectFilteredPoints.clear();
      //Add 3D object points such as the same index in m_queryFilteredKeyPoints and in m_objectFilteredPoints
      // matches to the same train object
      for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
        //m_matches is normally ordered following the queryDescriptor index
        m_objectFilteredPoints.push_back(m_trainPoints[(size_t) it->trainIdx]);
      }
    }
  }

  //Convert OpenCV type to ViSP type for compatibility
  vpConvert::convertFromOpenCV(m_queryFilteredKeyPoints, currentImagePointsList);
  vpConvert::convertFromOpenCV(m_filteredMatches, matchedReferencePoints);

  //error = std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
  error = DBL_MAX;
  m_ransacInliers.clear();
  m_ransacOutliers.clear();

  if(m_useRansacVVS) {
    std::vector<vpPoint> objectVpPoints(m_objectFilteredPoints.size());
    size_t cpt = 0;
    //Create a list of vpPoint with 2D coordinates (current keypoint location) + 3D coordinates (world/object coordinates)
    for(std::vector<cv::Point3f>::const_iterator it = m_objectFilteredPoints.begin(); it != m_objectFilteredPoints.end();
        ++it, cpt++) {
      vpPoint pt;
      pt.setWorldCoordinates(it->x, it->y, it->z);

      vpImagePoint imP(m_queryFilteredKeyPoints[cpt].pt.y, m_queryFilteredKeyPoints[cpt].pt.x);

      double x = 0.0, y = 0.0;
      vpPixelMeterConversion::convertPoint(cam, imP, x, y);
      pt.set_x(x);
      pt.set_y(y);

      objectVpPoints[cpt] = pt;
    }

    std::vector<vpPoint> inliers;
    std::vector<unsigned int> inlierIndex;

    bool res = computePose(objectVpPoints, cMo, inliers, inlierIndex, m_poseTime, func);

    std::map<unsigned int, bool> mapOfInlierIndex;
    m_matchRansacKeyPointsToPoints.clear();

    for (std::vector<unsigned int>::const_iterator it = inlierIndex.begin(); it != inlierIndex.end(); ++it) {
      m_matchRansacKeyPointsToPoints.push_back(std::pair<cv::KeyPoint, cv::Point3f>(m_queryFilteredKeyPoints[(size_t)(*it)],
                                              m_objectFilteredPoints[(size_t)(*it)]));
      mapOfInlierIndex[*it] = true;
    }

    for(size_t i = 0; i < m_queryFilteredKeyPoints.size(); i++) {
      if(mapOfInlierIndex.find((unsigned int) i) == mapOfInlierIndex.end()) {
        m_ransacOutliers.push_back(vpImagePoint(m_queryFilteredKeyPoints[i].pt.y, m_queryFilteredKeyPoints[i].pt.x));
      }
    }

    error = computePoseEstimationError(m_matchRansacKeyPointsToPoints, cam, cMo);

    m_ransacInliers.resize(m_matchRansacKeyPointsToPoints.size());
    std::transform(m_matchRansacKeyPointsToPoints.begin(), m_matchRansacKeyPointsToPoints.end(), m_ransacInliers.begin(),
                   matchRansacToVpImage);

    elapsedTime += m_poseTime;

    return res;
  } else {
    std::vector<cv::Point2f> imageFilteredPoints;
    cv::KeyPoint::convert(m_queryFilteredKeyPoints, imageFilteredPoints);
    std::vector<int> inlierIndex;
    bool res = computePose(imageFilteredPoints, m_objectFilteredPoints, cam, cMo, inlierIndex, m_poseTime);

    std::map<int, bool> mapOfInlierIndex;
    m_matchRansacKeyPointsToPoints.clear();

    for (std::vector<int>::const_iterator it = inlierIndex.begin(); it != inlierIndex.end(); ++it) {
      m_matchRansacKeyPointsToPoints.push_back(std::pair<cv::KeyPoint, cv::Point3f>(m_queryFilteredKeyPoints[(size_t)(*it)],
                                              m_objectFilteredPoints[(size_t)(*it)]));
      mapOfInlierIndex[*it] = true;
    }

    for(size_t i = 0; i < m_queryFilteredKeyPoints.size(); i++) {
      if(mapOfInlierIndex.find((int) i) == mapOfInlierIndex.end()) {
        m_ransacOutliers.push_back(vpImagePoint(m_queryFilteredKeyPoints[i].pt.y, m_queryFilteredKeyPoints[i].pt.x));
      }
    }

    error = computePoseEstimationError(m_matchRansacKeyPointsToPoints, cam, cMo);

    m_ransacInliers.resize(m_matchRansacKeyPointsToPoints.size());
    std::transform(m_matchRansacKeyPointsToPoints.begin(), m_matchRansacKeyPointsToPoints.end(), m_ransacInliers.begin(),
                   matchRansacToVpImage);

    elapsedTime += m_poseTime;

    return res;
  }
}

bool vpKeyPoint::matchPointAndDetect(const vpImage<unsigned char> &I, vpRect &boundingBox, vpImagePoint &centerOfGravity,
    const bool isPlanarObject, std::vector<vpImagePoint> *imPts1, std::vector<vpImagePoint> *imPts2,
    double *meanDescriptorDistance, double *detection_score, const vpRect& rectangle) {
  if(imPts1 != NULL && imPts2 != NULL) {
    imPts1->clear();
    imPts2->clear();
  }

  matchPoint(I, rectangle);

  double meanDescriptorDistanceTmp = 0.0;
  for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
    meanDescriptorDistanceTmp += (double) it->distance;
  }

  meanDescriptorDistanceTmp /= (double) m_filteredMatches.size();
  double score = (double) m_filteredMatches.size() / meanDescriptorDistanceTmp;

  if(meanDescriptorDistance != NULL) {
    *meanDescriptorDistance = meanDescriptorDistanceTmp;
  }
  if(detection_score != NULL) {
    *detection_score = score;
  }

  if(m_filteredMatches.size() >= 4) {
    //Training / Reference 2D points
    std::vector<cv::Point2f> points1(m_filteredMatches.size());
    //Query / Current 2D points
    std::vector<cv::Point2f> points2(m_filteredMatches.size());

    for(size_t i = 0; i < m_filteredMatches.size(); i++) {
      points1[i] = cv::Point2f(m_trainKeyPoints[(size_t)m_filteredMatches[i].trainIdx].pt);
      points2[i] = cv::Point2f(m_queryFilteredKeyPoints[(size_t)m_filteredMatches[i].queryIdx].pt);
    }

    std::vector<vpImagePoint> inliers;
    if(isPlanarObject) {
#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
      cv::Mat homographyMatrix = cv::findHomography(points1, points2, CV_RANSAC);
#else
      cv::Mat homographyMatrix = cv::findHomography(points1, points2, cv::RANSAC);
#endif

      for(size_t i = 0; i < m_filteredMatches.size(); i++ ) {
        //Compute reprojection error
        cv::Mat realPoint = cv::Mat(3, 1, CV_64F);
        realPoint.at<double>(0,0) = points1[i].x;
        realPoint.at<double>(1,0) = points1[i].y;
        realPoint.at<double>(2,0) = 1.f;

        cv::Mat reprojectedPoint = homographyMatrix * realPoint;
        double err_x = (reprojectedPoint.at<double>(0,0) / reprojectedPoint.at<double>(2,0)) - points2[i].x;
        double err_y = (reprojectedPoint.at<double>(1,0) / reprojectedPoint.at<double>(2,0)) - points2[i].y;
        double reprojectionError = std::sqrt(err_x*err_x + err_y*err_y);

        if(reprojectionError < 6.0) {
          inliers.push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
          if(imPts1 != NULL) {
            imPts1->push_back(vpImagePoint((double) points1[i].y, (double) points1[i].x));
          }

          if(imPts2 != NULL) {
            imPts2->push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
          }
        }
      }
    } else if(m_filteredMatches.size() >= 8) {
      cv::Mat fundamentalInliers;
      cv::Mat fundamentalMatrix = cv::findFundamentalMat(points1, points2, cv::FM_RANSAC, 3, 0.99, fundamentalInliers);

      for(size_t i = 0; i < (size_t) fundamentalInliers.rows; i++) {
        if(fundamentalInliers.at<uchar>((int) i, 0)) {
          inliers.push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));

          if(imPts1 != NULL) {
            imPts1->push_back(vpImagePoint((double) points1[i].y, (double) points1[i].x));
          }

          if(imPts2 != NULL) {
            imPts2->push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
          }
        }
      }
    }

    if(!inliers.empty()) {
      //Build a polygon with the list of inlier keypoints detected in the current image to get the bounding box
      vpPolygon polygon(inliers);
      boundingBox = polygon.getBoundingBox();

      //Compute the center of gravity
      double meanU = 0.0, meanV = 0.0;
      for(std::vector<vpImagePoint>::const_iterator it = inliers.begin(); it != inliers.end(); ++it) {
        meanU += it->get_u();
        meanV += it->get_v();
      }

      meanU /= (double) inliers.size();
      meanV /= (double) inliers.size();

      centerOfGravity.set_u(meanU);
      centerOfGravity.set_v(meanV);
    }
  } else {
    //Too few matches
    return false;
  }

  if(m_detectionMethod == detectionThreshold) {
    return meanDescriptorDistanceTmp < m_detectionThreshold;
  } else {
    return score > m_detectionScore;
  }
}

bool vpKeyPoint::matchPointAndDetect(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
                                     double &error, double &elapsedTime, vpRect &boundingBox, vpImagePoint &centerOfGravity,
                                     bool (*func)(vpHomogeneousMatrix *), const vpRect& rectangle) {
  bool isMatchOk = matchPoint(I, cam, cMo, error, elapsedTime, func, rectangle);
  if(isMatchOk) {
    //Use the pose estimated to project the model points in the image
    vpPoint pt;
    vpImagePoint imPt;
    std::vector<vpImagePoint> modelImagePoints(m_trainVpPoints.size());
    size_t cpt = 0;
    for(std::vector<vpPoint>::const_iterator it = m_trainVpPoints.begin(); it != m_trainVpPoints.end(); ++it, cpt++) {
      pt = *it;
      pt.project(cMo);
      vpMeterPixelConversion::convertPoint(cam, pt.get_x(), pt.get_y(), imPt);
      modelImagePoints[cpt] = imPt;
    }

    //Build a polygon with the list of model image points to get the bounding box
    vpPolygon polygon(modelImagePoints);
    boundingBox = polygon.getBoundingBox();

    //Compute the center of gravity of the current inlier keypoints
    double meanU = 0.0, meanV = 0.0;
    for(std::vector<vpImagePoint>::const_iterator it = m_ransacInliers.begin(); it != m_ransacInliers.end();
        ++it) {
      meanU += it->get_u();
      meanV += it->get_v();
    }

    meanU /= (double) m_ransacInliers.size();
    meanV /= (double) m_ransacInliers.size();

    centerOfGravity.set_u(meanU);
    centerOfGravity.set_v(meanV);
  }

  return isMatchOk;
}

void vpKeyPoint::detectExtractAffine(const vpImage<unsigned char> &I,std::vector<std::vector<cv::KeyPoint> >& listOfKeypoints,
    std::vector<cv::Mat>& listOfDescriptors, std::vector<vpImage<unsigned char> > *listOfAffineI) {
#if 0
  cv::Mat img;
  vpImageConvert::convert(I, img);
  listOfKeypoints.clear();
  listOfDescriptors.clear();

  for (int tl = 1; tl < 6; tl++) {
    double t = pow(2, 0.5 * tl);
    for (int phi = 0; phi < 180; phi += (int)(72.0 / t)) {
      std::vector<cv::KeyPoint> keypoints;
      cv::Mat descriptors;

      cv::Mat timg, mask, Ai;
      img.copyTo(timg);

      affineSkew(t, phi, timg, mask, Ai);


      if(listOfAffineI != NULL) {
        cv::Mat img_disp;
        bitwise_and(mask, timg, img_disp);
        vpImage<unsigned char> tI;
        vpImageConvert::convert(img_disp, tI);
        listOfAffineI->push_back(tI);
      }
#if 0
      cv::Mat img_disp;
      cv::bitwise_and(mask, timg, img_disp);
      cv::namedWindow( "Skew", cv::WINDOW_AUTOSIZE ); // Create a window for display.
      cv::imshow( "Skew", img_disp );
      cv::waitKey(0);
#endif

      for(std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin();
          it != m_detectors.end(); ++it) {
        std::vector<cv::KeyPoint> kp;
        it->second->detect(timg, kp, mask);
        keypoints.insert(keypoints.end(), kp.begin(), kp.end());
      }

      double elapsedTime;
      extract(timg, keypoints, descriptors, elapsedTime);

      for(unsigned int i = 0; i < keypoints.size(); i++) {
        cv::Point3f kpt(keypoints[i].pt.x, keypoints[i].pt.y, 1.f);
        cv::Mat kpt_t = Ai * cv::Mat(kpt);
        keypoints[i].pt.x = kpt_t.at<float>(0, 0);
        keypoints[i].pt.y = kpt_t.at<float>(1, 0);
      }

      listOfKeypoints.push_back(keypoints);
      listOfDescriptors.push_back(descriptors);
    }
  }

#else
  cv::Mat img;
  vpImageConvert::convert(I, img);

  //Create a vector for storing the affine skew parameters
  std::vector<std::pair<double, int> > listOfAffineParams;
  for (int tl = 1; tl < 6; tl++) {
    double t = pow(2, 0.5 * tl);
    for (int phi = 0; phi < 180; phi += (int)(72.0 / t)) {
      listOfAffineParams.push_back(std::pair<double, int>(t, phi));
    }
  }

  listOfKeypoints.resize(listOfAffineParams.size());
  listOfDescriptors.resize(listOfAffineParams.size());

  if(listOfAffineI != NULL) {
    listOfAffineI->resize(listOfAffineParams.size());
  }

#ifdef VISP_HAVE_OPENMP
  #pragma omp parallel for
#endif
  for(int cpt = 0; cpt < static_cast<int>(listOfAffineParams.size()); cpt++) {
    std::vector<cv::KeyPoint> keypoints;
    cv::Mat descriptors;

    cv::Mat timg, mask, Ai;
    img.copyTo(timg);

    affineSkew(listOfAffineParams[(size_t) cpt].first, listOfAffineParams[(size_t) cpt].second, timg, mask, Ai);


    if(listOfAffineI != NULL) {
      cv::Mat img_disp;
      bitwise_and(mask, timg, img_disp);
      vpImage<unsigned char> tI;
      vpImageConvert::convert(img_disp, tI);
      (*listOfAffineI)[(size_t) cpt] = tI;
    }

#if 0
    cv::Mat img_disp;
    cv::bitwise_and(mask, timg, img_disp);
    cv::namedWindow( "Skew", cv::WINDOW_AUTOSIZE ); // Create a window for display.
    cv::imshow( "Skew", img_disp );
    cv::waitKey(0);
#endif

    for(std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin();
        it != m_detectors.end(); ++it) {
      std::vector<cv::KeyPoint> kp;
      it->second->detect(timg, kp, mask);
      keypoints.insert(keypoints.end(), kp.begin(), kp.end());
    }

    double elapsedTime;
    extract(timg, keypoints, descriptors, elapsedTime);

    for(size_t i = 0; i < keypoints.size(); i++) {
      cv::Point3f kpt(keypoints[i].pt.x, keypoints[i].pt.y, 1.f);
      cv::Mat kpt_t = Ai * cv::Mat(kpt);
      keypoints[i].pt.x = kpt_t.at<float>(0, 0);
      keypoints[i].pt.y = kpt_t.at<float>(1, 0);
    }

    listOfKeypoints[(size_t) cpt] = keypoints;
    listOfDescriptors[(size_t) cpt] = descriptors;
  }
#endif
}

void vpKeyPoint::reset() {
  //vpBasicKeyPoint class
  referenceImagePointsList.clear(); currentImagePointsList.clear(); matchedReferencePoints.clear(); _reference_computed = false;


  m_computeCovariance = false; m_covarianceMatrix = vpMatrix(); m_currentImageId = 0; m_detectionMethod = detectionScore;
  m_detectionScore = 0.15; m_detectionThreshold = 100.0; m_detectionTime = 0.0; m_detectorNames.clear();
  m_detectors.clear(); m_extractionTime = 0.0; m_extractorNames.clear(); m_extractors.clear(); m_filteredMatches.clear();
  m_filterType = ratioDistanceThreshold;
  m_imageFormat = jpgImageFormat; m_knnMatches.clear(); m_mapOfImageId.clear(); m_mapOfImages.clear();
  m_matcher = cv::Ptr<cv::DescriptorMatcher>(); m_matcherName = "BruteForce-Hamming";
  m_matches.clear(); m_matchingFactorThreshold = 2.0; m_matchingRatioThreshold = 0.85; m_matchingTime = 0.0;
  m_matchRansacKeyPointsToPoints.clear(); m_nbRansacIterations = 200; m_nbRansacMinInlierCount = 100;
  m_objectFilteredPoints.clear();
  m_poseTime = 0.0; m_queryDescriptors = cv::Mat(); m_queryFilteredKeyPoints.clear(); m_queryKeyPoints.clear();
  m_ransacConsensusPercentage = 20.0; m_ransacInliers.clear(); m_ransacOutliers.clear(); m_ransacReprojectionError = 6.0;
  m_ransacThreshold = 0.01; m_trainDescriptors = cv::Mat(); m_trainKeyPoints.clear(); m_trainPoints.clear();
  m_trainVpPoints.clear(); m_useAffineDetection = false;
#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
  m_useBruteForceCrossCheck = true;
#endif
  m_useConsensusPercentage = false;
  m_useKnn = true; //as m_filterType == ratioDistanceThreshold
  m_useMatchTrainToQuery = false; m_useRansacVVS = true; m_useSingleMatchFilter = true;

  m_detectorNames.push_back("ORB");
  m_extractorNames.push_back("ORB");

  init();
}

void vpKeyPoint::saveLearningData(const std::string &filename, bool binaryMode, const bool saveTrainingImages) {
  std::string parent = vpIoTools::getParent(filename);
  if(!parent.empty()) {
    vpIoTools::makeDirectory(parent);
  }

  std::map<int, std::string> mapOfImgPath;
  if(saveTrainingImages) {
#ifdef VISP_HAVE_MODULE_IO
    //Save the training image files in the same directory
    int cpt = 0;

    for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
      if(cpt > 999) {
        throw vpException(vpException::fatalError, "The number of training images to save is too big !");
      }

      char buffer[4];
      sprintf(buffer, "%03d", cpt);
      std::stringstream ss;
      ss << "train_image_" << buffer;

      switch(m_imageFormat) {
      case jpgImageFormat:
        ss << ".jpg";
        break;

      case pngImageFormat:
        ss << ".png";
        break;

      case ppmImageFormat:
        ss << ".ppm";
        break;

      case pgmImageFormat:
        ss << ".pgm";
        break;

      default:
        ss << ".png";
        break;
      }

      std::string imgFilename = ss.str();
      mapOfImgPath[it->first] = imgFilename;
      vpImageIo::write(it->second, parent + (!parent.empty() ? "/" : "") + imgFilename);
    }
#else
    std::cout << "Warning: in vpKeyPoint::saveLearningData() training images are not saved because "
        "visp_io module is not available !" << std::endl;
#endif
  }

  bool have3DInfo = m_trainPoints.size() > 0;
  if(have3DInfo && m_trainPoints.size() != m_trainKeyPoints.size()) {
    throw vpException(vpException::fatalError, "List of keypoints and list of 3D points have different size !");
  }

  if(binaryMode) {
    //Save the learning data into little endian binary file.
    std::ofstream file(filename.c_str(), std::ofstream::binary);
    if(!file.is_open()) {
      throw vpException(vpException::ioError, "Cannot create the file.");
    }

    //Write info about training images
    int nbImgs = (int) mapOfImgPath.size();
//    file.write((char *)(&nbImgs), sizeof(nbImgs));
    writeBinaryIntLE(file, nbImgs);

#ifdef VISP_HAVE_MODULE_IO
    for(std::map<int, std::string>::const_iterator it = mapOfImgPath.begin(); it != mapOfImgPath.end(); ++it) {
      //Write image_id
      int id = it->first;
//      file.write((char *)(&id), sizeof(id));
      writeBinaryIntLE(file, id);

      //Write image path
      std::string path = it->second;
      int length = (int) path.length();
//      file.write((char *)(&length), sizeof(length));
      writeBinaryIntLE(file, length);

      for(int cpt = 0; cpt < length; cpt++) {
        file.write((char *) (&path[(size_t)cpt]), sizeof(path[(size_t)cpt]));
      }
    }
#endif

    //Write if we have 3D point information
    int have3DInfoInt = have3DInfo ? 1 : 0;
//    file.write((char *)(&have3DInfoInt), sizeof(have3DInfoInt));
    writeBinaryIntLE(file, have3DInfoInt);


    int nRows = m_trainDescriptors.rows,
        nCols = m_trainDescriptors.cols;
    int descriptorType = m_trainDescriptors.type();

    //Write the number of descriptors
//    file.write((char *)(&nRows), sizeof(nRows));
    writeBinaryIntLE(file, nRows);

    //Write the size of the descriptor
//    file.write((char *)(&nCols), sizeof(nCols));
    writeBinaryIntLE(file, nCols);

    //Write the type of the descriptor
//    file.write((char *)(&descriptorType), sizeof(descriptorType));
    writeBinaryIntLE(file, descriptorType);

    for (int i = 0; i < nRows; i++) {
      unsigned int i_ = (unsigned int) i;
      //Write u
      float u = m_trainKeyPoints[i_].pt.x;
//      file.write((char *)(&u), sizeof(u));
      writeBinaryFloatLE(file, u);

      //Write v
      float v = m_trainKeyPoints[i_].pt.y;
//      file.write((char *)(&v), sizeof(v));
      writeBinaryFloatLE(file, v);

      //Write size
      float size = m_trainKeyPoints[i_].size;
//      file.write((char *)(&size), sizeof(size));
      writeBinaryFloatLE(file, size);

      //Write angle
      float angle = m_trainKeyPoints[i_].angle;
//      file.write((char *)(&angle), sizeof(angle));
      writeBinaryFloatLE(file, angle);

      //Write response
      float response = m_trainKeyPoints[i_].response;
//      file.write((char *)(&response), sizeof(response));
      writeBinaryFloatLE(file, response);

      //Write octave
      int octave = m_trainKeyPoints[i_].octave;
//      file.write((char *)(&octave), sizeof(octave));
      writeBinaryIntLE(file, octave);

      //Write class_id
      int class_id = m_trainKeyPoints[i_].class_id;
//      file.write((char *)(&class_id), sizeof(class_id));
      writeBinaryIntLE(file, class_id);

      //Write image_id
#ifdef VISP_HAVE_MODULE_IO
      std::map<int, int>::const_iterator it_findImgId = m_mapOfImageId.find(m_trainKeyPoints[i_].class_id);
      int image_id = (saveTrainingImages && it_findImgId != m_mapOfImageId.end()) ? it_findImgId->second : -1;
//      file.write((char *)(&image_id), sizeof(image_id));
      writeBinaryIntLE(file, image_id);
#else
      int image_id = -1;
//      file.write((char *)(&image_id), sizeof(image_id));
      writeBinaryIntLE(file, image_id);
#endif

      if(have3DInfo) {
        float oX = m_trainPoints[i_].x, oY = m_trainPoints[i_].y, oZ = m_trainPoints[i_].z;
        //Write oX
//        file.write((char *)(&oX), sizeof(oX));
        writeBinaryFloatLE(file, oX);

        //Write oY
//        file.write((char *)(&oY), sizeof(oY));
        writeBinaryFloatLE(file, oY);

        //Write oZ
//        file.write((char *)(&oZ), sizeof(oZ));
        writeBinaryFloatLE(file, oZ);
      }

      for (int j = 0; j < nCols; j++) {
        //Write the descriptor value
        switch(descriptorType) {
        case CV_8U:
          file.write((char *)(&m_trainDescriptors.at<unsigned char>(i, j)), sizeof(m_trainDescriptors.at<unsigned char>(i, j)));
          break;

        case CV_8S:
          file.write((char *)(&m_trainDescriptors.at<char>(i, j)), sizeof(m_trainDescriptors.at<char>(i, j)));
          break;

        case CV_16U:
//          file.write((char *)(&m_trainDescriptors.at<unsigned short int>(i, j)), sizeof(m_trainDescriptors.at<unsigned short int>(i, j)));
          writeBinaryUShortLE(file, m_trainDescriptors.at<unsigned short int>(i, j));
          break;

        case CV_16S:
//          file.write((char *)(&m_trainDescriptors.at<short int>(i, j)), sizeof(m_trainDescriptors.at<short int>(i, j)));
          writeBinaryShortLE(file, m_trainDescriptors.at<short int>(i, j));
          break;

        case CV_32S:
//          file.write((char *)(&m_trainDescriptors.at<int>(i, j)), sizeof(m_trainDescriptors.at<int>(i, j)));
          writeBinaryIntLE(file, m_trainDescriptors.at<int>(i, j));
          break;

        case CV_32F:
//          file.write((char *)(&m_trainDescriptors.at<float>(i, j)), sizeof(m_trainDescriptors.at<float>(i, j)));
          writeBinaryFloatLE(file, m_trainDescriptors.at<float>(i, j));
          break;

        case CV_64F:
//          file.write((char *)(&m_trainDescriptors.at<double>(i, j)), sizeof(m_trainDescriptors.at<double>(i, j)));
          writeBinaryDoubleLE(file, m_trainDescriptors.at<double>(i, j));
          break;

        default:
          throw vpException(vpException::fatalError, "Problem with the data type of descriptors !");
          break;
        }
      }
    }


    file.close();
  } else {
#ifdef VISP_HAVE_XML2
    xmlDocPtr doc = NULL;
    xmlNodePtr root_node = NULL, image_node = NULL, image_info_node = NULL, descriptors_info_node = NULL,
        descriptor_node = NULL, desc_node = NULL;

    /*
     * this initialize the library and check potential ABI mismatches
     * between the version it was compiled for and the actual shared
     * library used.
     */
    LIBXML_TEST_VERSION

        doc = xmlNewDoc(BAD_CAST "1.0");
    if (doc == NULL) {
      throw vpException(vpException::ioError, "Error with file " + filename);
    }

    root_node = xmlNewNode(NULL, BAD_CAST "LearningData");
    xmlDocSetRootElement(doc, root_node);

    std::stringstream ss;

    //Write the training images info
    image_node = xmlNewChild(root_node, NULL, BAD_CAST "TrainingImageInfo", NULL);

#ifdef VISP_HAVE_MODULE_IO
    for(std::map<int, std::string>::const_iterator it = mapOfImgPath.begin(); it != mapOfImgPath.end(); ++it) {
      image_info_node = xmlNewChild(image_node, NULL, BAD_CAST "trainImg",
                                    BAD_CAST it->second.c_str());
      ss.str("");
      ss << it->first;
      xmlNewProp(image_info_node, BAD_CAST "image_id", BAD_CAST ss.str().c_str());
    }
#endif

    //Write information about descriptors
    descriptors_info_node = xmlNewChild(root_node, NULL, BAD_CAST "DescriptorsInfo", NULL);

    int nRows = m_trainDescriptors.rows,
        nCols = m_trainDescriptors.cols;
    int descriptorType = m_trainDescriptors.type();

    //Write the number of rows
    ss.str("");
    ss << nRows;
    xmlNewChild(descriptors_info_node, NULL, BAD_CAST "nrows", BAD_CAST ss.str().c_str());

    //Write the number of cols
    ss.str("");
    ss << nCols;
    xmlNewChild(descriptors_info_node, NULL, BAD_CAST "ncols", BAD_CAST ss.str().c_str());

    //Write the descriptors type
    ss.str("");
    ss << descriptorType;
    xmlNewChild(descriptors_info_node, NULL, BAD_CAST "type", BAD_CAST ss.str().c_str());

    for (int i = 0; i < nRows; i++) {
      unsigned int i_ = (unsigned int) i;
      descriptor_node = xmlNewChild(root_node, NULL, BAD_CAST "DescriptorInfo",
                                    NULL);

      ss.str("");
      //max_digits10 == 9 for float
      ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].pt.x;
      xmlNewChild(descriptor_node, NULL, BAD_CAST "u",
                               BAD_CAST ss.str().c_str());

      ss.str("");
      //max_digits10 == 9 for float
      ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].pt.y;
      xmlNewChild(descriptor_node, NULL, BAD_CAST "v",
                               BAD_CAST ss.str().c_str());

      ss.str("");
      //max_digits10 == 9 for float
      ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].size;
      xmlNewChild(descriptor_node, NULL, BAD_CAST "size",
                               BAD_CAST ss.str().c_str());

      ss.str("");
      //max_digits10 == 9 for float
      ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].angle;
      xmlNewChild(descriptor_node, NULL, BAD_CAST "angle",
                               BAD_CAST ss.str().c_str());

      ss.str("");
      //max_digits10 == 9 for float
      ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].response;
      xmlNewChild(descriptor_node, NULL, BAD_CAST "response",
                               BAD_CAST ss.str().c_str());

      ss.str("");
      ss << m_trainKeyPoints[i_].octave;
      xmlNewChild(descriptor_node, NULL, BAD_CAST "octave",
                               BAD_CAST ss.str().c_str());

      ss.str("");
      ss << m_trainKeyPoints[i_].class_id;
      xmlNewChild(descriptor_node, NULL, BAD_CAST "class_id",
                               BAD_CAST ss.str().c_str());

      ss.str("");
#ifdef VISP_HAVE_MODULE_IO
      std::map<int, int>::const_iterator it_findImgId = m_mapOfImageId.find(m_trainKeyPoints[i_].class_id);
      ss << ((saveTrainingImages && it_findImgId != m_mapOfImageId.end()) ? it_findImgId->second : -1);
      xmlNewChild(descriptor_node, NULL, BAD_CAST "image_id",
                               BAD_CAST ss.str().c_str());
#else
      ss << -1;
      xmlNewChild(descriptor_node, NULL, BAD_CAST "image_id",
                               BAD_CAST ss.str().c_str());
#endif

      if (have3DInfo) {
        ss.str("");
        //max_digits10 == 9 for float
        ss << std::fixed << std::setprecision(9) << m_trainPoints[i_].x;
        xmlNewChild(descriptor_node, NULL, BAD_CAST "oX",
                                 BAD_CAST ss.str().c_str());

        ss.str("");
        //max_digits10 == 9 for float
        ss << std::fixed << std::setprecision(9) << m_trainPoints[i_].y;
        xmlNewChild(descriptor_node, NULL, BAD_CAST "oY",
                                 BAD_CAST ss.str().c_str());

        ss.str("");
        //max_digits10 == 9 for float
        ss << std::fixed << std::setprecision(9) << m_trainPoints[i_].z;
        xmlNewChild(descriptor_node, NULL, BAD_CAST "oZ",
                                 BAD_CAST ss.str().c_str());
      }

      desc_node = xmlNewChild(descriptor_node, NULL, BAD_CAST "desc", NULL);

      for (int j = 0; j < nCols; j++) {
        ss.str("");

        switch(descriptorType) {
        case CV_8U:
          {
            //Promote an unsigned char to an int
            //val_tmp holds the numeric value that will be written
            //We save the value in numeric form otherwise libxml2 will not be able to parse
            //A better solution could be possible
            int val_tmp = m_trainDescriptors.at<unsigned char>(i, j);
            ss << val_tmp;
          }
          break;

          case CV_8S:
          {
            //Promote a char to an int
            //val_tmp holds the numeric value that will be written
            //We save the value in numeric form otherwise libxml2 will not be able to parse
            //A better solution could be possible
            int val_tmp = m_trainDescriptors.at<char>(i, j);
            ss << val_tmp;
          }
          break;

          case CV_16U:
            ss << m_trainDescriptors.at<unsigned short int>(i, j);
            break;

          case CV_16S:
            ss << m_trainDescriptors.at<short int>(i, j);
            break;

          case CV_32S:
            ss << m_trainDescriptors.at<int>(i, j);
            break;

          case CV_32F:
            //max_digits10 == 9 for float
            ss << std::fixed << std::setprecision(9) << m_trainDescriptors.at<float>(i, j);
            break;

          case CV_64F:
            //max_digits10 == 17 for double
            ss << std::fixed << std::setprecision(17) << m_trainDescriptors.at<double>(i, j);
            break;

          default:
            throw vpException(vpException::fatalError, "Problem with the data type of descriptors !");
            break;
        }
        xmlNewChild(desc_node, NULL, BAD_CAST "val",
                    BAD_CAST ss.str().c_str());
      }
    }

    xmlSaveFormatFileEnc(filename.c_str(), doc, "UTF-8", 1);

    /*free the document */
    xmlFreeDoc(doc);

    /*
     *Free the global variables that may
     *have been allocated by the parser.
     */
    xmlCleanupParser();
#else
    std::cerr << "Error: libxml2 is required !" << std::endl;
#endif
  }
}

#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x030000)
//From OpenCV 2.4.11 source code.
struct KeypointResponseGreaterThanThreshold {
  KeypointResponseGreaterThanThreshold(float _value) :
      value(_value) {
  }
  inline bool operator()(const cv::KeyPoint& kpt) const {
    return kpt.response >= value;
  }
  float value;
};

struct KeypointResponseGreater {
  inline bool operator()(const cv::KeyPoint& kp1,
      const cv::KeyPoint& kp2) const {
    return kp1.response > kp2.response;
  }
};

// takes keypoints and culls them by the response
void vpKeyPoint::KeyPointsFilter::retainBest(
    std::vector<cv::KeyPoint>& keypoints, int n_points) {
  //this is only necessary if the keypoints size is greater than the number of desired points.
  if (n_points >= 0 && keypoints.size() > (size_t) n_points) {
    if (n_points == 0) {
      keypoints.clear();
      return;
    }
    //first use nth element to partition the keypoints into the best and worst.
    std::nth_element(keypoints.begin(), keypoints.begin() + n_points,
        keypoints.end(), KeypointResponseGreater());
    //this is the boundary response, and in the case of FAST may be ambiguous
    float ambiguous_response = keypoints[(size_t) (n_points - 1)].response;
    //use std::partition to grab all of the keypoints with the boundary response.
    std::vector<cv::KeyPoint>::const_iterator new_end = std::partition(
        keypoints.begin() + n_points, keypoints.end(),
        KeypointResponseGreaterThanThreshold(ambiguous_response));
    //resize the keypoints, given this new end point. nth_element and partition reordered the points inplace
    keypoints.resize((size_t) (new_end - keypoints.begin()));
  }
}

struct RoiPredicate {
  RoiPredicate(const cv::Rect& _r) :
      r(_r) {
  }

  bool operator()(const cv::KeyPoint& keyPt) const {
    return !r.contains(keyPt.pt);
  }

  cv::Rect r;
};

void vpKeyPoint::KeyPointsFilter::runByImageBorder(
    std::vector<cv::KeyPoint>& keypoints, cv::Size imageSize, int borderSize) {
  if (borderSize > 0) {
    if (imageSize.height <= borderSize * 2 || imageSize.width <= borderSize * 2)
      keypoints.clear();
    else
      keypoints.erase(
          std::remove_if(keypoints.begin(), keypoints.end(),
              RoiPredicate(
                  cv::Rect(cv::Point(borderSize, borderSize),
                      cv::Point(imageSize.width - borderSize,
                          imageSize.height - borderSize)))), keypoints.end());
  }
}

struct SizePredicate {
  SizePredicate(float _minSize, float _maxSize) :
      minSize(_minSize), maxSize(_maxSize) {
  }

  bool operator()(const cv::KeyPoint& keyPt) const {
    float size = keyPt.size;
    return (size < minSize) || (size > maxSize);
  }

  float minSize, maxSize;
};

void vpKeyPoint::KeyPointsFilter::runByKeypointSize(
    std::vector<cv::KeyPoint>& keypoints, float minSize, float maxSize) {
  CV_Assert(minSize >= 0);
  CV_Assert(maxSize >= 0);
  CV_Assert(minSize <= maxSize);

  keypoints.erase(
      std::remove_if(keypoints.begin(), keypoints.end(),
          SizePredicate(minSize, maxSize)), keypoints.end());
}

class MaskPredicate {
public:
  MaskPredicate(const cv::Mat& _mask) :
      mask(_mask) {
  }
  bool operator()(const cv::KeyPoint& key_pt) const {
    return mask.at<uchar>((int) (key_pt.pt.y + 0.5f),
        (int) (key_pt.pt.x + 0.5f)) == 0;
  }

private:
  const cv::Mat mask;
  MaskPredicate& operator=(const MaskPredicate&);
};

void vpKeyPoint::KeyPointsFilter::runByPixelsMask(
    std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask) {
  if (mask.empty())
    return;

  keypoints.erase(
      std::remove_if(keypoints.begin(), keypoints.end(), MaskPredicate(mask)),
      keypoints.end());
}

struct KeyPoint_LessThan {
  KeyPoint_LessThan(const std::vector<cv::KeyPoint>& _kp) :
      kp(&_kp) {
  }
  bool operator()(/*int i, int j*/ size_t i, size_t j) const {
    const cv::KeyPoint& kp1 = (*kp)[/*(size_t)*/ i];
    const cv::KeyPoint& kp2 = (*kp)[/*(size_t)*/ j];
    if (!vpMath::equal(kp1.pt.x, kp2.pt.x, std::numeric_limits<float>::epsilon())) { //if (kp1.pt.x != kp2.pt.x) {
      return kp1.pt.x < kp2.pt.x;
    }

    if (!vpMath::equal(kp1.pt.y, kp2.pt.y, std::numeric_limits<float>::epsilon())) { //if (kp1.pt.y != kp2.pt.y) {
      return kp1.pt.y < kp2.pt.y;
    }

    if (!vpMath::equal(kp1.size, kp2.size, std::numeric_limits<float>::epsilon())) { //if (kp1.size != kp2.size) {
      return kp1.size > kp2.size;
    }

    if (!vpMath::equal(kp1.angle, kp2.angle, std::numeric_limits<float>::epsilon())) { //if (kp1.angle != kp2.angle) {
      return kp1.angle < kp2.angle;
    }

    if (!vpMath::equal(kp1.response, kp2.response, std::numeric_limits<float>::epsilon())) { //if (kp1.response != kp2.response) {
      return kp1.response > kp2.response;
    }

    if (kp1.octave != kp2.octave) {
      return kp1.octave > kp2.octave;
    }

    if (kp1.class_id != kp2.class_id) {
      return kp1.class_id > kp2.class_id;
    }

    return i < j;
  }
  const std::vector<cv::KeyPoint>* kp;
};

void vpKeyPoint::KeyPointsFilter::removeDuplicated(
    std::vector<cv::KeyPoint>& keypoints) {
  size_t i, j, n = keypoints.size();
  std::vector<size_t> kpidx(n);
  std::vector<uchar> mask(n, (uchar) 1);

  for (i = 0; i < n; i++) {
    kpidx[i] = i;
  }
  std::sort(kpidx.begin(), kpidx.end(), KeyPoint_LessThan(keypoints));
  for (i = 1, j = 0; i < n; i++) {
    cv::KeyPoint& kp1 = keypoints[kpidx[i]];
    cv::KeyPoint& kp2 = keypoints[kpidx[j]];
//    if (kp1.pt.x != kp2.pt.x || kp1.pt.y != kp2.pt.y || kp1.size != kp2.size || kp1.angle != kp2.angle) {
    if (!vpMath::equal(kp1.pt.x, kp2.pt.x, std::numeric_limits<float>::epsilon()) ||
        !vpMath::equal(kp1.pt.y, kp2.pt.y, std::numeric_limits<float>::epsilon()) ||
        !vpMath::equal(kp1.size, kp2.size, std::numeric_limits<float>::epsilon()) ||
        !vpMath::equal(kp1.angle, kp2.angle, std::numeric_limits<float>::epsilon())) {
      j = i;
    } else {
      mask[kpidx[i]] = 0;
    }
  }

  for (i = j = 0; i < n; i++) {
    if (mask[i]) {
      if (i != j) {
        keypoints[j] = keypoints[i];
      }
      j++;
    }
  }
  keypoints.resize(j);
}

/*
 *  PyramidAdaptedFeatureDetector
 */
vpKeyPoint::PyramidAdaptedFeatureDetector::PyramidAdaptedFeatureDetector(
    const cv::Ptr<cv::FeatureDetector>& _detector, int _maxLevel) :
    detector(_detector), maxLevel(_maxLevel) {
}

bool vpKeyPoint::PyramidAdaptedFeatureDetector::empty() const {
  return detector.empty() || (cv::FeatureDetector*) detector->empty();
}

void vpKeyPoint::PyramidAdaptedFeatureDetector::detect( cv::InputArray image, CV_OUT std::vector<cv::KeyPoint>& keypoints,
    cv::InputArray mask ) {
  detectImpl(image.getMat(), keypoints, mask.getMat());
}

void vpKeyPoint::PyramidAdaptedFeatureDetector::detectImpl(const cv::Mat& image,
    std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask) const {
  cv::Mat src = image;
  cv::Mat src_mask = mask;

  cv::Mat dilated_mask;
  if (!mask.empty()) {
    cv::dilate(mask, dilated_mask, cv::Mat());
    cv::Mat mask255(mask.size(), CV_8UC1, cv::Scalar(0));
    mask255.setTo(cv::Scalar(255), dilated_mask != 0);
    dilated_mask = mask255;
  }

  for (int l = 0, multiplier = 1; l <= maxLevel; ++l, multiplier *= 2) {
    // Detect on current level of the pyramid
    std::vector<cv::KeyPoint> new_pts;
    detector->detect(src, new_pts, src_mask);
    std::vector<cv::KeyPoint>::iterator it = new_pts.begin(), end =
        new_pts.end();
    for (; it != end; ++it) {
      it->pt.x *= multiplier;
      it->pt.y *= multiplier;
      it->size *= multiplier;
      it->octave = l;
    }
    keypoints.insert(keypoints.end(), new_pts.begin(), new_pts.end());

    // Downsample
    if (l < maxLevel) {
      cv::Mat dst;
      pyrDown(src, dst);
      src = dst;

      if (!mask.empty())
        resize(dilated_mask, src_mask, src.size(), 0, 0, CV_INTER_AREA);
    }
  }

  if (!mask.empty())
    vpKeyPoint::KeyPointsFilter::runByPixelsMask(keypoints, mask);
}
#endif

#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work around to avoid warning: libvisp_vision.a(vpKeyPoint.cpp.o) has no symbols
void dummy_vpKeyPoint() {};
#endif