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