Visual Servoing Platform  version 3.2.1 under development (2019-11-19)
vpKltOpencv.h
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 #ifndef vpKltOpencv_h
49 #define vpKltOpencv_h
50 
51 #include <visp3/core/vpColor.h>
52 #include <visp3/core/vpConfig.h>
53 #include <visp3/core/vpImage.h>
54 
55 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408))
56 
57 #include <opencv2/highgui/highgui.hpp>
58 #include <opencv2/imgproc/imgproc.hpp>
59 #include <opencv2/video/tracking.hpp>
60 
78 class VISP_EXPORT vpKltOpencv
79 {
80 public:
81  vpKltOpencv();
82  vpKltOpencv(const vpKltOpencv &copy);
83  virtual ~vpKltOpencv();
84 
85  void addFeature(const float &x, const float &y);
86  void addFeature(const long &id, const float &x, const float &y);
87  void addFeature(const cv::Point2f &f);
88 
89  void display(const vpImage<unsigned char> &I, const vpColor &color = vpColor::red, unsigned int thickness = 1);
90  static void display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
91  const vpColor &color = vpColor::green, unsigned int thickness = 1);
92  static void display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
93  const vpColor &color = vpColor::green, unsigned int thickness = 1);
94  static void display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
95  const std::vector<long> &featuresid, const vpColor &color = vpColor::green,
96  unsigned int thickness = 1);
97  static void display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
98  const std::vector<long> &featuresid, const vpColor &color = vpColor::green,
99  unsigned int thickness = 1);
100 
102  int getBlockSize() const { return m_blockSize; }
103  void getFeature(const int &index, long &id, float &x, float &y) const;
105  std::vector<cv::Point2f> getFeatures() const { return m_points[1]; }
106  // CvPoint2D32f* getFeatures() const {return features;}
108  std::vector<long> getFeaturesId() const { return m_points_id; }
109  // long* getFeaturesId() const {return featuresid;}
111  double getHarrisFreeParameter() const { return m_harris_k; }
113  // bool *getListOfLostFeature() const { return lostDuringTrack; }
115  int getMaxFeatures() const { return m_maxCount; }
118  double getMinDistance() const { return m_minDistance; }
120  int getNbFeatures() const { return (int)m_points[1].size(); }
122  int getNbPrevFeatures() const { return (int)m_points[0].size(); }
123  // void getPrevFeature(int index, int &id, float &x, float &y) const;
125  std::vector<cv::Point2f> getPrevFeatures() const { return m_points[0]; }
126  // CvPoint2D32f* getPrevFeatures() const {return prev_features;}
128  // long* getPrevFeaturesId() const {return prev_featuresid;}
130  int getPyramidLevels() const { return m_pyrMaxLevel; }
133  double getQuality() const { return m_qualityLevel; }
135  int getWindowSize() const { return m_winSize; }
136 
137  void initTracking(const cv::Mat &I, const cv::Mat &mask = cv::Mat());
138  void initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts);
139  void initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids);
140 
141  vpKltOpencv &operator=(const vpKltOpencv &copy);
142  void track(const cv::Mat &I);
143  void setBlockSize(const int blockSize);
144  void setHarrisFreeParameter(double harris_k);
145  void setInitialGuess(const std::vector<cv::Point2f> &guess_pts);
146  void setInitialGuess(const std::vector<cv::Point2f> &init_pts, const std::vector<cv::Point2f> &guess_pts,
147  const std::vector<long> &fid);
148  void setMaxFeatures(const int maxCount);
149  void setMinDistance(double minDistance);
150  void setMinEigThreshold(double minEigThreshold);
151  void setPyramidLevels(const int pyrMaxLevel);
152  void setQuality(double qualityLevel);
155  void setTrackerId(int tid) { (void)tid; }
156  void setUseHarris(const int useHarrisDetector);
157  void setWindowSize(const int winSize);
158  void suppressFeature(const int &index);
159 
160 protected:
161  cv::Mat m_gray, m_prevGray;
162  std::vector<cv::Point2f> m_points[2];
163  std::vector<long> m_points_id;
165  cv::TermCriteria m_termcrit;
170  double m_harris_k;
176 };
177 
178 #elif defined(VISP_HAVE_OPENCV)
179 #ifdef _CH_
180 #pragma package < opencv >
181 #endif
182 
183 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
184 #include <opencv2/highgui/highgui.hpp>
185 #include <opencv2/imgproc/imgproc.hpp>
186 #include <opencv2/legacy/legacy.hpp>
187 #include <opencv2/video/tracking.hpp>
188 #else
189 #ifndef _EiC
190 #include <ctype.h>
191 #include <cv.h>
192 #include <highgui.h>
193 #include <stdio.h>
194 #endif
195 #endif
196 
197 #include <visp3/core/vpConfig.h>
198 #include <visp3/core/vpDebug.h>
199 #include <visp3/core/vpDisplay.h>
200 #include <visp3/core/vpException.h>
201 #include <visp3/core/vpImage.h>
202 #include <visp3/core/vpTrackingException.h>
203 
204 typedef int (*funccheck)(int, double, double);
205 typedef void (*funcinfo)(int, int, int, double, double);
206 typedef void (*funcevent)(int);
207 
223 class VISP_EXPORT vpKltOpencv
224 {
225 private:
226  int initialized; // Is the tracker ready ?
227 
228  int maxFeatures; // Maximum number of features to track (Default 50)
229  int globalcountFeatures; // Keep over time for ID
230 
231  int win_size; // Size of search window for tracker (default 10)
232  double quality; // Multiplier for the maxmin eigenvalue; specifies minimal
233  // accepted quality of image corners (default 0.01)
234  double min_distance; // Limit, specifying minimum possible distance between
235  // returned corners; Euclidian distance is used.
236  // (default 10)
237  double harris_free_parameter; // Harris detector free parameter. (default 0.04)
238  int block_size; // Size of the averaging block used by the corner detector
239  // (default 3)
240  int use_harris; // 0 use a simple Minimum EigenValue Detector, != 0 use
241  // Harris (default 1)
242  int pyramid_level; // Number of level for the tracker's gaussian pyramid
243  // data (default 3)
244  int _tid; // tracker id for multiple trackers
245 
246  IplImage *image; // Image buffer
247  IplImage *prev_image; // Image buffer for the previous iteration
248  IplImage *pyramid; // Gaussian pyramid data
249  IplImage *prev_pyramid; // Gaussian pyramid data for the previous iteration
250  IplImage *swap_temp; // Internal
251 
252  int countFeatures; // Currently tracked features
253  int countPrevFeatures; // Previously tracked features
254 
255  CvPoint2D32f *features; // List of features
256  CvPoint2D32f *prev_features; // List of features for previous iteration
257 
258  long *featuresid; // Array of ids for features
259  long *prev_featuresid; // Array of ids for previous features
260 
261  int flags; // Flags for tracking (internal)
262 
263  bool initial_guess; // Bool to precise if the next call to track() uses an
264  // initial guess
265 
266  bool *lostDuringTrack; // Result of the tracker for every feature : 1 =
267  // lost, 0 = still present
268  char *status; // Result of the tracker for every features : 0 = lost, 1 =
269  // found
270 
271  // EVENT FUNCTION POINTERS
272  funcevent OnInitialize;
273  funcinfo OnFeatureLost;
274  funcinfo OnNewFeature;
275  funcinfo OnMeasureFeature;
276  funccheck IsFeatureValid;
277 
278 private:
279  // Internal
280  void clean();
281  void cleanAll();
282  void reset();
283 
284 public:
285  vpKltOpencv();
286  vpKltOpencv(const vpKltOpencv &copy);
287  virtual ~vpKltOpencv();
288 
289  void addFeature(const int &id, const float &x, const float &y);
290 
291  // Draw the tracked features on the given image
292  void display(const vpImage<unsigned char> &I, vpColor color = vpColor::red, unsigned int thickness = 1);
293 
295  int getBlockSize() const { return block_size; }
296  void getFeature(int index, long &id, float &x, float &y) const;
298  CvPoint2D32f *getFeatures() const { return features; }
300  long *getFeaturesId() const { return featuresid; }
302  double getHarrisFreeParameter() const { return harris_free_parameter; }
304  bool *getListOfLostFeature() const { return lostDuringTrack; }
306  int getMaxFeatures() const { return maxFeatures; }
308  double getMinDistance() const { return min_distance; }
310  int getNbFeatures() const { return countFeatures; }
312  int getNbPrevFeatures() const { return countPrevFeatures; }
313  void getPrevFeature(int index, int &id, float &x, float &y) const;
315  CvPoint2D32f *getPrevFeatures() const { return prev_features; }
317  long *getPrevFeaturesId() const { return prev_featuresid; }
319  int getPyramidLevels() const { return pyramid_level; }
321  double getQuality() const { return quality; }
323  int getWindowSize() const { return win_size; }
324 
325  // Detect corners in the image. Initialize the tracker
326  void initTracking(const IplImage *I, const IplImage *mask = NULL);
327  void initTracking(const IplImage *I, CvPoint2D32f *pts, int size);
328  void initTracking(const IplImage *I, CvPoint2D32f *pts, long *fid, int size);
329  vpKltOpencv &operator=(const vpKltOpencv &copy);
330  // Track !
331  void track(const IplImage *I);
332 
333  // Seters
342  void setBlockSize(const int input)
343  {
344  initialized = 0;
345  block_size = input;
346  }
355  void setHarrisFreeParameter(double input)
356  {
357  initialized = 0;
358  harris_free_parameter = input;
359  }
360  void setInitialGuess(CvPoint2D32f **guess_pts);
361  void setInitialGuess(CvPoint2D32f **init_pts, CvPoint2D32f **guess_pts, long *fid, int size);
366  void setIsFeatureValid(funccheck input) { IsFeatureValid = input; }
367 
368  /* Should be used only before initTracking */
369  void setMaxFeatures(const int input);
378  void setMinDistance(double input)
379  {
380  initialized = 0;
381  min_distance = input;
382  }
383 
384  // Functors
385 
386  // Event when tracker is initialized -> event(id_tracker)
387  void setOnInitialize(funcevent input) { OnInitialize = input; }
388  // Event when a feature is lost -> event(id_tracker, index, uid, x, y)
389  void setOnFeatureLost(funcinfo input) { OnFeatureLost = input; }
390  // Event when a new feature is found -> event(id_tracker, index, uid, x, y)
391  void setOnNewFeature(funcinfo input) { OnNewFeature = input; }
392  // Event when a feature is found while tracking -> event(id_tracker, index,
393  // uid, x, y)
394  void setOnMeasureFeature(funcinfo input) { OnMeasureFeature = input; }
404  void setPyramidLevels(const int input)
405  {
406  initialized = 0;
407  pyramid_level = input;
408  }
409  void setQuality(double input)
410  {
411  initialized = 0;
412  quality = input;
413  }
414  void setTrackerId(int tid) { _tid = tid; }
423  void setUseHarris(const int input)
424  {
425  initialized = 0;
426  use_harris = input;
427  }
428  void setWindowSize(const int input)
429  {
430  initialized = 0;
431  win_size = input;
432  }
433 
434  void suppressFeature(int index);
435 
436  // Static Functions
437 public:
438  static void display(const vpImage<unsigned char> &I, const CvPoint2D32f *features_list, const int &nbFeatures,
439  vpColor color = vpColor::green, unsigned int thickness = 1);
440  static void display(const vpImage<vpRGBa> &I, const CvPoint2D32f *features_list, const int &nbFeatures,
441  vpColor color = vpColor::green, unsigned int thickness = 1);
442 
443  static void display(const vpImage<unsigned char> &I, const CvPoint2D32f *features_list, const long *featuresid_list,
444  const int &nbFeatures, vpColor color = vpColor::green, unsigned int thickness = 1);
445  static void display(const vpImage<vpRGBa> &I, const CvPoint2D32f *features_list, const long *featuresid_list,
446  const int &nbFeatures, vpColor color = vpColor::green, unsigned int thickness = 1);
447 };
448 
449 #endif
450 #endif
double m_harris_k
Definition: vpKltOpencv.h:170
cv::Mat m_prevGray
Definition: vpKltOpencv.h:161
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
std::vector< cv::Point2f > getFeatures() const
Get the list of current features.
Definition: vpKltOpencv.h:105
int getNbFeatures() const
Get the number of current features.
Definition: vpKltOpencv.h:120
cv::TermCriteria m_termcrit
Definition: vpKltOpencv.h:165
static const vpColor green
Definition: vpColor.h:183
static const vpColor red
Definition: vpColor.h:180
bool m_initial_guess
Definition: vpKltOpencv.h:175
int getWindowSize() const
Get the window size used to refine the corner locations.
Definition: vpKltOpencv.h:135
std::vector< long > m_points_id
Keypoint id.
Definition: vpKltOpencv.h:163
int getMaxFeatures() const
Get the list of lost feature.
Definition: vpKltOpencv.h:115
void setTrackerId(int tid)
Definition: vpKltOpencv.h:155
int getPyramidLevels() const
Get the list of features id.
Definition: vpKltOpencv.h:130
double getMinDistance() const
Definition: vpKltOpencv.h:118
double m_minDistance
Definition: vpKltOpencv.h:168
double m_minEigThreshold
Definition: vpKltOpencv.h:169
double m_qualityLevel
Definition: vpKltOpencv.h:167
int m_useHarrisDetector
Definition: vpKltOpencv.h:172
double getHarrisFreeParameter() const
Get the free parameter of the Harris detector.
Definition: vpKltOpencv.h:111
int getNbPrevFeatures() const
Get the number of previous features.
Definition: vpKltOpencv.h:122
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:78
std::vector< long > getFeaturesId() const
Get the unique id of each feature.
Definition: vpKltOpencv.h:108
long m_next_points_id
Definition: vpKltOpencv.h:174
double getQuality() const
Definition: vpKltOpencv.h:133
int getBlockSize() const
Get the size of the averaging block used to track the features.
Definition: vpKltOpencv.h:102
int m_pyrMaxLevel
Definition: vpKltOpencv.h:173
std::vector< cv::Point2f > getPrevFeatures() const
Get the list of previous features.
Definition: vpKltOpencv.h:125