Visual Servoing Platform version 3.5.0
tutorial-klt-tracker-with-reinit.cpp
1
2#include <visp3/core/vpImageConvert.h>
3#include <visp3/gui/vpDisplayOpenCV.h>
4#include <visp3/io/vpVideoReader.h>
5#include <visp3/klt/vpKltOpencv.h>
6
7int main()
8{
9#ifdef VISP_HAVE_OPENCV
10 try {
11 vpVideoReader reader;
12 reader.setFileName("video-postcard.mp4");
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);
137 }
138
140
141#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
142 cvReleaseImage(&cvI);
143#endif
144
145 return 0;
146 } catch (const vpException &e) {
147 std::cout << "Catch an exception: " << e << std::endl;
148 }
149#endif
150}
static const vpColor red
Definition: vpColor.h:217
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
error that can be emited by ViSP classes.
Definition: vpException.h:72
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:79
int getMaxFeatures() const
Get the list of lost feature.
Definition: vpKltOpencv.h:115
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void track(const cv::Mat &I)
int getNbFeatures() const
Get the number of current features.
Definition: vpKltOpencv.h:120
void setHarrisFreeParameter(double harris_k)
void getFeature(const int &index, long &id, float &x, float &y) const
void setMaxFeatures(int maxCount)
void addFeature(const float &x, const float &y)
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
double getMinDistance() const
Definition: vpKltOpencv.h:118
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)
std::vector< cv::Point2f > getFeatures() const
Get the list of current features.
Definition: vpKltOpencv.h:105
static double sqr(double x)
Definition: vpMath.h:116
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
void acquire(vpImage< vpRGBa > &I)
void setFileName(const std::string &filename)
long getFrameIndex() const