Visual Servoing Platform  version 3.0.0
vpKeyPointSurf.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 Surf.
32  *
33  * Authors:
34  * Nicolas Melchior
35  *
36  *****************************************************************************/
37 
38 
39 #include <visp3/vision/vpKeyPointSurf.h>
40 
41 #if defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION >= 0x010100) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 1.1.0 < 3.0.0
42 
43 #include <visp3/core/vpImageConvert.h>
44 #include <visp3/core/vpImageTools.h>
45 #include <visp3/core/vpDisplay.h>
46 #include <visp3/core/vpDebug.h>
47 
48 #include <ctype.h>
49 #include <stdio.h>
50 #include <stdlib.h>
51 #include <vector>
52 #include <list>
53 
54 double compareSURFDescriptors( const float* d1, const float* d2, double best, int length );
55 int naiveNearestNeighbor( const float *vec, int laplacian,
56  const CvSeq *model_keypoints,
57  const CvSeq *model_descriptors );
58 int naiveNearestNeighbor( const float *vec,
59  const CvSeq *ref_keypoints,
60  const CvSeq *ref_descriptors );
61 void findPairs( const CvSeq* objectKeypoints,
62  const CvSeq* objectDescriptors,
63  const CvSeq* imageKeypoints,
64  const CvSeq* imageDescriptors,
65  std::vector<int>& ptpairs );
66 
67 // Compare two surf descriptors.
68 double compareSURFDescriptors( const float* d1, const float* d2, double best, int length )
69 {
70  double total_cost = 0;
71  int i;
72  assert( length % 4 == 0 );
73  for( i = 0; i < length; i += 4 )
74  {
75  double t0 = d1[i] - d2[i];
76  double t1 = d1[i+1] - d2[i+1];
77  double t2 = d1[i+2] - d2[i+2];
78  double t3 = d1[i+3] - d2[i+3];
79  total_cost += t0*t0 + t1*t1 + t2*t2 + t3*t3;
80  if( total_cost > best )
81  break;
82  }
83  return total_cost;
84 }
85 
86 
87 //Find for a point candidate the most similar point in the reference point list.
88 int naiveNearestNeighbor( const float *vec, int laplacian,
89  const CvSeq *model_keypoints,
90  const CvSeq *model_descriptors )
91 {
92  int length = (int)(model_descriptors->elem_size/(int)sizeof(float));
93  int i, neighbor = -1;
94  double d, dist1 = 1e6, dist2 = 1e6;
95  CvSeqReader reader, kreader;
96  cvStartReadSeq( model_keypoints, &kreader, 0 );
97  cvStartReadSeq( model_descriptors, &reader, 0 );
98 
99  for( i = 0; i < model_descriptors->total; i++ )
100  {
101  const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
102  const float* mvec = (const float*)reader.ptr;
103  CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
104  CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
105  if( laplacian != kp->laplacian )
106  continue;
107  d = compareSURFDescriptors( vec, mvec, dist2, length );
108  if( d < dist1 )
109  {
110  dist2 = dist1;
111  dist1 = d;
112  neighbor = i;
113  }
114  else if ( d < dist2 )
115  dist2 = d;
116  }
117  if ( dist1 < 0.6*dist2 )
118  return neighbor;
119  return -1;
120 }
121 
122 
123 //Find for a point candidate the most similar point in the reference point list.
124 int naiveNearestNeighbor( const float *vec,
125  const CvSeq *ref_keypoints,
126  const CvSeq *ref_descriptors )
127 {
128  int length = (int)(ref_descriptors->elem_size/(int)sizeof(float));
129  int i, neighbor = -1;
130  double d, dist1 = 1e6, dist2 = 1e6;
131  CvSeqReader reader, kreader;
132  cvStartReadSeq( ref_keypoints, &kreader, 0 );
133  cvStartReadSeq( ref_descriptors, &reader, 0 );
134 
135  for( i = 0; i < ref_descriptors->total; i++ )
136  {
137  const float* mvec = (const float*)reader.ptr;
138  CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
139  CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
140  d = compareSURFDescriptors( vec, mvec, dist2, length );
141  if( d < dist1 )
142  {
143  dist2 = dist1;
144  dist1 = d;
145  neighbor = i;
146  }
147  else if ( d < dist2 )
148  dist2 = d;
149  }
150  if ( dist1 < 0.6*dist2 )
151  return neighbor;
152  return -1;
153 }
154 
155 
156 
157 //Find all the matched points
158 void findPairs( const CvSeq* objectKeypoints,
159  const CvSeq* objectDescriptors,
160  const CvSeq* imageKeypoints,
161  const CvSeq* imageDescriptors,
162  std::vector<int>& ptpairs )
163 {
164  int i;
165  CvSeqReader reader, kreader;
166  cvStartReadSeq( objectKeypoints, &kreader );
167  cvStartReadSeq( objectDescriptors, &reader );
168  ptpairs.clear();
169 
170  for( i = 0; i < objectDescriptors->total; i++ )
171  {
172  const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
173  const float* descriptor = (const float*)reader.ptr;
174  CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
175  CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
176  int nearest_neighbor = naiveNearestNeighbor( descriptor,
177  kp->laplacian,
178  imageKeypoints,
179  imageDescriptors );
180  if( nearest_neighbor >= 0 )
181  {
182  ptpairs.push_back(i);
183  ptpairs.push_back(nearest_neighbor);
184  }
185  }
186 }
187 
188 
189 
198  : vpBasicKeyPoint(),
199  storage(NULL), params(), storage_cur(NULL), image_keypoints(NULL), image_descriptors(NULL),
200  ref_keypoints(NULL), ref_descriptors(NULL), hessianThreshold(500), descriptorType(extendedDescriptor)
201 {
202  init();
203 }
204 
208 void vpKeyPointSurf::init()
209 {
210 #if (VISP_HAVE_OPENCV_VERSION >= 0x020400) // Require opencv >= 2.4.0
211  cv::initModule_nonfree();
212 #endif
213 
214  storage = cvCreateMemStorage(0);
215  params = cvSURFParams(hessianThreshold, descriptorType);
216 }
217 
223 {
224  cvReleaseMemStorage(&storage);
225  if(storage_cur!=NULL){
226  cvReleaseMemStorage(&storage_cur);
227  }
228 }
229 
240 {
241  IplImage* model = NULL;
242 
243  if((I.getWidth() % 8) == 0){
244  int height = (int)I.getHeight();
245  int width = (int)I.getWidth();
246  CvSize size = cvSize(width, height);
247  model = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
248  model->imageData = (char*)I.bitmap;
249  }else{
250  vpImageConvert::convert(I,model);
251  }
252 
253  cvExtractSURF( model, 0, &ref_keypoints, &ref_descriptors, storage, params );
254 
255  const unsigned int nbPoints = (unsigned int)ref_keypoints->total;
256 
257  referenceImagePointsList.resize(nbPoints);
258 
259  for(unsigned int i = 0; i < nbPoints; i++ )
260  {
261  CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem(ref_keypoints, (int)i);
262 
263  referenceImagePointsList[i].set_i(r1->pt.y);
264  referenceImagePointsList[i].set_j(r1->pt.x);
265  }
266 
267  if((I.getWidth() % 8) == 0){
268  model->imageData = NULL;
269  cvReleaseImageHeader(&model);
270  }else{
271  cvReleaseImage(&model);
272  }
273 
274  _reference_computed = true;
275  return nbPoints;
276 }
277 
278 
297  const vpImagePoint &iP,
298  const unsigned int height, const unsigned int width)
299 {
300  if((iP.get_i()+height) >= I.getHeight()
301  || (iP.get_j()+width) >= I.getWidth())
302  {
303  vpTRACE("Bad size for the subimage");
305  "Bad size for the subimage"));
306  }
307 
308  vpImage<unsigned char> subImage;
309 
311  (unsigned int)iP.get_i(),
312  (unsigned int)iP.get_j(),
313  height, width, subImage);
314 
315  unsigned int nbRefPoint = this->buildReference(subImage);
316 
317  for(unsigned int k = 0; k < nbRefPoint; k++)
318  {
319  (referenceImagePointsList[k]).set_i((referenceImagePointsList[k]).get_i()
320  + iP.get_i());
321  (referenceImagePointsList[k]).set_j((referenceImagePointsList[k]).get_j()
322  + iP.get_j());
323  }
324  return(nbRefPoint);
325 }
326 
327 
342  const vpRect& rectangle)
343 {
344  vpImagePoint iP;
345  iP.set_i(rectangle.getTop());
346  iP.set_j(rectangle.getLeft());
347  return (this->buildReference(I, iP,
348  (unsigned int)rectangle.getHeight(),
349  (unsigned int)rectangle.getWidth()));
350 }
351 
352 
364 {
365  IplImage* currentImage = NULL;
366 
367  if((I.getWidth() % 8) == 0){
368  int height = (int)I.getHeight();
369  int width = (int)I.getWidth();
370  CvSize size = cvSize(width, height);
371  currentImage = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
372  currentImage->imageData = (char*)I.bitmap;
373  }else{
374  vpImageConvert::convert(I,currentImage);
375  }
376 
377  /* we release the memory storage for the current points (it has to be kept
378  allocated for the get descriptor points, ...) */
379  if(storage_cur != NULL){
380  cvReleaseMemStorage(&storage_cur);
381  storage_cur = NULL;
382  }
383  storage_cur = cvCreateMemStorage(0);
384 
385  cvExtractSURF( currentImage, 0, &image_keypoints, &image_descriptors,
386  storage_cur, params );
387 
388  CvSeqReader reader, kreader;
389  cvStartReadSeq( ref_keypoints, &kreader );
390  cvStartReadSeq( ref_descriptors, &reader );
391 
392 
393  std::list<int> indexImagePair;
394  std::list<int> indexReferencePair;
395 
396 
397  unsigned int nbrPair = 0;
398 
399  for(int i = 0; i < ref_descriptors->total; i++ )
400  {
401  const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
402  const float* descriptor = (const float*)reader.ptr;
403  CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
404  CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
405  int nearest_neighbor = naiveNearestNeighbor( descriptor,
406  kp->laplacian,
407  image_keypoints,
408  image_descriptors );
409  if( nearest_neighbor >= 0 )
410  {
411  indexReferencePair.push_back(i);
412  indexImagePair.push_back(nearest_neighbor);
413  nbrPair++;
414  }
415  }
416 
417  std::list<int>::const_iterator indexImagePairIter = indexImagePair.begin();
418  std::list<int>::const_iterator indexReferencePairIter = indexReferencePair.begin();
419 
420  matchedReferencePoints.resize(0);
421 
422  if (nbrPair == 0)
423  return (0);
424 
425  currentImagePointsList.resize(nbrPair);
426  matchedReferencePoints.resize(nbrPair);
427 
428  for (unsigned int i = 0; i < nbrPair; i++)
429  {
430  int index = *indexImagePairIter;
431 
432  CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem(image_keypoints, index);
433 
434  currentImagePointsList[i].set_i(r1->pt.y);
435  currentImagePointsList[i].set_j(r1->pt.x);
436 
437  matchedReferencePoints[i] = (unsigned int)*indexReferencePairIter;
438 
439 
440  ++indexImagePairIter;
441  ++indexReferencePairIter;
442  }
443 
444  if((I.getWidth() % 8) == 0){
445  currentImage->imageData = NULL;
446  cvReleaseImageHeader(&currentImage);
447  }else{
448  cvReleaseImage(&currentImage);
449  }
450 
451  return nbrPair;
452 }
453 
454 
474  const vpImagePoint &iP,
475  const unsigned int height, const unsigned int width)
476 {
477  if((iP.get_i()+height) >= I.getHeight()
478  || (iP.get_j()+width) >= I.getWidth())
479  {
480  vpTRACE("Bad size for the subimage");
482  "Bad size for the subimage"));
483  }
484 
485  vpImage<unsigned char> subImage;
486 
488  (unsigned int)iP.get_i(),
489  (unsigned int)iP.get_j(),
490  height, width, subImage);
491 
492  unsigned int nbMatchedPoint = this->matchPoint(subImage);
493 
494  for(unsigned int k = 0; k < nbMatchedPoint; k++)
495  {
496  (currentImagePointsList[k]).set_i((currentImagePointsList[k]).get_i()
497  + iP.get_i());
498  (currentImagePointsList[k]).set_j((currentImagePointsList[k]).get_j()
499  + iP.get_j());
500  }
501 
502  return(nbMatchedPoint);
503 }
504 
505 
521  const vpRect& rectangle)
522 {
523  vpImagePoint iP;
524  iP.set_i(rectangle.getTop());
525  iP.set_j(rectangle.getLeft());
526  return (this->matchPoint(I, iP,
527  (unsigned int)rectangle.getHeight(),
528  (unsigned int)rectangle.getWidth()));
529 }
530 
531 
551  const vpImage<unsigned char> &Icurrent, unsigned int size)
552 {
553 // matchedPointsCurrentImageList.front();
554 // matchedPointsReferenceImageList.front();
555 
556 // if (matchedPointsCurrentImageList.nbElements()
557 // != matchedPointsReferenceImageList.nbElements())
558 // {
559 // vpTRACE("Numbers of points mismatch");
560 // throw(vpException(vpException::fatalError,"Numbers of points mismatch"));
561 // }
562 
563  for (unsigned int i = 0; i < matchedReferencePoints.size(); i++)
564  {
567 // matchedPointsReferenceImageList.next();
568 // matchedPointsCurrentImageList.next();
569  }
570 }
571 
572 
585 void vpKeyPointSurf::display(const vpImage<unsigned char> &Icurrent, unsigned int size, const vpColor &color)
586 {
587 // matchedPointsCurrentImageList.front();
588 //
589 // vpImagePoint ipCur;
590 //
591  for (unsigned int i = 0; i < matchedReferencePoints.size(); i++)
592  {
593  vpDisplay::displayCross (Icurrent, currentImagePointsList[i], size, color);
594  }
595 }
596 
597 std::list<int*>* vpKeyPointSurf::matchPoint(std::list<float*> descriptorList, std::list<int> laplacianList)
598 {
599  std::list<int*>* pairPoints = new std::list<int*>;
600 
601  if(descriptorList.size() != laplacianList.size()){
602  vpTRACE("Error, the two lists have different number of element");
603  return pairPoints;
604  }
605 
606  CvSeqReader reader;
607  cvStartReadSeq( ref_descriptors, &reader );
608 
609  std::list<float*>::const_iterator descriptorListIter = descriptorList.begin();
610  std::list<int>::const_iterator laplacianListIter = laplacianList.begin();
611  descriptorList.front();
612  int indexList = 0;
613  while(descriptorListIter != descriptorList.end()){
614  float* descriptor = *descriptorListIter;
615 
616  int nearest_neighbor = naiveNearestNeighbor( descriptor, *laplacianListIter, ref_keypoints, ref_descriptors);
617 
618  if(nearest_neighbor >= 0){
619  int* tab;
620  tab = new int[2];
621  tab[0] = nearest_neighbor;
622  tab[1] = indexList;
623  pairPoints->push_back(tab);
624  }
625  indexList++;
626  ++descriptorListIter;
627  ++laplacianListIter;
628  }
629 
630  return pairPoints;
631 }
632 
639 {
640  if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0){
641  vpTRACE("Index of the reference point out of range");
642  throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
643  }
644 
645  float* descriptor = NULL;
646 
647  CvSeqReader reader;
648  cvStartReadSeq( ref_descriptors, &reader );
649 
650  for(int j = 0; j < ref_descriptors->total; j++ ){
651  if(j== index){
652  descriptor = (float*)reader.ptr;
653  break;
654  }
655  CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
656  }
657 
658  return descriptor;
659 }
660 
667 {
668  if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0){
669  vpTRACE("Index of the reference point out of range");
670  throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
671  }
672 
673  CvSeqReader reader;
674  cvStartReadSeq( ref_keypoints, &reader );
675 
676  int laplacian = 0;/* normally only -1, 0, +1 are possible */
677 
678  for(int j = 0; j < ref_keypoints->total; j++ ){
679  if(j== index){
680  const CvSURFPoint* kp = (const CvSURFPoint*)reader.ptr;
681  laplacian = kp->laplacian;
682  break;
683  }
684  CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
685  }
686 
687  return laplacian;
688 }
689 
697 void vpKeyPointSurf::getDescriptorParamReferencePoint (const int index, int& size, float& dir)
698 {
699  if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0){
700  vpTRACE("Index of the reference point out of range");
701  throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
702  }
703 
704  CvSeqReader reader;
705  cvStartReadSeq( ref_keypoints, &reader );
706 
707  for(int j = 0; j < ref_keypoints->total; j++ ){
708  if(j== index){
709  const CvSURFPoint* kp = (const CvSURFPoint*)reader.ptr;
710  size = kp->size;
711  dir = kp->dir;
712  break;
713  }
714  CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
715  }
716 }
717 
718 #elif !defined(VISP_BUILD_SHARED_LIBS)
719 // Work arround to avoid warning: libvisp_vision.a(vpKeyPointSurf.cpp.o) has no symbols
720 void dummy_vpKeyPointSurf() {};
721 #endif
722 
class that defines what is a Keypoint. This class provides all the basic elements to implement classe...
double getTop() const
Definition: vpRect.h:176
void getDescriptorParamReferencePoint(const int index, int &size, float &dir)
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)
Type * bitmap
points toward the bitmap
Definition: vpImage.h:116
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
int getLaplacianReferencePoint(const int index)
error that can be emited by ViSP classes.
Definition: vpException.h:73
unsigned int matchPoint(const vpImage< unsigned char > &I)
double getHeight() const
Definition: vpRect.h:151
void display(const vpImage< unsigned char > &Iref, const vpImage< unsigned char > &Icurrent, unsigned int size=3)
static const vpColor green
Definition: vpColor.h:166
double get_j() const
Definition: vpImagePoint.h:201
static const vpColor red
Definition: vpColor.h:163
void set_i(const double ii)
Definition: vpImagePoint.h:154
double getWidth() const
Definition: vpRect.h:195
#define vpTRACE
Definition: vpDebug.h:414
float * getDescriptorReferencePoint(const int index)
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
std::vector< vpImagePoint > referenceImagePointsList
unsigned int buildReference(const vpImage< unsigned char > &I)
void set_j(const double jj)
Definition: vpImagePoint.h:165
static void createSubImage(const vpImage< Type > &I, unsigned int i_sub, unsigned int j_sub, unsigned int nrow_sub, unsigned int ncol_sub, vpImage< Type > &S)
Definition: vpImageTools.h:132
std::vector< unsigned int > matchedReferencePoints
unsigned int getHeight() const
Definition: vpImage.h:152
Defines a rectangle in the plane.
Definition: vpRect.h:81
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
double getLeft() const
Definition: vpRect.h:157
virtual ~vpKeyPointSurf()