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