Visual Servoing Platform  version 3.0.0
vpKeyPoint.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Key point functionalities.
32  *
33  * Authors:
34  * Souriya Trinh
35  *
36  *****************************************************************************/
37 
38 #include <limits>
39 #include <iomanip>
40 #include <stdint.h> //uint32_t ; works also with >= VS2010 / _MSC_VER >= 1600
41 
42 #include <visp3/vision/vpKeyPoint.h>
43 #include <visp3/core/vpIoTools.h>
44 
45 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
46 
47 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
48 # include <opencv2/calib3d/calib3d.hpp>
49 #endif
50 
51 
52 //TODO: test saveLearningData with little / big endian platform
53 //Specific Type transformation functions
55 // Convert a list of cv::DMatch to a cv::DMatch (extract the first cv::DMatch, the nearest neighbor).
56 //
57 // \param knnMatches : List of cv::DMatch.
58 // \return The nearest neighbor.
59 // */
60 inline cv::DMatch knnToDMatch(const std::vector<cv::DMatch> &knnMatches) {
61  if(knnMatches.size() > 0) {
62  return knnMatches[0];
63  }
64 
65  return cv::DMatch();
66 }
67 
69 // Convert a cv::DMatch to an index (extract the train index).
70 //
71 // \param match : Point to convert in ViSP type.
72 // \return The train index.
73 // */
74 inline vpImagePoint matchRansacToVpImage(const std::pair<cv::KeyPoint, cv::Point3f> &pair) {
75  return vpImagePoint(pair.first.pt.y, pair.first.pt.x);
76 }
77 
78 bool isBigEndian() {
79  union {
80  uint32_t i;
81  char c[4];
82  } bint = { 0x01020304 };
83 
84  return bint.c[0] == 1;
85 }
86 
87 uint16_t reverse16bits(const uint16_t n) {
88  unsigned char *np = (unsigned char *) &n;
89 
90  return ((uint16_t) np[0] << 8) | (uint16_t) np[1];
91 }
92 
93 uint32_t reverse32bits(const uint32_t n) {
94  unsigned char *np = (unsigned char *) &n;
95 
96  return ((uint32_t) np[0] << 24) | ((uint32_t) np[1] << 16)
97  | ((uint32_t) np[2] << 8) | (uint32_t) np[3];
98 }
99 
100 float reverseFloat(const float f) {
101  union {
102  float f;
103  unsigned char b[4];
104  } dat1, dat2;
105 
106  dat1.f = f;
107  dat2.b[0] = dat1.b[3];
108  dat2.b[1] = dat1.b[2];
109  dat2.b[2] = dat1.b[1];
110  dat2.b[3] = dat1.b[0];
111  return dat2.f;
112 }
113 
114 double reverseDouble(const double d) {
115  union {
116  double d;
117  unsigned char b[8];
118  } dat1, dat2;
119 
120  dat1.d = d;
121  dat2.b[0] = dat1.b[7];
122  dat2.b[1] = dat1.b[6];
123  dat2.b[2] = dat1.b[5];
124  dat2.b[3] = dat1.b[4];
125  dat2.b[4] = dat1.b[3];
126  dat2.b[5] = dat1.b[2];
127  dat2.b[6] = dat1.b[1];
128  dat2.b[7] = dat1.b[0];
129  return dat2.d;
130 }
131 
132 void writeBinaryUShortLE(std::ofstream &file, const unsigned short ushort_value) {
133  if(isBigEndian()) {
134  //Reverse bytes order to little endian
135  uint16_t reverse_ushort = reverse16bits(ushort_value);
136  file.write((char *)(&reverse_ushort), sizeof(reverse_ushort));
137  } else {
138  file.write((char *)(&ushort_value), sizeof(ushort_value));
139  }
140 }
141 
142 void writeBinaryShortLE(std::ofstream &file, const short short_value) {
143  if(isBigEndian()) {
144  //Reverse bytes order to little endian
145  uint16_t reverse_short = reverse16bits((uint16_t) short_value);
146  file.write((char *)(&reverse_short), sizeof(reverse_short));
147  } else {
148  file.write((char *)(&short_value), sizeof(short_value));
149  }
150 }
151 
152 void writeBinaryUIntLE(std::ofstream &file, const unsigned int uint_value) {
153  if(isBigEndian()) {
154  //Reverse bytes order to little endian
155  if(sizeof(uint_value) == 4) {
156  uint32_t reverse_uint = reverse32bits(uint_value);
157  file.write((char *)(&reverse_uint), sizeof(reverse_uint));
158  } else {
159  uint16_t reverse_uint = reverse16bits(uint_value);
160  file.write((char *)(&reverse_uint), sizeof(reverse_uint));
161  }
162  } else {
163  file.write((char *)(&uint_value), sizeof(uint_value));
164  }
165 }
166 
167 void writeBinaryIntLE(std::ofstream &file, const int int_value) {
168  if(isBigEndian()) {
169  //Reverse bytes order to little endian
170  if(sizeof(int_value) == 4) {
171  uint32_t reverse_int = reverse32bits((uint32_t) int_value);
172  file.write((char *)(&reverse_int), sizeof(reverse_int));
173  } else {
174  uint16_t reverse_int = reverse16bits((uint16_t) int_value);
175  file.write((char *)(&reverse_int), sizeof(reverse_int));
176  }
177  } else {
178  file.write((char *)(&int_value), sizeof(int_value));
179  }
180 }
181 
182 void writeBinaryFloatLE(std::ofstream &file, const float float_value) {
183  if(isBigEndian()) {
184  //Reverse bytes order to little endian
185  float reverse_float = reverseFloat(float_value);
186  file.write((char *)(&reverse_float), sizeof(reverse_float));
187  } else {
188  file.write((char *)(&float_value), sizeof(float_value));
189  }
190 }
191 
192 void writeBinaryDoubleLE(std::ofstream &file, const double double_value) {
193  if(isBigEndian()) {
194  //Reverse bytes order to little endian
195  double reverse_double = reverseDouble(double_value);
196  file.write((char *)(&reverse_double), sizeof(reverse_double));
197  } else {
198  file.write((char *)(&double_value), sizeof(double_value));
199  }
200 }
201 
210 vpKeyPoint::vpKeyPoint(const std::string &detectorName, const std::string &extractorName,
211  const std::string &matcherName, const vpFilterMatchingType &filterType)
212  : m_computeCovariance(false), m_covarianceMatrix(), m_currentImageId(0), m_detectionMethod(detectionScore),
213  m_detectionScore(0.15), m_detectionThreshold(100.0), m_detectionTime(0.), m_detectorNames(),
214  m_detectors(), m_extractionTime(0.), m_extractorNames(), m_extractors(), m_filteredMatches(), m_filterType(filterType),
215  m_imageFormat(jpgImageFormat), m_knnMatches(), m_mapOfImageId(), m_mapOfImages(),
216  m_matcher(), m_matcherName(matcherName),
217  m_matches(), m_matchingFactorThreshold(2.0), m_matchingRatioThreshold(0.85), m_matchingTime(0.),
218  m_matchRansacKeyPointsToPoints(), m_nbRansacIterations(200), m_nbRansacMinInlierCount(100), m_objectFilteredPoints(),
219  m_poseTime(0.), m_queryDescriptors(), m_queryFilteredKeyPoints(), m_queryKeyPoints(),
220  m_ransacConsensusPercentage(20.0), m_ransacInliers(), m_ransacOutliers(), m_ransacReprojectionError(6.0),
221  m_ransacThreshold(0.01), m_trainDescriptors(), m_trainKeyPoints(), m_trainPoints(),
222  m_trainVpPoints(), m_useAffineDetection(false),
223  #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
224  m_useBruteForceCrossCheck(true),
225  #endif
226  m_useConsensusPercentage(false),
227  m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true), m_useSingleMatchFilter(true)
228 {
229  //Use k-nearest neighbors (knn) to retrieve the two best matches for a keypoint
230  //So this is useful only for ratioDistanceThreshold method
231  if(filterType == ratioDistanceThreshold || filterType == stdAndRatioDistanceThreshold) {
232  m_useKnn = true;
233  }
234 
235  m_detectorNames.push_back(detectorName);
236  m_extractorNames.push_back(extractorName);
237 
238  init();
239 }
240 
249 vpKeyPoint::vpKeyPoint(const std::vector<std::string> &detectorNames, const std::vector<std::string> &extractorNames,
250  const std::string &matcherName, const vpFilterMatchingType &filterType)
251  : m_computeCovariance(false), m_covarianceMatrix(), m_currentImageId(0), m_detectionMethod(detectionScore),
252  m_detectionScore(0.15), m_detectionThreshold(100.0), m_detectionTime(0.), m_detectorNames(detectorNames),
253  m_detectors(), m_extractionTime(0.), m_extractorNames(extractorNames), m_extractors(), m_filteredMatches(),
254  m_filterType(filterType), m_imageFormat(jpgImageFormat), m_knnMatches(), m_mapOfImageId(), m_mapOfImages(),
255  m_matcher(),
256  m_matcherName(matcherName), m_matches(), m_matchingFactorThreshold(2.0), m_matchingRatioThreshold(0.85), m_matchingTime(0.),
257  m_matchRansacKeyPointsToPoints(), m_nbRansacIterations(200), m_nbRansacMinInlierCount(100), m_objectFilteredPoints(),
258  m_poseTime(0.), m_queryDescriptors(), m_queryFilteredKeyPoints(), m_queryKeyPoints(),
259  m_ransacConsensusPercentage(20.0), m_ransacInliers(), m_ransacOutliers(), m_ransacReprojectionError(6.0),
260  m_ransacThreshold(0.01), m_trainDescriptors(), m_trainKeyPoints(), m_trainPoints(),
261  m_trainVpPoints(), m_useAffineDetection(false),
262  #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
263  m_useBruteForceCrossCheck(true),
264  #endif
265  m_useConsensusPercentage(false),
266  m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true), m_useSingleMatchFilter(true)
267 {
268  //Use k-nearest neighbors (knn) to retrieve the two best matches for a keypoint
269  //So this is useful only for ratioDistanceThreshold method
270  if(filterType == ratioDistanceThreshold || filterType == stdAndRatioDistanceThreshold) {
271  m_useKnn = true;
272  }
273 
274  init();
275 }
276 
285 void vpKeyPoint::affineSkew(double tilt, double phi, cv::Mat& img,
286  cv::Mat& mask, cv::Mat& Ai) {
287  int h = img.rows;
288  int w = img.cols;
289 
290  mask = cv::Mat(h, w, CV_8UC1, cv::Scalar(255));
291 
292  cv::Mat A = cv::Mat::eye(2, 3, CV_32F);
293 
294  //if (phi != 0.0) {
295  if (std::fabs(phi) > std::numeric_limits<double>::epsilon()) {
296  phi *= M_PI / 180.;
297  double s = sin(phi);
298  double c = cos(phi);
299 
300  A = (cv::Mat_<float>(2, 2) << c, -s, s, c);
301 
302  cv::Mat corners = (cv::Mat_<float>(4, 2) << 0, 0, w, 0, w, h, 0, h);
303  cv::Mat tcorners = corners * A.t();
304  cv::Mat tcorners_x, tcorners_y;
305  tcorners.col(0).copyTo(tcorners_x);
306  tcorners.col(1).copyTo(tcorners_y);
307  std::vector<cv::Mat> channels;
308  channels.push_back(tcorners_x);
309  channels.push_back(tcorners_y);
310  cv::merge(channels, tcorners);
311 
312  cv::Rect rect = cv::boundingRect(tcorners);
313  A = (cv::Mat_<float>(2, 3) << c, -s, -rect.x, s, c, -rect.y);
314 
315  cv::warpAffine(img, img, A, cv::Size(rect.width, rect.height),
316  cv::INTER_LINEAR, cv::BORDER_REPLICATE);
317  }
318  //if (tilt != 1.0) {
319  if (std::fabs(tilt - 1.0) > std::numeric_limits<double>::epsilon()) {
320  double s = 0.8 * sqrt(tilt * tilt - 1);
321  cv::GaussianBlur(img, img, cv::Size(0, 0), s, 0.01);
322  cv::resize(img, img, cv::Size(0, 0), 1.0 / tilt, 1.0, cv::INTER_NEAREST);
323  A.row(0) = A.row(0) / tilt;
324  }
325  //if (tilt != 1.0 || phi != 0.0) {
326  if (std::fabs(tilt - 1.0) > std::numeric_limits<double>::epsilon() || std::fabs(phi) > std::numeric_limits<double>::epsilon() ) {
327  h = img.rows;
328  w = img.cols;
329  cv::warpAffine(mask, mask, A, cv::Size(w, h), cv::INTER_NEAREST);
330  }
331  cv::invertAffineTransform(A, Ai);
332 }
333 
341  return buildReference(I, vpRect());
342 }
343 
354  const vpImagePoint &iP,
355  const unsigned int height, const unsigned int width) {
356 
357  return buildReference(I, vpRect(iP, width, height));
358 }
359 
368  const vpRect &rectangle) {
369  //Reset variables used when dealing with 3D models
370  //So as no 3D point list is passed, we dont need this variables
371  m_trainPoints.clear();
372  m_mapOfImageId.clear();
373  m_mapOfImages.clear();
374  m_currentImageId = 1;
375 
376  if(m_useAffineDetection) {
377  std::vector<std::vector<cv::KeyPoint> > listOfTrainKeyPoints;
378  std::vector<cv::Mat> listOfTrainDescriptors;
379 
380  //Detect keypoints and extract descriptors on multiple images
381  detectExtractAffine(I, listOfTrainKeyPoints, listOfTrainDescriptors);
382 
383  //Flatten the different train lists
384  m_trainKeyPoints.clear();
385  for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfTrainKeyPoints.begin();
386  it != listOfTrainKeyPoints.end(); ++it) {
387  m_trainKeyPoints.insert(m_trainKeyPoints.end(), it->begin(), it->end());
388  }
389 
390  bool first = true;
391  for(std::vector<cv::Mat>::const_iterator it = listOfTrainDescriptors.begin(); it != listOfTrainDescriptors.end(); ++it) {
392  if(first) {
393  first = false;
394  it->copyTo(m_trainDescriptors);
395  } else {
396  m_trainDescriptors.push_back(*it);
397  }
398  }
399  } else {
400  detect(I, m_trainKeyPoints, m_detectionTime, rectangle);
401  extract(I, m_trainKeyPoints, m_trainDescriptors, m_extractionTime);
402  }
403 
404  //Save the correspondence keypoint class_id with the training image_id in a map
405  //Used to display the matching with all the training images
406  for(std::vector<cv::KeyPoint>::const_iterator it = m_trainKeyPoints.begin(); it != m_trainKeyPoints.end(); ++it) {
407  m_mapOfImageId[it->class_id] = m_currentImageId;
408  }
409 
410  //Save the image in a map at a specific image_id
411  m_mapOfImages[m_currentImageId] = I;
412 
413  //Convert OpenCV type to ViSP type for compatibility
415 
416  _reference_computed = true;
417 
418  //Add train descriptors in matcher object
419  m_matcher->clear();
420  m_matcher->add(std::vector<cv::Mat>(1, m_trainDescriptors));
421 
422  return static_cast<unsigned int>(m_trainKeyPoints.size());
423 }
424 
434 void vpKeyPoint::buildReference(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &trainKeyPoints,
435  std::vector<cv::Point3f> &points3f, const bool append, const int class_id) {
436  cv::Mat trainDescriptors;
437  //Copy the input list of keypoints
438  std::vector<cv::KeyPoint> trainKeyPoints_tmp = trainKeyPoints;
439 
440  extract(I, trainKeyPoints, trainDescriptors, m_extractionTime, &points3f);
441 
442  if(trainKeyPoints.size() != trainKeyPoints_tmp.size()) {
443  //Keypoints have been removed
444  //Store the hash of a keypoint as the key and the index of the keypoint as the value
445  std::map<size_t, size_t> mapOfKeypointHashes;
446  size_t cpt = 0;
447  for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); ++it, cpt++) {
448  mapOfKeypointHashes[myKeypointHash(*it)] = cpt;
449  }
450 
451  std::vector<cv::Point3f> trainPoints_tmp;
452  for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
453  if(mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
454  trainPoints_tmp.push_back(points3f[mapOfKeypointHashes[myKeypointHash(*it)]]);
455  }
456  }
457 
458  //Copy trainPoints_tmp to points3f
459  points3f = trainPoints_tmp;
460  }
461 
462  buildReference(I, trainKeyPoints, trainDescriptors, points3f, append, class_id);
463 }
464 
475 void vpKeyPoint::buildReference(const vpImage<unsigned char> &I, const std::vector<cv::KeyPoint> &trainKeyPoints,
476  const cv::Mat &trainDescriptors, const std::vector<cv::Point3f> &points3f,
477  const bool append, const int class_id) {
478  if(!append) {
479  m_currentImageId = 0;
480  m_mapOfImageId.clear();
481  m_mapOfImages.clear();
482  this->m_trainKeyPoints.clear();
483  this->m_trainPoints.clear();
484  }
485 
486  m_currentImageId++;
487 
488  std::vector<cv::KeyPoint> trainKeyPoints_tmp = trainKeyPoints;
489  //Set class_id if != -1
490  if(class_id != -1) {
491  for(std::vector<cv::KeyPoint>::iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); ++it) {
492  it->class_id = class_id;
493  }
494  }
495 
496  //Save the correspondence keypoint class_id with the training image_id in a map
497  //Used to display the matching with all the training images
498  for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); ++it) {
499  m_mapOfImageId[it->class_id] = m_currentImageId;
500  }
501 
502  //Save the image in a map at a specific image_id
503  m_mapOfImages[m_currentImageId] = I;
504 
505  //Append reference lists
506  this->m_trainKeyPoints.insert(this->m_trainKeyPoints.end(), trainKeyPoints_tmp.begin(), trainKeyPoints_tmp.end());
507  if(!append) {
508  trainDescriptors.copyTo(this->m_trainDescriptors);
509  } else {
510  this->m_trainDescriptors.push_back(trainDescriptors);
511  }
512  this->m_trainPoints.insert(this->m_trainPoints.end(), points3f.begin(), points3f.end());
513 
514 
515  //Convert OpenCV type to ViSP type for compatibility
517  vpConvert::convertFromOpenCV(this->m_trainPoints, m_trainVpPoints);
518 
519  //Add train descriptors in matcher object
520  m_matcher->clear();
521  m_matcher->add(std::vector<cv::Mat>(1, m_trainDescriptors));
522 
523  _reference_computed = true;
524 }
525 
538 void vpKeyPoint::compute3D(const cv::KeyPoint &candidate, const std::vector<vpPoint> &roi,
539  const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, cv::Point3f &point) {
540  /* compute plane equation */
541  std::vector<vpPoint>::const_iterator it_roi = roi.begin();
542  vpPoint pts[3];
543  pts[0] = *it_roi;
544  ++it_roi;
545  pts[1] = *it_roi;
546  ++it_roi;
547  pts[2] = *it_roi;
548  vpPlane Po(pts[0], pts[1], pts[2]);
549  double xc = 0.0, yc = 0.0;
550  vpPixelMeterConversion::convertPoint(cam, candidate.pt.x, candidate.pt.y, xc, yc);
551  double Z = -Po.getD() / (Po.getA() * xc + Po.getB() * yc + Po.getC());
552  double X = xc * Z;
553  double Y = yc * Z;
554  vpColVector point_cam(4);
555  point_cam[0] = X;
556  point_cam[1] = Y;
557  point_cam[2] = Z;
558  point_cam[3] = 1;
559  vpColVector point_obj(4);
560  point_obj = cMo.inverse() * point_cam;
561  point = cv::Point3f((float) point_obj[0], (float) point_obj[1], (float) point_obj[2]);
562 }
563 
576 void vpKeyPoint::compute3D(const vpImagePoint &candidate, const std::vector<vpPoint> &roi,
577  const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, vpPoint &point) {
578  /* compute plane equation */
579  std::vector<vpPoint>::const_iterator it_roi = roi.begin();
580  vpPoint pts[3];
581  pts[0] = *it_roi;
582  ++it_roi;
583  pts[1] = *it_roi;
584  ++it_roi;
585  pts[2] = *it_roi;
586  vpPlane Po(pts[0], pts[1], pts[2]);
587  double xc = 0.0, yc = 0.0;
588  vpPixelMeterConversion::convertPoint(cam, candidate, xc, yc);
589  double Z = -Po.getD() / (Po.getA() * xc + Po.getB() * yc + Po.getC());
590  double X = xc * Z;
591  double Y = yc * Z;
592  vpColVector point_cam(4);
593  point_cam[0] = X;
594  point_cam[1] = Y;
595  point_cam[2] = Z;
596  point_cam[3] = 1;
597  vpColVector point_obj(4);
598  point_obj = cMo.inverse() * point_cam;
599  point.setWorldCoordinates(point_obj);
600 }
601 
616  std::vector<cv::KeyPoint> &candidates, const std::vector<vpPolygon> &polygons,
617  const std::vector<std::vector<vpPoint> > &roisPt, std::vector<cv::Point3f> &points, cv::Mat *descriptors) {
618 
619  std::vector<cv::KeyPoint> candidatesToCheck = candidates;
620  candidates.clear();
621  points.clear();
622  vpImagePoint imPt;
623  cv::Point3f pt;
624  cv::Mat desc;
625 
626  std::vector<std::pair<cv::KeyPoint, size_t> > pairOfCandidatesToCheck(candidatesToCheck.size());
627  for(size_t i = 0; i < candidatesToCheck.size(); i++) {
628  pairOfCandidatesToCheck[i] = std::pair<cv::KeyPoint, size_t>(candidatesToCheck[i], i);
629  }
630 
631  size_t cpt1 = 0;
632  std::vector<vpPolygon> polygons_tmp = polygons;
633  for (std::vector<vpPolygon>::iterator it1 = polygons_tmp.begin(); it1 != polygons_tmp.end(); ++it1, cpt1++) {
634  std::vector<std::pair<cv::KeyPoint, size_t> >::iterator it2 = pairOfCandidatesToCheck.begin();
635 
636  while(it2 != pairOfCandidatesToCheck.end()) {
637  imPt.set_ij(it2->first.pt.y, it2->first.pt.x);
638  if (it1->isInside(imPt)) {
639  candidates.push_back(it2->first);
640  vpKeyPoint::compute3D(it2->first, roisPt[cpt1], cam, cMo, pt);
641  points.push_back(pt);
642 
643  if(descriptors != NULL) {
644  desc.push_back(descriptors->row((int) it2->second));
645  }
646 
647  //Remove candidate keypoint which is located on the current polygon
648  it2 = pairOfCandidatesToCheck.erase(it2);
649  } else {
650  ++it2;
651  }
652  }
653  }
654 
655  if(descriptors != NULL) {
656  desc.copyTo(*descriptors);
657  }
658 }
659 
674  std::vector<vpImagePoint> &candidates, const std::vector<vpPolygon> &polygons,
675  const std::vector<std::vector<vpPoint> > &roisPt, std::vector<vpPoint> &points, cv::Mat *descriptors) {
676 
677  std::vector<vpImagePoint> candidatesToCheck = candidates;
678  candidates.clear();
679  points.clear();
680  vpPoint pt;
681  cv::Mat desc;
682 
683  std::vector<std::pair<vpImagePoint, size_t> > pairOfCandidatesToCheck(candidatesToCheck.size());
684  for(size_t i = 0; i < candidatesToCheck.size(); i++) {
685  pairOfCandidatesToCheck[i] = std::pair<vpImagePoint, size_t>(candidatesToCheck[i], i);
686  }
687 
688  size_t cpt1 = 0;
689  std::vector<vpPolygon> polygons_tmp = polygons;
690  for (std::vector<vpPolygon>::iterator it1 = polygons_tmp.begin(); it1 != polygons_tmp.end(); ++it1, cpt1++) {
691  std::vector<std::pair<vpImagePoint, size_t> >::iterator it2 = pairOfCandidatesToCheck.begin();
692 
693  while(it2 != pairOfCandidatesToCheck.end()) {
694  if (it1->isInside(it2->first)) {
695  candidates.push_back(it2->first);
696  vpKeyPoint::compute3D(it2->first, roisPt[cpt1], cam, cMo, pt);
697  points.push_back(pt);
698 
699  if(descriptors != NULL) {
700  desc.push_back(descriptors->row((int) it2->second));
701  }
702 
703  //Remove candidate keypoint which is located on the current polygon
704  it2 = pairOfCandidatesToCheck.erase(it2);
705  } else {
706  ++it2;
707  }
708  }
709  }
710 }
711 
724 bool vpKeyPoint::computePose(const std::vector<cv::Point2f> &imagePoints, const std::vector<cv::Point3f> &objectPoints,
725  const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, std::vector<int> &inlierIndex,
726  double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) {
727  double t = vpTime::measureTimeMs();
728 
729  if(imagePoints.size() < 4 || objectPoints.size() < 4 || imagePoints.size() != objectPoints.size()) {
730  elapsedTime = (vpTime::measureTimeMs() - t);
731  std::cerr << "Not enough points to compute the pose (at least 4 points are needed)." << std::endl;
732 
733  return false;
734  }
735 
736  cv::Mat cameraMatrix =
737  (cv::Mat_<double>(3, 3) << cam.get_px(), 0, cam.get_u0(), 0, cam.get_py(), cam.get_v0(), 0, 0, 1);
738  cv::Mat rvec, tvec;
739 
740 
741  //Bug with OpenCV < 2.4.0 when zero distorsion is provided by an empty array.
742  //http://code.opencv.org/issues/1700 ; http://code.opencv.org/issues/1718
743  //what(): Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8 or 8x1 floating-point vector in function cvProjectPoints2
744  //Fixed in OpenCV 2.4.0 (r7558)
745 // cv::Mat distCoeffs;
746  cv::Mat distCoeffs = cv::Mat::zeros(1, 5, CV_64F);
747 
748  try {
749 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
750  //OpenCV 3.0.0 (2014/12/12)
751  cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs,
752  rvec, tvec, false, m_nbRansacIterations, (float) m_ransacReprojectionError,
753  0.99,//confidence=0.99 (default) – The probability that the algorithm produces a useful result.
754  inlierIndex,
755  cv::SOLVEPNP_ITERATIVE);
756  //SOLVEPNP_ITERATIVE (default): Iterative method is based on Levenberg-Marquardt optimization.
757  //In this case the function finds such a pose that minimizes reprojection error, that is the sum of squared distances
758  //between the observed projections imagePoints and the projected (using projectPoints() ) objectPoints .
759  //SOLVEPNP_P3P: Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang “Complete Solution Classification
760  //for the Perspective-Three-Point Problem”. In this case the function requires exactly four object and image points.
761  //SOLVEPNP_EPNP: Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the paper “EPnP: Efficient
762  //Perspective-n-Point Camera Pose Estimation”.
763  //SOLVEPNP_DLS: Method is based on the paper of Joel A. Hesch and Stergios I. Roumeliotis. “A Direct Least-Squares (DLS)
764  //Method for PnP”.
765  //SOLVEPNP_UPNP Method is based on the paper of A.Penate-Sanchez, J.Andrade-Cetto, F.Moreno-Noguer.
766  //“Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation”. In this case the function also
767  //estimates the parameters f_x and f_y assuming that both have the same value. Then the cameraMatrix is updated with the
768  //estimated focal length.
769 #else
770  int nbInlierToReachConsensus = m_nbRansacMinInlierCount;
771  if(m_useConsensusPercentage) {
772  nbInlierToReachConsensus = (int) (m_ransacConsensusPercentage / 100.0 * (double) m_queryFilteredKeyPoints.size());
773  }
774 
775  cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs,
776  rvec, tvec, false, m_nbRansacIterations,
777  (float) m_ransacReprojectionError, nbInlierToReachConsensus,
778  inlierIndex);
779 #endif
780  } catch (cv::Exception &e) {
781  std::cerr << e.what() << std::endl;
782  elapsedTime = (vpTime::measureTimeMs() - t);
783  return false;
784  }
785  vpTranslationVector translationVec(tvec.at<double>(0),
786  tvec.at<double>(1), tvec.at<double>(2));
787  vpThetaUVector thetaUVector(rvec.at<double>(0), rvec.at<double>(1),
788  rvec.at<double>(2));
789  cMo = vpHomogeneousMatrix(translationVec, thetaUVector);
790 
791  if(func != NULL) {
792  //Check the final pose returned by solvePnPRansac to discard
793  //solutions which do not respect the pose criterion.
794  if(!func(&cMo)) {
795  elapsedTime = (vpTime::measureTimeMs() - t);
796  return false;
797  }
798  }
799 
800  elapsedTime = (vpTime::measureTimeMs() - t);
801  return true;
802 }
803 
815 bool vpKeyPoint::computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo,
816  std::vector<vpPoint> &inliers, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) {
817  std::vector<unsigned int> inlierIndex;
818  return computePose(objectVpPoints, cMo, inliers, inlierIndex, elapsedTime, func);
819 }
820 
833 bool vpKeyPoint::computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo,
834  std::vector<vpPoint> &inliers, std::vector<unsigned int> &inlierIndex,
835  double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) {
836  double t = vpTime::measureTimeMs();
837 
838  if(objectVpPoints.size() < 4) {
839  elapsedTime = (vpTime::measureTimeMs() - t);
840 // std::cerr << "Not enough points to compute the pose (at least 4 points are needed)." << std::endl;
841 
842  return false;
843  }
844 
845  vpPose pose;
846 
847  for(std::vector<vpPoint>::const_iterator it = objectVpPoints.begin(); it != objectVpPoints.end(); ++it) {
848  pose.addPoint(*it);
849  }
850 
851  unsigned int nbInlierToReachConsensus = (unsigned int) m_nbRansacMinInlierCount;
852  if(m_useConsensusPercentage) {
853  nbInlierToReachConsensus = (unsigned int) (m_ransacConsensusPercentage / 100.0 *
854  (double) m_queryFilteredKeyPoints.size());
855  }
856 
857  pose.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
858  pose.setRansacThreshold(m_ransacThreshold);
859  pose.setRansacMaxTrials(m_nbRansacIterations);
860 
861  bool isRansacPoseEstimationOk = false;
862  try {
863  pose.setCovarianceComputation(m_computeCovariance);
864  isRansacPoseEstimationOk = pose.computePose(vpPose::RANSAC, cMo, func);
865  inliers = pose.getRansacInliers();
866  inlierIndex = pose.getRansacInlierIndex();
867 
868  if(m_computeCovariance) {
869  m_covarianceMatrix = pose.getCovarianceMatrix();
870  }
871  } catch(vpException &e) {
872  std::cerr << "e=" << e.what() << std::endl;
873  elapsedTime = (vpTime::measureTimeMs() - t);
874  return false;
875  }
876 
877 // if(func != NULL && isRansacPoseEstimationOk) {
878 // //Check the final pose returned by the Ransac VVS pose estimation as in rare some cases
879 // //we can converge toward a final cMo that does not respect the pose criterion even
880 // //if the 4 minimal points picked to respect the pose criterion.
881 // if(!func(&cMo)) {
882 // elapsedTime = (vpTime::measureTimeMs() - t);
883 // return false;
884 // }
885 // }
886 
887  elapsedTime = (vpTime::measureTimeMs() - t);
888  return isRansacPoseEstimationOk;
889 }
890 
902 double vpKeyPoint::computePoseEstimationError(const std::vector<std::pair<cv::KeyPoint, cv::Point3f> > &matchKeyPoints,
903  const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo_est) {
904  if(matchKeyPoints.size() == 0) {
905  //return std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
906  return DBL_MAX;
907  }
908 
909  std::vector<double> errors(matchKeyPoints.size());
910  size_t cpt = 0;
911  vpPoint pt;
912  for(std::vector<std::pair<cv::KeyPoint, cv::Point3f> >::const_iterator it = matchKeyPoints.begin();
913  it != matchKeyPoints.end(); ++it, cpt++) {
914  pt.set_oX(it->second.x);
915  pt.set_oY(it->second.y);
916  pt.set_oZ(it->second.z);
917  pt.project(cMo_est);
918  double u = 0.0, v = 0.0;
919  vpMeterPixelConversion::convertPoint(cam, pt.get_x(), pt.get_y(), u, v);
920  errors[cpt] = std::sqrt((u-it->first.pt.x)*(u-it->first.pt.x) + (v-it->first.pt.y)*(v-it->first.pt.y));
921  }
922 
923  return std::accumulate(errors.begin(), errors.end(), 0.0) / errors.size();
924 }
925 
934  vpImage<unsigned char> &IMatching) {
935  //Image matching side by side
936  unsigned int width = IRef.getWidth() + ICurrent.getWidth();
937  unsigned int height = (std::max)(IRef.getHeight(), ICurrent.getHeight());
938 
939  IMatching = vpImage<unsigned char>(height, width);
940 }
941 
951  //Nb images in the training database + the current image we want to detect the object
952  unsigned int nbImg = (unsigned int) (m_mapOfImages.size() + 1);
953 
954  if(m_mapOfImages.empty()) {
955  std::cerr << "There is no training image loaded !" << std::endl;
956  return;
957  }
958 
959  if(nbImg == 2) {
960  //Only one training image, so we display them side by side
961  createImageMatching(m_mapOfImages.begin()->second, ICurrent, IMatching);
962  } else {
963  //Multiple training images, display them as a mosaic image
964  //(unsigned int) std::floor(std::sqrt((double) nbImg) + 0.5);
965  unsigned int nbImgSqrt = (unsigned int) vpMath::round(std::sqrt((double) nbImg));
966 
967  //Number of columns in the mosaic grid
968  unsigned int nbWidth = nbImgSqrt;
969  //Number of rows in the mosaic grid
970  unsigned int nbHeight = nbImgSqrt;
971 
972  //Deals with non square mosaic grid and the total number of images
973  if(nbImgSqrt * nbImgSqrt < nbImg) {
974  nbWidth++;
975  }
976 
977  unsigned int maxW = ICurrent.getWidth();
978  unsigned int maxH = ICurrent.getHeight();
979  for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
980  if(maxW < it->second.getWidth()) {
981  maxW = it->second.getWidth();
982  }
983 
984  if(maxH < it->second.getHeight()) {
985  maxH = it->second.getHeight();
986  }
987  }
988 
989  IMatching = vpImage<unsigned char>(maxH * nbHeight, maxW * nbWidth);
990  }
991 }
992 
1001 void vpKeyPoint::detect(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
1002  const vpRect &rectangle) {
1003  cv::Mat matImg;
1004  vpImageConvert::convert(I, matImg, false);
1005  cv::Mat mask = cv::Mat::zeros(matImg.rows, matImg.cols, CV_8U);
1006 
1007  if(rectangle.getWidth() > 0 && rectangle.getHeight() > 0) {
1008  cv::Point leftTop((int) rectangle.getLeft(), (int) rectangle.getTop()), rightBottom((int) rectangle.getRight(),
1009  (int) rectangle.getBottom());
1010  cv::rectangle(mask, leftTop, rightBottom, cv::Scalar(255), CV_FILLED);
1011  } else {
1012  mask = cv::Mat::ones(matImg.rows, matImg.cols, CV_8U) * 255;
1013  }
1014 
1015  detect(matImg, keyPoints, elapsedTime, mask);
1016 }
1017 
1026 void vpKeyPoint::detect(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
1027  const cv::Mat &mask) {
1028  double t = vpTime::measureTimeMs();
1029  keyPoints.clear();
1030 
1031  for(std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin(); it != m_detectors.end(); ++it) {
1032  std::vector<cv::KeyPoint> kp;
1033  it->second->detect(matImg, kp, mask);
1034  keyPoints.insert(keyPoints.end(), kp.begin(), kp.end());
1035  }
1036 
1037  elapsedTime = vpTime::measureTimeMs() - t;
1038 }
1039 
1048  const vpImage<unsigned char> &ICurrent, unsigned int size) {
1049  std::vector<vpImagePoint> vpQueryImageKeyPoints;
1050  getQueryKeyPoints(vpQueryImageKeyPoints);
1051  std::vector<vpImagePoint> vpTrainImageKeyPoints;
1052  getTrainKeyPoints(vpTrainImageKeyPoints);
1053 
1054  for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
1055  vpDisplay::displayCross(IRef, vpTrainImageKeyPoints[(size_t)(it->trainIdx)], size, vpColor::red);
1056  vpDisplay::displayCross(ICurrent, vpQueryImageKeyPoints[(size_t)(it->queryIdx)], size, vpColor::green);
1057  }
1058 }
1059 
1067 void vpKeyPoint::display(const vpImage<unsigned char> &ICurrent, unsigned int size, const vpColor &color) {
1068  std::vector<vpImagePoint> vpQueryImageKeyPoints;
1069  getQueryKeyPoints(vpQueryImageKeyPoints);
1070 
1071  for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
1072  vpDisplay::displayCross (ICurrent, vpQueryImageKeyPoints[(size_t)(it->queryIdx)], size, color);
1073  }
1074 }
1075 
1086  unsigned int crossSize, unsigned int lineThickness, const vpColor &color) {
1087  bool randomColor = (color == vpColor::none);
1088  srand((unsigned int) time(NULL));
1089  vpColor currentColor = color;
1090 
1091  std::vector<vpImagePoint> queryImageKeyPoints;
1092  getQueryKeyPoints(queryImageKeyPoints);
1093  std::vector<vpImagePoint> trainImageKeyPoints;
1094  getTrainKeyPoints(trainImageKeyPoints);
1095 
1096  vpImagePoint leftPt, rightPt;
1097  for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
1098  if(randomColor) {
1099  currentColor = vpColor((rand() % 256), (rand() % 256), (rand() % 256));
1100  }
1101 
1102  leftPt = trainImageKeyPoints[(size_t)(it->trainIdx)];
1103  rightPt = vpImagePoint(queryImageKeyPoints[(size_t)(it->queryIdx)].get_i(),
1104  queryImageKeyPoints[(size_t)it->queryIdx].get_j() + IRef.getWidth());
1105  vpDisplay::displayCross(IMatching, leftPt, crossSize, currentColor);
1106  vpDisplay::displayCross(IMatching, rightPt, crossSize, currentColor);
1107  vpDisplay::displayLine(IMatching, leftPt, rightPt, currentColor, lineThickness);
1108  }
1109 }
1110 
1122  const std::vector<vpImagePoint> &ransacInliers, unsigned int crossSize, unsigned int lineThickness) {
1123  if(m_mapOfImages.empty() || m_mapOfImageId.empty()) {
1124  //No training images so return
1125  std::cerr << "There is no training image loaded !" << std::endl;
1126  return;
1127  }
1128 
1129  //Nb images in the training database + the current image we want to detect the object
1130  int nbImg = (int) (m_mapOfImages.size() + 1);
1131 
1132  if(nbImg == 2) {
1133  //Only one training image, so we display the matching result side-by-side
1134  displayMatching(m_mapOfImages.begin()->second, IMatching, crossSize);
1135  } else {
1136  //Multiple training images, display them as a mosaic image
1137  int nbImgSqrt = vpMath::round(std::sqrt((double) nbImg)); //(int) std::floor(std::sqrt((double) nbImg) + 0.5);
1138  int nbWidth = nbImgSqrt;
1139  int nbHeight = nbImgSqrt;
1140 
1141  if(nbImgSqrt * nbImgSqrt < nbImg) {
1142  nbWidth++;
1143  }
1144 
1145  std::map<int, int> mapOfImageIdIndex;
1146  int cpt = 0;
1147  unsigned int maxW = ICurrent.getWidth(), maxH = ICurrent.getHeight();
1148  for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
1149  mapOfImageIdIndex[it->first] = cpt;
1150 
1151  if(maxW < it->second.getWidth()) {
1152  maxW = it->second.getWidth();
1153  }
1154 
1155  if(maxH < it->second.getHeight()) {
1156  maxH = it->second.getHeight();
1157  }
1158  }
1159 
1160  //Indexes of the current image in the grid made in order to the image is in the center square in the mosaic grid
1161  int medianI = nbHeight / 2;
1162  int medianJ = nbWidth / 2;
1163  int medianIndex = medianI * nbWidth + medianJ;
1164  for(std::vector<cv::KeyPoint>::const_iterator it = m_trainKeyPoints.begin(); it != m_trainKeyPoints.end(); ++it) {
1165  vpImagePoint topLeftCorner;
1166  int current_class_id_index = 0;
1167  if(mapOfImageIdIndex[m_mapOfImageId[it->class_id]] < medianIndex) {
1168  current_class_id_index = mapOfImageIdIndex[m_mapOfImageId[it->class_id]];
1169  } else {
1170  //Shift of one unity the index of the training images which are after the current image
1171  current_class_id_index = mapOfImageIdIndex[m_mapOfImageId[it->class_id]] + 1;
1172  }
1173 
1174  int indexI = current_class_id_index / nbWidth;
1175  int indexJ = current_class_id_index - (indexI * nbWidth);
1176  topLeftCorner.set_ij((int)maxH*indexI, (int)maxW*indexJ);
1177 
1178  //Display cross for keypoints in the learning database
1179  vpDisplay::displayCross(IMatching, (int) (it->pt.y + topLeftCorner.get_i()), (int) (it->pt.x + topLeftCorner.get_j()),
1180  crossSize, vpColor::red);
1181  }
1182 
1183  vpImagePoint topLeftCorner((int)maxH*medianI, (int)maxW*medianJ);
1184  for(std::vector<cv::KeyPoint>::const_iterator it = m_queryKeyPoints.begin(); it != m_queryKeyPoints.end(); ++it) {
1185  //Display cross for keypoints detected in the current image
1186  vpDisplay::displayCross(IMatching, (int) (it->pt.y + topLeftCorner.get_i()), (int) (it->pt.x + topLeftCorner.get_j()),
1187  crossSize, vpColor::red);
1188  }
1189  for(std::vector<vpImagePoint>::const_iterator it = ransacInliers.begin();
1190  it != ransacInliers.end(); ++it) {
1191  //Display green circle for RANSAC inliers
1192  vpDisplay::displayCircle(IMatching, (int) (it->get_v() + topLeftCorner.get_i()), (int) (it->get_u() +
1193  topLeftCorner.get_j()), 4, vpColor::green);
1194  }
1195  for(std::vector<vpImagePoint>::const_iterator it = m_ransacOutliers.begin(); it != m_ransacOutliers.end(); ++it) {
1196  //Display red circle for RANSAC outliers
1197  vpDisplay::displayCircle(IMatching, (int) (it->get_i() + topLeftCorner.get_i()), (int) (it->get_j() +
1198  topLeftCorner.get_j()), 4, vpColor::red);
1199  }
1200 
1201  for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
1202  int current_class_id = 0;
1203  if(mapOfImageIdIndex[m_mapOfImageId[m_trainKeyPoints[(size_t) it->trainIdx].class_id]] < medianIndex) {
1204  current_class_id = mapOfImageIdIndex[m_mapOfImageId[m_trainKeyPoints[(size_t) it->trainIdx].class_id]];
1205  } else {
1206  //Shift of one unity the index of the training images which are after the current image
1207  current_class_id = mapOfImageIdIndex[m_mapOfImageId[m_trainKeyPoints[(size_t) it->trainIdx].class_id]] + 1;
1208  }
1209 
1210  int indexI = current_class_id / nbWidth;
1211  int indexJ = current_class_id - (indexI * nbWidth);
1212 
1213  vpImagePoint end((int)maxH*indexI + m_trainKeyPoints[(size_t) it->trainIdx].pt.y,
1214  (int)maxW*indexJ + m_trainKeyPoints[(size_t) it->trainIdx].pt.x);
1215  vpImagePoint start((int)maxH*medianI + m_queryFilteredKeyPoints[(size_t) it->queryIdx].pt.y,
1216  (int)maxW*medianJ + m_queryFilteredKeyPoints[(size_t) it->queryIdx].pt.x);
1217 
1218  //Draw line for matching keypoints detected in the current image and those detected
1219  //in the training images
1220  vpDisplay::displayLine(IMatching, start, end, vpColor::green, lineThickness);
1221  }
1222  }
1223 }
1224 
1235 void vpKeyPoint::extract(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
1236  double &elapsedTime, std::vector<cv::Point3f> *trainPoints) {
1237  cv::Mat matImg;
1238  vpImageConvert::convert(I, matImg, false);
1239  extract(matImg, keyPoints, descriptors, elapsedTime, trainPoints);
1240 }
1241 
1252 void vpKeyPoint::extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
1253  double &elapsedTime, std::vector<cv::Point3f> *trainPoints) {
1254  double t = vpTime::measureTimeMs();
1255  bool first = true;
1256 
1257  for(std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator itd = m_extractors.begin();
1258  itd != m_extractors.end(); ++itd) {
1259  if(first) {
1260  first = false;
1261  //Check if we have 3D object points information
1262  if(trainPoints != NULL && !trainPoints->empty()) {
1263  //Copy the input list of keypoints, keypoints that cannot be computed are removed in the function compute
1264  std::vector<cv::KeyPoint> keyPoints_tmp = keyPoints;
1265 
1266  //Extract descriptors for the given list of keypoints
1267  itd->second->compute(matImg, keyPoints, descriptors);
1268 
1269  if(keyPoints.size() != keyPoints_tmp.size()) {
1270  //Keypoints have been removed
1271  //Store the hash of a keypoint as the key and the index of the keypoint as the value
1272  std::map<size_t, size_t> mapOfKeypointHashes;
1273  size_t cpt = 0;
1274  for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints_tmp.begin(); it != keyPoints_tmp.end(); ++it, cpt++) {
1275  mapOfKeypointHashes[myKeypointHash(*it)] = cpt;
1276  }
1277 
1278  std::vector<cv::Point3f> trainPoints_tmp;
1279  for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints.begin(); it != keyPoints.end(); ++it) {
1280  if(mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
1281  trainPoints_tmp.push_back((*trainPoints)[mapOfKeypointHashes[myKeypointHash(*it)]]);
1282  }
1283  }
1284 
1285  //Copy trainPoints_tmp to m_trainPoints
1286  *trainPoints = trainPoints_tmp;
1287  }
1288  } else {
1289  //Extract descriptors for the given list of keypoints
1290  itd->second->compute(matImg, keyPoints, descriptors);
1291  }
1292  } else {
1293  //Copy the input list of keypoints, keypoints that cannot be computed are removed in the function compute
1294  std::vector<cv::KeyPoint> keyPoints_tmp = keyPoints;
1295 
1296  cv::Mat desc;
1297  //Extract descriptors for the given list of keypoints
1298  itd->second->compute(matImg, keyPoints, desc);
1299 
1300  if(keyPoints.size() != keyPoints_tmp.size()) {
1301  //Keypoints have been removed
1302  //Store the hash of a keypoint as the key and the index of the keypoint as the value
1303  std::map<size_t, size_t> mapOfKeypointHashes;
1304  size_t cpt = 0;
1305  for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints_tmp.begin(); it != keyPoints_tmp.end(); ++it, cpt++) {
1306  mapOfKeypointHashes[myKeypointHash(*it)] = cpt;
1307  }
1308 
1309  std::vector<cv::Point3f> trainPoints_tmp;
1310  cv::Mat descriptors_tmp;
1311  for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints.begin(); it != keyPoints.end(); ++it) {
1312  if(mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
1313  if(trainPoints != NULL && !trainPoints->empty()) {
1314  trainPoints_tmp.push_back((*trainPoints)[mapOfKeypointHashes[myKeypointHash(*it)]]);
1315  }
1316 
1317  if(!descriptors.empty()) {
1318  descriptors_tmp.push_back(descriptors.row((int) mapOfKeypointHashes[myKeypointHash(*it)]));
1319  }
1320  }
1321  }
1322 
1323  if(trainPoints != NULL) {
1324  //Copy trainPoints_tmp to m_trainPoints
1325  *trainPoints = trainPoints_tmp;
1326  }
1327  //Copy descriptors_tmp to descriptors
1328  descriptors_tmp.copyTo(descriptors);
1329  }
1330 
1331  //Merge descriptors horizontally
1332  if(descriptors.empty()) {
1333  desc.copyTo(descriptors);
1334  } else {
1335  cv::hconcat(descriptors, desc, descriptors);
1336  }
1337  }
1338  }
1339 
1340  if(keyPoints.size() != (size_t) descriptors.rows) {
1341  std::cerr << "keyPoints.size() != (size_t) descriptors.rows" << std::endl;
1342  }
1343  elapsedTime = vpTime::measureTimeMs() - t;
1344 }
1345 
1349 void vpKeyPoint::filterMatches() {
1350  std::vector<cv::KeyPoint> queryKpts;
1351  std::vector<cv::Point3f> trainPts;
1352  std::vector<cv::DMatch> m;
1353 
1354  if(m_useKnn) {
1355  double max_dist = 0;
1356  //double min_dist = std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
1357  double min_dist = DBL_MAX;
1358  double mean = 0.0;
1359  std::vector<double> distance_vec(m_knnMatches.size());
1360 
1361  if(m_filterType == stdAndRatioDistanceThreshold) {
1362  for(size_t i = 0; i < m_knnMatches.size(); i++) {
1363  double dist = m_knnMatches[i][0].distance;
1364  mean += dist;
1365  distance_vec[i] = dist;
1366 
1367  if (dist < min_dist) {
1368  min_dist = dist;
1369  }
1370  if (dist > max_dist) {
1371  max_dist = dist;
1372  }
1373  }
1374  mean /= m_queryDescriptors.rows;
1375  }
1376 
1377  double sq_sum = std::inner_product(distance_vec.begin(), distance_vec.end(), distance_vec.begin(), 0.0);
1378  double stdev = std::sqrt(sq_sum / distance_vec.size() - mean * mean);
1379  double threshold = min_dist + stdev;
1380 
1381  for(size_t i = 0; i < m_knnMatches.size(); i++) {
1382  if(m_knnMatches[i].size() >= 2) {
1383  //Calculate ratio of the descriptor distance between the two nearest neighbors of the keypoint
1384  float ratio = m_knnMatches[i][0].distance / m_knnMatches[i][1].distance;
1385 // float ratio = std::sqrt((vecMatches[i][0].distance * vecMatches[i][0].distance)
1386 // / (vecMatches[i][1].distance * vecMatches[i][1].distance));
1387  double dist = m_knnMatches[i][0].distance;
1388 
1389  if(ratio < m_matchingRatioThreshold || (m_filterType == stdAndRatioDistanceThreshold && dist < threshold)) {
1390  m.push_back(cv::DMatch((int) queryKpts.size(), m_knnMatches[i][0].trainIdx, m_knnMatches[i][0].distance));
1391 
1392  if(!m_trainPoints.empty()) {
1393  trainPts.push_back(m_trainPoints[(size_t)m_knnMatches[i][0].trainIdx]);
1394  }
1395  queryKpts.push_back(m_queryKeyPoints[(size_t)m_knnMatches[i][0].queryIdx]);
1396  }
1397  }
1398  }
1399  } else {
1400  double max_dist = 0;
1401  // create an error under Windows. To fix it we have to add #undef max
1402  //double min_dist = std::numeric_limits<double>::max();
1403  double min_dist = DBL_MAX;
1404  double mean = 0.0;
1405  std::vector<double> distance_vec(m_matches.size());
1406  for(size_t i = 0; i < m_matches.size(); i++) {
1407  double dist = m_matches[i].distance;
1408  mean += dist;
1409  distance_vec[i] = dist;
1410 
1411  if (dist < min_dist) {
1412  min_dist = dist;
1413  }
1414  if (dist > max_dist) {
1415  max_dist = dist;
1416  }
1417  }
1418  mean /= m_queryDescriptors.rows;
1419 
1420  double sq_sum = std::inner_product(distance_vec.begin(), distance_vec.end(), distance_vec.begin(), 0.0);
1421  double stdev = std::sqrt(sq_sum / distance_vec.size() - mean * mean);
1422 
1423  //Define a threshold where we keep all keypoints whose the descriptor distance falls below a factor of the
1424  //minimum descriptor distance (for all the query keypoints)
1425  //or below the minimum descriptor distance + the standard deviation (calculated on all the query descriptor distances)
1426  double threshold = m_filterType == constantFactorDistanceThreshold ? m_matchingFactorThreshold * min_dist : min_dist + stdev;
1427 
1428  for (size_t i = 0; i < m_matches.size(); i++) {
1429  if(m_matches[i].distance <= threshold) {
1430  m.push_back(cv::DMatch((int) queryKpts.size(), m_matches[i].trainIdx, m_matches[i].distance));
1431 
1432  if(!m_trainPoints.empty()) {
1433  trainPts.push_back(m_trainPoints[(size_t)m_matches[i].trainIdx]);
1434  }
1435  queryKpts.push_back(m_queryKeyPoints[(size_t)m_matches[i].queryIdx]);
1436  }
1437  }
1438  }
1439 
1440  if(m_useSingleMatchFilter) {
1441  //Eliminate matches where multiple query keypoints are matched to the same train keypoint
1442  std::vector<cv::DMatch> mTmp;
1443  std::vector<cv::Point3f> trainPtsTmp;
1444  std::vector<cv::KeyPoint> queryKptsTmp;
1445 
1446  std::map<int, int> mapOfTrainIdx;
1447  //Count the number of query points matched to the same train point
1448  for(std::vector<cv::DMatch>::const_iterator it = m.begin(); it != m.end(); ++it) {
1449  mapOfTrainIdx[it->trainIdx]++;
1450  }
1451 
1452  //Keep matches with only one correspondence
1453  for(std::vector<cv::DMatch>::const_iterator it = m.begin(); it != m.end(); ++it) {
1454  if(mapOfTrainIdx[it->trainIdx] == 1) {
1455  mTmp.push_back(cv::DMatch((int) queryKptsTmp.size(), it->trainIdx, it->distance));
1456 
1457  if(!m_trainPoints.empty()) {
1458  trainPtsTmp.push_back(m_trainPoints[(size_t) it->trainIdx]);
1459  }
1460  queryKptsTmp.push_back(queryKpts[(size_t) it->queryIdx]);
1461  }
1462  }
1463 
1464  m_filteredMatches = mTmp;
1465  m_objectFilteredPoints = trainPtsTmp;
1466  m_queryFilteredKeyPoints = queryKptsTmp;
1467  } else {
1468  m_filteredMatches = m;
1469  m_objectFilteredPoints = trainPts;
1470  m_queryFilteredKeyPoints = queryKpts;
1471  }
1472 }
1473 
1480 void vpKeyPoint::getObjectPoints(std::vector<cv::Point3f> &objectPoints) const {
1481  objectPoints = m_objectFilteredPoints;
1482 }
1483 
1490 void vpKeyPoint::getObjectPoints(std::vector<vpPoint> &objectPoints) const {
1491  vpConvert::convertFromOpenCV(m_objectFilteredPoints, objectPoints);
1492 }
1493 
1499 void vpKeyPoint::getQueryKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const {
1500  keyPoints = m_queryFilteredKeyPoints;
1501 }
1502 
1508 void vpKeyPoint::getQueryKeyPoints(std::vector<vpImagePoint> &keyPoints) const {
1509  keyPoints = currentImagePointsList;
1510 }
1511 
1517 void vpKeyPoint::getTrainKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const {
1518  keyPoints = m_trainKeyPoints;
1519 }
1520 
1526 void vpKeyPoint::getTrainKeyPoints(std::vector<vpImagePoint> &keyPoints) const {
1527  keyPoints = referenceImagePointsList;
1528 }
1529 
1535 void vpKeyPoint::getTrainPoints(std::vector<cv::Point3f> &points) const {
1536  points = m_trainPoints;
1537 }
1538 
1544 void vpKeyPoint::getTrainPoints(std::vector<vpPoint> &points) const {
1545  points = m_trainVpPoints;
1546 }
1547 
1551 void vpKeyPoint::init() {
1552  // Require 2.4.0 <= opencv < 3.0.0
1553 #if defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION >= 0x020400) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
1554  //The following line must be called in order to use SIFT or SURF
1555  if (!cv::initModule_nonfree()) {
1556  std::cerr << "Cannot init module non free, SIFT or SURF cannot be used."
1557  << std::endl;
1558  }
1559 #endif
1560 
1561  initDetectors(m_detectorNames);
1562  initExtractors(m_extractorNames);
1563  initMatcher(m_matcherName);
1564 }
1565 
1571 void vpKeyPoint::initDetector(const std::string &detectorName) {
1572 #if (VISP_HAVE_OPENCV_VERSION < 0x030000)
1573  m_detectors[detectorName] = cv::FeatureDetector::create(detectorName);
1574 
1575  if(m_detectors[detectorName] == NULL) {
1576  std::stringstream ss_msg;
1577  ss_msg << "Fail to initialize the detector: " << detectorName << " or it is not available in OpenCV version: "
1578  << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
1579  throw vpException(vpException::fatalError, ss_msg.str());
1580  }
1581 #else
1582  std::string detectorNameTmp = detectorName;
1583  std::string pyramid = "Pyramid";
1584  std::size_t pos = detectorName.find(pyramid);
1585  bool usePyramid = false;
1586  if(pos != std::string::npos) {
1587  detectorNameTmp = detectorName.substr(pos + pyramid.size());
1588  usePyramid = true;
1589  }
1590 
1591  if(detectorNameTmp == "SIFT") {
1592 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1593  cv::Ptr<cv::FeatureDetector> siftDetector = cv::xfeatures2d::SIFT::create();
1594  if(!usePyramid) {
1595  m_detectors[detectorNameTmp] = siftDetector;
1596  } else {
1597  std::cerr << "Kind of non sense to use SIFT with Pyramid !" << std::endl;
1598  m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(siftDetector);
1599  }
1600 #else
1601  std::stringstream ss_msg;
1602  ss_msg << "Fail to initialize the detector: SIFT. OpenCV version "
1603  << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
1604  throw vpException(vpException::fatalError, ss_msg.str());
1605 #endif
1606  } else if(detectorNameTmp == "SURF") {
1607 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1608  cv::Ptr<cv::FeatureDetector> surfDetector = cv::xfeatures2d::SURF::create();
1609  if(!usePyramid) {
1610  m_detectors[detectorNameTmp] = surfDetector;
1611  } else {
1612  std::cerr << "Kind of non sense to use SURF with Pyramid !" << std::endl;
1613  m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(surfDetector);
1614  }
1615 #else
1616  std::stringstream ss_msg;
1617  ss_msg << "Fail to initialize the detector: SURF. OpenCV version "
1618  << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
1619  throw vpException(vpException::fatalError, ss_msg.str());
1620 #endif
1621  } else if(detectorNameTmp == "FAST") {
1622  cv::Ptr<cv::FeatureDetector> fastDetector = cv::FastFeatureDetector::create();
1623  if(!usePyramid) {
1624  m_detectors[detectorNameTmp] = fastDetector;
1625  } else {
1626 // m_detectors[detectorName] = new PyramidAdaptedFeatureDetector(cv::FastFeatureDetector::create());
1627  m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(fastDetector);
1628  }
1629  } else if(detectorNameTmp == "MSER") {
1630  cv::Ptr<cv::FeatureDetector> fastDetector = cv::MSER::create();
1631  if(!usePyramid) {
1632  m_detectors[detectorNameTmp] = fastDetector;
1633  } else {
1634 // m_detectors[detectorName] = new PyramidAdaptedFeatureDetector(cv::MSER::create());
1635  m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(fastDetector);
1636  }
1637  } else if(detectorNameTmp == "ORB") {
1638  cv::Ptr<cv::FeatureDetector> orbDetector = cv::ORB::create();
1639  if(!usePyramid) {
1640  m_detectors[detectorNameTmp] = orbDetector;
1641  } else {
1642  std::cerr << "Kind of non sense to use ORB with Pyramid !" << std::endl;
1643  m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(orbDetector);
1644  }
1645  } else if(detectorNameTmp == "BRISK") {
1646  cv::Ptr<cv::FeatureDetector> briskDetector = cv::BRISK::create();
1647  if(!usePyramid) {
1648  m_detectors[detectorNameTmp] = briskDetector;
1649  } else {
1650  std::cerr << "Kind of non sense to use BRISK with Pyramid !" << std::endl;
1651  m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(briskDetector);
1652  }
1653  } else if(detectorNameTmp == "KAZE") {
1654  cv::Ptr<cv::FeatureDetector> kazeDetector = cv::KAZE::create();
1655  if(!usePyramid) {
1656  m_detectors[detectorNameTmp] = kazeDetector;
1657  } else {
1658  std::cerr << "Kind of non sense to use KAZE with Pyramid !" << std::endl;
1659  m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(kazeDetector);
1660  }
1661  } else if(detectorNameTmp == "AKAZE") {
1662  cv::Ptr<cv::FeatureDetector> akazeDetector = cv::AKAZE::create();
1663  if(!usePyramid) {
1664  m_detectors[detectorNameTmp] = akazeDetector;
1665  } else {
1666  std::cerr << "Kind of non sense to use AKAZE with Pyramid !" << std::endl;
1667  m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(akazeDetector);
1668  }
1669  } else if(detectorNameTmp == "GFTT") {
1670  cv::Ptr<cv::FeatureDetector> gfttDetector = cv::GFTTDetector::create();
1671  if(!usePyramid) {
1672  m_detectors[detectorNameTmp] = gfttDetector;
1673  } else {
1674 // m_detectors[detectorName] = new PyramidAdaptedFeatureDetector(cv::GFTTDetector::create());
1675  m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(gfttDetector);
1676  }
1677  } else if(detectorNameTmp == "SimpleBlob") {
1678  cv::Ptr<cv::FeatureDetector> simpleBlobDetector = cv::SimpleBlobDetector::create();
1679  if(!usePyramid) {
1680  m_detectors[detectorNameTmp] = simpleBlobDetector;
1681  } else {
1682 // m_detectors[detectorName] = new PyramidAdaptedFeatureDetector(cv::SimpleBlobDetector::create());
1683  m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(simpleBlobDetector);
1684  }
1685  } else if(detectorNameTmp == "STAR") {
1686 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1687  cv::Ptr<cv::FeatureDetector> starDetector = cv::xfeatures2d::StarDetector::create();
1688  if(!usePyramid) {
1689  m_detectors[detectorNameTmp] = starDetector;
1690  } else {
1691 // m_detectors[detectorName] = new PyramidAdaptedFeatureDetector(cv::xfeatures2d::StarDetector::create());
1692  m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(starDetector);
1693  }
1694 #else
1695  std::stringstream ss_msg;
1696  ss_msg << "Fail to initialize the detector: STAR. OpenCV version "
1697  << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
1698  throw vpException(vpException::fatalError, ss_msg.str());
1699 #endif
1700  } else if(detectorNameTmp == "AGAST") {
1701  cv::Ptr<cv::FeatureDetector> agastDetector = cv::AgastFeatureDetector::create();
1702  if(!usePyramid) {
1703  m_detectors[detectorNameTmp] = agastDetector;
1704  } else {
1705  m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(agastDetector);
1706  }
1707  } else {
1708  std::cerr << "The detector:" << detectorNameTmp << " is not available." << std::endl;
1709  }
1710 
1711  bool detectorInitialized = false;
1712  if(!usePyramid) {
1713  detectorInitialized = (m_detectors[detectorNameTmp] != NULL);
1714  } else {
1715  detectorInitialized = (m_detectors[detectorName] != NULL);
1716  }
1717 
1718  if(!detectorInitialized) {
1719  std::stringstream ss_msg;
1720  ss_msg << "Fail to initialize the detector: " << detectorNameTmp << " or it is not available in OpenCV version: "
1721  << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
1722  throw vpException(vpException::fatalError, ss_msg.str());
1723  }
1724 
1725 #endif
1726 }
1727 
1733 void vpKeyPoint::initDetectors(const std::vector<std::string> &detectorNames) {
1734  for(std::vector<std::string>::const_iterator it = detectorNames.begin(); it != detectorNames.end(); ++it) {
1735  initDetector(*it);
1736  }
1737 }
1738 
1744 void vpKeyPoint::initExtractor(const std::string &extractorName) {
1745 #if (VISP_HAVE_OPENCV_VERSION < 0x030000)
1746  m_extractors[extractorName] = cv::DescriptorExtractor::create(extractorName);
1747 #else
1748  if(extractorName == "SIFT") {
1749 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1750  m_extractors[extractorName] = cv::xfeatures2d::SIFT::create();
1751 #else
1752  std::stringstream ss_msg;
1753  ss_msg << "Fail to initialize the extractor: SIFT. OpenCV version "
1754  << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
1755  throw vpException(vpException::fatalError, ss_msg.str());
1756 #endif
1757  } else if(extractorName == "SURF") {
1758 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1759  //Use extended set of SURF descriptors (128 instead of 64)
1760  m_extractors[extractorName] = cv::xfeatures2d::SURF::create(100, 4, 3, true);
1761 #else
1762  std::stringstream ss_msg;
1763  ss_msg << "Fail to initialize the extractor: SURF. OpenCV version "
1764  << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
1765  throw vpException(vpException::fatalError, ss_msg.str());
1766 #endif
1767  } else if(extractorName == "ORB") {
1768  m_extractors[extractorName] = cv::ORB::create();
1769  } else if(extractorName == "BRISK") {
1770  m_extractors[extractorName] = cv::BRISK::create();
1771  } else if(extractorName == "FREAK") {
1772 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1773  m_extractors[extractorName] = cv::xfeatures2d::FREAK::create();
1774 #else
1775  std::stringstream ss_msg;
1776  ss_msg << "Fail to initialize the extractor: FREAK. OpenCV version "
1777  << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
1778  throw vpException(vpException::fatalError, ss_msg.str());
1779 #endif
1780  } else if(extractorName == "BRIEF") {
1781 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1782  m_extractors[extractorName] = cv::xfeatures2d::BriefDescriptorExtractor::create();
1783 #else
1784  std::stringstream ss_msg;
1785  ss_msg << "Fail to initialize the extractor: BRIEF. OpenCV version "
1786  << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
1787  throw vpException(vpException::fatalError, ss_msg.str());
1788 #endif
1789  } else if(extractorName == "KAZE") {
1790  m_extractors[extractorName] = cv::KAZE::create();
1791  } else if(extractorName == "AKAZE") {
1792  m_extractors[extractorName] = cv::AKAZE::create();
1793  } else if(extractorName == "DAISY") {
1794 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1795  m_extractors[extractorName] = cv::xfeatures2d::DAISY::create();
1796 #else
1797  std::stringstream ss_msg;
1798  ss_msg << "Fail to initialize the extractor: DAISY. OpenCV version "
1799  << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
1800  throw vpException(vpException::fatalError, ss_msg.str());
1801 #endif
1802  } else if(extractorName == "LATCH") {
1803 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1804  m_extractors[extractorName] = cv::xfeatures2d::LATCH::create();
1805 #else
1806  std::stringstream ss_msg;
1807  ss_msg << "Fail to initialize the extractor: LATCH. OpenCV version "
1808  << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
1809  throw vpException(vpException::fatalError, ss_msg.str());
1810 #endif
1811  } else if(extractorName == "LUCID") {
1812 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1813  m_extractors[extractorName] = cv::xfeatures2d::LUCID::create(1, 2);
1814 #else
1815  std::stringstream ss_msg;
1816  ss_msg << "Fail to initialize the extractor: LUCID. OpenCV version "
1817  << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
1818  throw vpException(vpException::fatalError, ss_msg.str());
1819 #endif
1820  } else {
1821  std::cerr << "The extractor:" << extractorName << " is not available." << std::endl;
1822  }
1823 #endif
1824 
1825  if(m_extractors[extractorName] == NULL) {
1826  std::stringstream ss_msg;
1827  ss_msg << "Fail to initialize the extractor: " << extractorName << " or it is not available in OpenCV version: "
1828  << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
1829  throw vpException(vpException::fatalError, ss_msg.str());
1830  }
1831 
1832 #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
1833  if(extractorName == "SURF") {
1834  //Use extended set of SURF descriptors (128 instead of 64)
1835  m_extractors[extractorName]->set("extended", 1);
1836  }
1837 #endif
1838 }
1839 
1845 void vpKeyPoint::initExtractors(const std::vector<std::string> &extractorNames) {
1846  for(std::vector<std::string>::const_iterator it = extractorNames.begin(); it != extractorNames.end(); ++it) {
1847  initExtractor(*it);
1848  }
1849 
1850  int descriptorType = CV_32F;
1851  bool firstIteration = true;
1852  for(std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator it = m_extractors.begin();
1853  it != m_extractors.end(); ++it) {
1854  if(firstIteration) {
1855  firstIteration = false;
1856  descriptorType = it->second->descriptorType();
1857  } else {
1858  if(descriptorType != it->second->descriptorType()) {
1859  throw vpException(vpException::fatalError, "All the descriptors must have the same type !");
1860  }
1861  }
1862  }
1863 }
1864 
1870 void vpKeyPoint::initMatcher(const std::string &matcherName) {
1871  int descriptorType = CV_32F;
1872  bool firstIteration = true;
1873  for(std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator it = m_extractors.begin();
1874  it != m_extractors.end(); ++it) {
1875  if(firstIteration) {
1876  firstIteration = false;
1877  descriptorType = it->second->descriptorType();
1878  } else {
1879  if(descriptorType != it->second->descriptorType()) {
1880  throw vpException(vpException::fatalError, "All the descriptors must have the same type !");
1881  }
1882  }
1883  }
1884 
1885  if(matcherName == "FlannBased") {
1886  if(m_extractors.empty()) {
1887  std::cout << "Warning: No extractor initialized, by default use floating values (CV_32F) "
1888  "for descriptor type !" << std::endl;
1889  }
1890 
1891  if(descriptorType == CV_8U) {
1892 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
1893  m_matcher = cv::makePtr<cv::FlannBasedMatcher>(cv::makePtr<cv::flann::LshIndexParams>(12, 20, 2));
1894 #else
1895  m_matcher = new cv::FlannBasedMatcher(new cv::flann::LshIndexParams(12, 20, 2));
1896 #endif
1897  } else {
1898 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
1899  m_matcher = cv::makePtr<cv::FlannBasedMatcher>(cv::makePtr<cv::flann::KDTreeIndexParams>());
1900 #else
1901  m_matcher = new cv::FlannBasedMatcher(new cv::flann::KDTreeIndexParams());
1902 #endif
1903  }
1904  } else {
1905  m_matcher = cv::DescriptorMatcher::create(matcherName);
1906  }
1907 
1908 #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
1909  if(m_matcher != NULL && !m_useKnn && matcherName == "BruteForce") {
1910  m_matcher->set("crossCheck", m_useBruteForceCrossCheck);
1911  }
1912 #endif
1913 
1914  if(m_matcher == NULL) {
1915  std::stringstream ss_msg;
1916  ss_msg << "Fail to initialize the matcher: " << matcherName << " or it is not available in OpenCV version: "
1917  << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
1918  throw vpException(vpException::fatalError, ss_msg.str());
1919  }
1920 }
1921 
1931  vpImage<unsigned char> &IMatching) {
1932  vpImagePoint topLeftCorner(0, 0);
1933  IMatching.insert(IRef, topLeftCorner);
1934  topLeftCorner = vpImagePoint(0, IRef.getWidth());
1935  IMatching.insert(ICurrent, topLeftCorner);
1936 }
1937 
1946  //Nb images in the training database + the current image we want to detect the object
1947  int nbImg = (int) (m_mapOfImages.size() + 1);
1948 
1949  if(m_mapOfImages.empty()) {
1950  std::cerr << "There is no training image loaded !" << std::endl;
1951  return;
1952  }
1953 
1954  if(nbImg == 2) {
1955  //Only one training image, so we display them side by side
1956  insertImageMatching(m_mapOfImages.begin()->second, ICurrent, IMatching);
1957  } else {
1958  //Multiple training images, display them as a mosaic image
1959  int nbImgSqrt = vpMath::round(std::sqrt((double) nbImg)); //(int) std::floor(std::sqrt((double) nbImg) + 0.5);
1960  int nbWidth = nbImgSqrt;
1961  int nbHeight = nbImgSqrt;
1962 
1963  if(nbImgSqrt * nbImgSqrt < nbImg) {
1964  nbWidth++;
1965  }
1966 
1967  unsigned int maxW = ICurrent.getWidth(), maxH = ICurrent.getHeight();
1968  for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
1969  if(maxW < it->second.getWidth()) {
1970  maxW = it->second.getWidth();
1971  }
1972 
1973  if(maxH < it->second.getHeight()) {
1974  maxH = it->second.getHeight();
1975  }
1976  }
1977 
1978  //Indexes of the current image in the grid made in order to the image is in the center square in the mosaic grid
1979  int medianI = nbHeight / 2;
1980  int medianJ = nbWidth / 2;
1981  int medianIndex = medianI * nbWidth + medianJ;
1982 
1983  int cpt = 0;
1984  for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
1985  int local_cpt = cpt;
1986  if(cpt >= medianIndex) {
1987  //Shift of one unity the index of the training images which are after the current image
1988  local_cpt++;
1989  }
1990  int indexI = local_cpt / nbWidth;
1991  int indexJ = local_cpt - (indexI * nbWidth);
1992  vpImagePoint topLeftCorner((int)maxH*indexI, (int)maxW*indexJ);
1993 
1994  IMatching.insert(it->second, topLeftCorner);
1995  }
1996 
1997  vpImagePoint topLeftCorner((int)maxH*medianI, (int)maxW*medianJ);
1998  IMatching.insert(ICurrent, topLeftCorner);
1999  }
2000 }
2001 
2002 #ifdef VISP_HAVE_XML2
2003 
2008 void vpKeyPoint::loadConfigFile(const std::string &configFile) {
2010 
2011  try {
2012  //Reset detector and extractor
2013  m_detectorNames.clear();
2014  m_extractorNames.clear();
2015  m_detectors.clear();
2016  m_extractors.clear();
2017 
2018  std::cout << " *********** Parsing XML for configuration for vpKeyPoint ************ " << std::endl;
2019  xmlp.parse(configFile);
2020 
2021  m_detectorNames.push_back(xmlp.getDetectorName());
2022  m_extractorNames.push_back(xmlp.getExtractorName());
2023  m_matcherName = xmlp.getMatcherName();
2024 
2025  switch(xmlp.getMatchingMethod()) {
2027  m_filterType = constantFactorDistanceThreshold;
2028  break;
2029 
2031  m_filterType = stdDistanceThreshold;
2032  break;
2033 
2035  m_filterType = ratioDistanceThreshold;
2036  break;
2037 
2039  m_filterType = stdAndRatioDistanceThreshold;
2040  break;
2041 
2043  m_filterType = noFilterMatching;
2044  break;
2045 
2046  default:
2047  break;
2048  }
2049 
2050  m_matchingFactorThreshold = xmlp.getMatchingFactorThreshold();
2051  m_matchingRatioThreshold = xmlp.getMatchingRatioThreshold();
2052 
2053  m_useRansacVVS = xmlp.getUseRansacVVSPoseEstimation();
2054  m_useConsensusPercentage = xmlp.getUseRansacConsensusPercentage();
2055  m_nbRansacIterations = xmlp.getNbRansacIterations();
2056  m_ransacReprojectionError = xmlp.getRansacReprojectionError();
2057  m_nbRansacMinInlierCount = xmlp.getNbRansacMinInlierCount();
2058  m_ransacThreshold = xmlp.getRansacThreshold();
2059  m_ransacConsensusPercentage = xmlp.getRansacConsensusPercentage();
2060 
2061  if(m_filterType == ratioDistanceThreshold || m_filterType == stdAndRatioDistanceThreshold) {
2062  m_useKnn = true;
2063  } else {
2064  m_useKnn = false;
2065  }
2066 
2067  init();
2068  }
2069  catch(...) {
2070  throw vpException(vpException::ioError, "Can't open XML file \"%s\"\n ", configFile.c_str());
2071  }
2072 }
2073 #endif
2074 
2082 void vpKeyPoint::loadLearningData(const std::string &filename, const bool binaryMode, const bool append) {
2083  int startClassId = 0;
2084  int startImageId = 0;
2085  if(!append) {
2086  m_trainKeyPoints.clear();
2087  m_trainPoints.clear();
2088  m_mapOfImageId.clear();
2089  m_mapOfImages.clear();
2090  } else {
2091  //In append case, find the max index of keypoint class Id
2092  for(std::map<int, int>::const_iterator it = m_mapOfImageId.begin(); it != m_mapOfImageId.end(); ++it) {
2093  if(startClassId < it->first) {
2094  startClassId = it->first;
2095  }
2096  }
2097 
2098  //In append case, find the max index of images Id
2099  for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
2100  if(startImageId < it->first) {
2101  startImageId = it->first;
2102  }
2103  }
2104  }
2105 
2106  //Get parent directory
2107  std::string parent = vpIoTools::getParent(filename);
2108  if(!parent.empty()) {
2109  parent += "/";
2110  }
2111 
2112  if(binaryMode) {
2113  std::ifstream file(filename.c_str(), std::ifstream::binary);
2114  if(!file.is_open()){
2115  throw vpException(vpException::ioError, "Cannot open the file.");
2116  }
2117 
2118  //Read info about training images
2119  int nbImgs = 0;
2120  file.read((char *)(&nbImgs), sizeof(nbImgs));
2121 
2122 #if !defined(VISP_HAVE_MODULE_IO)
2123  if(nbImgs > 0) {
2124  std::cout << "Warning: The learning file contains image data that will not be loaded as visp_io module "
2125  "is not available !" << std::endl;
2126  }
2127 #endif
2128 
2129  for(int i = 0; i < nbImgs; i++) {
2130  //Read image_id
2131  int id = 0;
2132  file.read((char *)(&id), sizeof(id));
2133 
2134  int length = 0;
2135  file.read((char *)(&length), sizeof(length));
2136  //Will contain the path to the training images
2137  char* path = new char[length + 1];//char path[length + 1];
2138 
2139  for(int cpt = 0; cpt < length; cpt++) {
2140  char c;
2141  file.read((char *)(&c), sizeof(c));
2142  path[cpt] = c;
2143  }
2144  path[length] = '\0';
2145 
2147 #ifdef VISP_HAVE_MODULE_IO
2148  if(vpIoTools::isAbsolutePathname(std::string(path))) {
2149  vpImageIo::read(I, path);
2150  } else {
2151  vpImageIo::read(I, parent + path);
2152  }
2153 
2154  //Add the image previously loaded only if VISP_HAVE_MODULE_IO
2155  m_mapOfImages[id + startImageId] = I;
2156 #endif
2157 
2158  //Delete path
2159  delete[] path;
2160  }
2161 
2162  //Read if 3D point information are saved or not
2163  int have3DInfoInt = 0;
2164  file.read((char *)(&have3DInfoInt), sizeof(have3DInfoInt));
2165  bool have3DInfo = have3DInfoInt != 0;
2166 
2167  //Read the number of descriptors
2168  int nRows = 0;
2169  file.read((char *)(&nRows), sizeof(nRows));
2170 
2171  //Read the size of the descriptor
2172  int nCols = 0;
2173  file.read((char *)(&nCols), sizeof(nCols));
2174 
2175  //Read the type of the descriptor
2176  int descriptorType = 5; //CV_32F
2177  file.read((char *)(&descriptorType), sizeof(descriptorType));
2178 
2179  cv::Mat trainDescriptorsTmp = cv::Mat(nRows, nCols, descriptorType);
2180  for(int i = 0; i < nRows; i++) {
2181  //Read information about keyPoint
2182  float u, v, size, angle, response;
2183  int octave, class_id, image_id;
2184  file.read((char *)(&u), sizeof(u));
2185  file.read((char *)(&v), sizeof(v));
2186  file.read((char *)(&size), sizeof(size));
2187  file.read((char *)(&angle), sizeof(angle));
2188  file.read((char *)(&response), sizeof(response));
2189  file.read((char *)(&octave), sizeof(octave));
2190  file.read((char *)(&class_id), sizeof(class_id));
2191  file.read((char *)(&image_id), sizeof(image_id));
2192  cv::KeyPoint keyPoint(cv::Point2f(u, v), size, angle, response, octave, (class_id + startClassId));
2193  m_trainKeyPoints.push_back(keyPoint);
2194 
2195  if(image_id != -1) {
2196 #ifdef VISP_HAVE_MODULE_IO
2197  //No training images if image_id == -1
2198  m_mapOfImageId[class_id] = image_id + startImageId;
2199 #endif
2200  }
2201 
2202  if(have3DInfo) {
2203  //Read oX, oY, oZ
2204  float oX, oY, oZ;
2205  file.read((char *)(&oX), sizeof(oX));
2206  file.read((char *)(&oY), sizeof(oY));
2207  file.read((char *)(&oZ), sizeof(oZ));
2208  m_trainPoints.push_back(cv::Point3f(oX, oY, oZ));
2209  }
2210 
2211  for(int j = 0; j < nCols; j++) {
2212  //Read the value of the descriptor
2213  switch(descriptorType) {
2214  case CV_8U:
2215  {
2216  unsigned char value;
2217  file.read((char *)(&value), sizeof(value));
2218  trainDescriptorsTmp.at<unsigned char>(i, j) = value;
2219  }
2220  break;
2221 
2222  case CV_8S:
2223  {
2224  char value;
2225  file.read((char *)(&value), sizeof(value));
2226  trainDescriptorsTmp.at<char>(i, j) = value;
2227  }
2228  break;
2229 
2230  case CV_16U:
2231  {
2232  unsigned short int value;
2233  file.read((char *)(&value), sizeof(value));
2234  trainDescriptorsTmp.at<unsigned short int>(i, j) = value;
2235  }
2236  break;
2237 
2238  case CV_16S:
2239  {
2240  short int value;
2241  file.read((char *)(&value), sizeof(value));
2242  trainDescriptorsTmp.at<short int>(i, j) = value;
2243  }
2244  break;
2245 
2246  case CV_32S:
2247  {
2248  int value;
2249  file.read((char *)(&value), sizeof(value));
2250  trainDescriptorsTmp.at<int>(i, j) = value;
2251  }
2252  break;
2253 
2254  case CV_32F:
2255  {
2256  float value;
2257  file.read((char *)(&value), sizeof(value));
2258  trainDescriptorsTmp.at<float>(i, j) = value;
2259  }
2260  break;
2261 
2262  case CV_64F:
2263  {
2264  double value;
2265  file.read((char *)(&value), sizeof(value));
2266  trainDescriptorsTmp.at<double>(i, j) = value;
2267  }
2268  break;
2269 
2270  default:
2271  {
2272  float value;
2273  file.read((char *)(&value), sizeof(value));
2274  trainDescriptorsTmp.at<float>(i, j) = value;
2275  }
2276  break;
2277  }
2278  }
2279  }
2280 
2281  if(!append || m_trainDescriptors.empty()) {
2282  trainDescriptorsTmp.copyTo(m_trainDescriptors);
2283  } else {
2284  cv::vconcat(m_trainDescriptors, trainDescriptorsTmp, m_trainDescriptors);
2285  }
2286 
2287  file.close();
2288  } else {
2289 #ifdef VISP_HAVE_XML2
2290  xmlDocPtr doc = NULL;
2291  xmlNodePtr root_element = NULL;
2292 
2293  /*
2294  * this initialize the library and check potential ABI mismatches
2295  * between the version it was compiled for and the actual shared
2296  * library used.
2297  */
2298  LIBXML_TEST_VERSION
2299 
2300  /*parse the file and get the DOM */
2301  doc = xmlReadFile(filename.c_str(), NULL, 0);
2302 
2303  if (doc == NULL) {
2304  throw vpException(vpException::ioError, "Error with file " + filename);
2305  }
2306 
2307  root_element = xmlDocGetRootElement(doc);
2308 
2309  xmlNodePtr first_level_node = NULL;
2310  char *pEnd = NULL;
2311 
2312  int descriptorType = CV_32F; //float
2313  int nRows = 0, nCols = 0;
2314  int i = 0, j = 0;
2315 
2316  cv::Mat trainDescriptorsTmp;
2317 
2318  for (first_level_node = root_element->children; first_level_node;
2319  first_level_node = first_level_node->next) {
2320 
2321  std::string name((char *) first_level_node->name);
2322  if (first_level_node->type == XML_ELEMENT_NODE && name == "TrainingImageInfo") {
2323  xmlNodePtr image_info_node = NULL;
2324 
2325  for (image_info_node = first_level_node->children; image_info_node; image_info_node =
2326  image_info_node->next) {
2327  name = std::string ((char *) image_info_node->name);
2328 
2329  if(name == "trainImg") {
2330  //Read image_id
2331  xmlChar *image_id_property = xmlGetProp(image_info_node, BAD_CAST "image_id");
2332  int id = 0;
2333  if(image_id_property) {
2334  id = std::atoi((char *) image_id_property);
2335  }
2336  xmlFree(image_id_property);
2337 
2339 #ifdef VISP_HAVE_MODULE_IO
2340  std::string path((char *) image_info_node->children->content);
2341  //Read path to the training images
2342  if(vpIoTools::isAbsolutePathname(std::string(path))) {
2343  vpImageIo::read(I, path);
2344  } else {
2345  vpImageIo::read(I, parent + path);
2346  }
2347 
2348  //Add the image previously loaded only if VISP_HAVE_MODULE_IO
2349  m_mapOfImages[id + startImageId] = I;
2350 #endif
2351  }
2352  }
2353  } else if(first_level_node->type == XML_ELEMENT_NODE && name == "DescriptorsInfo") {
2354  xmlNodePtr descriptors_info_node = NULL;
2355  for (descriptors_info_node = first_level_node->children; descriptors_info_node; descriptors_info_node =
2356  descriptors_info_node->next) {
2357  if (descriptors_info_node->type == XML_ELEMENT_NODE) {
2358  name = std::string ((char *) descriptors_info_node->name);
2359 
2360  if(name == "nrows") {
2361  nRows = std::atoi((char *) descriptors_info_node->children->content);
2362  } else if(name == "ncols") {
2363  nCols = std::atoi((char *) descriptors_info_node->children->content);
2364  } else if(name == "type") {
2365  descriptorType = std::atoi((char *) descriptors_info_node->children->content);
2366  }
2367  }
2368  }
2369 
2370  trainDescriptorsTmp = cv::Mat(nRows, nCols, descriptorType);
2371  } else if (first_level_node->type == XML_ELEMENT_NODE && name == "DescriptorInfo") {
2372  xmlNodePtr point_node = NULL;
2373  double u = 0.0, v = 0.0, size = 0.0, angle = 0.0, response = 0.0;
2374  int octave = 0, class_id = 0, image_id = 0;
2375  double oX = 0.0, oY = 0.0, oZ = 0.0;
2376 
2377  std::stringstream ss;
2378 
2379  for (point_node = first_level_node->children; point_node; point_node =
2380  point_node->next) {
2381  if (point_node->type == XML_ELEMENT_NODE) {
2382  name = std::string ((char *) point_node->name);
2383 
2384  //Read information about keypoints
2385  if(name == "u") {
2386  u = std::strtod((char *) point_node->children->content, &pEnd);
2387  } else if(name == "v") {
2388  v = std::strtod((char *) point_node->children->content, &pEnd);
2389  } else if(name == "size") {
2390  size = std::strtod((char *) point_node->children->content, &pEnd);
2391  } else if(name == "angle") {
2392  angle = std::strtod((char *) point_node->children->content, &pEnd);
2393  } else if(name == "response") {
2394  response = std::strtod((char *) point_node->children->content, &pEnd);
2395  } else if(name == "octave") {
2396  octave = std::atoi((char *) point_node->children->content);
2397  } else if(name == "class_id") {
2398  class_id = std::atoi((char *) point_node->children->content);
2399  cv::KeyPoint keyPoint(cv::Point2f((float) u, (float) v), (float) size,
2400  (float) angle, (float) response, octave, (class_id + startClassId));
2401  m_trainKeyPoints.push_back(keyPoint);
2402  } else if(name == "image_id") {
2403  image_id = std::atoi((char *) point_node->children->content);
2404  if(image_id != -1) {
2405 #ifdef VISP_HAVE_MODULE_IO
2406  //No training images if image_id == -1
2407  m_mapOfImageId[m_trainKeyPoints.back().class_id] = image_id + startImageId;
2408 #endif
2409  }
2410  } else if (name == "oX") {
2411  oX = std::atof((char *) point_node->children->content);
2412  } else if (name == "oY") {
2413  oY = std::atof((char *) point_node->children->content);
2414  } else if (name == "oZ") {
2415  oZ = std::atof((char *) point_node->children->content);
2416  m_trainPoints.push_back(cv::Point3f((float) oX, (float) oY, (float) oZ));
2417  } else if (name == "desc") {
2418  xmlNodePtr descriptor_value_node = NULL;
2419  j = 0;
2420 
2421  for (descriptor_value_node = point_node->children;
2422  descriptor_value_node; descriptor_value_node =
2423  descriptor_value_node->next) {
2424 
2425  if (descriptor_value_node->type == XML_ELEMENT_NODE) {
2426  //Read descriptors values
2427  std::string parseStr((char *) descriptor_value_node->children->content);
2428  ss.clear();
2429  ss.str(parseStr);
2430 
2431  if(!ss.fail()) {
2432  switch(descriptorType) {
2433  case CV_8U:
2434  {
2435  //Parse the numeric value [0 ; 255] to an int
2436  int parseValue;
2437  ss >> parseValue;
2438  trainDescriptorsTmp.at<unsigned char>(i, j) = (unsigned char) parseValue;
2439  }
2440  break;
2441 
2442  case CV_8S:
2443  //Parse the numeric value [-128 ; 127] to an int
2444  int parseValue;
2445  ss >> parseValue;
2446  trainDescriptorsTmp.at<char>(i, j) = (char) parseValue;
2447  break;
2448 
2449  case CV_16U:
2450  ss >> trainDescriptorsTmp.at<unsigned short int>(i, j);
2451  break;
2452 
2453  case CV_16S:
2454  ss >> trainDescriptorsTmp.at<short int>(i, j);
2455  break;
2456 
2457  case CV_32S:
2458  ss >> trainDescriptorsTmp.at<int>(i, j);
2459  break;
2460 
2461  case CV_32F:
2462  ss >> trainDescriptorsTmp.at<float>(i, j);
2463  break;
2464 
2465  case CV_64F:
2466  ss >> trainDescriptorsTmp.at<double>(i, j);
2467  break;
2468 
2469  default:
2470  ss >> trainDescriptorsTmp.at<float>(i, j);
2471  break;
2472  }
2473  } else {
2474  std::cerr << "Error when converting:" << ss.str() << std::endl;
2475  }
2476 
2477  j++;
2478  }
2479  }
2480  }
2481  }
2482  }
2483  i++;
2484  }
2485  }
2486 
2487  if(!append || m_trainDescriptors.empty()) {
2488  trainDescriptorsTmp.copyTo(m_trainDescriptors);
2489  } else {
2490  cv::vconcat(m_trainDescriptors, trainDescriptorsTmp, m_trainDescriptors);
2491  }
2492 
2493  /*free the document */
2494  xmlFreeDoc(doc);
2495 
2496  /*
2497  *Free the global variables that may
2498  *have been allocated by the parser.
2499  */
2500  xmlCleanupParser();
2501 #else
2502  std::cout << "Error: libxml2 is required !" << std::endl;
2503 #endif
2504  }
2505 
2506  //Convert OpenCV type to ViSP type for compatibility
2508  vpConvert::convertFromOpenCV(this->m_trainPoints, m_trainVpPoints);
2509 
2510  //Add train descriptors in matcher object
2511  m_matcher->clear();
2512  m_matcher->add(std::vector<cv::Mat>(1, m_trainDescriptors));
2513 
2514  //Set _reference_computed to true as we load learning file
2515  _reference_computed = true;
2516 }
2517 
2526 void vpKeyPoint::match(const cv::Mat &trainDescriptors, const cv::Mat &queryDescriptors,
2527  std::vector<cv::DMatch> &matches, double &elapsedTime) {
2528  double t = vpTime::measureTimeMs();
2529 
2530  if(m_useKnn) {
2531  m_knnMatches.clear();
2532 
2533  if(m_useMatchTrainToQuery) {
2534  std::vector<std::vector<cv::DMatch> > knnMatchesTmp;
2535 
2536  //Match train descriptors to query descriptors
2537  cv::Ptr<cv::DescriptorMatcher> matcherTmp = m_matcher->clone(true);
2538  matcherTmp->knnMatch(trainDescriptors, queryDescriptors, knnMatchesTmp, 2);
2539 
2540  for(std::vector<std::vector<cv::DMatch> >::const_iterator it1 = knnMatchesTmp.begin(); it1 != knnMatchesTmp.end(); ++it1) {
2541  std::vector<cv::DMatch> tmp;
2542  for(std::vector<cv::DMatch>::const_iterator it2 = it1->begin(); it2 != it1->end(); ++it2) {
2543  tmp.push_back(cv::DMatch(it2->trainIdx, it2->queryIdx, it2->distance));
2544  }
2545  m_knnMatches.push_back(tmp);
2546  }
2547 
2548  matches.resize(m_knnMatches.size());
2549  std::transform(m_knnMatches.begin(), m_knnMatches.end(), matches.begin(), knnToDMatch);
2550  } else {
2551  //Match query descriptors to train descriptors
2552  m_matcher->knnMatch(queryDescriptors, m_knnMatches, 2);
2553  matches.resize(m_knnMatches.size());
2554  std::transform(m_knnMatches.begin(), m_knnMatches.end(), matches.begin(), knnToDMatch);
2555  }
2556  } else {
2557  matches.clear();
2558 
2559  if(m_useMatchTrainToQuery) {
2560  std::vector<cv::DMatch> matchesTmp;
2561  //Match train descriptors to query descriptors
2562  cv::Ptr<cv::DescriptorMatcher> matcherTmp = m_matcher->clone(true);
2563  matcherTmp->match(trainDescriptors, queryDescriptors, matchesTmp);
2564 
2565  for(std::vector<cv::DMatch>::const_iterator it = matchesTmp.begin(); it != matchesTmp.end(); ++it) {
2566  matches.push_back(cv::DMatch(it->trainIdx, it->queryIdx, it->distance));
2567  }
2568  } else {
2569  //Match query descriptors to train descriptors
2570  m_matcher->match(queryDescriptors, matches);
2571  }
2572  }
2573  elapsedTime = vpTime::measureTimeMs() - t;
2574 }
2575 
2583  return matchPoint(I, vpRect());
2584 }
2585 
2597  const vpImagePoint &iP,
2598  const unsigned int height, const unsigned int width) {
2599  return matchPoint(I, vpRect(iP, width, height));
2600 }
2601 
2611  const vpRect& rectangle) {
2612  if(m_trainDescriptors.empty()) {
2613  std::cerr << "Reference is empty." << std::endl;
2614  if(!_reference_computed) {
2615  std::cerr << "Reference is not computed." << std::endl;
2616  }
2617  std::cerr << "Matching is not possible." << std::endl;
2618 
2619  return 0;
2620  }
2621 
2622  if(m_useAffineDetection) {
2623  std::vector<std::vector<cv::KeyPoint> > listOfQueryKeyPoints;
2624  std::vector<cv::Mat> listOfQueryDescriptors;
2625 
2626  //Detect keypoints and extract descriptors on multiple images
2627  detectExtractAffine(I, listOfQueryKeyPoints, listOfQueryDescriptors);
2628 
2629  //Flatten the different train lists
2630  m_queryKeyPoints.clear();
2631  for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfQueryKeyPoints.begin();
2632  it != listOfQueryKeyPoints.end(); ++it) {
2633  m_queryKeyPoints.insert(m_queryKeyPoints.end(), it->begin(), it->end());
2634  }
2635 
2636  bool first = true;
2637  for(std::vector<cv::Mat>::const_iterator it = listOfQueryDescriptors.begin(); it != listOfQueryDescriptors.end(); ++it) {
2638  if(first) {
2639  first = false;
2640  it->copyTo(m_queryDescriptors);
2641  } else {
2642  m_queryDescriptors.push_back(*it);
2643  }
2644  }
2645  } else {
2646  detect(I, m_queryKeyPoints, m_detectionTime, rectangle);
2647  extract(I, m_queryKeyPoints, m_queryDescriptors, m_extractionTime);
2648  }
2649 
2650  match(m_trainDescriptors, m_queryDescriptors, m_matches, m_matchingTime);
2651 
2652  if(m_filterType != noFilterMatching) {
2653  m_queryFilteredKeyPoints.clear();
2654  m_objectFilteredPoints.clear();
2655  m_filteredMatches.clear();
2656 
2657  filterMatches();
2658  } else {
2659  if(m_useMatchTrainToQuery) {
2660  //Add only query keypoints matched with a train keypoints
2661  m_queryFilteredKeyPoints.clear();
2662  m_filteredMatches.clear();
2663  for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
2664  m_filteredMatches.push_back(cv::DMatch((int) m_queryFilteredKeyPoints.size(), it->trainIdx, it->distance));
2665  m_queryFilteredKeyPoints.push_back(m_queryKeyPoints[(size_t) it->queryIdx]);
2666  }
2667  } else {
2668  m_queryFilteredKeyPoints = m_queryKeyPoints;
2669  m_filteredMatches = m_matches;
2670  }
2671 
2672  if(!m_trainPoints.empty()) {
2673  m_objectFilteredPoints.clear();
2674  //Add 3D object points such as the same index in m_queryFilteredKeyPoints and in m_objectFilteredPoints
2675  // matches to the same train object
2676  for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
2677  //m_matches is normally ordered following the queryDescriptor index
2678  m_objectFilteredPoints.push_back(m_trainPoints[(size_t) it->trainIdx]);
2679  }
2680  }
2681  }
2682 
2683  //Convert OpenCV type to ViSP type for compatibility
2684  vpConvert::convertFromOpenCV(m_queryFilteredKeyPoints, currentImagePointsList);
2686 
2687  return static_cast<unsigned int>(m_filteredMatches.size());
2688 }
2689 
2705  double &error, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *), const vpRect& rectangle) {
2706  //Check if we have training descriptors
2707  if(m_trainDescriptors.empty()) {
2708  std::cerr << "Reference is empty." << std::endl;
2709  if(!_reference_computed) {
2710  std::cerr << "Reference is not computed." << std::endl;
2711  }
2712  std::cerr << "Matching is not possible." << std::endl;
2713 
2714  return false;
2715  }
2716 
2717  if(m_useAffineDetection) {
2718  std::vector<std::vector<cv::KeyPoint> > listOfQueryKeyPoints;
2719  std::vector<cv::Mat> listOfQueryDescriptors;
2720 
2721  //Detect keypoints and extract descriptors on multiple images
2722  detectExtractAffine(I, listOfQueryKeyPoints, listOfQueryDescriptors);
2723 
2724  //Flatten the different train lists
2725  m_queryKeyPoints.clear();
2726  for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfQueryKeyPoints.begin();
2727  it != listOfQueryKeyPoints.end(); ++it) {
2728  m_queryKeyPoints.insert(m_queryKeyPoints.end(), it->begin(), it->end());
2729  }
2730 
2731  bool first = true;
2732  for(std::vector<cv::Mat>::const_iterator it = listOfQueryDescriptors.begin(); it != listOfQueryDescriptors.end(); ++it) {
2733  if(first) {
2734  first = false;
2735  it->copyTo(m_queryDescriptors);
2736  } else {
2737  m_queryDescriptors.push_back(*it);
2738  }
2739  }
2740  } else {
2741  detect(I, m_queryKeyPoints, m_detectionTime, rectangle);
2742  extract(I, m_queryKeyPoints, m_queryDescriptors, m_extractionTime);
2743  }
2744 
2745  match(m_trainDescriptors, m_queryDescriptors, m_matches, m_matchingTime);
2746 
2747  elapsedTime = m_detectionTime + m_extractionTime + m_matchingTime;
2748 
2749  if(m_filterType != noFilterMatching) {
2750  m_queryFilteredKeyPoints.clear();
2751  m_objectFilteredPoints.clear();
2752  m_filteredMatches.clear();
2753 
2754  filterMatches();
2755  } else {
2756  if(m_useMatchTrainToQuery) {
2757  //Add only query keypoints matched with a train keypoints
2758  m_queryFilteredKeyPoints.clear();
2759  m_filteredMatches.clear();
2760  for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
2761  m_filteredMatches.push_back(cv::DMatch((int) m_queryFilteredKeyPoints.size(), it->trainIdx, it->distance));
2762  m_queryFilteredKeyPoints.push_back(m_queryKeyPoints[(size_t) it->queryIdx]);
2763  }
2764  } else {
2765  m_queryFilteredKeyPoints = m_queryKeyPoints;
2766  m_filteredMatches = m_matches;
2767  }
2768 
2769  if(!m_trainPoints.empty()) {
2770  m_objectFilteredPoints.clear();
2771  //Add 3D object points such as the same index in m_queryFilteredKeyPoints and in m_objectFilteredPoints
2772  // matches to the same train object
2773  for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
2774  //m_matches is normally ordered following the queryDescriptor index
2775  m_objectFilteredPoints.push_back(m_trainPoints[(size_t) it->trainIdx]);
2776  }
2777  }
2778  }
2779 
2780  //Convert OpenCV type to ViSP type for compatibility
2781  vpConvert::convertFromOpenCV(m_queryFilteredKeyPoints, currentImagePointsList);
2783 
2784  //error = std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
2785  error = DBL_MAX;
2786  m_ransacInliers.clear();
2787  m_ransacOutliers.clear();
2788 
2789  if(m_useRansacVVS) {
2790  std::vector<vpPoint> objectVpPoints(m_objectFilteredPoints.size());
2791  size_t cpt = 0;
2792  //Create a list of vpPoint with 2D coordinates (current keypoint location) + 3D coordinates (world/object coordinates)
2793  for(std::vector<cv::Point3f>::const_iterator it = m_objectFilteredPoints.begin(); it != m_objectFilteredPoints.end();
2794  ++it, cpt++) {
2795  vpPoint pt;
2796  pt.setWorldCoordinates(it->x, it->y, it->z);
2797 
2798  vpImagePoint imP(m_queryFilteredKeyPoints[cpt].pt.y, m_queryFilteredKeyPoints[cpt].pt.x);
2799 
2800  double x = 0.0, y = 0.0;
2801  vpPixelMeterConversion::convertPoint(cam, imP, x, y);
2802  pt.set_x(x);
2803  pt.set_y(y);
2804 
2805  objectVpPoints[cpt] = pt;
2806  }
2807 
2808  std::vector<vpPoint> inliers;
2809  std::vector<unsigned int> inlierIndex;
2810 
2811  bool res = computePose(objectVpPoints, cMo, inliers, inlierIndex, m_poseTime, func);
2812 
2813  std::map<unsigned int, bool> mapOfInlierIndex;
2814  m_matchRansacKeyPointsToPoints.clear();
2815 
2816  for (std::vector<unsigned int>::const_iterator it = inlierIndex.begin(); it != inlierIndex.end(); ++it) {
2817  m_matchRansacKeyPointsToPoints.push_back(std::pair<cv::KeyPoint, cv::Point3f>(m_queryFilteredKeyPoints[(size_t)(*it)],
2818  m_objectFilteredPoints[(size_t)(*it)]));
2819  mapOfInlierIndex[*it] = true;
2820  }
2821 
2822  for(size_t i = 0; i < m_queryFilteredKeyPoints.size(); i++) {
2823  if(mapOfInlierIndex.find((unsigned int) i) == mapOfInlierIndex.end()) {
2824  m_ransacOutliers.push_back(vpImagePoint(m_queryFilteredKeyPoints[i].pt.y, m_queryFilteredKeyPoints[i].pt.x));
2825  }
2826  }
2827 
2828  error = computePoseEstimationError(m_matchRansacKeyPointsToPoints, cam, cMo);
2829 
2830  m_ransacInliers.resize(m_matchRansacKeyPointsToPoints.size());
2831  std::transform(m_matchRansacKeyPointsToPoints.begin(), m_matchRansacKeyPointsToPoints.end(), m_ransacInliers.begin(),
2832  matchRansacToVpImage);
2833 
2834  elapsedTime += m_poseTime;
2835 
2836  return res;
2837  } else {
2838  std::vector<cv::Point2f> imageFilteredPoints;
2839  cv::KeyPoint::convert(m_queryFilteredKeyPoints, imageFilteredPoints);
2840  std::vector<int> inlierIndex;
2841  bool res = computePose(imageFilteredPoints, m_objectFilteredPoints, cam, cMo, inlierIndex, m_poseTime);
2842 
2843  std::map<int, bool> mapOfInlierIndex;
2844  m_matchRansacKeyPointsToPoints.clear();
2845 
2846  for (std::vector<int>::const_iterator it = inlierIndex.begin(); it != inlierIndex.end(); ++it) {
2847  m_matchRansacKeyPointsToPoints.push_back(std::pair<cv::KeyPoint, cv::Point3f>(m_queryFilteredKeyPoints[(size_t)(*it)],
2848  m_objectFilteredPoints[(size_t)(*it)]));
2849  mapOfInlierIndex[*it] = true;
2850  }
2851 
2852  for(size_t i = 0; i < m_queryFilteredKeyPoints.size(); i++) {
2853  if(mapOfInlierIndex.find((int) i) == mapOfInlierIndex.end()) {
2854  m_ransacOutliers.push_back(vpImagePoint(m_queryFilteredKeyPoints[i].pt.y, m_queryFilteredKeyPoints[i].pt.x));
2855  }
2856  }
2857 
2858  error = computePoseEstimationError(m_matchRansacKeyPointsToPoints, cam, cMo);
2859 
2860  m_ransacInliers.resize(m_matchRansacKeyPointsToPoints.size());
2861  std::transform(m_matchRansacKeyPointsToPoints.begin(), m_matchRansacKeyPointsToPoints.end(), m_ransacInliers.begin(),
2862  matchRansacToVpImage);
2863 
2864  elapsedTime += m_poseTime;
2865 
2866  return res;
2867  }
2868 }
2869 
2887 bool vpKeyPoint::matchPointAndDetect(const vpImage<unsigned char> &I, vpRect &boundingBox, vpImagePoint &centerOfGravity,
2888  const bool isPlanarObject, std::vector<vpImagePoint> *imPts1, std::vector<vpImagePoint> *imPts2,
2889  double *meanDescriptorDistance, double *detection_score, const vpRect& rectangle) {
2890  if(imPts1 != NULL && imPts2 != NULL) {
2891  imPts1->clear();
2892  imPts2->clear();
2893  }
2894 
2895  matchPoint(I, rectangle);
2896 
2897  double meanDescriptorDistanceTmp = 0.0;
2898  for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
2899  meanDescriptorDistanceTmp += (double) it->distance;
2900  }
2901 
2902  meanDescriptorDistanceTmp /= (double) m_filteredMatches.size();
2903  double score = (double) m_filteredMatches.size() / meanDescriptorDistanceTmp;
2904 
2905  if(meanDescriptorDistance != NULL) {
2906  *meanDescriptorDistance = meanDescriptorDistanceTmp;
2907  }
2908  if(detection_score != NULL) {
2909  *detection_score = score;
2910  }
2911 
2912  if(m_filteredMatches.size() >= 4) {
2913  //Training / Reference 2D points
2914  std::vector<cv::Point2f> points1(m_filteredMatches.size());
2915  //Query / Current 2D points
2916  std::vector<cv::Point2f> points2(m_filteredMatches.size());
2917 
2918  for(size_t i = 0; i < m_filteredMatches.size(); i++) {
2919  points1[i] = cv::Point2f(m_trainKeyPoints[(size_t)m_filteredMatches[i].trainIdx].pt);
2920  points2[i] = cv::Point2f(m_queryFilteredKeyPoints[(size_t)m_filteredMatches[i].queryIdx].pt);
2921  }
2922 
2923  std::vector<vpImagePoint> inliers;
2924  if(isPlanarObject) {
2925 #if (VISP_HAVE_OPENCV_VERSION < 0x030000)
2926  cv::Mat homographyMatrix = cv::findHomography(points1, points2, CV_RANSAC);
2927 #else
2928  cv::Mat homographyMatrix = cv::findHomography(points1, points2, cv::RANSAC);
2929 #endif
2930 
2931  for(size_t i = 0; i < m_filteredMatches.size(); i++ ) {
2932  //Compute reprojection error
2933  cv::Mat realPoint = cv::Mat(3, 1, CV_64F);
2934  realPoint.at<double>(0,0) = points1[i].x;
2935  realPoint.at<double>(1,0) = points1[i].y;
2936  realPoint.at<double>(2,0) = 1.f;
2937 
2938  cv::Mat reprojectedPoint = homographyMatrix * realPoint;
2939  double err_x = (reprojectedPoint.at<double>(0,0) / reprojectedPoint.at<double>(2,0)) - points2[i].x;
2940  double err_y = (reprojectedPoint.at<double>(1,0) / reprojectedPoint.at<double>(2,0)) - points2[i].y;
2941  double reprojectionError = std::sqrt(err_x*err_x + err_y*err_y);
2942 
2943  if(reprojectionError < 6.0) {
2944  inliers.push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
2945  if(imPts1 != NULL) {
2946  imPts1->push_back(vpImagePoint((double) points1[i].y, (double) points1[i].x));
2947  }
2948 
2949  if(imPts2 != NULL) {
2950  imPts2->push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
2951  }
2952  }
2953  }
2954  } else if(m_filteredMatches.size() >= 8) {
2955  cv::Mat fundamentalInliers;
2956  cv::Mat fundamentalMatrix = cv::findFundamentalMat(points1, points2, cv::FM_RANSAC, 3, 0.99, fundamentalInliers);
2957 
2958  for(size_t i = 0; i < (size_t) fundamentalInliers.rows; i++) {
2959  if(fundamentalInliers.at<uchar>((int) i, 0)) {
2960  inliers.push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
2961 
2962  if(imPts1 != NULL) {
2963  imPts1->push_back(vpImagePoint((double) points1[i].y, (double) points1[i].x));
2964  }
2965 
2966  if(imPts2 != NULL) {
2967  imPts2->push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
2968  }
2969  }
2970  }
2971  }
2972 
2973  if(!inliers.empty()) {
2974  //Build a polygon with the list of inlier keypoints detected in the current image to get the bounding box
2975  vpPolygon polygon(inliers);
2976  boundingBox = polygon.getBoundingBox();
2977 
2978  //Compute the center of gravity
2979  double meanU = 0.0, meanV = 0.0;
2980  for(std::vector<vpImagePoint>::const_iterator it = inliers.begin(); it != inliers.end(); ++it) {
2981  meanU += it->get_u();
2982  meanV += it->get_v();
2983  }
2984 
2985  meanU /= (double) inliers.size();
2986  meanV /= (double) inliers.size();
2987 
2988  centerOfGravity.set_u(meanU);
2989  centerOfGravity.set_v(meanV);
2990  }
2991  } else {
2992  //Too few matches
2993  return false;
2994  }
2995 
2996  if(m_detectionMethod == detectionThreshold) {
2997  return meanDescriptorDistanceTmp < m_detectionThreshold;
2998  } else {
2999  return score > m_detectionScore;
3000  }
3001 }
3002 
3022  double &error, double &elapsedTime, vpRect &boundingBox, vpImagePoint &centerOfGravity,
3023  bool (*func)(vpHomogeneousMatrix *), const vpRect& rectangle) {
3024  bool isMatchOk = matchPoint(I, cam, cMo, error, elapsedTime, func, rectangle);
3025  if(isMatchOk) {
3026  //Use the pose estimated to project the model points in the image
3027  vpPoint pt;
3028  vpImagePoint imPt;
3029  std::vector<vpImagePoint> modelImagePoints(m_trainVpPoints.size());
3030  size_t cpt = 0;
3031  for(std::vector<vpPoint>::const_iterator it = m_trainVpPoints.begin(); it != m_trainVpPoints.end(); ++it, cpt++) {
3032  pt = *it;
3033  pt.project(cMo);
3034  vpMeterPixelConversion::convertPoint(cam, pt.get_x(), pt.get_y(), imPt);
3035  modelImagePoints[cpt] = imPt;
3036  }
3037 
3038  //Build a polygon with the list of model image points to get the bounding box
3039  vpPolygon polygon(modelImagePoints);
3040  boundingBox = polygon.getBoundingBox();
3041 
3042  //Compute the center of gravity of the current inlier keypoints
3043  double meanU = 0.0, meanV = 0.0;
3044  for(std::vector<vpImagePoint>::const_iterator it = m_ransacInliers.begin(); it != m_ransacInliers.end();
3045  ++it) {
3046  meanU += it->get_u();
3047  meanV += it->get_v();
3048  }
3049 
3050  meanU /= (double) m_ransacInliers.size();
3051  meanV /= (double) m_ransacInliers.size();
3052 
3053  centerOfGravity.set_u(meanU);
3054  centerOfGravity.set_v(meanV);
3055  }
3056 
3057  return isMatchOk;
3058 }
3059 
3071 void vpKeyPoint::detectExtractAffine(const vpImage<unsigned char> &I,std::vector<std::vector<cv::KeyPoint> >& listOfKeypoints,
3072  std::vector<cv::Mat>& listOfDescriptors, std::vector<vpImage<unsigned char> > *listOfAffineI) {
3073 #if 0
3074  cv::Mat img;
3075  vpImageConvert::convert(I, img);
3076  listOfKeypoints.clear();
3077  listOfDescriptors.clear();
3078 
3079  for (int tl = 1; tl < 6; tl++) {
3080  double t = pow(2, 0.5 * tl);
3081  for (int phi = 0; phi < 180; phi += (int)(72.0 / t)) {
3082  std::vector<cv::KeyPoint> keypoints;
3083  cv::Mat descriptors;
3084 
3085  cv::Mat timg, mask, Ai;
3086  img.copyTo(timg);
3087 
3088  affineSkew(t, phi, timg, mask, Ai);
3089 
3090 
3091  if(listOfAffineI != NULL) {
3092  cv::Mat img_disp;
3093  bitwise_and(mask, timg, img_disp);
3095  vpImageConvert::convert(img_disp, tI);
3096  listOfAffineI->push_back(tI);
3097  }
3098 #if 0
3099  cv::Mat img_disp;
3100  cv::bitwise_and(mask, timg, img_disp);
3101  cv::namedWindow( "Skew", cv::WINDOW_AUTOSIZE ); // Create a window for display.
3102  cv::imshow( "Skew", img_disp );
3103  cv::waitKey(0);
3104 #endif
3105 
3106  for(std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin();
3107  it != m_detectors.end(); ++it) {
3108  std::vector<cv::KeyPoint> kp;
3109  it->second->detect(timg, kp, mask);
3110  keypoints.insert(keypoints.end(), kp.begin(), kp.end());
3111  }
3112 
3113  double elapsedTime;
3114  extract(timg, keypoints, descriptors, elapsedTime);
3115 
3116  for(unsigned int i = 0; i < keypoints.size(); i++) {
3117  cv::Point3f kpt(keypoints[i].pt.x, keypoints[i].pt.y, 1.f);
3118  cv::Mat kpt_t = Ai * cv::Mat(kpt);
3119  keypoints[i].pt.x = kpt_t.at<float>(0, 0);
3120  keypoints[i].pt.y = kpt_t.at<float>(1, 0);
3121  }
3122 
3123  listOfKeypoints.push_back(keypoints);
3124  listOfDescriptors.push_back(descriptors);
3125  }
3126  }
3127 
3128 #else
3129  cv::Mat img;
3130  vpImageConvert::convert(I, img);
3131 
3132  //Create a vector for storing the affine skew parameters
3133  std::vector<std::pair<double, int> > listOfAffineParams;
3134  for (int tl = 1; tl < 6; tl++) {
3135  double t = pow(2, 0.5 * tl);
3136  for (int phi = 0; phi < 180; phi += (int)(72.0 / t)) {
3137  listOfAffineParams.push_back(std::pair<double, int>(t, phi));
3138  }
3139  }
3140 
3141  listOfKeypoints.resize(listOfAffineParams.size());
3142  listOfDescriptors.resize(listOfAffineParams.size());
3143 
3144  if(listOfAffineI != NULL) {
3145  listOfAffineI->resize(listOfAffineParams.size());
3146  }
3147 
3148 #ifdef VISP_HAVE_OPENMP
3149  #pragma omp parallel for
3150 #endif
3151  for(int cpt = 0; cpt < static_cast<int>(listOfAffineParams.size()); cpt++) {
3152  std::vector<cv::KeyPoint> keypoints;
3153  cv::Mat descriptors;
3154 
3155  cv::Mat timg, mask, Ai;
3156  img.copyTo(timg);
3157 
3158  affineSkew(listOfAffineParams[(size_t) cpt].first, listOfAffineParams[(size_t) cpt].second, timg, mask, Ai);
3159 
3160 
3161  if(listOfAffineI != NULL) {
3162  cv::Mat img_disp;
3163  bitwise_and(mask, timg, img_disp);
3165  vpImageConvert::convert(img_disp, tI);
3166  (*listOfAffineI)[(size_t) cpt] = tI;
3167  }
3168 
3169 #if 0
3170  cv::Mat img_disp;
3171  cv::bitwise_and(mask, timg, img_disp);
3172  cv::namedWindow( "Skew", cv::WINDOW_AUTOSIZE ); // Create a window for display.
3173  cv::imshow( "Skew", img_disp );
3174  cv::waitKey(0);
3175 #endif
3176 
3177  for(std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin();
3178  it != m_detectors.end(); ++it) {
3179  std::vector<cv::KeyPoint> kp;
3180  it->second->detect(timg, kp, mask);
3181  keypoints.insert(keypoints.end(), kp.begin(), kp.end());
3182  }
3183 
3184  double elapsedTime;
3185  extract(timg, keypoints, descriptors, elapsedTime);
3186 
3187  for(size_t i = 0; i < keypoints.size(); i++) {
3188  cv::Point3f kpt(keypoints[i].pt.x, keypoints[i].pt.y, 1.f);
3189  cv::Mat kpt_t = Ai * cv::Mat(kpt);
3190  keypoints[i].pt.x = kpt_t.at<float>(0, 0);
3191  keypoints[i].pt.y = kpt_t.at<float>(1, 0);
3192  }
3193 
3194  listOfKeypoints[(size_t) cpt] = keypoints;
3195  listOfDescriptors[(size_t) cpt] = descriptors;
3196  }
3197 #endif
3198 }
3199 
3204  //vpBasicKeyPoint class
3206 
3207 
3208  m_computeCovariance = false; m_covarianceMatrix = vpMatrix(); m_currentImageId = 0; m_detectionMethod = detectionScore;
3209  m_detectionScore = 0.15; m_detectionThreshold = 100.0; m_detectionTime = 0.0; m_detectorNames.clear();
3210  m_detectors.clear(); m_extractionTime = 0.0; m_extractorNames.clear(); m_extractors.clear(); m_filteredMatches.clear();
3211  m_filterType = ratioDistanceThreshold;
3212  m_imageFormat = jpgImageFormat; m_knnMatches.clear(); m_mapOfImageId.clear(); m_mapOfImages.clear();
3213  m_matcher = cv::Ptr<cv::DescriptorMatcher>(); m_matcherName = "BruteForce-Hamming";
3214  m_matches.clear(); m_matchingFactorThreshold = 2.0; m_matchingRatioThreshold = 0.85; m_matchingTime = 0.0;
3215  m_matchRansacKeyPointsToPoints.clear(); m_nbRansacIterations = 200; m_nbRansacMinInlierCount = 100;
3216  m_objectFilteredPoints.clear();
3217  m_poseTime = 0.0; m_queryDescriptors = cv::Mat(); m_queryFilteredKeyPoints.clear(); m_queryKeyPoints.clear();
3218  m_ransacConsensusPercentage = 20.0; m_ransacInliers.clear(); m_ransacOutliers.clear(); m_ransacReprojectionError = 6.0;
3219  m_ransacThreshold = 0.01; m_trainDescriptors = cv::Mat(); m_trainKeyPoints.clear(); m_trainPoints.clear();
3220  m_trainVpPoints.clear(); m_useAffineDetection = false;
3221 #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
3222  m_useBruteForceCrossCheck = true;
3223 #endif
3224  m_useConsensusPercentage = false;
3225  m_useKnn = true; //as m_filterType == ratioDistanceThreshold
3226  m_useMatchTrainToQuery = false; m_useRansacVVS = true; m_useSingleMatchFilter = true;
3227 
3228  m_detectorNames.push_back("ORB");
3229  m_extractorNames.push_back("ORB");
3230 
3231  init();
3232 }
3233 
3241 void vpKeyPoint::saveLearningData(const std::string &filename, bool binaryMode, const bool saveTrainingImages) {
3242  std::string parent = vpIoTools::getParent(filename);
3243  if(!parent.empty()) {
3244  vpIoTools::makeDirectory(parent);
3245  }
3246 
3247  std::map<int, std::string> mapOfImgPath;
3248  if(saveTrainingImages) {
3249 #ifdef VISP_HAVE_MODULE_IO
3250  //Save the training image files in the same directory
3251  int cpt = 0;
3252 
3253  for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
3254  if(cpt > 999) {
3255  throw vpException(vpException::fatalError, "The number of training images to save is too big !");
3256  }
3257 
3258  char buffer[4];
3259  sprintf(buffer, "%03d", cpt);
3260  std::stringstream ss;
3261  ss << "train_image_" << buffer;
3262 
3263  switch(m_imageFormat) {
3264  case jpgImageFormat:
3265  ss << ".jpg";
3266  break;
3267 
3268  case pngImageFormat:
3269  ss << ".png";
3270  break;
3271 
3272  case ppmImageFormat:
3273  ss << ".ppm";
3274  break;
3275 
3276  case pgmImageFormat:
3277  ss << ".pgm";
3278  break;
3279 
3280  default:
3281  ss << ".png";
3282  break;
3283  }
3284 
3285  std::string imgFilename = ss.str();
3286  mapOfImgPath[it->first] = imgFilename;
3287  vpImageIo::write(it->second, parent + (!parent.empty() ? "/" : "") + imgFilename);
3288  }
3289 #else
3290  std::cout << "Warning: in vpKeyPoint::saveLearningData() training images are not saved because "
3291  "visp_io module is not available !" << std::endl;
3292 #endif
3293  }
3294 
3295  bool have3DInfo = m_trainPoints.size() > 0;
3296  if(have3DInfo && m_trainPoints.size() != m_trainKeyPoints.size()) {
3297  throw vpException(vpException::fatalError, "List of keypoints and list of 3D points have different size !");
3298  }
3299 
3300  if(binaryMode) {
3301  //Save the learning data into little endian binary file.
3302  std::ofstream file(filename.c_str(), std::ofstream::binary);
3303  if(!file.is_open()) {
3304  throw vpException(vpException::ioError, "Cannot create the file.");
3305  }
3306 
3307  //Write info about training images
3308  int nbImgs = (int) mapOfImgPath.size();
3309 // file.write((char *)(&nbImgs), sizeof(nbImgs));
3310  writeBinaryIntLE(file, nbImgs);
3311 
3312 #ifdef VISP_HAVE_MODULE_IO
3313  for(std::map<int, std::string>::const_iterator it = mapOfImgPath.begin(); it != mapOfImgPath.end(); ++it) {
3314  //Write image_id
3315  int id = it->first;
3316 // file.write((char *)(&id), sizeof(id));
3317  writeBinaryIntLE(file, id);
3318 
3319  //Write image path
3320  std::string path = it->second;
3321  int length = (int) path.length();
3322 // file.write((char *)(&length), sizeof(length));
3323  writeBinaryIntLE(file, length);
3324 
3325  for(int cpt = 0; cpt < length; cpt++) {
3326  file.write((char *) (&path[(size_t)cpt]), sizeof(path[(size_t)cpt]));
3327  }
3328  }
3329 #endif
3330 
3331  //Write if we have 3D point information
3332  int have3DInfoInt = have3DInfo ? 1 : 0;
3333 // file.write((char *)(&have3DInfoInt), sizeof(have3DInfoInt));
3334  writeBinaryIntLE(file, have3DInfoInt);
3335 
3336 
3337  int nRows = m_trainDescriptors.rows,
3338  nCols = m_trainDescriptors.cols;
3339  int descriptorType = m_trainDescriptors.type();
3340 
3341  //Write the number of descriptors
3342 // file.write((char *)(&nRows), sizeof(nRows));
3343  writeBinaryIntLE(file, nRows);
3344 
3345  //Write the size of the descriptor
3346 // file.write((char *)(&nCols), sizeof(nCols));
3347  writeBinaryIntLE(file, nCols);
3348 
3349  //Write the type of the descriptor
3350 // file.write((char *)(&descriptorType), sizeof(descriptorType));
3351  writeBinaryIntLE(file, descriptorType);
3352 
3353  for (int i = 0; i < nRows; i++) {
3354  unsigned int i_ = (unsigned int) i;
3355  //Write u
3356  float u = m_trainKeyPoints[i_].pt.x;
3357 // file.write((char *)(&u), sizeof(u));
3358  writeBinaryFloatLE(file, u);
3359 
3360  //Write v
3361  float v = m_trainKeyPoints[i_].pt.y;
3362 // file.write((char *)(&v), sizeof(v));
3363  writeBinaryFloatLE(file, v);
3364 
3365  //Write size
3366  float size = m_trainKeyPoints[i_].size;
3367 // file.write((char *)(&size), sizeof(size));
3368  writeBinaryFloatLE(file, size);
3369 
3370  //Write angle
3371  float angle = m_trainKeyPoints[i_].angle;
3372 // file.write((char *)(&angle), sizeof(angle));
3373  writeBinaryFloatLE(file, angle);
3374 
3375  //Write response
3376  float response = m_trainKeyPoints[i_].response;
3377 // file.write((char *)(&response), sizeof(response));
3378  writeBinaryFloatLE(file, response);
3379 
3380  //Write octave
3381  int octave = m_trainKeyPoints[i_].octave;
3382 // file.write((char *)(&octave), sizeof(octave));
3383  writeBinaryIntLE(file, octave);
3384 
3385  //Write class_id
3386  int class_id = m_trainKeyPoints[i_].class_id;
3387 // file.write((char *)(&class_id), sizeof(class_id));
3388  writeBinaryIntLE(file, class_id);
3389 
3390  //Write image_id
3391 #ifdef VISP_HAVE_MODULE_IO
3392  std::map<int, int>::const_iterator it_findImgId = m_mapOfImageId.find(m_trainKeyPoints[i_].class_id);
3393  int image_id = (saveTrainingImages && it_findImgId != m_mapOfImageId.end()) ? it_findImgId->second : -1;
3394 // file.write((char *)(&image_id), sizeof(image_id));
3395  writeBinaryIntLE(file, image_id);
3396 #else
3397  int image_id = -1;
3398 // file.write((char *)(&image_id), sizeof(image_id));
3399  writeBinaryIntLE(file, image_id);
3400 #endif
3401 
3402  if(have3DInfo) {
3403  float oX = m_trainPoints[i_].x, oY = m_trainPoints[i_].y, oZ = m_trainPoints[i_].z;
3404  //Write oX
3405 // file.write((char *)(&oX), sizeof(oX));
3406  writeBinaryFloatLE(file, oX);
3407 
3408  //Write oY
3409 // file.write((char *)(&oY), sizeof(oY));
3410  writeBinaryFloatLE(file, oY);
3411 
3412  //Write oZ
3413 // file.write((char *)(&oZ), sizeof(oZ));
3414  writeBinaryFloatLE(file, oZ);
3415  }
3416 
3417  for (int j = 0; j < nCols; j++) {
3418  //Write the descriptor value
3419  switch(descriptorType) {
3420  case CV_8U:
3421  file.write((char *)(&m_trainDescriptors.at<unsigned char>(i, j)), sizeof(m_trainDescriptors.at<unsigned char>(i, j)));
3422  break;
3423 
3424  case CV_8S:
3425  file.write((char *)(&m_trainDescriptors.at<char>(i, j)), sizeof(m_trainDescriptors.at<char>(i, j)));
3426  break;
3427 
3428  case CV_16U:
3429 // file.write((char *)(&m_trainDescriptors.at<unsigned short int>(i, j)), sizeof(m_trainDescriptors.at<unsigned short int>(i, j)));
3430  writeBinaryUShortLE(file, m_trainDescriptors.at<unsigned short int>(i, j));
3431  break;
3432 
3433  case CV_16S:
3434 // file.write((char *)(&m_trainDescriptors.at<short int>(i, j)), sizeof(m_trainDescriptors.at<short int>(i, j)));
3435  writeBinaryShortLE(file, m_trainDescriptors.at<short int>(i, j));
3436  break;
3437 
3438  case CV_32S:
3439 // file.write((char *)(&m_trainDescriptors.at<int>(i, j)), sizeof(m_trainDescriptors.at<int>(i, j)));
3440  writeBinaryIntLE(file, m_trainDescriptors.at<int>(i, j));
3441  break;
3442 
3443  case CV_32F:
3444 // file.write((char *)(&m_trainDescriptors.at<float>(i, j)), sizeof(m_trainDescriptors.at<float>(i, j)));
3445  writeBinaryFloatLE(file, m_trainDescriptors.at<float>(i, j));
3446  break;
3447 
3448  case CV_64F:
3449 // file.write((char *)(&m_trainDescriptors.at<double>(i, j)), sizeof(m_trainDescriptors.at<double>(i, j)));
3450  writeBinaryDoubleLE(file, m_trainDescriptors.at<double>(i, j));
3451  break;
3452 
3453  default:
3454  throw vpException(vpException::fatalError, "Problem with the data type of descriptors !");
3455  break;
3456  }
3457  }
3458  }
3459 
3460 
3461  file.close();
3462  } else {
3463 #ifdef VISP_HAVE_XML2
3464  xmlDocPtr doc = NULL;
3465  xmlNodePtr root_node = NULL, image_node = NULL, image_info_node = NULL, descriptors_info_node = NULL,
3466  descriptor_node = NULL, desc_node = NULL;
3467 
3468  /*
3469  * this initialize the library and check potential ABI mismatches
3470  * between the version it was compiled for and the actual shared
3471  * library used.
3472  */
3473  LIBXML_TEST_VERSION
3474 
3475  doc = xmlNewDoc(BAD_CAST "1.0");
3476  if (doc == NULL) {
3477  throw vpException(vpException::ioError, "Error with file " + filename);
3478  }
3479 
3480  root_node = xmlNewNode(NULL, BAD_CAST "LearningData");
3481  xmlDocSetRootElement(doc, root_node);
3482 
3483  std::stringstream ss;
3484 
3485  //Write the training images info
3486  image_node = xmlNewChild(root_node, NULL, BAD_CAST "TrainingImageInfo", NULL);
3487 
3488 #ifdef VISP_HAVE_MODULE_IO
3489  for(std::map<int, std::string>::const_iterator it = mapOfImgPath.begin(); it != mapOfImgPath.end(); ++it) {
3490  image_info_node = xmlNewChild(image_node, NULL, BAD_CAST "trainImg",
3491  BAD_CAST it->second.c_str());
3492  ss.str("");
3493  ss << it->first;
3494  xmlNewProp(image_info_node, BAD_CAST "image_id", BAD_CAST ss.str().c_str());
3495  }
3496 #endif
3497 
3498  //Write information about descriptors
3499  descriptors_info_node = xmlNewChild(root_node, NULL, BAD_CAST "DescriptorsInfo", NULL);
3500 
3501  int nRows = m_trainDescriptors.rows,
3502  nCols = m_trainDescriptors.cols;
3503  int descriptorType = m_trainDescriptors.type();
3504 
3505  //Write the number of rows
3506  ss.str("");
3507  ss << nRows;
3508  xmlNewChild(descriptors_info_node, NULL, BAD_CAST "nrows", BAD_CAST ss.str().c_str());
3509 
3510  //Write the number of cols
3511  ss.str("");
3512  ss << nCols;
3513  xmlNewChild(descriptors_info_node, NULL, BAD_CAST "ncols", BAD_CAST ss.str().c_str());
3514 
3515  //Write the descriptors type
3516  ss.str("");
3517  ss << descriptorType;
3518  xmlNewChild(descriptors_info_node, NULL, BAD_CAST "type", BAD_CAST ss.str().c_str());
3519 
3520  for (int i = 0; i < nRows; i++) {
3521  unsigned int i_ = (unsigned int) i;
3522  descriptor_node = xmlNewChild(root_node, NULL, BAD_CAST "DescriptorInfo",
3523  NULL);
3524 
3525  ss.str("");
3526  //max_digits10 == 9 for float
3527  ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].pt.x;
3528  xmlNewChild(descriptor_node, NULL, BAD_CAST "u",
3529  BAD_CAST ss.str().c_str());
3530 
3531  ss.str("");
3532  //max_digits10 == 9 for float
3533  ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].pt.y;
3534  xmlNewChild(descriptor_node, NULL, BAD_CAST "v",
3535  BAD_CAST ss.str().c_str());
3536 
3537  ss.str("");
3538  //max_digits10 == 9 for float
3539  ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].size;
3540  xmlNewChild(descriptor_node, NULL, BAD_CAST "size",
3541  BAD_CAST ss.str().c_str());
3542 
3543  ss.str("");
3544  //max_digits10 == 9 for float
3545  ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].angle;
3546  xmlNewChild(descriptor_node, NULL, BAD_CAST "angle",
3547  BAD_CAST ss.str().c_str());
3548 
3549  ss.str("");
3550  //max_digits10 == 9 for float
3551  ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].response;
3552  xmlNewChild(descriptor_node, NULL, BAD_CAST "response",
3553  BAD_CAST ss.str().c_str());
3554 
3555  ss.str("");
3556  ss << m_trainKeyPoints[i_].octave;
3557  xmlNewChild(descriptor_node, NULL, BAD_CAST "octave",
3558  BAD_CAST ss.str().c_str());
3559 
3560  ss.str("");
3561  ss << m_trainKeyPoints[i_].class_id;
3562  xmlNewChild(descriptor_node, NULL, BAD_CAST "class_id",
3563  BAD_CAST ss.str().c_str());
3564 
3565  ss.str("");
3566 #ifdef VISP_HAVE_MODULE_IO
3567  std::map<int, int>::const_iterator it_findImgId = m_mapOfImageId.find(m_trainKeyPoints[i_].class_id);
3568  ss << ((saveTrainingImages && it_findImgId != m_mapOfImageId.end()) ? it_findImgId->second : -1);
3569  xmlNewChild(descriptor_node, NULL, BAD_CAST "image_id",
3570  BAD_CAST ss.str().c_str());
3571 #else
3572  ss << -1;
3573  xmlNewChild(descriptor_node, NULL, BAD_CAST "image_id",
3574  BAD_CAST ss.str().c_str());
3575 #endif
3576 
3577  if (have3DInfo) {
3578  ss.str("");
3579  //max_digits10 == 9 for float
3580  ss << std::fixed << std::setprecision(9) << m_trainPoints[i_].x;
3581  xmlNewChild(descriptor_node, NULL, BAD_CAST "oX",
3582  BAD_CAST ss.str().c_str());
3583 
3584  ss.str("");
3585  //max_digits10 == 9 for float
3586  ss << std::fixed << std::setprecision(9) << m_trainPoints[i_].y;
3587  xmlNewChild(descriptor_node, NULL, BAD_CAST "oY",
3588  BAD_CAST ss.str().c_str());
3589 
3590  ss.str("");
3591  //max_digits10 == 9 for float
3592  ss << std::fixed << std::setprecision(9) << m_trainPoints[i_].z;
3593  xmlNewChild(descriptor_node, NULL, BAD_CAST "oZ",
3594  BAD_CAST ss.str().c_str());
3595  }
3596 
3597  desc_node = xmlNewChild(descriptor_node, NULL, BAD_CAST "desc", NULL);
3598 
3599  for (int j = 0; j < nCols; j++) {
3600  ss.str("");
3601 
3602  switch(descriptorType) {
3603  case CV_8U:
3604  {
3605  //Promote an unsigned char to an int
3606  //val_tmp holds the numeric value that will be written
3607  //We save the value in numeric form otherwise libxml2 will not be able to parse
3608  //A better solution could be possible
3609  int val_tmp = m_trainDescriptors.at<unsigned char>(i, j);
3610  ss << val_tmp;
3611  }
3612  break;
3613 
3614  case CV_8S:
3615  {
3616  //Promote a char to an int
3617  //val_tmp holds the numeric value that will be written
3618  //We save the value in numeric form otherwise libxml2 will not be able to parse
3619  //A better solution could be possible
3620  int val_tmp = m_trainDescriptors.at<char>(i, j);
3621  ss << val_tmp;
3622  }
3623  break;
3624 
3625  case CV_16U:
3626  ss << m_trainDescriptors.at<unsigned short int>(i, j);
3627  break;
3628 
3629  case CV_16S:
3630  ss << m_trainDescriptors.at<short int>(i, j);
3631  break;
3632 
3633  case CV_32S:
3634  ss << m_trainDescriptors.at<int>(i, j);
3635  break;
3636 
3637  case CV_32F:
3638  //max_digits10 == 9 for float
3639  ss << std::fixed << std::setprecision(9) << m_trainDescriptors.at<float>(i, j);
3640  break;
3641 
3642  case CV_64F:
3643  //max_digits10 == 17 for double
3644  ss << std::fixed << std::setprecision(17) << m_trainDescriptors.at<double>(i, j);
3645  break;
3646 
3647  default:
3648  throw vpException(vpException::fatalError, "Problem with the data type of descriptors !");
3649  break;
3650  }
3651  xmlNewChild(desc_node, NULL, BAD_CAST "val",
3652  BAD_CAST ss.str().c_str());
3653  }
3654  }
3655 
3656  xmlSaveFormatFileEnc(filename.c_str(), doc, "UTF-8", 1);
3657 
3658  /*free the document */
3659  xmlFreeDoc(doc);
3660 
3661  /*
3662  *Free the global variables that may
3663  *have been allocated by the parser.
3664  */
3665  xmlCleanupParser();
3666 #else
3667  std::cerr << "Error: libxml2 is required !" << std::endl;
3668 #endif
3669  }
3670 }
3671 
3672 #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x030000)
3673 //From OpenCV 2.4.11 source code.
3674 struct KeypointResponseGreaterThanThreshold {
3675  KeypointResponseGreaterThanThreshold(float _value) :
3676  value(_value) {
3677  }
3678  inline bool operator()(const cv::KeyPoint& kpt) const {
3679  return kpt.response >= value;
3680  }
3681  float value;
3682 };
3683 
3684 struct KeypointResponseGreater {
3685  inline bool operator()(const cv::KeyPoint& kp1,
3686  const cv::KeyPoint& kp2) const {
3687  return kp1.response > kp2.response;
3688  }
3689 };
3690 
3691 // takes keypoints and culls them by the response
3692 void vpKeyPoint::KeyPointsFilter::retainBest(
3693  std::vector<cv::KeyPoint>& keypoints, int n_points) {
3694  //this is only necessary if the keypoints size is greater than the number of desired points.
3695  if (n_points >= 0 && keypoints.size() > (size_t) n_points) {
3696  if (n_points == 0) {
3697  keypoints.clear();
3698  return;
3699  }
3700  //first use nth element to partition the keypoints into the best and worst.
3701  std::nth_element(keypoints.begin(), keypoints.begin() + n_points,
3702  keypoints.end(), KeypointResponseGreater());
3703  //this is the boundary response, and in the case of FAST may be ambiguous
3704  float ambiguous_response = keypoints[(size_t) (n_points - 1)].response;
3705  //use std::partition to grab all of the keypoints with the boundary response.
3706  std::vector<cv::KeyPoint>::const_iterator new_end = std::partition(
3707  keypoints.begin() + n_points, keypoints.end(),
3708  KeypointResponseGreaterThanThreshold(ambiguous_response));
3709  //resize the keypoints, given this new end point. nth_element and partition reordered the points inplace
3710  keypoints.resize((size_t) (new_end - keypoints.begin()));
3711  }
3712 }
3713 
3714 struct RoiPredicate {
3715  RoiPredicate(const cv::Rect& _r) :
3716  r(_r) {
3717  }
3718 
3719  bool operator()(const cv::KeyPoint& keyPt) const {
3720  return !r.contains(keyPt.pt);
3721  }
3722 
3723  cv::Rect r;
3724 };
3725 
3726 void vpKeyPoint::KeyPointsFilter::runByImageBorder(
3727  std::vector<cv::KeyPoint>& keypoints, cv::Size imageSize, int borderSize) {
3728  if (borderSize > 0) {
3729  if (imageSize.height <= borderSize * 2 || imageSize.width <= borderSize * 2)
3730  keypoints.clear();
3731  else
3732  keypoints.erase(
3733  std::remove_if(keypoints.begin(), keypoints.end(),
3734  RoiPredicate(
3735  cv::Rect(cv::Point(borderSize, borderSize),
3736  cv::Point(imageSize.width - borderSize,
3737  imageSize.height - borderSize)))), keypoints.end());
3738  }
3739 }
3740 
3741 struct SizePredicate {
3742  SizePredicate(float _minSize, float _maxSize) :
3743  minSize(_minSize), maxSize(_maxSize) {
3744  }
3745 
3746  bool operator()(const cv::KeyPoint& keyPt) const {
3747  float size = keyPt.size;
3748  return (size < minSize) || (size > maxSize);
3749  }
3750 
3751  float minSize, maxSize;
3752 };
3753 
3754 void vpKeyPoint::KeyPointsFilter::runByKeypointSize(
3755  std::vector<cv::KeyPoint>& keypoints, float minSize, float maxSize) {
3756  CV_Assert(minSize >= 0);
3757  CV_Assert(maxSize >= 0);
3758  CV_Assert(minSize <= maxSize);
3759 
3760  keypoints.erase(
3761  std::remove_if(keypoints.begin(), keypoints.end(),
3762  SizePredicate(minSize, maxSize)), keypoints.end());
3763 }
3764 
3765 class MaskPredicate {
3766 public:
3767  MaskPredicate(const cv::Mat& _mask) :
3768  mask(_mask) {
3769  }
3770  bool operator()(const cv::KeyPoint& key_pt) const {
3771  return mask.at<uchar>((int) (key_pt.pt.y + 0.5f),
3772  (int) (key_pt.pt.x + 0.5f)) == 0;
3773  }
3774 
3775 private:
3776  const cv::Mat mask;
3777  MaskPredicate& operator=(const MaskPredicate&);
3778 };
3779 
3780 void vpKeyPoint::KeyPointsFilter::runByPixelsMask(
3781  std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask) {
3782  if (mask.empty())
3783  return;
3784 
3785  keypoints.erase(
3786  std::remove_if(keypoints.begin(), keypoints.end(), MaskPredicate(mask)),
3787  keypoints.end());
3788 }
3789 
3790 struct KeyPoint_LessThan {
3791  KeyPoint_LessThan(const std::vector<cv::KeyPoint>& _kp) :
3792  kp(&_kp) {
3793  }
3794  bool operator()(/*int i, int j*/ size_t i, size_t j) const {
3795  const cv::KeyPoint& kp1 = (*kp)[/*(size_t)*/ i];
3796  const cv::KeyPoint& kp2 = (*kp)[/*(size_t)*/ j];
3797  if (!vpMath::equal(kp1.pt.x, kp2.pt.x, std::numeric_limits<float>::epsilon())) { //if (kp1.pt.x != kp2.pt.x) {
3798  return kp1.pt.x < kp2.pt.x;
3799  }
3800 
3801  if (!vpMath::equal(kp1.pt.y, kp2.pt.y, std::numeric_limits<float>::epsilon())) { //if (kp1.pt.y != kp2.pt.y) {
3802  return kp1.pt.y < kp2.pt.y;
3803  }
3804 
3805  if (!vpMath::equal(kp1.size, kp2.size, std::numeric_limits<float>::epsilon())) { //if (kp1.size != kp2.size) {
3806  return kp1.size > kp2.size;
3807  }
3808 
3809  if (!vpMath::equal(kp1.angle, kp2.angle, std::numeric_limits<float>::epsilon())) { //if (kp1.angle != kp2.angle) {
3810  return kp1.angle < kp2.angle;
3811  }
3812 
3813  if (!vpMath::equal(kp1.response, kp2.response, std::numeric_limits<float>::epsilon())) { //if (kp1.response != kp2.response) {
3814  return kp1.response > kp2.response;
3815  }
3816 
3817  if (kp1.octave != kp2.octave) {
3818  return kp1.octave > kp2.octave;
3819  }
3820 
3821  if (kp1.class_id != kp2.class_id) {
3822  return kp1.class_id > kp2.class_id;
3823  }
3824 
3825  return i < j;
3826  }
3827  const std::vector<cv::KeyPoint>* kp;
3828 };
3829 
3830 void vpKeyPoint::KeyPointsFilter::removeDuplicated(
3831  std::vector<cv::KeyPoint>& keypoints) {
3832  size_t i, j, n = keypoints.size();
3833  std::vector<size_t> kpidx(n);
3834  std::vector<uchar> mask(n, (uchar) 1);
3835 
3836  for (i = 0; i < n; i++) {
3837  kpidx[i] = i;
3838  }
3839  std::sort(kpidx.begin(), kpidx.end(), KeyPoint_LessThan(keypoints));
3840  for (i = 1, j = 0; i < n; i++) {
3841  cv::KeyPoint& kp1 = keypoints[kpidx[i]];
3842  cv::KeyPoint& kp2 = keypoints[kpidx[j]];
3843 // if (kp1.pt.x != kp2.pt.x || kp1.pt.y != kp2.pt.y || kp1.size != kp2.size || kp1.angle != kp2.angle) {
3844  if (!vpMath::equal(kp1.pt.x, kp2.pt.x, std::numeric_limits<float>::epsilon()) ||
3845  !vpMath::equal(kp1.pt.y, kp2.pt.y, std::numeric_limits<float>::epsilon()) ||
3846  !vpMath::equal(kp1.size, kp2.size, std::numeric_limits<float>::epsilon()) ||
3847  !vpMath::equal(kp1.angle, kp2.angle, std::numeric_limits<float>::epsilon())) {
3848  j = i;
3849  } else {
3850  mask[kpidx[i]] = 0;
3851  }
3852  }
3853 
3854  for (i = j = 0; i < n; i++) {
3855  if (mask[i]) {
3856  if (i != j) {
3857  keypoints[j] = keypoints[i];
3858  }
3859  j++;
3860  }
3861  }
3862  keypoints.resize(j);
3863 }
3864 
3865 /*
3866  * PyramidAdaptedFeatureDetector
3867  */
3868 vpKeyPoint::PyramidAdaptedFeatureDetector::PyramidAdaptedFeatureDetector(
3869  const cv::Ptr<cv::FeatureDetector>& _detector, int _maxLevel) :
3870  detector(_detector), maxLevel(_maxLevel) {
3871 }
3872 
3873 bool vpKeyPoint::PyramidAdaptedFeatureDetector::empty() const {
3874  return detector.empty() || (cv::FeatureDetector*) detector->empty();
3875 }
3876 
3877 void vpKeyPoint::PyramidAdaptedFeatureDetector::detect( cv::InputArray image, CV_OUT std::vector<cv::KeyPoint>& keypoints,
3878  cv::InputArray mask ) {
3879  detectImpl(image.getMat(), keypoints, mask.getMat());
3880 }
3881 
3882 void vpKeyPoint::PyramidAdaptedFeatureDetector::detectImpl(const cv::Mat& image,
3883  std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask) const {
3884  cv::Mat src = image;
3885  cv::Mat src_mask = mask;
3886 
3887  cv::Mat dilated_mask;
3888  if (!mask.empty()) {
3889  cv::dilate(mask, dilated_mask, cv::Mat());
3890  cv::Mat mask255(mask.size(), CV_8UC1, cv::Scalar(0));
3891  mask255.setTo(cv::Scalar(255), dilated_mask != 0);
3892  dilated_mask = mask255;
3893  }
3894 
3895  for (int l = 0, multiplier = 1; l <= maxLevel; ++l, multiplier *= 2) {
3896  // Detect on current level of the pyramid
3897  std::vector<cv::KeyPoint> new_pts;
3898  detector->detect(src, new_pts, src_mask);
3899  std::vector<cv::KeyPoint>::iterator it = new_pts.begin(), end =
3900  new_pts.end();
3901  for (; it != end; ++it) {
3902  it->pt.x *= multiplier;
3903  it->pt.y *= multiplier;
3904  it->size *= multiplier;
3905  it->octave = l;
3906  }
3907  keypoints.insert(keypoints.end(), new_pts.begin(), new_pts.end());
3908 
3909  // Downsample
3910  if (l < maxLevel) {
3911  cv::Mat dst;
3912  pyrDown(src, dst);
3913  src = dst;
3914 
3915  if (!mask.empty())
3916  resize(dilated_mask, src_mask, src.size(), 0, 0, CV_INTER_AREA);
3917  }
3918  }
3919 
3920  if (!mask.empty())
3921  vpKeyPoint::KeyPointsFilter::runByPixelsMask(keypoints, mask);
3922 }
3923 #endif
3924 
3925 #elif !defined(VISP_BUILD_SHARED_LIBS)
3926 // Work around to avoid warning: libvisp_vision.a(vpKeyPoint.cpp.o) has no symbols
3927 void dummy_vpKeyPoint() {};
3928 #endif
virtual void displayCircle(const vpImagePoint &center, unsigned int radius, const vpColor &color, bool fill=false, unsigned int thickness=1)=0
static void write(const vpImage< unsigned char > &I, const char *filename)
Definition: vpImageIo.cpp:472
void getTrainKeyPoints(std::vector< cv::KeyPoint > &keyPoints) const
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
void displayMatching(const vpImage< unsigned char > &IRef, vpImage< unsigned char > &IMatching, unsigned int crossSize, unsigned int lineThickness=1, const vpColor &color=vpColor::green)
double getTop() const
Definition: vpRect.h:176
static void compute3D(const cv::KeyPoint &candidate, const std::vector< vpPoint > &roi, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, cv::Point3f &point)
Definition: vpKeyPoint.cpp:538
void set_oZ(const double oZ)
Set the point Z coordinate in the object frame.
Definition: vpPoint.cpp:491
void detectExtractAffine(const vpImage< unsigned char > &I, std::vector< std::vector< cv::KeyPoint > > &listOfKeypoints, std::vector< cv::Mat > &listOfDescriptors, std::vector< vpImage< unsigned char > > *listOfAffineI=NULL)
double get_u0() const
void getQueryKeyPoints(std::vector< cv::KeyPoint > &keyPoints) const
double get_i() const
Definition: vpImagePoint.h:190
unsigned int getWidth() const
Definition: vpImage.h:161
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static bool isAbsolutePathname(const std::string &pathname)
Definition: vpIoTools.cpp:1311
void createImageMatching(vpImage< unsigned char > &IRef, vpImage< unsigned char > &ICurrent, vpImage< unsigned char > &IMatching)
Definition: vpKeyPoint.cpp:933
Implementation of an homogeneous matrix and operations on such kind of matrices.
void detect(const vpImage< unsigned char > &I, std::vector< cv::KeyPoint > &keyPoints, double &elapsedTime, const vpRect &rectangle=vpRect())
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Point coordinates conversion from normalized coordinates in meter to pixel coordinates ...
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:297
static const vpColor none
Definition: vpColor.h:175
error that can be emited by ViSP classes.
Definition: vpException.h:73
void setRansacThreshold(const double &t)
Definition: vpPose.h:167
void set_x(const double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:496
double getHeight() const
Definition: vpRect.h:151
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Point coordinates conversion from pixel coordinates to normalized coordinates in meter...
std::vector< unsigned int > getRansacInlierIndex() const
Definition: vpPose.h:177
double get_py() const
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:458
double getRight() const
Definition: vpRect.h:163
static std::string getParent(const std::string &pathname)
Definition: vpIoTools.cpp:1240
static const vpColor green
Definition: vpColor.h:166
void match(const cv::Mat &trainDescriptors, const cv::Mat &queryDescriptors, std::vector< cv::DMatch > &matches, double &elapsedTime)
static int round(const double x)
Definition: vpMath.h:248
double get_j() const
Definition: vpImagePoint.h:201
void insertImageMatching(const vpImage< unsigned char > &IRef, const vpImage< unsigned char > &ICurrent, vpImage< unsigned char > &IMatching)
static const vpColor red
Definition: vpColor.h:163
Class that defines what is a point.
Definition: vpPoint.h:59
static void makeDirectory(const char *dirname)
Definition: vpIoTools.cpp:404
Defines a generic 2D polygon.
Definition: vpPolygon.h:98
const char * what() const
double getBottom() const
Definition: vpRect.h:99
vpRect getBoundingBox() const
Definition: vpPolygon.h:164
double getWidth() const
Definition: vpRect.h:195
double get_v0() const
void set_u(const double u)
Definition: vpImagePoint.h:212
static void compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, std::vector< cv::KeyPoint > &candidate, const std::vector< vpPolygon > &polygons, const std::vector< std::vector< vpPoint > > &roisPt, std::vector< cv::Point3f > &points, cv::Mat *descriptors=NULL)
Definition: vpKeyPoint.cpp:615
bool computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo, bool(*func)(vpHomogeneousMatrix *)=NULL)
compute the pose for a given method
Definition: vpPose.cpp:382
bool _reference_computed
flag to indicate if the reference has been built.
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
void set_v(const double v)
Definition: vpImagePoint.h:223
Class used for pose computation from N points (pose from point only).
Definition: vpPose.h:74
Generic class defining intrinsic camera parameters.
void set_y(const double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:498
void set_oX(const double oX)
Set the point X coordinate in the object frame.
Definition: vpPoint.cpp:487
void display(const vpImage< unsigned char > &IRef, const vpImage< unsigned char > &ICurrent, unsigned int size=3)
unsigned int buildReference(const vpImage< unsigned char > &I)
Definition: vpKeyPoint.cpp:340
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:456
vpMatrix getCovarianceMatrix() const
Definition: vpPose.h:194
vpKeyPoint(const std::string &detectorName="ORB", const std::string &extractorName="ORB", const std::string &matcherName="BruteForce-Hamming", const vpFilterMatchingType &filterType=ratioDistanceThreshold)
Definition: vpKeyPoint.cpp:210
std::vector< vpImagePoint > referenceImagePointsList
void getObjectPoints(std::vector< cv::Point3f > &objectPoints) const
double get_px() const
bool computePose(const std::vector< cv::Point2f > &imagePoints, const std::vector< cv::Point3f > &objectPoints, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, std::vector< int > &inlierIndex, double &elapsedTime, bool(*func)(vpHomogeneousMatrix *)=NULL)
Definition: vpKeyPoint.cpp:724
unsigned int matchPoint(const vpImage< unsigned char > &I)
void setRansacMaxTrials(const int &rM)
Definition: vpPose.h:175
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:166
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
void loadLearningData(const std::string &filename, const bool binaryMode=false, const bool append=false)
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:111
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void set_oY(const double oY)
Set the point Y coordinate in the object frame.
Definition: vpPoint.cpp:489
void saveLearningData(const std::string &filename, const bool binaryMode=false, const bool saveTrainingImages=true)
void getTrainPoints(std::vector< cv::Point3f > &points) const
void insert(const vpImage< Type > &src, const vpImagePoint topLeft)
Definition: vpImage.h:935
vpHomogeneousMatrix inverse() const
vpFilterMatchingType
Definition: vpKeyPoint.h:218
double getB() const
Definition: vpPlane.h:108
void extract(const vpImage< unsigned char > &I, std::vector< cv::KeyPoint > &keyPoints, cv::Mat &descriptors, double &elapsedTime, std::vector< cv::Point3f > *trainPoints=NULL)
bool matchPointAndDetect(const vpImage< unsigned char > &I, vpRect &boundingBox, vpImagePoint &centerOfGravity, const bool isPlanarObject=true, std::vector< vpImagePoint > *imPts1=NULL, std::vector< vpImagePoint > *imPts2=NULL, double *meanDescriptorDistance=NULL, double *detectionScore=NULL, const vpRect &rectangle=vpRect())
std::vector< unsigned int > matchedReferencePoints
double getA() const
Definition: vpPlane.h:106
unsigned int getHeight() const
Definition: vpImage.h:152
Defines a rectangle in the plane.
Definition: vpRect.h:81
std::vector< vpImagePoint > currentImagePointsList
double getC() const
Definition: vpPlane.h:110
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
void loadConfigFile(const std::string &configFile)
void reset()
static void read(vpImage< unsigned char > &I, const char *filename)
Definition: vpImageIo.cpp:274
void addPoint(const vpPoint &P)
Add a new point in this array.
Definition: vpPose.cpp:151
double getLeft() const
Definition: vpRect.h:157
static void convertFromOpenCV(const cv::KeyPoint &from, vpImagePoint &to)
Definition: vpConvert.cpp:213
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
std::vector< vpPoint > getRansacInliers() const
Definition: vpPose.h:178
void setCovarianceComputation(const bool &flag)
Definition: vpPose.h:185
double getD() const
Definition: vpPlane.h:112
void set_ij(const double ii, const double jj)
Definition: vpImagePoint.h:176
vpMatchingMethodEnum getMatchingMethod() const
void initMatcher(const std::string &matcherName)
void parse(const std::string &filename)