ViSP  2.9.0
vpKeyPointSurf.cpp
1 /****************************************************************************
2  *
3  * $Id: vpKeyPointSurf.cpp 4649 2014-02-07 14:57:11Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Key point Surf.
36  *
37  * Authors:
38  * Nicolas Melchior
39  *
40  *****************************************************************************/
41 
42 
43 #include <visp/vpKeyPointSurf.h>
44 
45 #if defined (VISP_HAVE_OPENCV_NONFREE)
46 #if VISP_HAVE_OPENCV_VERSION >= 0x010100 // Require opencv >= 1.1.0
47 
48 #include <visp/vpImageConvert.h>
49 #include <visp/vpImageTools.h>
50 #include <visp/vpDisplay.h>
51 #include <visp/vpDebug.h>
52 
53 #include <ctype.h>
54 #include <stdio.h>
55 #include <stdlib.h>
56 #include <vector>
57 #include <list>
58 
59 double compareSURFDescriptors( const float* d1, const float* d2, double best, int length );
60 int naiveNearestNeighbor( const float *vec, int laplacian,
61  const CvSeq *model_keypoints,
62  const CvSeq *model_descriptors );
63 int naiveNearestNeighbor( const float *vec,
64  const CvSeq *ref_keypoints,
65  const CvSeq *ref_descriptors );
66 void findPairs( const CvSeq* objectKeypoints,
67  const CvSeq* objectDescriptors,
68  const CvSeq* imageKeypoints,
69  const CvSeq* imageDescriptors,
70  std::vector<int>& ptpairs );
71 
72 // Compare two surf descriptors.
73 double compareSURFDescriptors( const float* d1, const float* d2, double best, int length )
74 {
75  double total_cost = 0;
76  int i;
77  assert( length % 4 == 0 );
78  for( i = 0; i < length; i += 4 )
79  {
80  double t0 = d1[i] - d2[i];
81  double t1 = d1[i+1] - d2[i+1];
82  double t2 = d1[i+2] - d2[i+2];
83  double t3 = d1[i+3] - d2[i+3];
84  total_cost += t0*t0 + t1*t1 + t2*t2 + t3*t3;
85  if( total_cost > best )
86  break;
87  }
88  return total_cost;
89 }
90 
91 
92 //Find for a point candidate the most similar point in the reference point list.
93 int naiveNearestNeighbor( const float *vec, int laplacian,
94  const CvSeq *model_keypoints,
95  const CvSeq *model_descriptors )
96 {
97  int length = (int)(model_descriptors->elem_size/(int)sizeof(float));
98  int i, neighbor = -1;
99  double d, dist1 = 1e6, dist2 = 1e6;
100  CvSeqReader reader, kreader;
101  cvStartReadSeq( model_keypoints, &kreader, 0 );
102  cvStartReadSeq( model_descriptors, &reader, 0 );
103 
104  for( i = 0; i < model_descriptors->total; i++ )
105  {
106  const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
107  const float* mvec = (const float*)reader.ptr;
108  CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
109  CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
110  if( laplacian != kp->laplacian )
111  continue;
112  d = compareSURFDescriptors( vec, mvec, dist2, length );
113  if( d < dist1 )
114  {
115  dist2 = dist1;
116  dist1 = d;
117  neighbor = i;
118  }
119  else if ( d < dist2 )
120  dist2 = d;
121  }
122  if ( dist1 < 0.6*dist2 )
123  return neighbor;
124  return -1;
125 }
126 
127 
128 //Find for a point candidate the most similar point in the reference point list.
129 int naiveNearestNeighbor( const float *vec,
130  const CvSeq *ref_keypoints,
131  const CvSeq *ref_descriptors )
132 {
133  int length = (int)(ref_descriptors->elem_size/(int)sizeof(float));
134  int i, neighbor = -1;
135  double d, dist1 = 1e6, dist2 = 1e6;
136  CvSeqReader reader, kreader;
137  cvStartReadSeq( ref_keypoints, &kreader, 0 );
138  cvStartReadSeq( ref_descriptors, &reader, 0 );
139 
140  for( i = 0; i < ref_descriptors->total; i++ )
141  {
142  const float* mvec = (const float*)reader.ptr;
143  CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
144  CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
145  d = compareSURFDescriptors( vec, mvec, dist2, length );
146  if( d < dist1 )
147  {
148  dist2 = dist1;
149  dist1 = d;
150  neighbor = i;
151  }
152  else if ( d < dist2 )
153  dist2 = d;
154  }
155  if ( dist1 < 0.6*dist2 )
156  return neighbor;
157  return -1;
158 }
159 
160 
161 
162 //Find all the matched points
163 void findPairs( const CvSeq* objectKeypoints,
164  const CvSeq* objectDescriptors,
165  const CvSeq* imageKeypoints,
166  const CvSeq* imageDescriptors,
167  std::vector<int>& ptpairs )
168 {
169  int i;
170  CvSeqReader reader, kreader;
171  cvStartReadSeq( objectKeypoints, &kreader );
172  cvStartReadSeq( objectDescriptors, &reader );
173  ptpairs.clear();
174 
175  for( i = 0; i < objectDescriptors->total; i++ )
176  {
177  const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
178  const float* descriptor = (const float*)reader.ptr;
179  CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
180  CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
181  int nearest_neighbor = naiveNearestNeighbor( descriptor,
182  kp->laplacian,
183  imageKeypoints,
184  imageDescriptors );
185  if( nearest_neighbor >= 0 )
186  {
187  ptpairs.push_back(i);
188  ptpairs.push_back(nearest_neighbor);
189  }
190  }
191 }
192 
193 
194 
203  : vpBasicKeyPoint(),
204  storage(NULL), params(), storage_cur(NULL), image_keypoints(NULL), image_descriptors(NULL),
205  ref_keypoints(NULL), ref_descriptors(NULL), hessianThreshold(500), descriptorType(extendedDescriptor)
206 {
207  init();
208 }
209 
213 void vpKeyPointSurf::init()
214 {
215 #if (VISP_HAVE_OPENCV_VERSION >= 0x020400) // Require opencv >= 2.4.0
216  cv::initModule_nonfree();
217 #endif
218 
219  storage = cvCreateMemStorage(0);
220  params = cvSURFParams(hessianThreshold, descriptorType);
221 }
222 
228 {
229  cvReleaseMemStorage(&storage);
230  if(storage_cur!=NULL){
231  cvReleaseMemStorage(&storage_cur);
232  }
233 }
234 
245 {
246  IplImage* model = NULL;
247 
248  if((I.getWidth() % 8) == 0){
249  int height = (int)I.getHeight();
250  int width = (int)I.getWidth();
251  CvSize size = cvSize(width, height);
252  model = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
253  model->imageData = (char*)I.bitmap;
254  }else{
255  vpImageConvert::convert(I,model);
256  }
257 
258  cvExtractSURF( model, 0, &ref_keypoints, &ref_descriptors, storage, params );
259 
260  const unsigned int nbPoints = (unsigned int)ref_keypoints->total;
261 
262  referenceImagePointsList.resize(nbPoints);
263 
264  for(unsigned int i = 0; i < nbPoints; i++ )
265  {
266  CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem(ref_keypoints, (int)i);
267 
268  referenceImagePointsList[i].set_i(r1->pt.y);
269  referenceImagePointsList[i].set_j(r1->pt.x);
270  }
271 
272  if((I.getWidth() % 8) == 0){
273  model->imageData = NULL;
274  cvReleaseImageHeader(&model);
275  }else{
276  cvReleaseImage(&model);
277  }
278 
279  _reference_computed = true;
280  return nbPoints;
281 }
282 
283 
302  const vpImagePoint &iP,
303  const unsigned int height, const unsigned int width)
304 {
305  if((iP.get_i()+height) >= I.getHeight()
306  || (iP.get_j()+width) >= I.getWidth())
307  {
308  vpTRACE("Bad size for the subimage");
310  "Bad size for the subimage"));
311  }
312 
313  vpImage<unsigned char> subImage;
314 
316  (unsigned int)iP.get_i(),
317  (unsigned int)iP.get_j(),
318  height, width, subImage);
319 
320  unsigned int nbRefPoint = this->buildReference(subImage);
321 
322  for(unsigned int k = 0; k < nbRefPoint; k++)
323  {
324  (referenceImagePointsList[k]).set_i((referenceImagePointsList[k]).get_i()
325  + iP.get_i());
326  (referenceImagePointsList[k]).set_j((referenceImagePointsList[k]).get_j()
327  + iP.get_j());
328  }
329  return(nbRefPoint);
330 }
331 
332 
347  const vpRect& rectangle)
348 {
349  vpImagePoint iP;
350  iP.set_i(rectangle.getTop());
351  iP.set_j(rectangle.getLeft());
352  return (this->buildReference(I, iP,
353  (unsigned int)rectangle.getHeight(),
354  (unsigned int)rectangle.getWidth()));
355 }
356 
357 
369 {
370  IplImage* currentImage = NULL;
371 
372  if((I.getWidth() % 8) == 0){
373  int height = (int)I.getHeight();
374  int width = (int)I.getWidth();
375  CvSize size = cvSize(width, height);
376  currentImage = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
377  currentImage->imageData = (char*)I.bitmap;
378  }else{
379  vpImageConvert::convert(I,currentImage);
380  }
381 
382  /* we release the memory storage for the current points (it has to be kept
383  allocated for the get descriptor points, ...) */
384  if(storage_cur != NULL){
385  cvReleaseMemStorage(&storage_cur);
386  storage_cur = NULL;
387  }
388  storage_cur = cvCreateMemStorage(0);
389 
390  cvExtractSURF( currentImage, 0, &image_keypoints, &image_descriptors,
391  storage_cur, params );
392 
393  CvSeqReader reader, kreader;
394  cvStartReadSeq( ref_keypoints, &kreader );
395  cvStartReadSeq( ref_descriptors, &reader );
396 
397 
398  std::list<int> indexImagePair;
399  std::list<int> indexReferencePair;
400 
401 
402  unsigned int nbrPair = 0;
403 
404  for(int i = 0; i < ref_descriptors->total; i++ )
405  {
406  const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
407  const float* descriptor = (const float*)reader.ptr;
408  CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
409  CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
410  int nearest_neighbor = naiveNearestNeighbor( descriptor,
411  kp->laplacian,
412  image_keypoints,
413  image_descriptors );
414  if( nearest_neighbor >= 0 )
415  {
416  indexReferencePair.push_back(i);
417  indexImagePair.push_back(nearest_neighbor);
418  nbrPair++;
419  }
420  }
421 
422  std::list<int>::const_iterator indexImagePairIter = indexImagePair.begin();
423  std::list<int>::const_iterator indexReferencePairIter = indexReferencePair.begin();
424 
425  matchedReferencePoints.resize(0);
426 
427  if (nbrPair == 0)
428  return (0);
429 
430  currentImagePointsList.resize(nbrPair);
431  matchedReferencePoints.resize(nbrPair);
432 
433  for (unsigned int i = 0; i < nbrPair; i++)
434  {
435  int index = *indexImagePairIter;
436 
437  CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem(image_keypoints, index);
438 
439  currentImagePointsList[i].set_i(r1->pt.y);
440  currentImagePointsList[i].set_j(r1->pt.x);
441 
442  matchedReferencePoints[i] = (unsigned int)*indexReferencePairIter;
443 
444 
445  ++indexImagePairIter;
446  ++indexReferencePairIter;
447  }
448 
449  if((I.getWidth() % 8) == 0){
450  currentImage->imageData = NULL;
451  cvReleaseImageHeader(&currentImage);
452  }else{
453  cvReleaseImage(&currentImage);
454  }
455 
456  return nbrPair;
457 }
458 
459 
479  const vpImagePoint &iP,
480  const unsigned int height, const unsigned int width)
481 {
482  if((iP.get_i()+height) >= I.getHeight()
483  || (iP.get_j()+width) >= I.getWidth())
484  {
485  vpTRACE("Bad size for the subimage");
487  "Bad size for the subimage"));
488  }
489 
490  vpImage<unsigned char> subImage;
491 
493  (unsigned int)iP.get_i(),
494  (unsigned int)iP.get_j(),
495  height, width, subImage);
496 
497  unsigned int nbMatchedPoint = this->matchPoint(subImage);
498 
499  for(unsigned int k = 0; k < nbMatchedPoint; k++)
500  {
501  (currentImagePointsList[k]).set_i((currentImagePointsList[k]).get_i()
502  + iP.get_i());
503  (currentImagePointsList[k]).set_j((currentImagePointsList[k]).get_j()
504  + iP.get_j());
505  }
506 
507  return(nbMatchedPoint);
508 }
509 
510 
526  const vpRect& rectangle)
527 {
528  vpImagePoint iP;
529  iP.set_i(rectangle.getTop());
530  iP.set_j(rectangle.getLeft());
531  return (this->matchPoint(I, iP,
532  (unsigned int)rectangle.getHeight(),
533  (unsigned int)rectangle.getWidth()));
534 }
535 
536 
556  const vpImage<unsigned char> &Icurrent, unsigned int size)
557 {
558 // matchedPointsCurrentImageList.front();
559 // matchedPointsReferenceImageList.front();
560 
561 // if (matchedPointsCurrentImageList.nbElements()
562 // != matchedPointsReferenceImageList.nbElements())
563 // {
564 // vpTRACE("Numbers of points mismatch");
565 // throw(vpException(vpException::fatalError,"Numbers of points mismatch"));
566 // }
567 
568  for (unsigned int i = 0; i < matchedReferencePoints.size(); i++)
569  {
572 // matchedPointsReferenceImageList.next();
573 // matchedPointsCurrentImageList.next();
574  }
575 }
576 
577 
590 void vpKeyPointSurf::display(const vpImage<unsigned char> &Icurrent, unsigned int size, const vpColor &color)
591 {
592 // matchedPointsCurrentImageList.front();
593 //
594 // vpImagePoint ipCur;
595 //
596  for (unsigned int i = 0; i < matchedReferencePoints.size(); i++)
597  {
598  vpDisplay::displayCross (Icurrent, currentImagePointsList[i], size, color);
599  }
600 }
601 
602 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
603 
622 vp_deprecated vpList<int*>* vpKeyPointSurf::matchPoint(vpList<float*> descriptorList, vpList<int> laplacianList)
623 {
624  vpList<int*>* pairPoints = new vpList<int*>;
625 
626  if(descriptorList.nb != laplacianList.nb){
627  vpTRACE("Error, the two lists have different number of element");
628  return pairPoints;
629  }
630 
631  CvSeqReader reader;
632  cvStartReadSeq( ref_descriptors, &reader );
633 
634  descriptorList.front();
635  pairPoints->front();
636  laplacianList.front();
637  int indexList = 0;
638  while(!descriptorList.outside()){
639  float* descriptor = descriptorList.value();
640 
641  int nearest_neighbor = naiveNearestNeighbor( descriptor, laplacianList.value(), ref_keypoints, ref_descriptors);
642 
643  if(nearest_neighbor >= 0){
644  int* tab;
645  tab = new int[2];
646  tab[0] = nearest_neighbor;
647  tab[1] = indexList;
648  pairPoints->addRight(tab);
649  }
650  indexList++;
651  descriptorList.next();
652  laplacianList.next();
653  }
654 
655  return pairPoints;
656 }
657 #endif
658 
659 std::list<int*>* vpKeyPointSurf::matchPoint(std::list<float*> descriptorList, std::list<int> laplacianList)
660 {
661  std::list<int*>* pairPoints = new std::list<int*>;
662 
663  if(descriptorList.size() != laplacianList.size()){
664  vpTRACE("Error, the two lists have different number of element");
665  return pairPoints;
666  }
667 
668  CvSeqReader reader;
669  cvStartReadSeq( ref_descriptors, &reader );
670 
671  std::list<float*>::const_iterator descriptorListIter = descriptorList.begin();
672  std::list<int>::const_iterator laplacianListIter = laplacianList.begin();
673  descriptorList.front();
674  int indexList = 0;
675  while(descriptorListIter != descriptorList.end()){
676  float* descriptor = *descriptorListIter;
677 
678  int nearest_neighbor = naiveNearestNeighbor( descriptor, *laplacianListIter, ref_keypoints, ref_descriptors);
679 
680  if(nearest_neighbor >= 0){
681  int* tab;
682  tab = new int[2];
683  tab[0] = nearest_neighbor;
684  tab[1] = indexList;
685  pairPoints->push_back(tab);
686  }
687  indexList++;
688  ++descriptorListIter;
689  ++laplacianListIter;
690  }
691 
692  return pairPoints;
693 }
694 
701 {
702  if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0){
703  vpTRACE("Index of the reference point out of range");
704  throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
705  }
706 
707  float* descriptor = NULL;
708 
709  CvSeqReader reader;
710  cvStartReadSeq( ref_descriptors, &reader );
711 
712  for(int j = 0; j < ref_descriptors->total; j++ ){
713  if(j== index){
714  descriptor = (float*)reader.ptr;
715  break;
716  }
717  CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
718  }
719 
720  return descriptor;
721 }
722 
729 {
730  if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0){
731  vpTRACE("Index of the reference point out of range");
732  throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
733  }
734 
735  CvSeqReader reader;
736  cvStartReadSeq( ref_keypoints, &reader );
737 
738  int laplacian = 0;/* normally only -1, 0, +1 are possible */
739 
740  for(int j = 0; j < ref_keypoints->total; j++ ){
741  if(j== index){
742  const CvSURFPoint* kp = (const CvSURFPoint*)reader.ptr;
743  laplacian = kp->laplacian;
744  break;
745  }
746  CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
747  }
748 
749  return laplacian;
750 }
751 
759 void vpKeyPointSurf::getDescriptorParamReferencePoint (const int index, int& size, float& dir)
760 {
761  if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0){
762  vpTRACE("Index of the reference point out of range");
763  throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
764  }
765 
766  CvSeqReader reader;
767  cvStartReadSeq( ref_keypoints, &reader );
768 
769  for(int j = 0; j < ref_keypoints->total; j++ ){
770  if(j== index){
771  const CvSURFPoint* kp = (const CvSURFPoint*)reader.ptr;
772  size = kp->size;
773  dir = kp->dir;
774  break;
775  }
776  CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
777  }
778 }
779 
780 
781 #endif
782 #endif
783 
class that defines what is a Keypoint. This class provides all the basic elements to implement classe...
double getTop() const
Definition: vpRect.h:174
void getDescriptorParamReferencePoint(const int index, int &size, float &dir)
double get_i() const
Definition: vpImagePoint.h:194
bool outside(void) const
Test if the current element is outside the list (on the virtual element)
Definition: vpList.h:429
unsigned int getWidth() const
Definition: vpImage.h:159
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
#define vpTRACE
Definition: vpDebug.h:418
Provide simple list management.
Definition: vpList.h:113
Type * bitmap
points toward the bitmap
Definition: vpImage.h:120
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
int getLaplacianReferencePoint(const int index)
error that can be emited by ViSP classes.
Definition: vpException.h:76
unsigned int matchPoint(const vpImage< unsigned char > &I)
double getHeight() const
Definition: vpRect.h:155
void display(const vpImage< unsigned char > &Iref, const vpImage< unsigned char > &Icurrent, unsigned int size=3)
static const vpColor green
Definition: vpColor.h:170
double get_j() const
Definition: vpImagePoint.h:205
void next(void)
position the current element on the next one
Definition: vpList.h:273
static const vpColor red
Definition: vpColor.h:167
void set_i(const double ii)
Definition: vpImagePoint.h:158
double getWidth() const
Definition: vpRect.h:193
unsigned int nb
Definition: vpList.h:119
void front(void)
Position the current element on the first element of the list.
Definition: vpList.h:384
float * getDescriptorReferencePoint(const int index)
type & value(void)
return the value of the current element
Definition: vpList.h:301
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 addRight(const type &el)
add a new element in the list, at the right of the current one
Definition: vpList.h:478
std::vector< vpImagePoint > referenceImagePointsList
unsigned int buildReference(const vpImage< unsigned char > &I)
void set_j(const double jj)
Definition: vpImagePoint.h:169
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:133
std::vector< unsigned int > matchedReferencePoints
unsigned int getHeight() const
Definition: vpImage.h:150
Defines a rectangle in the plane.
Definition: vpRect.h:85
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:92
double getLeft() const
Definition: vpRect.h:161
virtual ~vpKeyPointSurf()