Visual Servoing Platform version 3.5.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
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
54double compareSURFDescriptors(const float *d1, const float *d2, double best, int length);
55int naiveNearestNeighbor(const float *vec, int laplacian, const CvSeq *model_keypoints, const CvSeq *model_descriptors);
56int naiveNearestNeighbor(const float *vec, const CvSeq *ref_keypoints, const CvSeq *ref_descriptors);
57void findPairs(const CvSeq *objectKeypoints, const CvSeq *objectDescriptors, const CvSeq *imageKeypoints,
58 const CvSeq *imageDescriptors, std::vector<int> &ptpairs);
59
60// Compare two surf descriptors.
61double 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.
80int 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.
111int 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
138void 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
167vpKeyPointSurf::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
177void 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
191vpKeyPointSurf::~vpKeyPointSurf()
192{
193 cvReleaseMemStorage(&storage);
194 if (storage_cur != NULL) {
195 cvReleaseMemStorage(&storage_cur);
196 }
197}
198
208unsigned 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
263unsigned 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
297unsigned 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
315unsigned 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
414unsigned 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
450unsigned 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
477void 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++) {
492 vpDisplay::displayCross(Ireference, referenceImagePointsList[matchedReferencePoints[i]], size, vpColor::red);
493 vpDisplay::displayCross(Icurrent, currentImagePointsList[i], size, vpColor::green);
494 // matchedPointsReferenceImageList.next();
495 // matchedPointsCurrentImageList.next();
496 }
497}
498
512void 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
523std::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
565float *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
594int 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
626void 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
650void dummy_vpKeyPointSurf(){};
651#endif
class that defines what is a Keypoint. This class provides all the basic elements to implement classe...
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static const vpColor green
Definition: vpColor.h:220
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ fatalError
Fatal error.
Definition: vpException.h:96
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void set_j(double jj)
Definition: vpImagePoint.h:177
double get_j() const
Definition: vpImagePoint.h:214
void set_i(double ii)
Definition: vpImagePoint.h:166
double get_i() const
Definition: vpImagePoint.h:203
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
unsigned int getWidth() const
Definition: vpImage.h:246
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
unsigned int getHeight() const
Definition: vpImage.h:188
void init(unsigned int h, unsigned int w, Type value)
Definition: vpImage.h:631
Defines a rectangle in the plane.
Definition: vpRect.h:80
double getWidth() const
Definition: vpRect.h:228
double getLeft() const
Definition: vpRect.h:174
double getHeight() const
Definition: vpRect.h:167
double getTop() const
Definition: vpRect.h:193
#define vpTRACE
Definition: vpDebug.h:416
Class that implements the SURF key points and technics thanks to the OpenCV library.