Visual Servoing Platform  version 3.4.0
vpKeyPointSurf.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Key point Surf.
33  *
34  * Authors:
35  * Nicolas Melchior
36  *
37  *****************************************************************************/
38 
39 #include <visp3/vision/vpKeyPointSurf.h>
40 
41 #if defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
42 
43 #include <visp3/core/vpDebug.h>
44 #include <visp3/core/vpDisplay.h>
45 #include <visp3/core/vpImageConvert.h>
46 #include <visp3/core/vpImageTools.h>
47 
48 #include <ctype.h>
49 #include <list>
50 #include <stdio.h>
51 #include <stdlib.h>
52 #include <vector>
53 
54 double compareSURFDescriptors(const float *d1, const float *d2, double best, int length);
55 int naiveNearestNeighbor(const float *vec, int laplacian, const CvSeq *model_keypoints, const CvSeq *model_descriptors);
56 int naiveNearestNeighbor(const float *vec, const CvSeq *ref_keypoints, const CvSeq *ref_descriptors);
57 void findPairs(const CvSeq *objectKeypoints, const CvSeq *objectDescriptors, const CvSeq *imageKeypoints,
58  const CvSeq *imageDescriptors, std::vector<int> &ptpairs);
59 
60 // Compare two surf descriptors.
61 double compareSURFDescriptors(const float *d1, const float *d2, double best, int length)
62 {
63  double total_cost = 0;
64  int i;
65  assert(length % 4 == 0);
66  for (i = 0; i < length; i += 4) {
67  double t0 = d1[i] - d2[i];
68  double t1 = d1[i + 1] - d2[i + 1];
69  double t2 = d1[i + 2] - d2[i + 2];
70  double t3 = d1[i + 3] - d2[i + 3];
71  total_cost += t0 * t0 + t1 * t1 + t2 * t2 + t3 * t3;
72  if (total_cost > best)
73  break;
74  }
75  return total_cost;
76 }
77 
78 // Find for a point candidate the most similar point in the reference point
79 // list.
80 int naiveNearestNeighbor(const float *vec, int laplacian, const CvSeq *model_keypoints, const CvSeq *model_descriptors)
81 {
82  int length = (int)(model_descriptors->elem_size / (int)sizeof(float));
83  int i, neighbor = -1;
84  double d, dist1 = 1e6, dist2 = 1e6;
85  CvSeqReader reader, kreader;
86  cvStartReadSeq(model_keypoints, &kreader, 0);
87  cvStartReadSeq(model_descriptors, &reader, 0);
88 
89  for (i = 0; i < model_descriptors->total; i++) {
90  const CvSURFPoint *kp = (const CvSURFPoint *)kreader.ptr;
91  const float *mvec = (const float *)reader.ptr;
92  CV_NEXT_SEQ_ELEM(kreader.seq->elem_size, kreader);
93  CV_NEXT_SEQ_ELEM(reader.seq->elem_size, reader);
94  if (laplacian != kp->laplacian)
95  continue;
96  d = compareSURFDescriptors(vec, mvec, dist2, length);
97  if (d < dist1) {
98  dist2 = dist1;
99  dist1 = d;
100  neighbor = i;
101  } else if (d < dist2)
102  dist2 = d;
103  }
104  if (dist1 < 0.6 * dist2)
105  return neighbor;
106  return -1;
107 }
108 
109 // Find for a point candidate the most similar point in the reference point
110 // list.
111 int naiveNearestNeighbor(const float *vec, const CvSeq *ref_keypoints, const CvSeq *ref_descriptors)
112 {
113  int length = (int)(ref_descriptors->elem_size / (int)sizeof(float));
114  int i, neighbor = -1;
115  double dist1 = 1e6, dist2 = 1e6;
116  CvSeqReader reader, kreader;
117  cvStartReadSeq(ref_keypoints, &kreader, 0);
118  cvStartReadSeq(ref_descriptors, &reader, 0);
119 
120  for (i = 0; i < ref_descriptors->total; i++) {
121  const float *mvec = (const float *)reader.ptr;
122  CV_NEXT_SEQ_ELEM(kreader.seq->elem_size, kreader);
123  CV_NEXT_SEQ_ELEM(reader.seq->elem_size, reader);
124  double d = compareSURFDescriptors(vec, mvec, dist2, length);
125  if (d < dist1) {
126  dist2 = dist1;
127  dist1 = d;
128  neighbor = i;
129  } else if (d < dist2)
130  dist2 = d;
131  }
132  if (dist1 < 0.6 * dist2)
133  return neighbor;
134  return -1;
135 }
136 
137 // Find all the matched points
138 void findPairs(const CvSeq *objectKeypoints, const CvSeq *objectDescriptors, const CvSeq *imageKeypoints,
139  const CvSeq *imageDescriptors, std::vector<int> &ptpairs)
140 {
141  int i;
142  CvSeqReader reader, kreader;
143  cvStartReadSeq(objectKeypoints, &kreader);
144  cvStartReadSeq(objectDescriptors, &reader);
145  ptpairs.clear();
146 
147  for (i = 0; i < objectDescriptors->total; i++) {
148  const CvSURFPoint *kp = (const CvSURFPoint *)kreader.ptr;
149  const float *descriptor = (const float *)reader.ptr;
150  CV_NEXT_SEQ_ELEM(kreader.seq->elem_size, kreader);
151  CV_NEXT_SEQ_ELEM(reader.seq->elem_size, reader);
152  int nearest_neighbor = naiveNearestNeighbor(descriptor, kp->laplacian, imageKeypoints, imageDescriptors);
153  if (nearest_neighbor >= 0) {
154  ptpairs.push_back(i);
155  ptpairs.push_back(nearest_neighbor);
156  }
157  }
158 }
159 
167 vpKeyPointSurf::vpKeyPointSurf()
168  : vpBasicKeyPoint(), storage(NULL), params(), storage_cur(NULL), image_keypoints(NULL), image_descriptors(NULL),
169  ref_keypoints(NULL), ref_descriptors(NULL), hessianThreshold(500), descriptorType(extendedDescriptor)
170 {
171  init();
172 }
173 
177 void vpKeyPointSurf::init()
178 {
179 #if (VISP_HAVE_OPENCV_VERSION >= 0x020400) // Require opencv >= 2.4.0
180  cv::initModule_nonfree();
181 #endif
182 
183  storage = cvCreateMemStorage(0);
184  params = cvSURFParams(hessianThreshold, descriptorType);
185 }
186 
191 vpKeyPointSurf::~vpKeyPointSurf()
192 {
193  cvReleaseMemStorage(&storage);
194  if (storage_cur != NULL) {
195  cvReleaseMemStorage(&storage_cur);
196  }
197 }
198 
208 unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I)
209 {
210  IplImage *model = NULL;
211 
212  if ((I.getWidth() % 8) == 0) {
213  int height = (int)I.getHeight();
214  int width = (int)I.getWidth();
215  CvSize size = cvSize(width, height);
216  model = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
217  model->imageData = (char *)I.bitmap;
218  } else {
219  vpImageConvert::convert(I, model);
220  }
221 
222  cvExtractSURF(model, 0, &ref_keypoints, &ref_descriptors, storage, params);
223 
224  unsigned int nbPoints = (unsigned int)ref_keypoints->total;
225 
226  referenceImagePointsList.resize(nbPoints);
227 
228  for (unsigned int i = 0; i < nbPoints; i++) {
229  CvSURFPoint *r1 = (CvSURFPoint *)cvGetSeqElem(ref_keypoints, (int)i);
230 
231  referenceImagePointsList[i].set_i(r1->pt.y);
232  referenceImagePointsList[i].set_j(r1->pt.x);
233  }
234 
235  if ((I.getWidth() % 8) == 0) {
236  model->imageData = NULL;
237  cvReleaseImageHeader(&model);
238  } else {
239  cvReleaseImage(&model);
240  }
241 
242  _reference_computed = true;
243  return nbPoints;
244 }
245 
263 unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I, const vpImagePoint &iP,
264  unsigned int height, unsigned int width)
265 {
266  if ((iP.get_i() + height) >= I.getHeight() || (iP.get_j() + width) >= I.getWidth()) {
267  vpTRACE("Bad size for the subimage");
268  throw(vpException(vpImageException::notInTheImage, "Bad size for the subimage"));
269  }
270 
271  vpImage<unsigned char> subImage;
272 
273  vpImageTools::crop(I, (unsigned int)iP.get_i(), (unsigned int)iP.get_j(), height, width, subImage);
274 
275  unsigned int nbRefPoint = this->buildReference(subImage);
276 
277  for (unsigned int k = 0; k < nbRefPoint; k++) {
278  (referenceImagePointsList[k]).set_i((referenceImagePointsList[k]).get_i() + iP.get_i());
279  (referenceImagePointsList[k]).set_j((referenceImagePointsList[k]).get_j() + iP.get_j());
280  }
281  return (nbRefPoint);
282 }
283 
297 unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I, const vpRect &rectangle)
298 {
299  vpImagePoint iP;
300  iP.set_i(rectangle.getTop());
301  iP.set_j(rectangle.getLeft());
302  return (this->buildReference(I, iP, (unsigned int)rectangle.getHeight(), (unsigned int)rectangle.getWidth()));
303 }
304 
315 unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I)
316 {
317  IplImage *currentImage = NULL;
318 
319  if ((I.getWidth() % 8) == 0) {
320  int height = (int)I.getHeight();
321  int width = (int)I.getWidth();
322  CvSize size = cvSize(width, height);
323  currentImage = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
324  currentImage->imageData = (char *)I.bitmap;
325  } else {
326  vpImageConvert::convert(I, currentImage);
327  }
328 
329  /* we release the memory storage for the current points (it has to be kept
330  allocated for the get descriptor points, ...) */
331  if (storage_cur != NULL) {
332  cvReleaseMemStorage(&storage_cur);
333  storage_cur = NULL;
334  }
335  storage_cur = cvCreateMemStorage(0);
336 
337  cvExtractSURF(currentImage, 0, &image_keypoints, &image_descriptors, storage_cur, params);
338 
339  CvSeqReader reader, kreader;
340  cvStartReadSeq(ref_keypoints, &kreader);
341  cvStartReadSeq(ref_descriptors, &reader);
342 
343  std::list<int> indexImagePair;
344  std::list<int> indexReferencePair;
345 
346  unsigned int nbrPair = 0;
347 
348  for (int i = 0; i < ref_descriptors->total; i++) {
349  const CvSURFPoint *kp = (const CvSURFPoint *)kreader.ptr;
350  const float *descriptor = (const float *)reader.ptr;
351  CV_NEXT_SEQ_ELEM(kreader.seq->elem_size, kreader);
352  CV_NEXT_SEQ_ELEM(reader.seq->elem_size, reader);
353  int nearest_neighbor = naiveNearestNeighbor(descriptor, kp->laplacian, image_keypoints, image_descriptors);
354  if (nearest_neighbor >= 0) {
355  indexReferencePair.push_back(i);
356  indexImagePair.push_back(nearest_neighbor);
357  nbrPair++;
358  }
359  }
360 
361  std::list<int>::const_iterator indexImagePairIter = indexImagePair.begin();
362  std::list<int>::const_iterator indexReferencePairIter = indexReferencePair.begin();
363 
364  matchedReferencePoints.resize(0);
365 
366  if (nbrPair == 0)
367  return (0);
368 
369  currentImagePointsList.resize(nbrPair);
370  matchedReferencePoints.resize(nbrPair);
371 
372  for (unsigned int i = 0; i < nbrPair; i++) {
373  int index = *indexImagePairIter;
374 
375  CvSURFPoint *r1 = (CvSURFPoint *)cvGetSeqElem(image_keypoints, index);
376 
377  currentImagePointsList[i].set_i(r1->pt.y);
378  currentImagePointsList[i].set_j(r1->pt.x);
379 
380  matchedReferencePoints[i] = (unsigned int)*indexReferencePairIter;
381 
382  ++indexImagePairIter;
383  ++indexReferencePairIter;
384  }
385 
386  if ((I.getWidth() % 8) == 0) {
387  currentImage->imageData = NULL;
388  cvReleaseImageHeader(&currentImage);
389  } else {
390  cvReleaseImage(&currentImage);
391  }
392 
393  return nbrPair;
394 }
395 
414 unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP,
415  unsigned int height, unsigned int width)
416 {
417  if ((iP.get_i() + height) >= I.getHeight() || (iP.get_j() + width) >= I.getWidth()) {
418  vpTRACE("Bad size for the subimage");
419  throw(vpException(vpImageException::notInTheImage, "Bad size for the subimage"));
420  }
421 
422  vpImage<unsigned char> subImage;
423 
424  vpImageTools::crop(I, (unsigned int)iP.get_i(), (unsigned int)iP.get_j(), height, width, subImage);
425 
426  unsigned int nbMatchedPoint = this->matchPoint(subImage);
427 
428  for (unsigned int k = 0; k < nbMatchedPoint; k++) {
429  (currentImagePointsList[k]).set_i((currentImagePointsList[k]).get_i() + iP.get_i());
430  (currentImagePointsList[k]).set_j((currentImagePointsList[k]).get_j() + iP.get_j());
431  }
432 
433  return (nbMatchedPoint);
434 }
435 
450 unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I, const vpRect &rectangle)
451 {
452  vpImagePoint iP;
453  iP.set_i(rectangle.getTop());
454  iP.set_j(rectangle.getLeft());
455  return (this->matchPoint(I, iP, (unsigned int)rectangle.getHeight(), (unsigned int)rectangle.getWidth()));
456 }
457 
477 void vpKeyPointSurf::display(const vpImage<unsigned char> &Ireference, const vpImage<unsigned char> &Icurrent,
478  unsigned int size)
479 {
480  // matchedPointsCurrentImageList.front();
481  // matchedPointsReferenceImageList.front();
482 
483  // if (matchedPointsCurrentImageList.nbElements()
484  // != matchedPointsReferenceImageList.nbElements())
485  // {
486  // vpTRACE("Numbers of points mismatch");
487  // throw(vpException(vpException::fatalError,"Numbers of points
488  // mismatch"));
489  // }
490 
491  for (unsigned int i = 0; i < matchedReferencePoints.size(); i++) {
494  // matchedPointsReferenceImageList.next();
495  // matchedPointsCurrentImageList.next();
496  }
497 }
498 
512 void vpKeyPointSurf::display(const vpImage<unsigned char> &Icurrent, unsigned int size, const vpColor &color)
513 {
514  // matchedPointsCurrentImageList.front();
515  //
516  // vpImagePoint ipCur;
517  //
518  for (unsigned int i = 0; i < matchedReferencePoints.size(); i++) {
519  vpDisplay::displayCross(Icurrent, currentImagePointsList[i], size, color);
520  }
521 }
522 
523 std::list<int *> *vpKeyPointSurf::matchPoint(std::list<float *> descriptorList, std::list<int> laplacianList)
524 {
525  std::list<int *> *pairPoints = new std::list<int *>;
526 
527  if (descriptorList.size() != laplacianList.size()) {
528  vpTRACE("Error, the two lists have different number of element");
529  return pairPoints;
530  }
531 
532  CvSeqReader reader;
533  cvStartReadSeq(ref_descriptors, &reader);
534 
535  std::list<float *>::const_iterator descriptorListIter = descriptorList.begin();
536  std::list<int>::const_iterator laplacianListIter = laplacianList.begin();
537  descriptorList.front();
538  int indexList = 0;
539  while (descriptorListIter != descriptorList.end()) {
540  float *descriptor = *descriptorListIter;
541 
542  int nearest_neighbor = naiveNearestNeighbor(descriptor, *laplacianListIter, ref_keypoints, ref_descriptors);
543 
544  if (nearest_neighbor >= 0) {
545  int *tab;
546  tab = new int[2];
547  tab[0] = nearest_neighbor;
548  tab[1] = indexList;
549  pairPoints->push_back(tab);
550  }
551  indexList++;
552  ++descriptorListIter;
553  ++laplacianListIter;
554  }
555 
556  return pairPoints;
557 }
558 
565 float *vpKeyPointSurf::getDescriptorReferencePoint(int index)
566 {
567  if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0) {
568  vpTRACE("Index of the reference point out of range");
569  throw(vpException(vpException::fatalError, "Index of the reference point out of range"));
570  }
571 
572  float *descriptor = NULL;
573 
574  CvSeqReader reader;
575  cvStartReadSeq(ref_descriptors, &reader);
576 
577  for (int j = 0; j < ref_descriptors->total; j++) {
578  if (j == index) {
579  descriptor = (float *)reader.ptr;
580  break;
581  }
582  CV_NEXT_SEQ_ELEM(reader.seq->elem_size, reader);
583  }
584 
585  return descriptor;
586 }
587 
594 int vpKeyPointSurf::getLaplacianReferencePoint(int index)
595 {
596  if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0) {
597  vpTRACE("Index of the reference point out of range");
598  throw(vpException(vpException::fatalError, "Index of the reference point out of range"));
599  }
600 
601  CvSeqReader reader;
602  cvStartReadSeq(ref_keypoints, &reader);
603 
604  int laplacian = 0; /* normally only -1, 0, +1 are possible */
605 
606  for (int j = 0; j < ref_keypoints->total; j++) {
607  if (j == index) {
608  const CvSURFPoint *kp = (const CvSURFPoint *)reader.ptr;
609  laplacian = kp->laplacian;
610  break;
611  }
612  CV_NEXT_SEQ_ELEM(reader.seq->elem_size, reader);
613  }
614 
615  return laplacian;
616 }
617 
626 void vpKeyPointSurf::getDescriptorParamReferencePoint(int index, int &size, float &dir)
627 {
628  if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0) {
629  vpTRACE("Index of the reference point out of range");
630  throw(vpException(vpException::fatalError, "Index of the reference point out of range"));
631  }
632 
633  CvSeqReader reader;
634  cvStartReadSeq(ref_keypoints, &reader);
635 
636  for (int j = 0; j < ref_keypoints->total; j++) {
637  if (j == index) {
638  const CvSURFPoint *kp = (const CvSURFPoint *)reader.ptr;
639  size = kp->size;
640  dir = kp->dir;
641  break;
642  }
643  CV_NEXT_SEQ_ELEM(reader.seq->elem_size, reader);
644  }
645 }
646 
647 #elif !defined(VISP_BUILD_SHARED_LIBS)
648 // Work arround to avoid warning: libvisp_vision.a(vpKeyPointSurf.cpp.o) has
649 // no symbols
650 void dummy_vpKeyPointSurf(){};
651 #endif
class that defines what is a Keypoint. This class provides all the basic elements to implement classe...
double getTop() const
Definition: vpRect.h:193
double get_i() const
Definition: vpImagePoint.h:203
unsigned int getWidth() const
Definition: vpImage.h:246
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
error that can be emited by ViSP classes.
Definition: vpException.h:71
double getHeight() const
Definition: vpRect.h:167
static const vpColor green
Definition: vpColor.h:220
double get_j() const
Definition: vpImagePoint.h:214
static const vpColor red
Definition: vpColor.h:217
double getWidth() const
Definition: vpRect.h:228
void set_i(double ii)
Definition: vpImagePoint.h:166
#define vpTRACE
Definition: vpDebug.h:416
bool _reference_computed
flag to indicate if the reference has been built.
unsigned int buildReference(const vpImage< unsigned char > &I)
Definition: vpKeyPoint.cpp:238
std::vector< vpImagePoint > referenceImagePointsList
unsigned int matchPoint(const vpImage< unsigned char > &I)
void set_j(double jj)
Definition: vpImagePoint.h:177
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void crop(const vpImage< Type > &I, double roi_top, double roi_left, unsigned int roi_height, unsigned int roi_width, vpImage< Type > &crop, unsigned int v_scale=1, unsigned int h_scale=1)
Definition: vpImageTools.h:305
std::vector< unsigned int > matchedReferencePoints
unsigned int getHeight() const
Definition: vpImage.h:188
Defines a rectangle in the plane.
Definition: vpRect.h:79
std::vector< vpImagePoint > currentImagePointsList
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
double getLeft() const
Definition: vpRect.h:174