Visual Servoing Platform  version 3.5.1 under development (2023-03-29)
vpKltOpencv.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2022 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  * Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented
33  * with opencv.
34  *
35  *****************************************************************************/
36 
44 #include <visp3/core/vpConfig.h>
45 
46 #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408)
47 
48 #include <string>
49 
50 #include <visp3/core/vpDisplay.h>
51 #include <visp3/core/vpTrackingException.h>
52 #include <visp3/klt/vpKltOpencv.h>
53 
58  : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01),
59  m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1),
60  m_pyrMaxLevel(3), m_next_points_id(0), m_initial_guess(false)
61 {
62  m_termcrit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 20, 0.03);
63 }
64 
69  : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01),
70  m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1),
71  m_pyrMaxLevel(3), m_next_points_id(0), m_initial_guess(false)
72 {
73  *this = copy;
74 }
75 
80 {
81  m_gray = copy.m_gray;
82  m_prevGray = copy.m_prevGray;
83  m_points[0] = copy.m_points[0];
84  m_points[1] = copy.m_points[1];
85  m_points_id = copy.m_points_id;
86  m_maxCount = copy.m_maxCount;
87  m_termcrit = copy.m_termcrit;
88  m_winSize = copy.m_winSize;
92  m_harris_k = copy.m_harris_k;
93  m_blockSize = copy.m_blockSize;
98 
99  return *this;
100 }
101 
103 
114 void vpKltOpencv::initTracking(const cv::Mat &I, const cv::Mat &mask)
115 {
116  m_next_points_id = 0;
117 
118  // cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
119  I.copyTo(m_gray);
120 
121  for (size_t i = 0; i < 2; i++) {
122  m_points[i].clear();
123  }
124 
125  m_points_id.clear();
126 
127  cv::goodFeaturesToTrack(m_gray, m_points[1], m_maxCount, m_qualityLevel, m_minDistance, mask, m_blockSize, false,
128  m_harris_k);
129 
130  if (m_points[1].size() > 0) {
131  cv::cornerSubPix(m_gray, m_points[1], cv::Size(m_winSize, m_winSize), cv::Size(-1, -1), m_termcrit);
132 
133  for (size_t i = 0; i < m_points[1].size(); i++)
134  m_points_id.push_back(m_next_points_id++);
135  }
136 }
137 
143 void vpKltOpencv::track(const cv::Mat &I)
144 {
145  if (m_points[1].size() == 0)
146  throw vpTrackingException(vpTrackingException::fatalError, "Not enough key points to track.");
147 
148  std::vector<float> err;
149  int flags = 0;
150 
151  cv::swap(m_prevGray, m_gray);
152 
153  if (m_initial_guess) {
154  flags |= cv::OPTFLOW_USE_INITIAL_FLOW;
155  m_initial_guess = false;
156  } else {
157  std::swap(m_points[1], m_points[0]);
158  }
159 
160  // cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
161  I.copyTo(m_gray);
162 
163  if (m_prevGray.empty()) {
164  m_gray.copyTo(m_prevGray);
165  }
166 
167  std::vector<uchar> status;
168 
169  cv::calcOpticalFlowPyrLK(m_prevGray, m_gray, m_points[0], m_points[1], status, err, cv::Size(m_winSize, m_winSize),
171 
172  // Remove points that are lost
173  for (int i = (int)status.size() - 1; i >= 0; i--) {
174  if (status[(size_t)i] == 0) { // point is lost
175  m_points[0].erase(m_points[0].begin() + i);
176  m_points[1].erase(m_points[1].begin() + i);
177  m_points_id.erase(m_points_id.begin() + i);
178  }
179  }
180 }
181 
195 void vpKltOpencv::getFeature(const int &index, long &id, float &x, float &y) const
196 {
197  if ((size_t)index >= m_points[1].size()) {
198  throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
199  }
200 
201  x = m_points[1][(size_t)index].x;
202  y = m_points[1][(size_t)index].y;
203  id = m_points_id[(size_t)index];
204 }
205 
213 void vpKltOpencv::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness)
214 {
215  vpKltOpencv::display(I, m_points[1], m_points_id, color, thickness);
216 }
217 
230 void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
231  const vpColor &color, unsigned int thickness)
232 {
233  vpImagePoint ip;
234  for (size_t i = 0; i < features.size(); i++) {
235  ip.set_u(vpMath::round(features[i].x));
236  ip.set_v(vpMath::round(features[i].y));
237  vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
238  }
239 }
240 
253 void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features, const vpColor &color,
254  unsigned int thickness)
255 {
256  vpImagePoint ip;
257  for (size_t i = 0; i < features.size(); i++) {
258  ip.set_u(vpMath::round(features[i].x));
259  ip.set_v(vpMath::round(features[i].y));
260  vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
261  }
262 }
263 
278 void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
279  const std::vector<long> &featuresid, const vpColor &color, unsigned int thickness)
280 {
281  vpImagePoint ip;
282  for (size_t i = 0; i < features.size(); i++) {
283  ip.set_u(vpMath::round(features[i].x));
284  ip.set_v(vpMath::round(features[i].y));
285  vpDisplay::displayCross(I, ip, 10, color, thickness);
286 
287  std::ostringstream id;
288  id << featuresid[i];
289  ip.set_u(vpMath::round(features[i].x + 5));
290  vpDisplay::displayText(I, ip, id.str(), color);
291  }
292 }
293 
308 void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
309  const std::vector<long> &featuresid, const vpColor &color, unsigned int thickness)
310 {
311  vpImagePoint ip;
312  for (size_t i = 0; i < features.size(); i++) {
313  ip.set_u(vpMath::round(features[i].x));
314  ip.set_v(vpMath::round(features[i].y));
315  vpDisplay::displayCross(I, ip, 10, color, thickness);
316 
317  std::ostringstream id;
318  id << featuresid[i];
319  ip.set_u(vpMath::round(features[i].x + 5));
320  vpDisplay::displayText(I, ip, id.str(), color);
321  }
322 }
323 
330 void vpKltOpencv::setMaxFeatures(int maxCount) { m_maxCount = maxCount; }
331 
339 void vpKltOpencv::setWindowSize(int winSize) { m_winSize = winSize; }
340 
352 void vpKltOpencv::setQuality(double qualityLevel) { m_qualityLevel = qualityLevel; }
353 
360 void vpKltOpencv::setHarrisFreeParameter(double harris_k) { m_harris_k = harris_k; }
361 
368 void vpKltOpencv::setUseHarris(int useHarrisDetector) { m_useHarrisDetector = useHarrisDetector; }
369 
377 void vpKltOpencv::setMinDistance(double minDistance) { m_minDistance = minDistance; }
378 
384 void vpKltOpencv::setMinEigThreshold(double minEigThreshold) { m_minEigThreshold = minEigThreshold; }
385 
395 void vpKltOpencv::setBlockSize(int blockSize) { m_blockSize = blockSize; }
396 
405 void vpKltOpencv::setPyramidLevels(int pyrMaxLevel) { m_pyrMaxLevel = pyrMaxLevel; }
406 
419 void vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &guess_pts)
420 {
421  if (guess_pts.size() != m_points[1].size()) {
423  "Cannot set initial guess: size feature vector [%d] "
424  "and guess vector [%d] doesn't match",
425  m_points[1].size(), guess_pts.size()));
426  }
427 
428  m_points[1] = guess_pts;
429  m_initial_guess = true;
430 }
431 
446 void vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &init_pts, const std::vector<cv::Point2f> &guess_pts,
447  const std::vector<long> &fid)
448 {
449  if (guess_pts.size() != init_pts.size()) {
451  "Cannot set initial guess: size init vector [%d] and "
452  "guess vector [%d] doesn't match",
453  init_pts.size(), guess_pts.size()));
454  }
455 
456  m_points[0] = init_pts;
457  m_points[1] = guess_pts;
458  m_points_id = fid;
459  m_initial_guess = true;
460 }
461 
470 void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts)
471 {
472  m_initial_guess = false;
473  m_points[1] = pts;
474  m_next_points_id = 0;
475  m_points_id.clear();
476  for (size_t i = 0; i < m_points[1].size(); i++) {
477  m_points_id.push_back(m_next_points_id++);
478  }
479 
480  I.copyTo(m_gray);
481 }
482 
483 void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids)
484 {
485  m_initial_guess = false;
486  m_points[1] = pts;
487  m_points_id.clear();
488 
489  if (ids.size() != pts.size()) {
490  m_next_points_id = 0;
491  for (size_t i = 0; i < m_points[1].size(); i++)
492  m_points_id.push_back(m_next_points_id++);
493  } else {
494  long max = 0;
495  for (size_t i = 0; i < m_points[1].size(); i++) {
496  m_points_id.push_back(ids[i]);
497  if (ids[i] > max)
498  max = ids[i];
499  }
500  m_next_points_id = max + 1;
501  }
502 
503  I.copyTo(m_gray);
504 }
505 
513 void vpKltOpencv::addFeature(const float &x, const float &y)
514 {
515  cv::Point2f f(x, y);
516  m_points[1].push_back(f);
517  m_points_id.push_back(m_next_points_id++);
518 }
519 
532 void vpKltOpencv::addFeature(const long &id, const float &x, const float &y)
533 {
534  cv::Point2f f(x, y);
535  m_points[1].push_back(f);
536  m_points_id.push_back(id);
537  if (id >= m_next_points_id)
538  m_next_points_id = id + 1;
539 }
540 
548 void vpKltOpencv::addFeature(const cv::Point2f &f)
549 {
550  m_points[1].push_back(f);
551  m_points_id.push_back(m_next_points_id++);
552 }
553 
558 void vpKltOpencv::suppressFeature(const int &index)
559 {
560  if ((size_t)index >= m_points[1].size()) {
561  throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
562  }
563 
564  m_points[1].erase(m_points[1].begin() + index);
565  m_points_id.erase(m_points_id.begin() + index);
566 }
567 
568 #elif defined(VISP_HAVE_OPENCV)
569 
570 #include <string>
571 
572 #include <visp3/klt/vpKltOpencv.h>
573 
574 void vpKltOpencv::clean()
575 {
576  if (image)
577  cvReleaseImage(&image);
578  if (prev_image)
579  cvReleaseImage(&prev_image);
580  if (pyramid)
581  cvReleaseImage(&pyramid);
582  if (prev_pyramid)
583  cvReleaseImage(&prev_pyramid);
584 
585  image = NULL;
586  prev_image = NULL;
587  pyramid = NULL;
588  prev_pyramid = NULL;
589 
590  swap_temp = NULL;
591  countFeatures = 0;
592  countPrevFeatures = 0;
593  flags = 0;
594  initialized = 0;
595  globalcountFeatures = 0;
596 }
597 
598 void vpKltOpencv::cleanAll()
599 {
600  clean();
601  if (features)
602  cvFree(&features);
603  if (prev_features)
604  cvFree(&prev_features);
605  if (status)
606  cvFree(&status);
607  if (lostDuringTrack)
608  cvFree(&lostDuringTrack);
609  if (featuresid)
610  cvFree(&featuresid);
611  if (prev_featuresid)
612  cvFree(&prev_featuresid);
613  features = NULL;
614  prev_features = NULL;
615  status = NULL;
616  lostDuringTrack = 0;
617  featuresid = NULL;
618  prev_featuresid = NULL;
619 }
620 
621 void vpKltOpencv::reset() { clean(); }
622 
627  : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01), min_distance(10),
628  harris_free_parameter(0.04), block_size(3), use_harris(1), pyramid_level(3), _tid(-1), image(NULL),
629  prev_image(NULL), pyramid(NULL), prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
630  features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL), flags(0), initial_guess(false),
631  lostDuringTrack(0), status(0), OnInitialize(0), OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0),
632  IsFeatureValid(0)
633 {
634  features = (CvPoint2D32f *)cvAlloc((unsigned int)maxFeatures * sizeof(features[0]));
635  prev_features = (CvPoint2D32f *)cvAlloc((unsigned int)maxFeatures * sizeof(prev_features[0]));
636  status = (char *)cvAlloc((size_t)maxFeatures);
637  lostDuringTrack = (bool *)cvAlloc((size_t)maxFeatures);
638  featuresid = (long *)cvAlloc((unsigned int)maxFeatures * sizeof(long));
639  prev_featuresid = (long *)cvAlloc((unsigned int)maxFeatures * sizeof(long));
640 }
641 
646  : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01), min_distance(10),
647  harris_free_parameter(0.04), block_size(3), use_harris(1), pyramid_level(3), _tid(-1), image(NULL),
648  prev_image(NULL), pyramid(NULL), prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
649  features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL), flags(0), initial_guess(false),
650  lostDuringTrack(0), status(0), OnInitialize(0), OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0),
651  IsFeatureValid(0)
652 {
653  *this = copy;
654 }
655 
660 {
661  // Shallow copy of primitives
662  initialized = copy.initialized;
663  maxFeatures = copy.maxFeatures;
664  countFeatures = copy.countFeatures;
665  countPrevFeatures = copy.countPrevFeatures;
666  globalcountFeatures = copy.globalcountFeatures;
667  flags = copy.flags;
668  win_size = copy.win_size;
669  quality = copy.quality;
670  min_distance = copy.min_distance;
671  harris_free_parameter = copy.harris_free_parameter;
672  block_size = copy.block_size;
673  use_harris = copy.use_harris;
674  pyramid_level = copy.pyramid_level;
675  _tid = copy._tid;
676 
677  OnInitialize = copy.OnInitialize;
678  OnFeatureLost = copy.OnFeatureLost;
679  OnNewFeature = copy.OnNewFeature;
680  OnMeasureFeature = copy.OnMeasureFeature;
681  IsFeatureValid = copy.IsFeatureValid;
682 
683  initial_guess = copy.initial_guess;
684  lostDuringTrack = copy.lostDuringTrack;
685 
686  if (!initialized) {
687  status = 0;
688  lostDuringTrack = 0;
689  countFeatures = 0;
690  countPrevFeatures = 0;
691  flags = 0;
692  initialized = 0;
693  globalcountFeatures = 0;
694  }
695 
696  if (copy.image) {
697  image = cvCreateImage(cvGetSize(copy.image), 8, 1);
698  // /*IplImage **/cvCopyImage(copy.image,image);
699  cvCopy(copy.image, image, 0);
700  }
701 
702  if (copy.prev_image) {
703  prev_image = cvCreateImage(cvGetSize(copy.prev_image), IPL_DEPTH_8U, 1);
704  // /*IplImage **/ cvCopyImage(copy.prev_image,prev_image);
705  cvCopy(copy.prev_image, prev_image, 0);
706  }
707 
708  if (copy.pyramid) {
709  pyramid = cvCreateImage(cvGetSize(copy.pyramid), IPL_DEPTH_8U, 1);
710  // /*IplImage **/cvCopyImage(copy.pyramid,pyramid);
711  cvCopy(copy.pyramid, pyramid, 0);
712  }
713 
714  if (copy.prev_pyramid) {
715  prev_pyramid = cvCreateImage(cvGetSize(copy.prev_pyramid), IPL_DEPTH_8U, 1);
716  // /*IplImage **/cvCopyImage(copy.prev_pyramid,prev_pyramid);
717  cvCopy(copy.prev_pyramid, prev_pyramid, 0);
718  }
719 
720  // Deep copy of arrays
721  if (copy.features) {
722  /*CvPoint2D32f **/ features = (CvPoint2D32f *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(CvPoint2D32f));
723  for (int i = 0; i < copy.maxFeatures; i++)
724  features[i] = copy.features[i];
725  }
726 
727  if (copy.prev_features) {
728  /*CvPoint2D32f **/ prev_features = (CvPoint2D32f *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(CvPoint2D32f));
729  for (int i = 0; i < copy.maxFeatures; i++)
730  prev_features[i] = copy.prev_features[i];
731  }
732 
733  if (copy.featuresid) {
734  /*long **/ featuresid = (long *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(long));
735  for (int i = 0; i < copy.maxFeatures; i++)
736  featuresid[i] = copy.featuresid[i];
737  }
738 
739  if (copy.prev_featuresid) {
740  /*long **/ prev_featuresid = (long *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(long));
741  for (int i = 0; i < copy.maxFeatures; i++)
742  prev_featuresid[i] = copy.prev_featuresid[i];
743  }
744 
745  if (copy.status) {
746  /*char **/ status = (char *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(char));
747  for (int i = 0; i < copy.maxFeatures; i++)
748  status[i] = copy.status[i];
749  }
750 
751  if (copy.lostDuringTrack) {
752  /*bool **/ lostDuringTrack = (bool *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(bool));
753  for (int i = 0; i < copy.maxFeatures; i++)
754  lostDuringTrack[i] = copy.lostDuringTrack[i];
755  }
756 
757  return *this;
758 }
759 
760 vpKltOpencv::~vpKltOpencv() { cleanAll(); }
761 
769 void vpKltOpencv::setMaxFeatures(int input)
770 {
771  initialized = 0;
772  maxFeatures = input;
773 
774  if (features)
775  cvFree(&features);
776  if (prev_features)
777  cvFree(&prev_features);
778  if (status)
779  cvFree(&status);
780  if (lostDuringTrack)
781  cvFree(&lostDuringTrack);
782  if (featuresid)
783  cvFree(&featuresid);
784  if (prev_featuresid)
785  cvFree(&prev_featuresid);
786 
787  features = (CvPoint2D32f *)cvAlloc((unsigned int)maxFeatures * sizeof(CvPoint2D32f));
788  prev_features = (CvPoint2D32f *)cvAlloc((unsigned int)maxFeatures * sizeof(CvPoint2D32f));
789  status = (char *)cvAlloc((unsigned int)maxFeatures * sizeof(char));
790  lostDuringTrack = (bool *)cvAlloc((unsigned int)maxFeatures * sizeof(bool));
791  featuresid = (long *)cvAlloc((unsigned int)maxFeatures * sizeof(long));
792  prev_featuresid = (long *)cvAlloc((unsigned int)maxFeatures * sizeof(long));
793 }
794 
805 void vpKltOpencv::initTracking(const IplImage *I, const IplImage *mask)
806 {
807  if (!I) {
808  throw(vpException(vpTrackingException::initializationError, "Image Not initialized"));
809  }
810 
811  if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
812  throw(vpException(vpTrackingException::initializationError, "Bad Image format"));
813  }
814 
815  if (mask) {
816  if (mask->depth != IPL_DEPTH_8U || I->nChannels != 1) {
817  throw(vpException(vpTrackingException::initializationError, "Bad Image format"));
818  }
819  }
820 
821  // Creation des buffers
822  CvSize Sizeim, SizeI;
823  SizeI = cvGetSize(I);
824  bool b_imOK = true;
825  if (image != NULL) {
826  Sizeim = cvGetSize(image);
827  if (SizeI.width != Sizeim.width || SizeI.height != Sizeim.height)
828  b_imOK = false;
829  }
830  if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) {
831  reset();
832  image = cvCreateImage(cvGetSize(I), 8, 1);
833  image->origin = I->origin;
834  prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
835  pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
836  prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
837  } else {
838  swap_temp = 0;
839  countFeatures = 0;
840  countPrevFeatures = 0;
841  flags = 0;
842  initialized = 0;
843  globalcountFeatures = 0;
844  }
845 
846  initialized = 1;
847 
848  // Import
849  cvCopy(I, image, 0);
850 
851  // Recherche de points d'interets
852  countFeatures = maxFeatures;
853  countPrevFeatures = 0;
854  IplImage *eig = cvCreateImage(cvGetSize(image), 32, 1);
855  IplImage *temp = cvCreateImage(cvGetSize(image), 32, 1);
856  cvGoodFeaturesToTrack(image, eig, temp, features, &countFeatures, quality, min_distance, mask, block_size, use_harris,
857  harris_free_parameter);
858  cvFindCornerSubPix(image, features, countFeatures, cvSize(win_size, win_size), cvSize(-1, -1),
859  cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
860  cvReleaseImage(&eig);
861  cvReleaseImage(&temp);
862 
863  if (OnInitialize)
864  OnInitialize(_tid);
865 
866  // printf("Number of features at init: %d\n", countFeatures);
867  for (int boucle = 0; boucle < countFeatures; boucle++) {
868  featuresid[boucle] = globalcountFeatures;
869  globalcountFeatures++;
870 
871  if (OnNewFeature) {
872  OnNewFeature(_tid, boucle, featuresid[boucle], features[boucle].x, features[boucle].y);
873  }
874  }
875 }
876 
885 void vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, int size)
886 {
887  if (size > maxFeatures)
888  throw(vpException(vpTrackingException::initializationError, "Cannot initialize tracker from points"));
889 
890  // Creation des buffers
891  CvSize Sizeim, SizeI;
892  SizeI = cvGetSize(I);
893  bool b_imOK = true;
894  if (image != NULL) {
895  Sizeim = cvGetSize(image);
896  if (SizeI.width != Sizeim.width || SizeI.height != Sizeim.height)
897  b_imOK = false;
898  }
899  if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) {
900  reset();
901  image = cvCreateImage(cvGetSize(I), 8, 1);
902  image->origin = I->origin;
903  prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
904  pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
905  prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
906  } else {
907  flags = 0;
908  }
909  // Save current features as previous features
910  countFeatures = size;
911  for (int i = 0; i < countFeatures; i++) {
912  features[i] = pts[i];
913  featuresid[i] = i;
914  }
915 
916  globalcountFeatures = size;
917  initialized = 1;
918 
919  cvCopy(I, image, 0);
920 }
921 
922 void vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, long *fid, int size)
923 {
924  if (size > maxFeatures)
925  throw(vpException(vpTrackingException::initializationError, "Cannot initialize tracker from points"));
926 
927  // Creation des buffers
928  CvSize Sizeim, SizeI;
929  SizeI = cvGetSize(I);
930  bool b_imOK = true;
931  if (image != NULL) {
932  Sizeim = cvGetSize(image);
933  if (SizeI.width != Sizeim.width || SizeI.height != Sizeim.height)
934  b_imOK = false;
935  }
936  if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) {
937  reset();
938  image = cvCreateImage(cvGetSize(I), 8, 1);
939  image->origin = I->origin;
940  prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
941  pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
942  prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
943  } else {
944  flags = 0;
945  }
946  // Save current features as previous features
947  countFeatures = size;
948  long max = 0;
949  for (int i = 0; i < countFeatures; i++) {
950  features[i] = pts[i];
951  featuresid[i] = fid[i];
952  if (fid[i] > max)
953  max = fid[i];
954  }
955 
956  globalcountFeatures = max + 1;
957  initialized = 1;
958 
959  cvCopy(I, image, 0);
960 }
961 
962 void vpKltOpencv::track(const IplImage *I)
963 {
964  if (!initialized) {
965  vpERROR_TRACE("KLT Not initialized");
966  throw(vpException(vpTrackingException::initializationError, "KLT Not initialized"));
967  }
968 
969  if (!I) {
970  throw(vpException(vpTrackingException::initializationError, "Image Not initialized"));
971  }
972 
973  if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
974  throw(vpException(vpTrackingException::initializationError, "Bad Image format"));
975  }
976 
977  CV_SWAP(prev_image, image, swap_temp);
978  CV_SWAP(prev_pyramid, pyramid, swap_temp);
979 
980  cvCopy(I, image, 0);
981 
982  if (!initial_guess) {
983  // Save current features as previous features
984  countPrevFeatures = countFeatures;
985  for (int boucle = 0; boucle < countFeatures; boucle++) {
986  prev_featuresid[boucle] = featuresid[boucle];
987  }
988 
989  CvPoint2D32f *swap_features = 0;
990  CV_SWAP(prev_features, features, swap_features);
991  }
992 
993  if (countFeatures <= 0)
994  return;
995 
996  cvCalcOpticalFlowPyrLK(prev_image, image, prev_pyramid, pyramid, prev_features, features, countFeatures,
997  cvSize(win_size, win_size), pyramid_level, status, 0,
998  cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03), flags);
999 
1000  if (!initial_guess)
1001  flags |= CV_LKFLOW_PYR_A_READY;
1002  else {
1003  flags = CV_LKFLOW_PYR_A_READY;
1004  initial_guess = false;
1005  }
1006 
1007  int i, k;
1008  for (i = k = 0; i < countFeatures; i++) {
1009  if (!status[i]) {
1010  lostDuringTrack[i] = 1;
1011  if (OnFeatureLost)
1012  OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
1013  continue;
1014  }
1015 
1016  if (IsFeatureValid) {
1017  if (!IsFeatureValid(_tid, features[i].x, features[i].y)) {
1018  lostDuringTrack[i] = 1;
1019  if (OnFeatureLost)
1020  OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
1021  continue;
1022  }
1023  }
1024  features[k] = features[i];
1025  featuresid[k] = featuresid[i];
1026 
1027  if (OnMeasureFeature)
1028  OnMeasureFeature(_tid, k, featuresid[k], features[k].x, features[k].y);
1029 
1030  lostDuringTrack[i] = 0;
1031  k++;
1032  }
1033  countFeatures = k;
1034 }
1035 
1043 void vpKltOpencv::display(const vpImage<unsigned char> &I, vpColor color, unsigned int thickness)
1044 {
1045  if ((features == 0) || (I.bitmap == 0) || (!initialized)) {
1046  vpERROR_TRACE(" Memory problem ");
1047  throw(vpException(vpException::memoryAllocationError, " Memory problem"));
1048  }
1049 
1050  vpKltOpencv::display(I, features, featuresid, countFeatures, color, thickness);
1051 }
1052 
1066 void vpKltOpencv::getFeature(int index, long &id, float &x, float &y) const
1067 {
1068  if (index >= countFeatures) {
1069  vpERROR_TRACE(" Memory problem ");
1070  throw(vpException(vpException::memoryAllocationError, " Memory problem"));
1071  }
1072 
1073  x = features[index].x;
1074  y = features[index].y;
1075  id = featuresid[index];
1076 }
1077 
1087 void vpKltOpencv::setInitialGuess(CvPoint2D32f **guess_pts)
1088 {
1089  // Save current features as previous features
1090  countPrevFeatures = countFeatures;
1091  for (int boucle = 0; boucle < countFeatures; boucle++) {
1092  prev_featuresid[boucle] = featuresid[boucle];
1093  }
1094 
1095  CvPoint2D32f *swap_features = NULL;
1096  CV_SWAP(prev_features, *guess_pts, swap_features);
1097 
1098  CV_SWAP(features, prev_features, swap_features);
1099 
1100  flags |= CV_LKFLOW_INITIAL_GUESSES;
1101 
1102  initial_guess = true;
1103 }
1104 
1120 void vpKltOpencv::setInitialGuess(CvPoint2D32f **init_pts, CvPoint2D32f **guess_pts, long *fid, int size)
1121 {
1122  countPrevFeatures = size;
1123  countFeatures = size;
1124  for (int boucle = 0; boucle < size; boucle++) {
1125  prev_featuresid[boucle] = fid[boucle];
1126  featuresid[boucle] = fid[boucle];
1127  }
1128 
1129  CvPoint2D32f *swap_features = NULL;
1130  CvPoint2D32f *swap_features2 = NULL;
1131  CV_SWAP(prev_features, *init_pts, swap_features);
1132 
1133  // if(swap_features) cvFree(&swap_features);
1134  // swap_features = NULL;
1135 
1136  CV_SWAP(features, *guess_pts, swap_features2);
1137 
1138  flags |= CV_LKFLOW_INITIAL_GUESSES;
1139 
1140  initial_guess = true;
1141 }
1142 
1151 void vpKltOpencv::getPrevFeature(int index, int &id, float &x, float &y) const
1152 {
1153  if (index >= countPrevFeatures) {
1154  vpERROR_TRACE(" Memory problem ");
1155  throw(vpException(vpException::memoryAllocationError, " Memory problem"));
1156  }
1157 
1158  x = prev_features[index].x;
1159  y = prev_features[index].y;
1160  id = prev_featuresid[index];
1161 }
1162 
1169 void vpKltOpencv::addFeature(const int &id, const float &x, const float &y)
1170 {
1171  if (maxFeatures == countFeatures) {
1172  vpERROR_TRACE(" Cannot add the feature ");
1173  return;
1174  }
1175 
1176  CvPoint2D32f f;
1177  f.x = x;
1178  f.y = y;
1179  features[countFeatures] = f;
1180  featuresid[countFeatures] = id;
1181  countFeatures++;
1182 }
1183 
1184 void vpKltOpencv::suppressFeature(int index)
1185 {
1186  if (index >= countFeatures) {
1187  vpERROR_TRACE(" Memory problem ");
1188  throw(vpException(vpException::memoryAllocationError, " Memory problem"));
1189  }
1190 
1191  countFeatures--;
1192 
1193  for (int i = index; i < countFeatures; i++) {
1194  features[i] = features[i + 1];
1195  featuresid[i] = featuresid[i + 1];
1196  }
1197 }
1198 
1213 void vpKltOpencv::display(const vpImage<unsigned char> &I, const CvPoint2D32f *features_list, const int &nbFeatures,
1214  vpColor color, unsigned int thickness)
1215 {
1216  vpImagePoint ip;
1217  for (int i = 0; i < nbFeatures; i++) {
1218  ip.set_u(vpMath::round(features_list[i].x));
1219  ip.set_v(vpMath::round(features_list[i].y));
1220  vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
1221  }
1222 }
1237 void vpKltOpencv::display(const vpImage<vpRGBa> &I, const CvPoint2D32f *features_list, const int &nbFeatures,
1238  vpColor color, unsigned int thickness)
1239 {
1240  vpImagePoint ip;
1241  for (int i = 0; i < nbFeatures; i++) {
1242  ip.set_u(vpMath::round(features_list[i].x));
1243  ip.set_v(vpMath::round(features_list[i].y));
1244  vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
1245  }
1246 }
1247 
1264 void vpKltOpencv::display(const vpImage<unsigned char> &I, const CvPoint2D32f *features_list,
1265  const long *featuresid_list, const int &nbFeatures, vpColor color, unsigned int thickness)
1266 {
1267  vpImagePoint ip;
1268  for (int i = 0; i < nbFeatures; i++) {
1269  ip.set_u(vpMath::round(features_list[i].x));
1270  ip.set_v(vpMath::round(features_list[i].y));
1271  vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
1272 
1273  std::stringstream id;
1274  id << featuresid_list[i];
1275  ip.set_u(vpMath::round(features_list[i].x + 5));
1276  vpDisplay::displayText(I, ip, id.str(), color);
1277  }
1278 }
1279 
1296 void vpKltOpencv::display(const vpImage<vpRGBa> &I, const CvPoint2D32f *features_list, const long *featuresid_list,
1297  const int &nbFeatures, vpColor color, unsigned int thickness)
1298 {
1299  vpImagePoint ip;
1300  for (int i = 0; i < nbFeatures; i++) {
1301  ip.set_u(vpMath::round(features_list[i].x));
1302  ip.set_v(vpMath::round(features_list[i].y));
1303  vpDisplay::displayCross(I, ip, 10, color, thickness);
1304 
1305  std::stringstream id;
1306  id << featuresid_list[i];
1307  ip.set_u(vpMath::round(features_list[i].x + 5));
1308  vpDisplay::displayText(I, ip, id.str(), color);
1309  }
1310 }
1311 #else
1312 
1313 // Work around to avoid visp_klt library empty when OpenCV is not installed
1314 // or used
1315 class VISP_EXPORT dummy_vpKltOpencv
1316 {
1317 public:
1318  dummy_vpKltOpencv(){};
1319 };
1320 
1321 #if !defined(VISP_BUILD_SHARED_LIBS)
1322 // Work around to avoid warning: libvisp_klt.a(vpKltOpenCV.cpp.o) has no
1323 // symbols
1324 void dummy_vpKltOpenCV_fct(){};
1325 #endif
1326 
1327 #endif
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
@ memoryAllocationError
Memory allocation error.
Definition: vpException.h:88
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
void set_u(double u)
Definition: vpImagePoint.h:335
void set_v(double v)
Definition: vpImagePoint.h:346
Type * bitmap
points toward the bitmap
Definition: vpImage.h:144
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:79
virtual ~vpKltOpencv()
std::vector< long > m_points_id
Keypoint id.
Definition: vpKltOpencv.h:163
int m_useHarrisDetector
Definition: vpKltOpencv.h:172
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void track(const cv::Mat &I)
double m_minDistance
Definition: vpKltOpencv.h:168
vpKltOpencv & operator=(const vpKltOpencv &copy)
Definition: vpKltOpencv.cpp:79
cv::TermCriteria m_termcrit
Definition: vpKltOpencv.h:165
double m_minEigThreshold
Definition: vpKltOpencv.h:169
void setHarrisFreeParameter(double harris_k)
void getFeature(const int &index, long &id, float &x, float &y) const
int m_pyrMaxLevel
Definition: vpKltOpencv.h:173
std::vector< cv::Point2f > m_points[2]
Previous [0] and current [1] keypoint location.
Definition: vpKltOpencv.h:162
double m_qualityLevel
Definition: vpKltOpencv.h:167
void suppressFeature(const int &index)
bool m_initial_guess
Definition: vpKltOpencv.h:175
cv::Mat m_gray
Definition: vpKltOpencv.h:161
void setMaxFeatures(int maxCount)
void setMinEigThreshold(double minEigThreshold)
void setInitialGuess(const std::vector< cv::Point2f > &guess_pts)
void addFeature(const float &x, const float &y)
double m_harris_k
Definition: vpKltOpencv.h:170
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
long m_next_points_id
Definition: vpKltOpencv.h:174
cv::Mat m_prevGray
Definition: vpKltOpencv.h:161
void setMinDistance(double minDistance)
void display(const vpImage< unsigned char > &I, const vpColor &color=vpColor::red, unsigned int thickness=1)
void setUseHarris(int useHarrisDetector)
void setWindowSize(int winSize)
void setPyramidLevels(int pyrMaxLevel)
static int round(double x)
Definition: vpMath.h:325
Error that can be emited by the vpTracker class and its derivates.
#define vpERROR_TRACE
Definition: vpDebug.h:393