Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpKltOpencv.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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  * Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented
32  * with opencv.
33  *
34  * Authors:
35  * Fabien Servant
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
47 #include <visp3/core/vpConfig.h>
48 
49 #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408)
50 
51 #include <string>
52 
53 #include <visp3/core/vpDisplay.h>
54 #include <visp3/klt/vpKltOpencv.h>
55 #include <visp3/core/vpTrackingException.h>
56 
61  : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01),
62  m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1), m_pyrMaxLevel(3),
63  m_next_points_id(0), m_initial_guess(false)
64 {
65  m_termcrit = cv::TermCriteria(cv::TermCriteria::COUNT|cv::TermCriteria::EPS, 20, 0.03);
66 
67 }
68 
73  : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01),
74  m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1), m_pyrMaxLevel(3),
75  m_next_points_id(0), m_initial_guess(false)
76 {
77  *this = copy;
78 }
79 
84 {
85  m_gray = copy.m_gray;
86  m_prevGray = copy.m_prevGray;
87  m_points[0] = copy.m_points[0];
88  m_points[1] = copy.m_points[1];
89  m_points_id = copy.m_points_id;
90  m_maxCount = copy.m_maxCount;
91  m_termcrit = copy.m_termcrit;
92  m_winSize = copy.m_winSize;
96  m_harris_k = copy.m_harris_k;
97  m_blockSize = copy.m_blockSize;
102 
103  return *this;
104 }
105 
107 {
108 }
109 
120 void vpKltOpencv::initTracking(const cv::Mat &I, const cv::Mat &mask)
121 {
122  m_next_points_id = 0;
123 
124  //cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
125  I.copyTo(m_gray);
126 
127  for (size_t i=0; i<2; i++) {
128  m_points[i].clear();
129  }
130 
131  m_points_id.clear();
132 
133  cv::goodFeaturesToTrack(m_gray, m_points[1], m_maxCount, m_qualityLevel, m_minDistance, mask, m_blockSize, 0, m_harris_k);
134 
135  if(m_points[1].size() > 0){
136  cv::cornerSubPix(m_gray, m_points[1], cv::Size(m_winSize, m_winSize), cv::Size(-1,-1), m_termcrit);
137 
138  for (size_t i=0; i < m_points[1].size(); i++)
139  m_points_id.push_back(m_next_points_id++);
140  }
141 }
142 
148 void vpKltOpencv::track(const cv::Mat &I)
149 {
150  if(m_points[1].size() == 0)
151  throw vpTrackingException(vpTrackingException::fatalError, "Not enough key points to track.");
152 
153  std::vector<float> err;
154  int flags = 0;
155 
156  cv::swap(m_prevGray, m_gray);
157 
158  if (m_initial_guess) {
159  flags |= cv::OPTFLOW_USE_INITIAL_FLOW;
160  m_initial_guess = false;
161  }
162  else {
163  std::swap(m_points[1], m_points[0]);
164  }
165 
166  //cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
167  I.copyTo(m_gray);
168 
169  if(m_prevGray.empty()){
170  m_gray.copyTo(m_prevGray);
171  }
172 
173  std::vector<uchar> status;
174 
175  cv::calcOpticalFlowPyrLK(m_prevGray, m_gray, m_points[0], m_points[1], status, err, cv::Size(m_winSize, m_winSize),
177 
178  // Remove points that are lost
179  for (int i=(int)status.size()-1; i>=0; i--) {
180  if (status[(size_t)i] == 0) { // point is lost
181  m_points[0].erase(m_points[0].begin()+i);
182  m_points[1].erase(m_points[1].begin()+i);
183  m_points_id.erase(m_points_id.begin()+i);
184  }
185  }
186 }
187 
201 void vpKltOpencv::getFeature(const int &index, long &id, float &x, float &y) const
202 {
203  if ((size_t)index >= m_points[1].size()){
204  throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
205  }
206 
207  x = m_points[1][(size_t)index].x;
208  y = m_points[1][(size_t)index].y;
209  id = m_points_id[(size_t)index];
210 }
211 
220  const vpColor &color, unsigned int thickness)
221 {
222  vpKltOpencv::display(I, m_points[1], m_points_id, color, thickness);
223 }
224 
237 void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
238  const vpColor &color, unsigned int thickness)
239 {
240  vpImagePoint ip;
241  for (size_t i = 0 ; i < features.size() ; i++) {
242  ip.set_u( vpMath::round(features[i].x ) );
243  ip.set_v( vpMath::round(features[i].y ) );
244  vpDisplay::displayCross(I, ip, 10+thickness, color, thickness);
245  }
246 }
247 
260 void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
261  const vpColor &color, unsigned int thickness)
262 {
263  vpImagePoint ip;
264  for (size_t i = 0 ; i < features.size() ; i++) {
265  ip.set_u( vpMath::round(features[i].x ) );
266  ip.set_v( vpMath::round(features[i].y ) );
267  vpDisplay::displayCross(I, ip, 10+thickness, color, thickness);
268  }
269 }
270 
285 void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
286  const std::vector<long> &featuresid,
287  const vpColor &color, unsigned int thickness)
288 {
289  vpImagePoint ip;
290  for (size_t i = 0; i < features.size(); i++) {
291  ip.set_u( vpMath::round(features[i].x ) );
292  ip.set_v( vpMath::round(features[i].y ) );
293  vpDisplay::displayCross(I, ip, 10, color, thickness);
294 
295  std::ostringstream id;
296  id << featuresid[i];
297  ip.set_u( vpMath::round( features[i].x + 5 ) );
298  vpDisplay::displayText(I, ip, id.str(), color);
299  }
300 }
301 
316 void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
317  const std::vector<long> &featuresid,
318  const vpColor &color, unsigned int thickness)
319 {
320  vpImagePoint ip;
321  for (size_t i = 0 ; i < features.size() ; i++) {
322  ip.set_u( vpMath::round(features[i].x ) );
323  ip.set_v( vpMath::round(features[i].y ) );
324  vpDisplay::displayCross(I, ip, 10, color, thickness);
325 
326  std::ostringstream id;
327  id << featuresid[i];
328  ip.set_u( vpMath::round( features[i].x + 5 ) );
329  vpDisplay::displayText(I, ip, id.str(), color);
330  }
331 }
332 
338 void vpKltOpencv::setMaxFeatures(const int maxCount)
339 {
340  m_maxCount = maxCount;
341 }
342 
349 void vpKltOpencv::setWindowSize(const int winSize)
350 {
351  m_winSize = winSize;
352 }
353 
362 void vpKltOpencv::setQuality(double qualityLevel)
363 {
364  m_qualityLevel = qualityLevel;
365 }
366 
373 {
374  m_harris_k = harris_k;
375 }
376 
382 void vpKltOpencv::setUseHarris(const int useHarrisDetector)
383 {
384  m_useHarrisDetector = useHarrisDetector;
385 }
386 
393 void vpKltOpencv::setMinDistance(double minDistance)
394 {
395  m_minDistance = minDistance;
396 }
397 
402 void vpKltOpencv::setMinEigThreshold(double minEigThreshold)
403 {
404  m_minEigThreshold = minEigThreshold;
405 }
406 
416 void vpKltOpencv::setBlockSize(const int blockSize)
417 {
418  m_blockSize = blockSize;
419 }
420 
428 void vpKltOpencv::setPyramidLevels(const int pyrMaxLevel)
429 {
430  m_pyrMaxLevel = pyrMaxLevel;
431 }
432 
444 void
445 vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &guess_pts)
446 {
447  if(guess_pts.size() != m_points[1].size()){
449  "Cannot set initial guess: size feature vector [%d] and guess vector [%d] doesn't match",
450  m_points[1].size(), guess_pts.size()));
451  }
452 
453  m_points[1] = guess_pts;
454  m_initial_guess = true;
455 }
456 
470 void
471 vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &init_pts, const std::vector<cv::Point2f> &guess_pts, const std::vector<long> &fid)
472 {
473  if(guess_pts.size() != init_pts.size()){
475  "Cannot set initial guess: size init vector [%d] and guess vector [%d] doesn't match",
476  init_pts.size(), guess_pts.size()));
477  }
478 
479  m_points[0] = init_pts;
480  m_points[1] = guess_pts;
481  m_points_id = fid;
482  m_initial_guess = true;
483 }
484 
492 void
493 vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts)
494 {
495  m_initial_guess = false;
496  m_points[1] = pts;
497  m_next_points_id = 0;
498  m_points_id.clear();
499  for(size_t i=0; i < m_points[1].size(); i++) {
500  m_points_id.push_back(m_next_points_id ++);
501  }
502 
503  I.copyTo(m_gray);
504 }
505 
506 void
507 vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids)
508 {
509  m_initial_guess = false;
510  m_points[1] = pts;
511  m_points_id.clear();
512 
513  if(ids.size() != pts.size()){
514  m_next_points_id = 0;
515  for(size_t i=0; i < m_points[1].size(); i++)
516  m_points_id.push_back(m_next_points_id ++);
517  }
518  else{
519  long max = 0;
520  for(size_t i=0; i < m_points[1].size(); i++){
521  m_points_id.push_back(ids[i]);
522  if(ids[i] > max) max = ids[i];
523  }
524  m_next_points_id = max + 1;
525  }
526 
527  I.copyTo(m_gray);
528 }
529 
536 void vpKltOpencv::addFeature(const float &x, const float &y)
537 {
538  cv::Point2f f(x, y);
539  m_points[1].push_back(f);
540  m_points_id.push_back(m_next_points_id++);
541 }
542 
554 void vpKltOpencv::addFeature(const long &id, const float &x, const float &y)
555 {
556  cv::Point2f f(x, y);
557  m_points[1].push_back(f);
558  m_points_id.push_back(id);
559  if (id >= m_next_points_id)
560  m_next_points_id = id + 1;
561 }
562 
569 void vpKltOpencv::addFeature(const cv::Point2f &f)
570 {
571  m_points[1].push_back(f);
572  m_points_id.push_back(m_next_points_id++);
573 }
574 
579 void vpKltOpencv::suppressFeature(const int &index)
580 {
581  if ((size_t)index >= m_points[1].size()){
582  throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
583  }
584 
585  m_points[1].erase(m_points[1].begin()+index);
586  m_points_id.erase(m_points_id.begin()+index);
587 }
588 
589 
590 #elif defined(VISP_HAVE_OPENCV)
591 
592 #include <string>
593 
594 #include <visp3/klt/vpKltOpencv.h>
595 
596 void vpKltOpencv::clean()
597 {
598  if (image) cvReleaseImage(&image);
599  if (prev_image) cvReleaseImage(&prev_image);
600  if (pyramid) cvReleaseImage(&pyramid);
601  if (prev_pyramid) cvReleaseImage(&prev_pyramid);
602 
603  image = NULL;
604  prev_image = NULL;
605  pyramid = NULL;
606  prev_pyramid = NULL;
607 
608  swap_temp = NULL;
609  countFeatures = 0;
610  countPrevFeatures = 0;
611  flags = 0;
612  initialized = 0;
613  globalcountFeatures = 0;
614 }
615 
616 void vpKltOpencv::cleanAll()
617 {
618  clean();
619  if (features) cvFree(&features);
620  if (prev_features) cvFree(&prev_features);
621  if (status) cvFree(&status);
622  if (lostDuringTrack) cvFree(&lostDuringTrack);
623  if (featuresid) cvFree(&featuresid);
624  if (prev_featuresid) cvFree(&prev_featuresid);
625  features = NULL;
626  prev_features = NULL;
627  status = NULL;
628  lostDuringTrack = 0;
629  featuresid = NULL;
630  prev_featuresid = NULL;
631 }
632 
633 void vpKltOpencv::reset()
634 {
635  clean();
636 
637 }
638 
643  : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01),
644  min_distance(10), harris_free_parameter(0.04), block_size(3), use_harris(1),
645  pyramid_level(3), _tid(-1), image(NULL), prev_image(NULL), pyramid(NULL),
646  prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
647  features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL),
648  flags(0), initial_guess(false), lostDuringTrack(0), status(0), OnInitialize(0),
649  OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0), IsFeatureValid(0)
650 {
651  features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(features[0]));
652  prev_features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(prev_features[0]));
653  status = (char*)cvAlloc((size_t)maxFeatures);
654  lostDuringTrack = (bool*)cvAlloc((size_t)maxFeatures);
655  featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
656  prev_featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
657 }
658 
663  : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01),
664  min_distance(10), harris_free_parameter(0.04), block_size(3), use_harris(1),
665  pyramid_level(3), _tid(-1), image(NULL), prev_image(NULL), pyramid(NULL),
666  prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
667  features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL),
668  flags(0), initial_guess(false), lostDuringTrack(0), status(0), OnInitialize(0),
669  OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0), IsFeatureValid(0)
670 {
671  *this = copy;
672 }
673 
678 {
679  //Shallow copy of primitives
680  initialized = copy.initialized;
681  maxFeatures = copy.maxFeatures;
682  countFeatures = copy.countFeatures;
683  countPrevFeatures = copy.countPrevFeatures;
684  globalcountFeatures = copy.globalcountFeatures;
685  flags = copy.flags;
686  win_size = copy.win_size;
687  quality = copy.quality;
688  min_distance = copy.min_distance;
689  harris_free_parameter = copy.harris_free_parameter;
690  block_size = copy.block_size;
691  use_harris = copy.use_harris;
692  pyramid_level = copy.pyramid_level;
693  _tid = copy._tid;
694 
695  OnInitialize = copy.OnInitialize;
696  OnFeatureLost = copy.OnFeatureLost;
697  OnNewFeature = copy.OnNewFeature;
698  OnMeasureFeature = copy.OnMeasureFeature;
699  IsFeatureValid = copy.IsFeatureValid;
700 
701  initial_guess = copy.initial_guess;
702  lostDuringTrack = copy.lostDuringTrack;
703 
704  if (!initialized) {
705  status = 0;
706  lostDuringTrack = 0;
707  countFeatures = 0;
708  countPrevFeatures = 0;
709  flags = 0;
710  initialized = 0;
711  globalcountFeatures = 0;
712  }
713 
714  if (copy.image)
715  {
716  image = cvCreateImage(cvGetSize(copy.image), 8, 1);
717  // /*IplImage **/cvCopyImage(copy.image,image);
718  cvCopy(copy.image, image, 0);
719  }
720 
721  if (copy.prev_image)
722  {
723  prev_image = cvCreateImage(cvGetSize(copy.prev_image), IPL_DEPTH_8U, 1);
724  // /*IplImage **/ cvCopyImage(copy.prev_image,prev_image);
725  cvCopy(copy.prev_image, prev_image, 0);
726  }
727 
728  if (copy.pyramid)
729  {
730  pyramid = cvCreateImage(cvGetSize(copy.pyramid), IPL_DEPTH_8U, 1);
731  // /*IplImage **/cvCopyImage(copy.pyramid,pyramid);
732  cvCopy(copy.pyramid, pyramid, 0);
733  }
734 
735  if (copy.prev_pyramid)
736  {
737  prev_pyramid = cvCreateImage(cvGetSize(copy.prev_pyramid), IPL_DEPTH_8U, 1);
738  // /*IplImage **/cvCopyImage(copy.prev_pyramid,prev_pyramid);
739  cvCopy(copy.prev_pyramid, prev_pyramid, 0);
740  }
741 
742  //Deep copy of arrays
743  if (copy.features) {
744  /*CvPoint2D32f **/features =
745  (CvPoint2D32f*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(CvPoint2D32f));
746  for (int i = 0; i < copy.maxFeatures; i++)
747  features[i] = copy.features[i];
748  }
749 
750  if (copy.prev_features) {
751  /*CvPoint2D32f **/prev_features =
752  (CvPoint2D32f*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(CvPoint2D32f));
753  for (int i = 0; i < copy.maxFeatures; i++)
754  prev_features[i] = copy.prev_features[i];
755  }
756 
757  if (copy.featuresid) {
758  /*long **/featuresid = (long*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(long));
759  for (int i = 0; i < copy.maxFeatures; i++)
760  featuresid[i] = copy.featuresid[i];
761  }
762 
763  if (copy.prev_featuresid) {
764  /*long **/prev_featuresid = (long*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(long));
765  for (int i = 0; i < copy.maxFeatures; i++)
766  prev_featuresid[i] = copy.prev_featuresid[i];
767  }
768 
769  if (copy.status) {
770  /*char **/status = (char*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(char));
771  for (int i = 0; i < copy.maxFeatures; i++)
772  status[i] = copy.status[i];
773  }
774 
775  if (copy.lostDuringTrack) {
776  /*bool **/lostDuringTrack = (bool*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(bool));
777  for (int i = 0; i < copy.maxFeatures; i++)
778  lostDuringTrack[i] = copy.lostDuringTrack[i];
779  }
780 
781  return *this;
782 }
783 
785 {
786  cleanAll();
787 }
788 
796 void vpKltOpencv::setMaxFeatures(const int input) {
797  initialized = 0; maxFeatures=input;
798 
799  if (features) cvFree(&features);
800  if (prev_features) cvFree(&prev_features);
801  if (status) cvFree(&status);
802  if (lostDuringTrack) cvFree(&lostDuringTrack);
803  if (featuresid) cvFree(&featuresid);
804  if (prev_featuresid) cvFree(&prev_featuresid);
805 
806 
807  features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(CvPoint2D32f));
808  prev_features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(CvPoint2D32f));
809  status = (char*)cvAlloc((unsigned int)maxFeatures*sizeof(char));
810  lostDuringTrack = (bool*)cvAlloc((unsigned int)maxFeatures*sizeof(bool));
811  featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
812  prev_featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
813 }
814 
825 void vpKltOpencv::initTracking(const IplImage *I, const IplImage *mask)
826 {
827  if (!I) {
828  throw(vpException(vpTrackingException::initializationError, "Image Not initialized")) ;
829  }
830 
831  if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
832  throw(vpException(vpTrackingException::initializationError, "Bad Image format")) ;
833  }
834 
835  if (mask) {
836  if (mask->depth != IPL_DEPTH_8U || I->nChannels != 1) {
837  throw(vpException(vpTrackingException::initializationError, "Bad Image format")) ;
838  }
839  }
840 
841  //Creation des buffers
842  CvSize Sizeim, SizeI;
843  SizeI = cvGetSize(I);
844  bool b_imOK = true;
845  if(image != NULL){
846  Sizeim = cvGetSize(image);
847  if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
848  }
849  if(image == NULL || prev_image == NULL || pyramid==NULL || prev_pyramid ==NULL || !b_imOK){
850  reset();
851  image = cvCreateImage(cvGetSize(I), 8, 1);image->origin = I->origin;
852  prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
853  pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
854  prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
855  }else{
856  swap_temp = 0;
857  countFeatures = 0;
858  countPrevFeatures = 0;
859  flags = 0;
860  initialized = 0;
861  globalcountFeatures = 0;
862  }
863 
864  initialized = 1;
865 
866  //Import
867  cvCopy(I, image, 0);
868 
869  //Recherche de points d'interets
870  countFeatures = maxFeatures;
871  countPrevFeatures = 0;
872  IplImage* eig = cvCreateImage(cvGetSize(image), 32, 1);
873  IplImage* temp = cvCreateImage(cvGetSize(image), 32, 1);
874  cvGoodFeaturesToTrack(image, eig, temp, features,
875  &countFeatures, quality, min_distance,
876  mask, block_size, use_harris, harris_free_parameter);
877  cvFindCornerSubPix(image, features, countFeatures, cvSize(win_size, win_size),
878  cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_ITER|
879  CV_TERMCRIT_EPS,20,0.03));
880  cvReleaseImage(&eig);
881  cvReleaseImage(&temp);
882 
883  if (OnInitialize)
884  OnInitialize(_tid);
885 
886  //printf("Number of features at init: %d\n", countFeatures);
887  for (int boucle=0; boucle<countFeatures;boucle++) {
888  featuresid[boucle] = globalcountFeatures;
889  globalcountFeatures++;
890 
891  if (OnNewFeature){
892  OnNewFeature(_tid, boucle, featuresid[boucle], features[boucle].x,
893  features[boucle].y);
894  }
895  }
896 }
897 
905 void
906 vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, int size)
907 {
908  if (size > maxFeatures)
910  "Cannot initialize tracker from points"));
911 
912  //Creation des buffers
913  CvSize Sizeim, SizeI;
914  SizeI = cvGetSize(I);
915  bool b_imOK = true;
916  if(image != NULL){
917  Sizeim = cvGetSize(image);
918  if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
919  }
920  if(image == NULL || prev_image == NULL || pyramid==NULL || prev_pyramid ==NULL || !b_imOK){
921  reset();
922  image = cvCreateImage(cvGetSize(I), 8, 1);image->origin = I->origin;
923  prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
924  pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
925  prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
926  } else {
927  flags = 0;
928  }
929  // Save current features as previous features
930  countFeatures = size;
931  for (int i=0; i<countFeatures;i++) {
932  features[i] = pts[i];
933  featuresid[i] = i;
934  }
935 
936  globalcountFeatures = size;
937  initialized = 1;
938 
939  cvCopy(I, image, 0);
940 }
941 
942 void
943 vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, long *fid, int size)
944 {
945  if (size > maxFeatures)
947  "Cannot initialize tracker from points"));
948 
949  //Creation des buffers
950  CvSize Sizeim, SizeI;
951  SizeI = cvGetSize(I);
952  bool b_imOK = true;
953  if(image != NULL){
954  Sizeim = cvGetSize(image);
955  if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
956  }
957  if(image == NULL || prev_image == NULL || pyramid==NULL || prev_pyramid ==NULL || !b_imOK){
958  reset();
959  image = cvCreateImage(cvGetSize(I), 8, 1);image->origin = I->origin;
960  prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
961  pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
962  prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
963  } else {
964  flags = 0;
965  }
966  // Save current features as previous features
967  countFeatures = size;
968  long max = 0;
969  for (int i=0; i<countFeatures;i++) {
970  features[i] = pts[i];
971  featuresid[i] = fid[i];
972  if (fid[i] > max)
973  max = fid[i];
974  }
975 
976  globalcountFeatures = max + 1;
977  initialized = 1;
978 
979  cvCopy(I, image, 0);
980 }
981 
982 void vpKltOpencv::track(const IplImage *I)
983 {
984  if (!initialized) {
985  vpERROR_TRACE("KLT Not initialized") ;
987  "KLT Not initialized")) ;
988  }
989 
990  if (!I) {
992  "Image Not initialized")) ;
993  }
994 
995  if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
997  "Bad Image format")) ;
998  }
999 
1000 
1001 
1002  CV_SWAP(prev_image, image, swap_temp);
1003  CV_SWAP(prev_pyramid, pyramid, swap_temp);
1004 
1005  cvCopy(I, image, 0);
1006 
1007  if(!initial_guess){
1008  // Save current features as previous features
1009  countPrevFeatures = countFeatures;
1010  for (int boucle=0; boucle<countFeatures;boucle++) {
1011  prev_featuresid[boucle] = featuresid[boucle];
1012  }
1013 
1014  CvPoint2D32f *swap_features = 0;
1015  CV_SWAP(prev_features, features, swap_features);
1016  }
1017 
1018  if (countFeatures <= 0) return;
1019 
1020  cvCalcOpticalFlowPyrLK( prev_image, image, prev_pyramid, pyramid,
1021  prev_features, features, countFeatures,
1022  cvSize(win_size, win_size), pyramid_level,
1023  status, 0, cvTermCriteria(CV_TERMCRIT_ITER
1024  |CV_TERMCRIT_EPS,20,0.03),
1025  flags );
1026 
1027  if(!initial_guess)
1028  flags |= CV_LKFLOW_PYR_A_READY;
1029  else{
1030  flags = CV_LKFLOW_PYR_A_READY;
1031  initial_guess = false;
1032  }
1033 
1034  int i,k;
1035  for (i = k = 0; i < countFeatures ; i++) {
1036  if (!status[i]) {
1037  lostDuringTrack[i] = 1;
1038  if (OnFeatureLost)
1039  OnFeatureLost(_tid, i, featuresid[i], features[i].x,
1040  features[i].y);
1041  continue;
1042  }
1043 
1044  if (IsFeatureValid) {
1045  if (!IsFeatureValid(_tid, features[i].x, features[i].y)) {
1046  lostDuringTrack[i] = 1;
1047  if (OnFeatureLost)
1048  OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
1049  continue;
1050  }
1051  }
1052  features[k] = features[i];
1053  featuresid[k] = featuresid[i];
1054 
1055  if (OnMeasureFeature) OnMeasureFeature(_tid, k, featuresid[k], features[k].x, features[k].y);
1056 
1057  lostDuringTrack[i] = 0;
1058  k++;
1059  }
1060  countFeatures = k;
1061 }
1062 
1071  vpColor color, unsigned int thickness)
1072 {
1073  if ((features == 0) || (I.bitmap==0) || (!initialized))
1074  {
1075  vpERROR_TRACE(" Memory problem ");
1076  throw(vpException(vpException::memoryAllocationError," Memory problem"));
1077  }
1078 
1079  vpKltOpencv::display(I, features, featuresid, countFeatures, color, thickness);
1080 }
1081 
1095 void vpKltOpencv::getFeature(int index, long &id, float &x, float &y) const
1096 {
1097  if (index >= countFeatures)
1098  {
1099  vpERROR_TRACE(" Memory problem ");
1100  throw(vpException(vpException::memoryAllocationError," Memory problem"));
1101  }
1102 
1103  x = features[index].x;
1104  y = features[index].y;
1105  id = featuresid[index];
1106 }
1107 
1108 
1116 void
1117 vpKltOpencv::setInitialGuess(CvPoint2D32f **guess_pts)
1118 {
1119  // Save current features as previous features
1120  countPrevFeatures = countFeatures;
1121  for (int boucle=0; boucle<countFeatures;boucle++) {
1122  prev_featuresid[boucle] = featuresid[boucle];
1123  }
1124 
1125  CvPoint2D32f *swap_features = NULL;
1126  CV_SWAP(prev_features, *guess_pts, swap_features);
1127 
1128  CV_SWAP(features, prev_features, swap_features);
1129 
1130  flags |= CV_LKFLOW_INITIAL_GUESSES;
1131 
1132  initial_guess = true;
1133 }
1134 
1149 void
1150 vpKltOpencv::setInitialGuess(CvPoint2D32f **init_pts, CvPoint2D32f **guess_pts, long *fid, int size)
1151 {
1152  countPrevFeatures = size;
1153  countFeatures = size;
1154  for (int boucle=0; boucle<size;boucle++) {
1155  prev_featuresid[boucle] = fid[boucle];
1156  featuresid[boucle] = fid[boucle];
1157  }
1158 
1159  CvPoint2D32f *swap_features = NULL;
1160  CvPoint2D32f *swap_features2 = NULL;
1161  CV_SWAP(prev_features, *init_pts, swap_features);
1162 
1163 // if(swap_features) cvFree(&swap_features);
1164 // swap_features = NULL;
1165 
1166  CV_SWAP(features, *guess_pts, swap_features2);
1167 
1168  flags |= CV_LKFLOW_INITIAL_GUESSES;
1169 
1170  initial_guess = true;
1171 }
1172 
1181 void vpKltOpencv::getPrevFeature(int index, int &id, float &x, float &y) const
1182 {
1183  if (index >= countPrevFeatures)
1184  {
1185  vpERROR_TRACE(" Memory problem ");
1186  throw(vpException(vpException::memoryAllocationError," Memory problem"));
1187  }
1188 
1189  x = prev_features[index].x;
1190  y = prev_features[index].y;
1191  id = prev_featuresid[index];
1192 }
1193 
1200 void vpKltOpencv::addFeature(const int &id,
1201  const float &x, const float &y)
1202 {
1203  if (maxFeatures == countFeatures)
1204  {
1205  vpERROR_TRACE(" Cannot add the feature ");
1206  return;
1207  }
1208 
1209  CvPoint2D32f f;
1210  f.x = x;
1211  f.y = y;
1212  features[countFeatures] = f;
1213  featuresid[countFeatures] = id;
1214  countFeatures ++;
1215 }
1216 
1217 void vpKltOpencv::suppressFeature(int index)
1218 {
1219  if (index >= countFeatures)
1220  {
1221  vpERROR_TRACE(" Memory problem ");
1222  throw(vpException(vpException::memoryAllocationError," Memory problem"));
1223  }
1224 
1225  countFeatures --;
1226 
1227  for (int i=index ; i < countFeatures; i ++) {
1228  features[i] = features[i+1];
1229  featuresid[i] = featuresid[i+1];
1230  }
1231 }
1232 
1247 void vpKltOpencv::display(const vpImage<unsigned char>& I,const CvPoint2D32f* features_list,
1248  const int &nbFeatures, vpColor color, unsigned int thickness)
1249 {
1250  vpImagePoint ip;
1251  for (int i = 0 ; i < nbFeatures ; i++)
1252  {
1253  ip.set_u( vpMath::round(features_list[i].x ) );
1254  ip.set_v( vpMath::round(features_list[i].y ) );
1255  vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
1256  }
1257 }
1272 void vpKltOpencv::display(const vpImage<vpRGBa>& I,const CvPoint2D32f* features_list,
1273  const int &nbFeatures, vpColor color, unsigned int thickness)
1274 {
1275  vpImagePoint ip;
1276  for (int i = 0 ; i < nbFeatures ; i++)
1277  {
1278  ip.set_u( vpMath::round(features_list[i].x ) );
1279  ip.set_v( vpMath::round(features_list[i].y ) );
1280  vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
1281  }
1282 }
1283 
1300 void vpKltOpencv::display(const vpImage<unsigned char>& I,const CvPoint2D32f* features_list,
1301  const long *featuresid_list, const int &nbFeatures,
1302  vpColor color, unsigned int thickness)
1303 {
1304  vpImagePoint ip;
1305  for (int i = 0 ; i < nbFeatures ; i++)
1306  {
1307  ip.set_u( vpMath::round(features_list[i].x ) );
1308  ip.set_v( vpMath::round(features_list[i].y ) );
1309  vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
1310 
1311  char id[10];
1312  sprintf(id, "%ld", featuresid_list[i]);
1313  ip.set_u( vpMath::round( features_list[i].x + 5 ) );
1314  vpDisplay::displayText(I, ip, id, color);
1315  }
1316 }
1317 
1334 void vpKltOpencv::display(const vpImage<vpRGBa>& I,const CvPoint2D32f* features_list,
1335  const long *featuresid_list, const int &nbFeatures,
1336  vpColor color, unsigned int thickness)
1337 {
1338  vpImagePoint ip;
1339  for (int i = 0 ; i < nbFeatures ; i++)
1340  {
1341  ip.set_u( vpMath::round(features_list[i].x ) );
1342  ip.set_v( vpMath::round(features_list[i].y ) );
1343  vpDisplay::displayCross(I, ip, 10, color, thickness) ;
1344 
1345  char id[10];
1346  sprintf(id, "%ld", featuresid_list[i]);
1347  ip.set_u( vpMath::round( features_list[i].x + 5 ) );
1348  vpDisplay::displayText(I, ip, id, color);
1349  }
1350 }
1351 #else
1352 
1353 // Work arround to avoid visp_klt library empty when OpenCV is not installed or used
1354 class VISP_EXPORT dummy_vpKltOpencv
1355 {
1356 public:
1357  dummy_vpKltOpencv() {};
1358 };
1359 
1360 #if !defined(VISP_BUILD_SHARED_LIBS)
1361 // Work arround to avoid warning: libvisp_klt.a(vpKltOpenCV.cpp.o) has no symbols
1362 void dummy_vpKltOpenCV_fct() {};
1363 #endif
1364 
1365 #endif
void addFeature(const float &x, const float &y)
double m_harris_k
Definition: vpKltOpencv.h:165
cv::Mat m_gray
Definition: vpKltOpencv.h:156
void setHarrisFreeParameter(double harris_k)
Type * bitmap
points toward the bitmap
Definition: vpImage.h:134
vpKltOpencv & operator=(const vpKltOpencv &copy)
Definition: vpKltOpencv.cpp:83
cv::Mat m_prevGray
Definition: vpKltOpencv.h:156
#define vpERROR_TRACE
Definition: vpDebug.h:391
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
void setMaxFeatures(const int maxCount)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
void setMinEigThreshold(double minEigThreshold)
void setMinDistance(double minDistance)
error that can be emited by ViSP classes.
Definition: vpException.h:73
virtual ~vpKltOpencv()
cv::TermCriteria m_termcrit
Definition: vpKltOpencv.h:160
static int round(const double x)
Definition: vpMath.h:249
void display(const vpImage< unsigned char > &I, const vpColor &color=vpColor::red, unsigned int thickness=1)
void setQuality(double qualityLevel)
bool m_initial_guess
Definition: vpKltOpencv.h:170
std::vector< long > m_points_id
Keypoint id.
Definition: vpKltOpencv.h:158
Error that can be emited by the vpTracker class and its derivates.
void set_u(const double u)
Definition: vpImagePoint.h:221
void set_v(const double v)
Definition: vpImagePoint.h:232
double m_minDistance
Definition: vpKltOpencv.h:163
double m_minEigThreshold
Definition: vpKltOpencv.h:164
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
void setInitialGuess(const std::vector< cv::Point2f > &guess_pts)
void setPyramidLevels(const int pyrMaxLevel)
double m_qualityLevel
Definition: vpKltOpencv.h:162
int m_useHarrisDetector
Definition: vpKltOpencv.h:167
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
void setWindowSize(const int winSize)
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:76
void suppressFeature(const int &index)
void setBlockSize(const int blockSize)
void getFeature(const int &index, long &id, float &x, float &y) const
std::vector< cv::Point2f > m_points[2]
Previous [0] and current [1] keypoint location.
Definition: vpKltOpencv.h:157
long m_next_points_id
Definition: vpKltOpencv.h:169
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void setUseHarris(const int useHarrisDetector)
int m_pyrMaxLevel
Definition: vpKltOpencv.h:168
void track(const cv::Mat &I)