Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
tutorial-klt-tracker-with-reinit.cpp
1 #include <visp3/core/vpImageConvert.h>
3 #include <visp3/klt/vpKltOpencv.h>
4 #include <visp3/gui/vpDisplayOpenCV.h>
5 #include <visp3/io/vpVideoReader.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  {
45  reader.acquire(I);
46  std::cout << "acquire image " << reader.getFrameIndex() << std::endl;
48 
50 
52  // Restart the initialization to detect new keypoints
53  if (reader.getFrameIndex() == 25) {
54  std::cout << "Re initialize the tracker" << std::endl;
55 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
56  // Save of previous features
57  std::vector<cv::Point2f> prev_features = tracker.getFeatures();
58 
59  // Start a new feature detection
60  tracker.initTracking(cvI);
61  std::vector<cv::Point2f> new_features = tracker.getFeatures();
62 
63  // Add previous features if they are not to close to detected one
64  double distance, minDistance_ = tracker.getMinDistance();
65  bool is_redundant;
66  for (size_t i=0; i < prev_features.size(); i++) {
67  // Test if a previous feature is not redundant with one of the newly detected
68  is_redundant = false;
69  for (size_t j=0; j < new_features.size(); j++){
70  distance = sqrt(vpMath::sqr(new_features[j].x-prev_features[i].x) + 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 << std::endl;
80  tracker.addFeature(prev_features[i]);
81  }
82 #else
83  // Save of previous features
84  int prev_nfeatures = tracker.getNbFeatures();
85  float x,y;
86  long id;
87  int j=0;
88 
89  CvPoint2D32f *prev_features = (CvPoint2D32f*)cvAlloc(prev_nfeatures*sizeof(CvPoint2D32f));
90 
91  for (int i=0; i <prev_nfeatures ; i ++) {
92  tracker.getFeature(i, id, x, y);
93  prev_features[i].x=x;
94  prev_features[i].y=y;
95  //printf("prev feature %d: id %d coord: %g %g\n", i, id, x, y);
96  }
97 
98  // Start a new feature detection
99  tracker.initTracking(cvI);
100  std::cout << "Detection of " << tracker.getNbFeatures() << " new features" << std::endl;
101 
102  // Add previous features if they are not to close to detected one
103  double distance, minDistance_ = tracker.getMinDistance();
104  for(int i = tracker.getNbFeatures() ;
105  j<prev_nfeatures && i<tracker.getMaxFeatures() ;
106  j++){
107  // Test if a previous feature is not redundant with new the one that 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 << std::endl;
122  tracker.addFeature(i, prev_features[j].x, prev_features[j].y);
123  i++;
124  }
125  cvFree(&prev_features);
126 #endif
127  }
128  // Track the features
129  tracker.track(cvI);
131 
132  std::cout << "tracking of " << tracker.getNbFeatures() << " features" << std::endl;
133 
134  tracker.display(I, vpColor::red);
135  vpDisplay::flush(I);
136  }
137 
139 
140 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
141  cvReleaseImage(&cvI);
142 #endif
143 
144  return 0;
145  }
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)
long getFrameIndex() const
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setHarrisFreeParameter(double harris_k)
int getMaxFeatures() const
Get the list of lost feature.
Definition: vpKltOpencv.h:114
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
std::vector< cv::Point2f > getFeatures() const
Get the list of current features.
Definition: vpKltOpencv.h:104
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)
error that can be emited by ViSP classes.
Definition: vpException.h:73
static void flush(const vpImage< unsigned char > &I)
static const vpColor red
Definition: vpColor.h:163
void display(const vpImage< unsigned char > &I, const vpColor &color=vpColor::red, unsigned int thickness=1)
void setQuality(double qualityLevel)
static double sqr(double x)
Definition: vpMath.h:110
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)
void setFileName(const char *filename)
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
void setPyramidLevels(const int pyrMaxLevel)
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 setBlockSize(const int blockSize)
void getFeature(const int &index, long &id, float &x, float &y) const
int getNbFeatures() const
Get the number of current features.
Definition: vpKltOpencv.h:118
void setUseHarris(const int useHarrisDetector)
double getMinDistance() const
Get the minimal Euclidean distance between detected corners during initialization.
Definition: vpKltOpencv.h:116
void track(const cv::Mat &I)