ViSP  2.9.0
vpKltOpencv.cpp
1 /****************************************************************************
2  *
3  * $Id: vpKltOpencv.cpp 4632 2014-02-03 17:06:40Z 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  * Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented
36  * with opencv.
37  *
38  * Authors:
39  * Fabien Servant
40  * Fabien Spindler
41  *
42  *****************************************************************************/
43 
51 #include "visp/vpConfig.h"
52 
53 #ifdef VISP_HAVE_OPENCV
54 
55 #include <string>
56 
57 #include "vpKltOpencv.h"
58 
59 void vpKltOpencv::clean()
60 {
61  if (image) cvReleaseImage(&image);
62  if (prev_image) cvReleaseImage(&prev_image);
63  if (pyramid) cvReleaseImage(&pyramid);
64  if (prev_pyramid) cvReleaseImage(&prev_pyramid);
65 
66  image = NULL;
67  prev_image = NULL;
68  pyramid = NULL;
69  prev_pyramid = NULL;
70 
71  swap_temp = NULL;
72  countFeatures = 0;
73  countPrevFeatures = 0;
74  flags = 0;
75  initialized = 0;
76  globalcountFeatures = 0;
77 }
78 
79 void vpKltOpencv::cleanAll()
80 {
81  clean();
82  if (features) cvFree(&features);
83  if (prev_features) cvFree(&prev_features);
84  if (status) cvFree(&status);
85  if (lostDuringTrack) cvFree(&lostDuringTrack);
86  if (featuresid) cvFree(&featuresid);
87  if (prev_featuresid) cvFree(&prev_featuresid);
88  features = NULL;
89  prev_features = NULL;
90  status = NULL;
91  lostDuringTrack = 0;
92  featuresid = NULL;
93  prev_featuresid = NULL;
94 }
95 
96 void vpKltOpencv::reset()
97 {
98  clean();
99 
100 }
101 
106  : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01),
107  min_distance(10), harris_free_parameter(0.04), block_size(3), use_harris(1),
108  pyramid_level(3), _tid(-1), image(NULL), prev_image(NULL), pyramid(NULL),
109  prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
110  features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL),
111  flags(0), initial_guess(false), lostDuringTrack(0), status(0), OnInitialize(0),
112  OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0), IsFeatureValid(0)
113 {
114  features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(features[0]));
115  prev_features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(prev_features[0]));
116  status = (char*)cvAlloc((size_t)maxFeatures);
117  lostDuringTrack = (bool*)cvAlloc((size_t)maxFeatures);
118  featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
119  prev_featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
120 }
121 
126  : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01),
127  min_distance(10), harris_free_parameter(0.04), block_size(3), use_harris(1),
128  pyramid_level(3), _tid(-1), image(NULL), prev_image(NULL), pyramid(NULL),
129  prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
130  features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL),
131  flags(0), initial_guess(false), lostDuringTrack(0), status(0), OnInitialize(0),
132  OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0), IsFeatureValid(0)
133 {
134  *this = copy;
135 }
136 
141 {
142  //Shallow copy of primitives
143  initialized = copy.initialized;
144  maxFeatures = copy.maxFeatures;
145  countFeatures = copy.countFeatures;
146  countPrevFeatures = copy.countPrevFeatures;
147  globalcountFeatures = copy.globalcountFeatures;
148  flags = copy.flags;
149  win_size = copy.win_size;
150  quality = copy.quality;
151  min_distance = copy.min_distance;
152  harris_free_parameter = copy.harris_free_parameter;
153  block_size = copy.block_size;
154  use_harris = copy.use_harris;
155  pyramid_level = copy.pyramid_level;
156  _tid = copy._tid;
157 
158  OnInitialize = copy.OnInitialize;
159  OnFeatureLost = copy.OnFeatureLost;
160  OnNewFeature = copy.OnNewFeature;
161  OnMeasureFeature = copy.OnMeasureFeature;
162  IsFeatureValid = copy.IsFeatureValid;
163 
164  initial_guess = copy.initial_guess;
165  lostDuringTrack = copy.lostDuringTrack;
166 
167  if (!initialized) {
168  status = 0;
169  lostDuringTrack = 0;
170  countFeatures = 0;
171  countPrevFeatures = 0;
172  flags = 0;
173  initialized = 0;
174  globalcountFeatures = 0;
175  }
176 
177  if (copy.image)
178  {
179  image = cvCreateImage(cvGetSize(copy.image), 8, 1);
180  // /*IplImage **/cvCopyImage(copy.image,image);
181  cvCopy(copy.image, image, 0);
182  }
183 
184  if (copy.prev_image)
185  {
186  prev_image = cvCreateImage(cvGetSize(copy.prev_image), IPL_DEPTH_8U, 1);
187  // /*IplImage **/ cvCopyImage(copy.prev_image,prev_image);
188  cvCopy(copy.prev_image, prev_image, 0);
189  }
190 
191  if (copy.pyramid)
192  {
193  pyramid = cvCreateImage(cvGetSize(copy.pyramid), IPL_DEPTH_8U, 1);
194  // /*IplImage **/cvCopyImage(copy.pyramid,pyramid);
195  cvCopy(copy.pyramid, pyramid, 0);
196  }
197 
198  if (copy.prev_pyramid)
199  {
200  prev_pyramid = cvCreateImage(cvGetSize(copy.prev_pyramid), IPL_DEPTH_8U, 1);
201  // /*IplImage **/cvCopyImage(copy.prev_pyramid,prev_pyramid);
202  cvCopy(copy.prev_pyramid, prev_pyramid, 0);
203  }
204 
205  //Deep copy of arrays
206  if (copy.features) {
207  /*CvPoint2D32f **/features =
208  (CvPoint2D32f*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(CvPoint2D32f));
209  for (int i = 0; i < copy.maxFeatures; i++)
210  features[i] = copy.features[i];
211  }
212 
213  if (copy.prev_features) {
214  /*CvPoint2D32f **/prev_features =
215  (CvPoint2D32f*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(CvPoint2D32f));
216  for (int i = 0; i < copy.maxFeatures; i++)
217  prev_features[i] = copy.prev_features[i];
218  }
219 
220  if (copy.featuresid) {
221  /*long **/featuresid = (long*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(long));
222  for (int i = 0; i < copy.maxFeatures; i++)
223  featuresid[i] = copy.featuresid[i];
224  }
225 
226  if (copy.prev_featuresid) {
227  /*long **/prev_featuresid = (long*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(long));
228  for (int i = 0; i < copy.maxFeatures; i++)
229  prev_featuresid[i] = copy.prev_featuresid[i];
230  }
231 
232  if (copy.status) {
233  /*char **/status = (char*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(char));
234  for (int i = 0; i < copy.maxFeatures; i++)
235  status[i] = copy.status[i];
236  }
237 
238  if (copy.lostDuringTrack) {
239  /*bool **/lostDuringTrack = (bool*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(bool));
240  for (int i = 0; i < copy.maxFeatures; i++)
241  lostDuringTrack[i] = copy.lostDuringTrack[i];
242  }
243 
244  return *this;
245 }
246 
248 {
249  cleanAll();
250 }
251 
259 void vpKltOpencv::setMaxFeatures(const int input) {
260  initialized = 0; maxFeatures=input;
261 
262  if (features) cvFree(&features);
263  if (prev_features) cvFree(&prev_features);
264  if (status) cvFree(&status);
265  if (lostDuringTrack) cvFree(&lostDuringTrack);
266  if (featuresid) cvFree(&featuresid);
267  if (prev_featuresid) cvFree(&prev_featuresid);
268 
269 
270  features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(CvPoint2D32f));
271  prev_features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(CvPoint2D32f));
272  status = (char*)cvAlloc((unsigned int)maxFeatures*sizeof(char));
273  lostDuringTrack = (bool*)cvAlloc((unsigned int)maxFeatures*sizeof(bool));
274  featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
275  prev_featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
276 }
277 
288 void vpKltOpencv::initTracking(const IplImage *I, const IplImage *mask)
289 {
290  if (!I) {
291  throw(vpException(vpTrackingException::initializationError, "Image Not initialized")) ;
292  }
293 
294  if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
295  throw(vpException(vpTrackingException::initializationError, "Bad Image format")) ;
296  }
297 
298  if (mask) {
299  if (mask->depth != IPL_DEPTH_8U || I->nChannels != 1) {
300  throw(vpException(vpTrackingException::initializationError, "Bad Image format")) ;
301  }
302  }
303 
304  //Creation des buffers
305  CvSize Sizeim, SizeI;
306  SizeI = cvGetSize(I);
307  bool b_imOK = true;
308  if(image != NULL){
309  Sizeim = cvGetSize(image);
310  if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
311  }
312  if(image == NULL || prev_image == NULL || pyramid==NULL || prev_pyramid ==NULL || !b_imOK){
313  reset();
314  image = cvCreateImage(cvGetSize(I), 8, 1);image->origin = I->origin;
315  prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
316  pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
317  prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
318  }else{
319  swap_temp = 0;
320  countFeatures = 0;
321  countPrevFeatures = 0;
322  flags = 0;
323  initialized = 0;
324  globalcountFeatures = 0;
325  }
326 
327  initialized = 1;
328 
329  //Import
330  cvCopy(I, image, 0);
331 
332  //Recherche de points d'int�rets
333  countFeatures = maxFeatures;
334  countPrevFeatures = 0;
335  IplImage* eig = cvCreateImage(cvGetSize(image), 32, 1);
336  IplImage* temp = cvCreateImage(cvGetSize(image), 32, 1);
337  cvGoodFeaturesToTrack(image, eig, temp, features,
338  &countFeatures, quality, min_distance,
339  mask, block_size, use_harris, harris_free_parameter);
340  cvFindCornerSubPix(image, features, countFeatures, cvSize(win_size, win_size),
341  cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_ITER|
342  CV_TERMCRIT_EPS,20,0.03));
343  cvReleaseImage(&eig);
344  cvReleaseImage(&temp);
345 
346  if (OnInitialize)
347  OnInitialize(_tid);
348 
349  //printf("Number of features at init: %d\n", countFeatures);
350  for (int boucle=0; boucle<countFeatures;boucle++) {
351  featuresid[boucle] = globalcountFeatures;
352  globalcountFeatures++;
353 
354  if (OnNewFeature){
355  OnNewFeature(_tid, boucle, featuresid[boucle], features[boucle].x,
356  features[boucle].y);
357  }
358  }
359 }
360 
361 
362 void vpKltOpencv::track(const IplImage *I)
363 {
364  if (!initialized) {
365  vpERROR_TRACE("KLT Not initialized") ;
367  "KLT Not initialized")) ;
368  }
369 
370  if (!I) {
372  "Image Not initialized")) ;
373  }
374 
375  if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
377  "Bad Image format")) ;
378  }
379 
380 
381 
382  CV_SWAP(prev_image, image, swap_temp);
383  CV_SWAP(prev_pyramid, pyramid, swap_temp);
384 
385  cvCopy(I, image, 0);
386 
387  if(!initial_guess){
388  // Save current features as previous features
389  countPrevFeatures = countFeatures;
390  for (int boucle=0; boucle<countFeatures;boucle++) {
391  prev_featuresid[boucle] = featuresid[boucle];
392  }
393 
394  CvPoint2D32f *swap_features = 0;
395  CV_SWAP(prev_features, features, swap_features);
396  }
397 
398  if (countFeatures <= 0) return;
399 
400  cvCalcOpticalFlowPyrLK( prev_image, image, prev_pyramid, pyramid,
401  prev_features, features, countFeatures,
402  cvSize(win_size, win_size), pyramid_level,
403  status, 0, cvTermCriteria(CV_TERMCRIT_ITER
404  |CV_TERMCRIT_EPS,20,0.03),
405  flags );
406 
407  if(!initial_guess)
408  flags |= CV_LKFLOW_PYR_A_READY;
409  else{
410  flags = CV_LKFLOW_PYR_A_READY;
411  initial_guess = false;
412  }
413 
414  int i,k;
415  for (i = k = 0; i < countFeatures ; i++) {
416  if (!status[i]) {
417  lostDuringTrack[i] = 1;
418  if (OnFeatureLost)
419  OnFeatureLost(_tid, i, featuresid[i], features[i].x,
420  features[i].y);
421  continue;
422  }
423 
424  if (IsFeatureValid) {
425  if (!IsFeatureValid(_tid, features[i].x, features[i].y)) {
426  lostDuringTrack[i] = 1;
427  if (OnFeatureLost)
428  OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
429  continue;
430  }
431  }
432  features[k] = features[i];
433  featuresid[k] = featuresid[i];
434 
435  if (OnMeasureFeature) OnMeasureFeature(_tid, k, featuresid[k], features[k].x, features[k].y);
436 
437  lostDuringTrack[i] = 0;
438  k++;
439  }
440  countFeatures = k;
441 }
442 
451  vpColor color, unsigned int thickness)
452 {
453  if ((features == 0) || (I.bitmap==0) || (!initialized))
454  {
455  vpERROR_TRACE(" Memory problem ");
456  throw(vpException(vpException::memoryAllocationError," Memory problem"));
457  }
458 
459  vpKltOpencv::display(I, features, featuresid, countFeatures, color, thickness);
460 }
461 
475 void vpKltOpencv::getFeature(int index, int &id, float &x, float &y) const
476 {
477  if (index >= countFeatures)
478  {
479  vpERROR_TRACE(" Memory problem ");
480  throw(vpException(vpException::memoryAllocationError," Memory problem"));
481  }
482 
483  x = features[index].x;
484  y = features[index].y;
485  id = featuresid[index];
486 }
487 
488 
496 void
497 vpKltOpencv::setInitialGuess(CvPoint2D32f **guess_pts)
498 {
499  // Save current features as previous features
500  countPrevFeatures = countFeatures;
501  for (int boucle=0; boucle<countFeatures;boucle++) {
502  prev_featuresid[boucle] = featuresid[boucle];
503  }
504 
505  CvPoint2D32f *swap_features = NULL;
506  CV_SWAP(prev_features, *guess_pts, swap_features);
507 
508  CV_SWAP(features, prev_features, swap_features);
509 
510  flags |= CV_LKFLOW_INITIAL_GUESSES;
511 
512  initial_guess = true;
513 }
514 
523 void vpKltOpencv::getPrevFeature(int index, int &id, float &x, float &y) const
524 {
525  if (index >= countPrevFeatures)
526  {
527  vpERROR_TRACE(" Memory problem ");
528  throw(vpException(vpException::memoryAllocationError," Memory problem"));
529  }
530 
531  x = prev_features[index].x;
532  y = prev_features[index].y;
533  id = prev_featuresid[index];
534 }
535 
542 void vpKltOpencv::addFeature(const int &id,
543  const float &x, const float &y)
544 {
545  if (maxFeatures == countFeatures)
546  {
547  vpERROR_TRACE(" Cannot add the feature ");
548  return;
549  }
550 
551  CvPoint2D32f f;
552  f.x = x;
553  f.y = y;
554  features[countFeatures] = f;
555  featuresid[countFeatures] = id;
556  countFeatures ++;
557 }
558 
560 {
561  if (index >= countFeatures)
562  {
563  vpERROR_TRACE(" Memory problem ");
564  throw(vpException(vpException::memoryAllocationError," Memory problem"));
565  }
566 
567  countFeatures --;
568 
569  for (int i=index ; i < countFeatures; i ++) {
570  features[i] = features[i+1];
571  featuresid[i] = featuresid[i+1];
572  }
573 }
574 
589 void vpKltOpencv::display(const vpImage<unsigned char>& I,const CvPoint2D32f* features_list,
590  const int &nbFeatures, vpColor color, unsigned int thickness)
591 {
592  vpImagePoint ip;
593  for (int i = 0 ; i < nbFeatures ; i++)
594  {
595  ip.set_u( vpMath::round(features_list[i].x ) );
596  ip.set_v( vpMath::round(features_list[i].y ) );
597  vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
598  }
599 }
614 void vpKltOpencv::display(const vpImage<vpRGBa>& I,const CvPoint2D32f* features_list,
615  const int &nbFeatures, vpColor color, unsigned int thickness)
616 {
617  vpImagePoint ip;
618  for (int i = 0 ; i < nbFeatures ; i++)
619  {
620  ip.set_u( vpMath::round(features_list[i].x ) );
621  ip.set_v( vpMath::round(features_list[i].y ) );
622  vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
623  }
624 }
625 
642 void vpKltOpencv::display(const vpImage<unsigned char>& I,const CvPoint2D32f* features_list,
643  const long *featuresid_list, const int &nbFeatures,
644  vpColor color, unsigned int thickness)
645 {
646  vpImagePoint ip;
647  for (int i = 0 ; i < nbFeatures ; i++)
648  {
649  ip.set_u( vpMath::round(features_list[i].x ) );
650  ip.set_v( vpMath::round(features_list[i].y ) );
651  vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
652 
653  char id[10];
654  sprintf(id, "%ld", featuresid_list[i]);
655  ip.set_u( vpMath::round( features_list[i].x + 5 ) );
656  vpDisplay::displayCharString(I, ip, id, color);
657  }
658 }
659 
676 void vpKltOpencv::display(const vpImage<vpRGBa>& I,const CvPoint2D32f* features_list,
677  const long *featuresid_list, const int &nbFeatures,
678  vpColor color, unsigned int thickness)
679 {
680  vpImagePoint ip;
681  for (int i = 0 ; i < nbFeatures ; i++)
682  {
683  ip.set_u( vpMath::round(features_list[i].x ) );
684  ip.set_v( vpMath::round(features_list[i].y ) );
685  vpDisplay::displayCross(I, ip, 10, color, thickness) ;
686 
687  char id[10];
688  sprintf(id, "%ld", featuresid_list[i]);
689  ip.set_u( vpMath::round( features_list[i].x + 5 ) );
690  vpDisplay::displayCharString(I, ip, id, color);
691  }
692 }
693 
694 #endif
void track(const IplImage *I)
#define vpERROR_TRACE
Definition: vpDebug.h:395
Type * bitmap
points toward the bitmap
Definition: vpImage.h:120
vpKltOpencv & operator=(const vpKltOpencv &copy)
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
void addFeature(const int &id, const float &x, const float &y)
void display(const vpImage< unsigned char > &I, vpColor color=vpColor::red, unsigned int thickness=1)
error that can be emited by ViSP classes.
Definition: vpException.h:76
void setInitialGuess(CvPoint2D32f **guess_pts)
virtual ~vpKltOpencv()
static int round(const double x)
Definition: vpMath.h:228
void getFeature(int index, int &id, float &x, float &y) const
void set_u(const double u)
Definition: vpImagePoint.h:216
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
void set_v(const double v)
Definition: vpImagePoint.h:227
void getPrevFeature(int index, int &id, float &x, float &y) const
void initTracking(const IplImage *I, const IplImage *mask=NULL)
virtual void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color=vpColor::green)=0
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV.
Definition: vpKltOpencv.h:103
void suppressFeature(int index)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:92
void setMaxFeatures(const int input)