Visual Servoing Platform  version 3.4.0
vpKltOpencv.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  * Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented
33  * with opencv.
34  *
35  * Authors:
36  * Fabien Servant
37  * Fabien Spindler
38  *
39  *****************************************************************************/
40 
48 #include <visp3/core/vpConfig.h>
49 
50 #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408)
51 
52 #include <string>
53 
54 #include <visp3/core/vpDisplay.h>
55 #include <visp3/core/vpTrackingException.h>
56 #include <visp3/klt/vpKltOpencv.h>
57 
62  : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01),
63  m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1),
64  m_pyrMaxLevel(3), m_next_points_id(0), m_initial_guess(false)
65 {
66  m_termcrit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 20, 0.03);
67 }
68 
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 
118 void vpKltOpencv::initTracking(const cv::Mat &I, const cv::Mat &mask)
119 {
120  m_next_points_id = 0;
121 
122  // cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
123  I.copyTo(m_gray);
124 
125  for (size_t i = 0; i < 2; i++) {
126  m_points[i].clear();
127  }
128 
129  m_points_id.clear();
130 
131  cv::goodFeaturesToTrack(m_gray, m_points[1], m_maxCount, m_qualityLevel, m_minDistance, mask, m_blockSize, false,
132  m_harris_k);
133 
134  if (m_points[1].size() > 0) {
135  cv::cornerSubPix(m_gray, m_points[1], cv::Size(m_winSize, m_winSize), cv::Size(-1, -1), m_termcrit);
136 
137  for (size_t i = 0; i < m_points[1].size(); i++)
138  m_points_id.push_back(m_next_points_id++);
139  }
140 }
141 
147 void vpKltOpencv::track(const cv::Mat &I)
148 {
149  if (m_points[1].size() == 0)
150  throw vpTrackingException(vpTrackingException::fatalError, "Not enough key points to track.");
151 
152  std::vector<float> err;
153  int flags = 0;
154 
155  cv::swap(m_prevGray, m_gray);
156 
157  if (m_initial_guess) {
158  flags |= cv::OPTFLOW_USE_INITIAL_FLOW;
159  m_initial_guess = false;
160  } else {
161  std::swap(m_points[1], m_points[0]);
162  }
163 
164  // cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
165  I.copyTo(m_gray);
166 
167  if (m_prevGray.empty()) {
168  m_gray.copyTo(m_prevGray);
169  }
170 
171  std::vector<uchar> status;
172 
173  cv::calcOpticalFlowPyrLK(m_prevGray, m_gray, m_points[0], m_points[1], status, err, cv::Size(m_winSize, m_winSize),
175 
176  // Remove points that are lost
177  for (int i = (int)status.size() - 1; i >= 0; i--) {
178  if (status[(size_t)i] == 0) { // point is lost
179  m_points[0].erase(m_points[0].begin() + i);
180  m_points[1].erase(m_points[1].begin() + i);
181  m_points_id.erase(m_points_id.begin() + i);
182  }
183  }
184 }
185 
199 void vpKltOpencv::getFeature(const int &index, long &id, float &x, float &y) const
200 {
201  if ((size_t)index >= m_points[1].size()) {
202  throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
203  }
204 
205  x = m_points[1][(size_t)index].x;
206  y = m_points[1][(size_t)index].y;
207  id = m_points_id[(size_t)index];
208 }
209 
217 void vpKltOpencv::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness)
218 {
219  vpKltOpencv::display(I, m_points[1], m_points_id, color, thickness);
220 }
221 
234 void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
235  const vpColor &color, unsigned int thickness)
236 {
237  vpImagePoint ip;
238  for (size_t i = 0; i < features.size(); i++) {
239  ip.set_u(vpMath::round(features[i].x));
240  ip.set_v(vpMath::round(features[i].y));
241  vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
242  }
243 }
244 
257 void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features, const vpColor &color,
258  unsigned int thickness)
259 {
260  vpImagePoint ip;
261  for (size_t i = 0; i < features.size(); i++) {
262  ip.set_u(vpMath::round(features[i].x));
263  ip.set_v(vpMath::round(features[i].y));
264  vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
265  }
266 }
267 
282 void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
283  const std::vector<long> &featuresid, const vpColor &color, unsigned int thickness)
284 {
285  vpImagePoint ip;
286  for (size_t i = 0; i < features.size(); i++) {
287  ip.set_u(vpMath::round(features[i].x));
288  ip.set_v(vpMath::round(features[i].y));
289  vpDisplay::displayCross(I, ip, 10, color, thickness);
290 
291  std::ostringstream id;
292  id << featuresid[i];
293  ip.set_u(vpMath::round(features[i].x + 5));
294  vpDisplay::displayText(I, ip, id.str(), color);
295  }
296 }
297 
312 void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
313  const std::vector<long> &featuresid, const vpColor &color, unsigned int thickness)
314 {
315  vpImagePoint ip;
316  for (size_t i = 0; i < features.size(); i++) {
317  ip.set_u(vpMath::round(features[i].x));
318  ip.set_v(vpMath::round(features[i].y));
319  vpDisplay::displayCross(I, ip, 10, color, thickness);
320 
321  std::ostringstream id;
322  id << featuresid[i];
323  ip.set_u(vpMath::round(features[i].x + 5));
324  vpDisplay::displayText(I, ip, id.str(), color);
325  }
326 }
327 
334 void vpKltOpencv::setMaxFeatures(int maxCount) { m_maxCount = maxCount; }
335 
343 void vpKltOpencv::setWindowSize(int winSize) { m_winSize = winSize; }
344 
356 void vpKltOpencv::setQuality(double qualityLevel) { m_qualityLevel = qualityLevel; }
357 
364 void vpKltOpencv::setHarrisFreeParameter(double harris_k) { m_harris_k = harris_k; }
365 
372 void vpKltOpencv::setUseHarris(int useHarrisDetector) { m_useHarrisDetector = useHarrisDetector; }
373 
381 void vpKltOpencv::setMinDistance(double minDistance) { m_minDistance = minDistance; }
382 
388 void vpKltOpencv::setMinEigThreshold(double minEigThreshold) { m_minEigThreshold = minEigThreshold; }
389 
399 void vpKltOpencv::setBlockSize(int blockSize) { m_blockSize = blockSize; }
400 
409 void vpKltOpencv::setPyramidLevels(int pyrMaxLevel) { m_pyrMaxLevel = pyrMaxLevel; }
410 
423 void vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &guess_pts)
424 {
425  if (guess_pts.size() != m_points[1].size()) {
427  "Cannot set initial guess: size feature vector [%d] "
428  "and guess vector [%d] doesn't match",
429  m_points[1].size(), guess_pts.size()));
430  }
431 
432  m_points[1] = guess_pts;
433  m_initial_guess = true;
434 }
435 
450 void vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &init_pts, const std::vector<cv::Point2f> &guess_pts,
451  const std::vector<long> &fid)
452 {
453  if (guess_pts.size() != init_pts.size()) {
455  "Cannot set initial guess: size init vector [%d] and "
456  "guess vector [%d] doesn't match",
457  init_pts.size(), guess_pts.size()));
458  }
459 
460  m_points[0] = init_pts;
461  m_points[1] = guess_pts;
462  m_points_id = fid;
463  m_initial_guess = true;
464 }
465 
474 void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts)
475 {
476  m_initial_guess = false;
477  m_points[1] = pts;
478  m_next_points_id = 0;
479  m_points_id.clear();
480  for (size_t i = 0; i < m_points[1].size(); i++) {
481  m_points_id.push_back(m_next_points_id++);
482  }
483 
484  I.copyTo(m_gray);
485 }
486 
487 void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids)
488 {
489  m_initial_guess = false;
490  m_points[1] = pts;
491  m_points_id.clear();
492 
493  if (ids.size() != pts.size()) {
494  m_next_points_id = 0;
495  for (size_t i = 0; i < m_points[1].size(); i++)
496  m_points_id.push_back(m_next_points_id++);
497  } else {
498  long max = 0;
499  for (size_t i = 0; i < m_points[1].size(); i++) {
500  m_points_id.push_back(ids[i]);
501  if (ids[i] > max)
502  max = ids[i];
503  }
504  m_next_points_id = max + 1;
505  }
506 
507  I.copyTo(m_gray);
508 }
509 
517 void vpKltOpencv::addFeature(const float &x, const float &y)
518 {
519  cv::Point2f f(x, y);
520  m_points[1].push_back(f);
521  m_points_id.push_back(m_next_points_id++);
522 }
523 
536 void vpKltOpencv::addFeature(const long &id, 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(id);
541  if (id >= m_next_points_id)
542  m_next_points_id = id + 1;
543 }
544 
552 void vpKltOpencv::addFeature(const cv::Point2f &f)
553 {
554  m_points[1].push_back(f);
555  m_points_id.push_back(m_next_points_id++);
556 }
557 
562 void vpKltOpencv::suppressFeature(const int &index)
563 {
564  if ((size_t)index >= m_points[1].size()) {
565  throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
566  }
567 
568  m_points[1].erase(m_points[1].begin() + index);
569  m_points_id.erase(m_points_id.begin() + index);
570 }
571 
572 #elif defined(VISP_HAVE_OPENCV)
573 
574 #include <string>
575 
576 #include <visp3/klt/vpKltOpencv.h>
577 
578 void vpKltOpencv::clean()
579 {
580  if (image)
581  cvReleaseImage(&image);
582  if (prev_image)
583  cvReleaseImage(&prev_image);
584  if (pyramid)
585  cvReleaseImage(&pyramid);
586  if (prev_pyramid)
587  cvReleaseImage(&prev_pyramid);
588 
589  image = NULL;
590  prev_image = NULL;
591  pyramid = NULL;
592  prev_pyramid = NULL;
593 
594  swap_temp = NULL;
595  countFeatures = 0;
596  countPrevFeatures = 0;
597  flags = 0;
598  initialized = 0;
599  globalcountFeatures = 0;
600 }
601 
602 void vpKltOpencv::cleanAll()
603 {
604  clean();
605  if (features)
606  cvFree(&features);
607  if (prev_features)
608  cvFree(&prev_features);
609  if (status)
610  cvFree(&status);
611  if (lostDuringTrack)
612  cvFree(&lostDuringTrack);
613  if (featuresid)
614  cvFree(&featuresid);
615  if (prev_featuresid)
616  cvFree(&prev_featuresid);
617  features = NULL;
618  prev_features = NULL;
619  status = NULL;
620  lostDuringTrack = 0;
621  featuresid = NULL;
622  prev_featuresid = NULL;
623 }
624 
625 void vpKltOpencv::reset() { clean(); }
626 
631  : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01), min_distance(10),
632  harris_free_parameter(0.04), block_size(3), use_harris(1), pyramid_level(3), _tid(-1), image(NULL),
633  prev_image(NULL), pyramid(NULL), prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
634  features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL), flags(0), initial_guess(false),
635  lostDuringTrack(0), status(0), OnInitialize(0), OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0),
636  IsFeatureValid(0)
637 {
638  features = (CvPoint2D32f *)cvAlloc((unsigned int)maxFeatures * sizeof(features[0]));
639  prev_features = (CvPoint2D32f *)cvAlloc((unsigned int)maxFeatures * sizeof(prev_features[0]));
640  status = (char *)cvAlloc((size_t)maxFeatures);
641  lostDuringTrack = (bool *)cvAlloc((size_t)maxFeatures);
642  featuresid = (long *)cvAlloc((unsigned int)maxFeatures * sizeof(long));
643  prev_featuresid = (long *)cvAlloc((unsigned int)maxFeatures * sizeof(long));
644 }
645 
650  : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01), min_distance(10),
651  harris_free_parameter(0.04), block_size(3), use_harris(1), pyramid_level(3), _tid(-1), image(NULL),
652  prev_image(NULL), pyramid(NULL), prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
653  features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL), flags(0), initial_guess(false),
654  lostDuringTrack(0), status(0), OnInitialize(0), OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0),
655  IsFeatureValid(0)
656 {
657  *this = copy;
658 }
659 
664 {
665  // Shallow copy of primitives
666  initialized = copy.initialized;
667  maxFeatures = copy.maxFeatures;
668  countFeatures = copy.countFeatures;
669  countPrevFeatures = copy.countPrevFeatures;
670  globalcountFeatures = copy.globalcountFeatures;
671  flags = copy.flags;
672  win_size = copy.win_size;
673  quality = copy.quality;
674  min_distance = copy.min_distance;
675  harris_free_parameter = copy.harris_free_parameter;
676  block_size = copy.block_size;
677  use_harris = copy.use_harris;
678  pyramid_level = copy.pyramid_level;
679  _tid = copy._tid;
680 
681  OnInitialize = copy.OnInitialize;
682  OnFeatureLost = copy.OnFeatureLost;
683  OnNewFeature = copy.OnNewFeature;
684  OnMeasureFeature = copy.OnMeasureFeature;
685  IsFeatureValid = copy.IsFeatureValid;
686 
687  initial_guess = copy.initial_guess;
688  lostDuringTrack = copy.lostDuringTrack;
689 
690  if (!initialized) {
691  status = 0;
692  lostDuringTrack = 0;
693  countFeatures = 0;
694  countPrevFeatures = 0;
695  flags = 0;
696  initialized = 0;
697  globalcountFeatures = 0;
698  }
699 
700  if (copy.image) {
701  image = cvCreateImage(cvGetSize(copy.image), 8, 1);
702  // /*IplImage **/cvCopyImage(copy.image,image);
703  cvCopy(copy.image, image, 0);
704  }
705 
706  if (copy.prev_image) {
707  prev_image = cvCreateImage(cvGetSize(copy.prev_image), IPL_DEPTH_8U, 1);
708  // /*IplImage **/ cvCopyImage(copy.prev_image,prev_image);
709  cvCopy(copy.prev_image, prev_image, 0);
710  }
711 
712  if (copy.pyramid) {
713  pyramid = cvCreateImage(cvGetSize(copy.pyramid), IPL_DEPTH_8U, 1);
714  // /*IplImage **/cvCopyImage(copy.pyramid,pyramid);
715  cvCopy(copy.pyramid, pyramid, 0);
716  }
717 
718  if (copy.prev_pyramid) {
719  prev_pyramid = cvCreateImage(cvGetSize(copy.prev_pyramid), IPL_DEPTH_8U, 1);
720  // /*IplImage **/cvCopyImage(copy.prev_pyramid,prev_pyramid);
721  cvCopy(copy.prev_pyramid, prev_pyramid, 0);
722  }
723 
724  // Deep copy of arrays
725  if (copy.features) {
726  /*CvPoint2D32f **/ features = (CvPoint2D32f *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(CvPoint2D32f));
727  for (int i = 0; i < copy.maxFeatures; i++)
728  features[i] = copy.features[i];
729  }
730 
731  if (copy.prev_features) {
732  /*CvPoint2D32f **/ prev_features = (CvPoint2D32f *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(CvPoint2D32f));
733  for (int i = 0; i < copy.maxFeatures; i++)
734  prev_features[i] = copy.prev_features[i];
735  }
736 
737  if (copy.featuresid) {
738  /*long **/ featuresid = (long *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(long));
739  for (int i = 0; i < copy.maxFeatures; i++)
740  featuresid[i] = copy.featuresid[i];
741  }
742 
743  if (copy.prev_featuresid) {
744  /*long **/ prev_featuresid = (long *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(long));
745  for (int i = 0; i < copy.maxFeatures; i++)
746  prev_featuresid[i] = copy.prev_featuresid[i];
747  }
748 
749  if (copy.status) {
750  /*char **/ status = (char *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(char));
751  for (int i = 0; i < copy.maxFeatures; i++)
752  status[i] = copy.status[i];
753  }
754 
755  if (copy.lostDuringTrack) {
756  /*bool **/ lostDuringTrack = (bool *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(bool));
757  for (int i = 0; i < copy.maxFeatures; i++)
758  lostDuringTrack[i] = copy.lostDuringTrack[i];
759  }
760 
761  return *this;
762 }
763 
764 vpKltOpencv::~vpKltOpencv() { cleanAll(); }
765 
773 void vpKltOpencv::setMaxFeatures(int input)
774 {
775  initialized = 0;
776  maxFeatures = input;
777 
778  if (features)
779  cvFree(&features);
780  if (prev_features)
781  cvFree(&prev_features);
782  if (status)
783  cvFree(&status);
784  if (lostDuringTrack)
785  cvFree(&lostDuringTrack);
786  if (featuresid)
787  cvFree(&featuresid);
788  if (prev_featuresid)
789  cvFree(&prev_featuresid);
790 
791  features = (CvPoint2D32f *)cvAlloc((unsigned int)maxFeatures * sizeof(CvPoint2D32f));
792  prev_features = (CvPoint2D32f *)cvAlloc((unsigned int)maxFeatures * sizeof(CvPoint2D32f));
793  status = (char *)cvAlloc((unsigned int)maxFeatures * sizeof(char));
794  lostDuringTrack = (bool *)cvAlloc((unsigned int)maxFeatures * sizeof(bool));
795  featuresid = (long *)cvAlloc((unsigned int)maxFeatures * sizeof(long));
796  prev_featuresid = (long *)cvAlloc((unsigned int)maxFeatures * sizeof(long));
797 }
798 
809 void vpKltOpencv::initTracking(const IplImage *I, const IplImage *mask)
810 {
811  if (!I) {
812  throw(vpException(vpTrackingException::initializationError, "Image Not initialized"));
813  }
814 
815  if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
816  throw(vpException(vpTrackingException::initializationError, "Bad Image format"));
817  }
818 
819  if (mask) {
820  if (mask->depth != IPL_DEPTH_8U || I->nChannels != 1) {
821  throw(vpException(vpTrackingException::initializationError, "Bad Image format"));
822  }
823  }
824 
825  // Creation des buffers
826  CvSize Sizeim, SizeI;
827  SizeI = cvGetSize(I);
828  bool b_imOK = true;
829  if (image != NULL) {
830  Sizeim = cvGetSize(image);
831  if (SizeI.width != Sizeim.width || SizeI.height != Sizeim.height)
832  b_imOK = false;
833  }
834  if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) {
835  reset();
836  image = cvCreateImage(cvGetSize(I), 8, 1);
837  image->origin = I->origin;
838  prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
839  pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
840  prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
841  } else {
842  swap_temp = 0;
843  countFeatures = 0;
844  countPrevFeatures = 0;
845  flags = 0;
846  initialized = 0;
847  globalcountFeatures = 0;
848  }
849 
850  initialized = 1;
851 
852  // Import
853  cvCopy(I, image, 0);
854 
855  // Recherche de points d'interets
856  countFeatures = maxFeatures;
857  countPrevFeatures = 0;
858  IplImage *eig = cvCreateImage(cvGetSize(image), 32, 1);
859  IplImage *temp = cvCreateImage(cvGetSize(image), 32, 1);
860  cvGoodFeaturesToTrack(image, eig, temp, features, &countFeatures, quality, min_distance, mask, block_size, use_harris,
861  harris_free_parameter);
862  cvFindCornerSubPix(image, features, countFeatures, cvSize(win_size, win_size), cvSize(-1, -1),
863  cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
864  cvReleaseImage(&eig);
865  cvReleaseImage(&temp);
866 
867  if (OnInitialize)
868  OnInitialize(_tid);
869 
870  // printf("Number of features at init: %d\n", countFeatures);
871  for (int boucle = 0; boucle < countFeatures; boucle++) {
872  featuresid[boucle] = globalcountFeatures;
873  globalcountFeatures++;
874 
875  if (OnNewFeature) {
876  OnNewFeature(_tid, boucle, featuresid[boucle], features[boucle].x, features[boucle].y);
877  }
878  }
879 }
880 
889 void vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, int size)
890 {
891  if (size > maxFeatures)
892  throw(vpException(vpTrackingException::initializationError, "Cannot initialize tracker from points"));
893 
894  // Creation des buffers
895  CvSize Sizeim, SizeI;
896  SizeI = cvGetSize(I);
897  bool b_imOK = true;
898  if (image != NULL) {
899  Sizeim = cvGetSize(image);
900  if (SizeI.width != Sizeim.width || SizeI.height != Sizeim.height)
901  b_imOK = false;
902  }
903  if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) {
904  reset();
905  image = cvCreateImage(cvGetSize(I), 8, 1);
906  image->origin = I->origin;
907  prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
908  pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
909  prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
910  } else {
911  flags = 0;
912  }
913  // Save current features as previous features
914  countFeatures = size;
915  for (int i = 0; i < countFeatures; i++) {
916  features[i] = pts[i];
917  featuresid[i] = i;
918  }
919 
920  globalcountFeatures = size;
921  initialized = 1;
922 
923  cvCopy(I, image, 0);
924 }
925 
926 void vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, long *fid, int size)
927 {
928  if (size > maxFeatures)
929  throw(vpException(vpTrackingException::initializationError, "Cannot initialize tracker from points"));
930 
931  // Creation des buffers
932  CvSize Sizeim, SizeI;
933  SizeI = cvGetSize(I);
934  bool b_imOK = true;
935  if (image != NULL) {
936  Sizeim = cvGetSize(image);
937  if (SizeI.width != Sizeim.width || SizeI.height != Sizeim.height)
938  b_imOK = false;
939  }
940  if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) {
941  reset();
942  image = cvCreateImage(cvGetSize(I), 8, 1);
943  image->origin = I->origin;
944  prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
945  pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
946  prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
947  } else {
948  flags = 0;
949  }
950  // Save current features as previous features
951  countFeatures = size;
952  long max = 0;
953  for (int i = 0; i < countFeatures; i++) {
954  features[i] = pts[i];
955  featuresid[i] = fid[i];
956  if (fid[i] > max)
957  max = fid[i];
958  }
959 
960  globalcountFeatures = max + 1;
961  initialized = 1;
962 
963  cvCopy(I, image, 0);
964 }
965 
966 void vpKltOpencv::track(const IplImage *I)
967 {
968  if (!initialized) {
969  vpERROR_TRACE("KLT Not initialized");
970  throw(vpException(vpTrackingException::initializationError, "KLT Not initialized"));
971  }
972 
973  if (!I) {
974  throw(vpException(vpTrackingException::initializationError, "Image Not initialized"));
975  }
976 
977  if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
978  throw(vpException(vpTrackingException::initializationError, "Bad Image format"));
979  }
980 
981  CV_SWAP(prev_image, image, swap_temp);
982  CV_SWAP(prev_pyramid, pyramid, swap_temp);
983 
984  cvCopy(I, image, 0);
985 
986  if (!initial_guess) {
987  // Save current features as previous features
988  countPrevFeatures = countFeatures;
989  for (int boucle = 0; boucle < countFeatures; boucle++) {
990  prev_featuresid[boucle] = featuresid[boucle];
991  }
992 
993  CvPoint2D32f *swap_features = 0;
994  CV_SWAP(prev_features, features, swap_features);
995  }
996 
997  if (countFeatures <= 0)
998  return;
999 
1000  cvCalcOpticalFlowPyrLK(prev_image, image, prev_pyramid, pyramid, prev_features, features, countFeatures,
1001  cvSize(win_size, win_size), pyramid_level, status, 0,
1002  cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03), flags);
1003 
1004  if (!initial_guess)
1005  flags |= CV_LKFLOW_PYR_A_READY;
1006  else {
1007  flags = CV_LKFLOW_PYR_A_READY;
1008  initial_guess = false;
1009  }
1010 
1011  int i, k;
1012  for (i = k = 0; i < countFeatures; i++) {
1013  if (!status[i]) {
1014  lostDuringTrack[i] = 1;
1015  if (OnFeatureLost)
1016  OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
1017  continue;
1018  }
1019 
1020  if (IsFeatureValid) {
1021  if (!IsFeatureValid(_tid, features[i].x, features[i].y)) {
1022  lostDuringTrack[i] = 1;
1023  if (OnFeatureLost)
1024  OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
1025  continue;
1026  }
1027  }
1028  features[k] = features[i];
1029  featuresid[k] = featuresid[i];
1030 
1031  if (OnMeasureFeature)
1032  OnMeasureFeature(_tid, k, featuresid[k], features[k].x, features[k].y);
1033 
1034  lostDuringTrack[i] = 0;
1035  k++;
1036  }
1037  countFeatures = k;
1038 }
1039 
1047 void vpKltOpencv::display(const vpImage<unsigned char> &I, vpColor color, unsigned int thickness)
1048 {
1049  if ((features == 0) || (I.bitmap == 0) || (!initialized)) {
1050  vpERROR_TRACE(" Memory problem ");
1051  throw(vpException(vpException::memoryAllocationError, " Memory problem"));
1052  }
1053 
1054  vpKltOpencv::display(I, features, featuresid, countFeatures, color, thickness);
1055 }
1056 
1070 void vpKltOpencv::getFeature(int index, long &id, float &x, float &y) const
1071 {
1072  if (index >= countFeatures) {
1073  vpERROR_TRACE(" Memory problem ");
1074  throw(vpException(vpException::memoryAllocationError, " Memory problem"));
1075  }
1076 
1077  x = features[index].x;
1078  y = features[index].y;
1079  id = featuresid[index];
1080 }
1081 
1091 void vpKltOpencv::setInitialGuess(CvPoint2D32f **guess_pts)
1092 {
1093  // Save current features as previous features
1094  countPrevFeatures = countFeatures;
1095  for (int boucle = 0; boucle < countFeatures; boucle++) {
1096  prev_featuresid[boucle] = featuresid[boucle];
1097  }
1098 
1099  CvPoint2D32f *swap_features = NULL;
1100  CV_SWAP(prev_features, *guess_pts, swap_features);
1101 
1102  CV_SWAP(features, prev_features, swap_features);
1103 
1104  flags |= CV_LKFLOW_INITIAL_GUESSES;
1105 
1106  initial_guess = true;
1107 }
1108 
1124 void vpKltOpencv::setInitialGuess(CvPoint2D32f **init_pts, CvPoint2D32f **guess_pts, long *fid, int size)
1125 {
1126  countPrevFeatures = size;
1127  countFeatures = size;
1128  for (int boucle = 0; boucle < size; boucle++) {
1129  prev_featuresid[boucle] = fid[boucle];
1130  featuresid[boucle] = fid[boucle];
1131  }
1132 
1133  CvPoint2D32f *swap_features = NULL;
1134  CvPoint2D32f *swap_features2 = NULL;
1135  CV_SWAP(prev_features, *init_pts, swap_features);
1136 
1137  // if(swap_features) cvFree(&swap_features);
1138  // swap_features = NULL;
1139 
1140  CV_SWAP(features, *guess_pts, swap_features2);
1141 
1142  flags |= CV_LKFLOW_INITIAL_GUESSES;
1143 
1144  initial_guess = true;
1145 }
1146 
1155 void vpKltOpencv::getPrevFeature(int index, int &id, float &x, float &y) const
1156 {
1157  if (index >= countPrevFeatures) {
1158  vpERROR_TRACE(" Memory problem ");
1159  throw(vpException(vpException::memoryAllocationError, " Memory problem"));
1160  }
1161 
1162  x = prev_features[index].x;
1163  y = prev_features[index].y;
1164  id = prev_featuresid[index];
1165 }
1166 
1173 void vpKltOpencv::addFeature(const int &id, const float &x, const float &y)
1174 {
1175  if (maxFeatures == countFeatures) {
1176  vpERROR_TRACE(" Cannot add the feature ");
1177  return;
1178  }
1179 
1180  CvPoint2D32f f;
1181  f.x = x;
1182  f.y = y;
1183  features[countFeatures] = f;
1184  featuresid[countFeatures] = id;
1185  countFeatures++;
1186 }
1187 
1188 void vpKltOpencv::suppressFeature(int index)
1189 {
1190  if (index >= countFeatures) {
1191  vpERROR_TRACE(" Memory problem ");
1192  throw(vpException(vpException::memoryAllocationError, " Memory problem"));
1193  }
1194 
1195  countFeatures--;
1196 
1197  for (int i = index; i < countFeatures; i++) {
1198  features[i] = features[i + 1];
1199  featuresid[i] = featuresid[i + 1];
1200  }
1201 }
1202 
1217 void vpKltOpencv::display(const vpImage<unsigned char> &I, const CvPoint2D32f *features_list, const int &nbFeatures,
1218  vpColor color, unsigned int thickness)
1219 {
1220  vpImagePoint ip;
1221  for (int i = 0; i < nbFeatures; i++) {
1222  ip.set_u(vpMath::round(features_list[i].x));
1223  ip.set_v(vpMath::round(features_list[i].y));
1224  vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
1225  }
1226 }
1241 void vpKltOpencv::display(const vpImage<vpRGBa> &I, const CvPoint2D32f *features_list, const int &nbFeatures,
1242  vpColor color, unsigned int thickness)
1243 {
1244  vpImagePoint ip;
1245  for (int i = 0; i < nbFeatures; i++) {
1246  ip.set_u(vpMath::round(features_list[i].x));
1247  ip.set_v(vpMath::round(features_list[i].y));
1248  vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
1249  }
1250 }
1251 
1268 void vpKltOpencv::display(const vpImage<unsigned char> &I, const CvPoint2D32f *features_list,
1269  const long *featuresid_list, const int &nbFeatures, vpColor color, unsigned int thickness)
1270 {
1271  vpImagePoint ip;
1272  for (int i = 0; i < nbFeatures; i++) {
1273  ip.set_u(vpMath::round(features_list[i].x));
1274  ip.set_v(vpMath::round(features_list[i].y));
1275  vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
1276 
1277  char id[10];
1278  sprintf(id, "%ld", featuresid_list[i]);
1279  ip.set_u(vpMath::round(features_list[i].x + 5));
1280  vpDisplay::displayText(I, ip, id, color);
1281  }
1282 }
1283 
1300 void vpKltOpencv::display(const vpImage<vpRGBa> &I, const CvPoint2D32f *features_list, const long *featuresid_list,
1301  const int &nbFeatures, vpColor color, unsigned int thickness)
1302 {
1303  vpImagePoint ip;
1304  for (int i = 0; i < nbFeatures; i++) {
1305  ip.set_u(vpMath::round(features_list[i].x));
1306  ip.set_v(vpMath::round(features_list[i].y));
1307  vpDisplay::displayCross(I, ip, 10, color, thickness);
1308 
1309  char id[10];
1310  sprintf(id, "%ld", featuresid_list[i]);
1311  ip.set_u(vpMath::round(features_list[i].x + 5));
1312  vpDisplay::displayText(I, ip, id, color);
1313  }
1314 }
1315 #else
1316 
1317 // Work arround to avoid visp_klt library empty when OpenCV is not installed
1318 // or used
1319 class VISP_EXPORT dummy_vpKltOpencv
1320 {
1321 public:
1322  dummy_vpKltOpencv(){};
1323 };
1324 
1325 #if !defined(VISP_BUILD_SHARED_LIBS)
1326 // Work arround to avoid warning: libvisp_klt.a(vpKltOpenCV.cpp.o) has no
1327 // symbols
1328 void dummy_vpKltOpenCV_fct(){};
1329 #endif
1330 
1331 #endif
void addFeature(const float &x, const float &y)
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
double m_harris_k
Definition: vpKltOpencv.h:170
cv::Mat m_gray
Definition: vpKltOpencv.h:161
void setMaxFeatures(int maxCount)
void setHarrisFreeParameter(double harris_k)
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
void set_u(double u)
Definition: vpImagePoint.h:225
vpKltOpencv & operator=(const vpKltOpencv &copy)
Definition: vpKltOpencv.cpp:83
cv::Mat m_prevGray
Definition: vpKltOpencv.h:161
#define vpERROR_TRACE
Definition: vpDebug.h:393
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
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:71
virtual ~vpKltOpencv()
cv::TermCriteria m_termcrit
Definition: vpKltOpencv.h:165
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:175
std::vector< long > m_points_id
Keypoint id.
Definition: vpKltOpencv.h:163
Error that can be emited by the vpTracker class and its derivates.
void setPyramidLevels(int pyrMaxLevel)
Memory allocation error.
Definition: vpException.h:88
void setWindowSize(int winSize)
double m_minDistance
Definition: vpKltOpencv.h:168
double m_minEigThreshold
Definition: vpKltOpencv.h:169
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
void setInitialGuess(const std::vector< cv::Point2f > &guess_pts)
double m_qualityLevel
Definition: vpKltOpencv.h:167
int m_useHarrisDetector
Definition: vpKltOpencv.h:172
static int round(double x)
Definition: vpMath.h:245
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
void set_v(double v)
Definition: vpImagePoint.h:236
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:78
void suppressFeature(const int &index)
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:162
void setUseHarris(int useHarrisDetector)
long m_next_points_id
Definition: vpKltOpencv.h:174
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
void setBlockSize(int blockSize)
int m_pyrMaxLevel
Definition: vpKltOpencv.h:173
void track(const cv::Mat &I)