Visual Servoing Platform  version 3.1.0
tutorial-klt-tracker-with-reinit.cpp
1 #include <visp3/core/vpImageConvert.h>
3 #include <visp3/gui/vpDisplayOpenCV.h>
4 #include <visp3/io/vpVideoReader.h>
5 #include <visp3/klt/vpKltOpencv.h>
6 
7 int main()
8 {
9 #ifdef VISP_HAVE_OPENCV
10  try {
11  vpVideoReader reader;
12  reader.setFileName("video-postcard.mpeg");
13 
15  reader.acquire(I);
16 
17 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
18  IplImage *cvI = NULL;
19 #else
20  cv::Mat cvI;
21 #endif
23 
24  // Display initialisation
25  vpDisplayOpenCV d(I, 0, 0, "Klt tracking");
28 
29  vpKltOpencv tracker;
30  // Set tracker parameters
31  tracker.setMaxFeatures(200);
32  tracker.setWindowSize(10);
33  tracker.setQuality(0.01);
34  tracker.setMinDistance(15);
35  tracker.setHarrisFreeParameter(0.04);
36  tracker.setBlockSize(9);
37  tracker.setUseHarris(1);
38  tracker.setPyramidLevels(3);
39 
40  // Initialise the tracking
41  tracker.initTracking(cvI);
42 
43  while (!reader.end()) {
44  reader.acquire(I);
45  std::cout << "Process image " << reader.getFrameIndex() << std::endl;
47 
49 
51  // Restart the initialization to detect new keypoints
52  if (reader.getFrameIndex() == 25) {
53  std::cout << "Re initialize the tracker" << std::endl;
54 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
55  // Save of previous features
56  std::vector<cv::Point2f> prev_features = tracker.getFeatures();
57 
58  // Start a new feature detection
59  tracker.initTracking(cvI);
60  std::vector<cv::Point2f> new_features = tracker.getFeatures();
61 
62  // Add previous features if they are not to close to detected one
63  double distance, minDistance_ = tracker.getMinDistance();
64  for (size_t i = 0; i < prev_features.size(); i++) {
65  // Test if a previous feature is not redundant with one of the newly
66  // detected
67  bool is_redundant = false;
68  for (size_t j = 0; j < new_features.size(); j++) {
69  distance = sqrt(vpMath::sqr(new_features[j].x - prev_features[i].x) +
70  vpMath::sqr(new_features[j].y - prev_features[i].y));
71  if (distance < minDistance_) {
72  is_redundant = true;
73  break;
74  }
75  }
76  if (is_redundant) {
77  continue;
78  }
79  // std::cout << "Add previous feature with index " << i <<
80  // std::endl;
81  tracker.addFeature(prev_features[i]);
82  }
83 #else
84  // Save of previous features
85  int prev_nfeatures = tracker.getNbFeatures();
86  float x, y;
87  long id;
88  int j = 0;
89 
90  CvPoint2D32f *prev_features = (CvPoint2D32f *)cvAlloc(prev_nfeatures * sizeof(CvPoint2D32f));
91 
92  for (int i = 0; i < prev_nfeatures; i++) {
93  tracker.getFeature(i, id, x, y);
94  prev_features[i].x = x;
95  prev_features[i].y = y;
96  // printf("prev feature %d: id %d coord: %g %g\n", i, id, x, y);
97  }
98 
99  // Start a new feature detection
100  tracker.initTracking(cvI);
101  std::cout << "Detection of " << tracker.getNbFeatures() << " new features" << std::endl;
102 
103  // Add previous features if they are not to close to detected one
104  double distance, minDistance_ = tracker.getMinDistance();
105  for (int i = tracker.getNbFeatures(); j < prev_nfeatures && i < tracker.getMaxFeatures(); j++) {
106  // Test if a previous feature is not redundant with new the one that
107  // are newly detected
108  bool is_redundant = false;
109  for (int k = 0; k < tracker.getNbFeatures(); k++) {
110  tracker.getFeature(k, id, x, y);
111  // printf("curr feature %d: id %d coord: %g %g\n", k, id, x, y);
112  distance = sqrt(vpMath::sqr(x - prev_features[j].x) + vpMath::sqr(y - prev_features[j].y));
113  if (distance < minDistance_) {
114  is_redundant = true;
115  break;
116  }
117  }
118  if (is_redundant) {
119  continue;
120  }
121  // std::cout << "Add previous feature with index " << i <<
122  // std::endl;
123  tracker.addFeature(i, prev_features[j].x, prev_features[j].y);
124  i++;
125  }
126  cvFree(&prev_features);
127 #endif
128  }
129  // Track the features
130  tracker.track(cvI);
132 
133  std::cout << "tracking of " << tracker.getNbFeatures() << " features" << std::endl;
134 
135  tracker.display(I, vpColor::red);
136  vpDisplay::flush(I);
137  }
138 
140 
141 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
142  cvReleaseImage(&cvI);
143 #endif
144 
145  return 0;
146  } catch (vpException &e) {
147  std::cout << "Catch an exception: " << e << std::endl;
148  }
149 #endif
150 }
void addFeature(const float &x, const float &y)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setHarrisFreeParameter(double harris_k)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void setMaxFeatures(const int maxCount)
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
void setMinDistance(double minDistance)
std::vector< cv::Point2f > getFeatures() const
Get the list of current features.
Definition: vpKltOpencv.h:105
error that can be emited by ViSP classes.
Definition: vpException.h:71
int getNbFeatures() const
Get the number of current features.
Definition: vpKltOpencv.h:120
static void flush(const vpImage< unsigned char > &I)
static const vpColor red
Definition: vpColor.h:180
void display(const vpImage< unsigned char > &I, const vpColor &color=vpColor::red, unsigned int thickness=1)
void setQuality(double qualityLevel)
int getMaxFeatures() const
Get the list of lost feature.
Definition: vpKltOpencv.h:115
static double sqr(double x)
Definition: vpMath.h:108
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
void acquire(vpImage< vpRGBa > &I)
double getMinDistance() const
Definition: vpKltOpencv.h:118
void setFileName(const char *filename)
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
void setPyramidLevels(const int pyrMaxLevel)
void getFeature(const int &index, long &id, float &x, float &y) const
void setWindowSize(const int winSize)
long getFrameIndex() const
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:78
void setBlockSize(const int blockSize)
void setUseHarris(const int useHarrisDetector)
void track(const cv::Mat &I)