43 #include <visp/vpKeyPointSurf.h>
45 #if VISP_HAVE_OPENCV_VERSION >= 0x010100 // Require opencv >= 1.1.0
47 #include <visp/vpImageConvert.h>
48 #include <visp/vpImageTools.h>
49 #include <visp/vpDisplay.h>
50 #include <visp/vpDebug.h>
59 double compareSURFDescriptors(
const float* d1,
const float* d2,
60 double best,
int length )
62 double total_cost = 0;
64 assert( length % 4 == 0 );
65 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 )
80 int naiveNearestNeighbor(
const float *vec,
int laplacian,
81 const CvSeq *model_keypoints,
82 const CvSeq *model_descriptors )
84 int length = (int)(model_descriptors->elem_size/(
int)
sizeof(float));
86 double d, dist1 = 1e6, dist2 = 1e6;
87 CvSeqReader reader, kreader;
88 cvStartReadSeq( model_keypoints, &kreader, 0 );
89 cvStartReadSeq( model_descriptors, &reader, 0 );
91 for( i = 0; i < model_descriptors->total; i++ )
93 const CvSURFPoint* kp = (
const CvSURFPoint*)kreader.ptr;
94 const float* mvec = (
const float*)reader.ptr;
95 CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
96 CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
97 if( laplacian != kp->laplacian )
99 d = compareSURFDescriptors( vec, mvec, dist2, length );
106 else if ( d < dist2 )
109 if ( dist1 < 0.6*dist2 )
116 int naiveNearestNeighbor(
const float *vec,
117 const CvSeq *ref_keypoints,
118 const CvSeq *ref_descriptors )
120 int length = (int)(ref_descriptors->elem_size/(
int)
sizeof(float));
121 int i, neighbor = -1;
122 double d, dist1 = 1e6, dist2 = 1e6;
123 CvSeqReader reader, kreader;
124 cvStartReadSeq( ref_keypoints, &kreader, 0 );
125 cvStartReadSeq( ref_descriptors, &reader, 0 );
127 for( i = 0; i < ref_descriptors->total; i++ )
129 const float* mvec = (
const float*)reader.ptr;
130 CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
131 CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
132 d = compareSURFDescriptors( vec, mvec, dist2, length );
139 else if ( d < dist2 )
142 if ( dist1 < 0.6*dist2 )
150 void findPairs(
const CvSeq* objectKeypoints,
151 const CvSeq* objectDescriptors,
152 const CvSeq* imageKeypoints,
153 const CvSeq* imageDescriptors,
154 std::vector<int>& ptpairs )
157 CvSeqReader reader, kreader;
158 cvStartReadSeq( objectKeypoints, &kreader );
159 cvStartReadSeq( objectDescriptors, &reader );
162 for( i = 0; i < objectDescriptors->total; i++ )
164 const CvSURFPoint* kp = (
const CvSURFPoint*)kreader.ptr;
165 const float* descriptor = (
const float*)reader.ptr;
166 CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
167 CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
168 int nearest_neighbor = naiveNearestNeighbor( descriptor,
172 if( nearest_neighbor >= 0 )
174 ptpairs.push_back(i);
175 ptpairs.push_back(nearest_neighbor);
192 hessianThreshold = 500;
195 image_keypoints = NULL;
196 image_descriptors = NULL;
197 ref_keypoints = NULL;
198 ref_descriptors = NULL;
206 void vpKeyPointSurf::init()
208 #if (VISP_HAVE_OPENCV_VERSION >= 0x020400) // Require opencv >= 2.4.0
209 cv::initModule_nonfree();
212 storage = cvCreateMemStorage(0);
213 params = cvSURFParams(hessianThreshold, descriptorType);
222 cvReleaseMemStorage(&storage);
223 if(storage_cur!=NULL){
224 cvReleaseMemStorage(&storage_cur);
239 IplImage* model = NULL;
244 CvSize size = cvSize(width, height);
245 model = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
246 model->imageData = (
char*)I.
bitmap;
251 cvExtractSURF( model, 0, &ref_keypoints, &ref_descriptors, storage, params );
253 const unsigned int nbPoints = (
unsigned int)ref_keypoints->total;
257 for(
unsigned int i = 0; i < nbPoints; i++ )
259 CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem(ref_keypoints, (
int)i);
266 model->imageData = NULL;
267 cvReleaseImageHeader(&model);
269 cvReleaseImage(&model);
296 const unsigned int height,
const unsigned int width)
301 vpTRACE(
"Bad size for the subimage");
303 "Bad size for the subimage"));
309 (
unsigned int)iP.
get_i(),
310 (
unsigned int)iP.
get_j(),
311 height, width, subImage);
315 for(
unsigned int k = 0; k < nbRefPoint; k++)
347 (
unsigned int)rectangle.
getWidth()));
363 IplImage* currentImage = NULL;
368 CvSize size = cvSize(width, height);
369 currentImage = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
370 currentImage->imageData = (
char*)I.
bitmap;
377 if(storage_cur != NULL){
378 cvReleaseMemStorage(&storage_cur);
381 storage_cur = cvCreateMemStorage(0);
383 cvExtractSURF( currentImage, 0, &image_keypoints, &image_descriptors,
384 storage_cur, params );
386 CvSeqReader reader, kreader;
387 cvStartReadSeq( ref_keypoints, &kreader );
388 cvStartReadSeq( ref_descriptors, &reader );
391 std::list<int> indexImagePair;
392 std::list<int> indexReferencePair;
395 unsigned int nbrPair = 0;
397 for(
int i = 0; i < ref_descriptors->total; i++ )
399 const CvSURFPoint* kp = (
const CvSURFPoint*)kreader.ptr;
400 const float* descriptor = (
const float*)reader.ptr;
401 CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
402 CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
403 int nearest_neighbor = naiveNearestNeighbor( descriptor,
407 if( nearest_neighbor >= 0 )
409 indexReferencePair.push_back(i);
410 indexImagePair.push_back(nearest_neighbor);
415 std::list<int>::const_iterator indexImagePairIter = indexImagePair.begin();
416 std::list<int>::const_iterator indexReferencePairIter = indexReferencePair.begin();
426 for (
unsigned int i = 0; i < nbrPair; i++)
428 int index = *indexImagePairIter;
430 CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem(image_keypoints, index);
438 ++indexImagePairIter;
439 ++indexReferencePairIter;
443 currentImage->imageData = NULL;
444 cvReleaseImageHeader(¤tImage);
446 cvReleaseImage(¤tImage);
473 const unsigned int height,
const unsigned int width)
478 vpTRACE(
"Bad size for the subimage");
480 "Bad size for the subimage"));
486 (
unsigned int)iP.
get_i(),
487 (
unsigned int)iP.
get_j(),
488 height, width, subImage);
490 unsigned int nbMatchedPoint = this->
matchPoint(subImage);
492 for(
unsigned int k = 0; k < nbMatchedPoint; k++)
500 return(nbMatchedPoint);
526 (
unsigned int)rectangle.
getWidth()));
595 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
619 if(descriptorList.
nb != laplacianList.
nb){
620 vpTRACE(
"Error, the two lists have different number of element");
625 cvStartReadSeq( ref_descriptors, &reader );
627 descriptorList.
front();
629 laplacianList.
front();
631 while(!descriptorList.
outside()){
632 float* descriptor = descriptorList.
value();
634 int nearest_neighbor = naiveNearestNeighbor( descriptor, laplacianList.
value(), ref_keypoints, ref_descriptors);
636 if(nearest_neighbor >= 0){
639 tab[0] = nearest_neighbor;
644 descriptorList.
next();
645 laplacianList.
next();
654 std::list<int*>* pairPoints =
new std::list<int*>;
656 if(descriptorList.size() != laplacianList.size()){
657 vpTRACE(
"Error, the two lists have different number of element");
662 cvStartReadSeq( ref_descriptors, &reader );
664 std::list<float*>::const_iterator descriptorListIter = descriptorList.begin();
665 std::list<int>::const_iterator laplacianListIter = laplacianList.begin();
666 descriptorList.front();
668 while(descriptorListIter != descriptorList.end()){
669 float* descriptor = *descriptorListIter;
671 int nearest_neighbor = naiveNearestNeighbor( descriptor, *laplacianListIter, ref_keypoints, ref_descriptors);
673 if(nearest_neighbor >= 0){
676 tab[0] = nearest_neighbor;
678 pairPoints->push_back(tab);
681 ++descriptorListIter;
696 vpTRACE(
"Index of the reference point out of range");
700 float* descriptor = NULL;
703 cvStartReadSeq( ref_descriptors, &reader );
705 for(
int j = 0; j < ref_descriptors->total; j++ ){
707 descriptor = (
float*)reader.ptr;
710 CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
724 vpTRACE(
"Index of the reference point out of range");
729 cvStartReadSeq( ref_keypoints, &reader );
733 for(
int j = 0; j < ref_keypoints->total; j++ ){
735 const CvSURFPoint* kp = (
const CvSURFPoint*)reader.ptr;
736 laplacian = kp->laplacian;
739 CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
755 vpTRACE(
"Index of the reference point out of range");
760 cvStartReadSeq( ref_keypoints, &reader );
762 for(
int j = 0; j < ref_keypoints->total; j++ ){
764 const CvSURFPoint* kp = (
const CvSURFPoint*)reader.ptr;
769 CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
void set_j(const double j)
class that defines what is a Keypoint. This class provides all the basic elements to implement classe...
void getDescriptorParamReferencePoint(const int index, int &size, float &dir)
bool outside(void) const
Test if the current element is outside the list (on the virtual element)
unsigned int getWidth() const
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Provide simple list management.
Type * bitmap
points toward the bitmap
Class to define colors available for display functionnalities.
int getLaplacianReferencePoint(const int index)
void set_i(const double i)
unsigned int matchPoint(const vpImage< unsigned char > &I)
void display(const vpImage< unsigned char > &Iref, const vpImage< unsigned char > &Icurrent, unsigned int size=3)
static const vpColor green
void next(void)
position the current element on the next one
void front(void)
Position the current element on the first element of the list.
float * getDescriptorReferencePoint(const int index)
type & value(void)
return the value of the current element
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
std::vector< vpImagePoint > referenceImagePointsList
unsigned int buildReference(const vpImage< unsigned char > &I)
std::vector< unsigned int > matchedReferencePoints
unsigned int getHeight() const
Defines a rectangle in the plane.
std::vector< vpImagePoint > currentImagePointsList
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
virtual ~vpKeyPointSurf()