Visual Servoing Platform  version 3.3.0
tutorial-klt-tracker-with-reinit.cpp
#include <visp3/core/vpImageConvert.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/io/vpVideoReader.h>
#include <visp3/klt/vpKltOpencv.h>
int main()
{
#ifdef VISP_HAVE_OPENCV
try {
vpVideoReader reader;
reader.setFileName("video-postcard.mpeg");
reader.acquire(I);
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
IplImage *cvI = NULL;
#else
cv::Mat cvI;
#endif
// Display initialisation
vpDisplayOpenCV d(I, 0, 0, "Klt tracking");
vpKltOpencv tracker;
// Set tracker parameters
tracker.setMaxFeatures(200);
tracker.setWindowSize(10);
tracker.setQuality(0.01);
tracker.setMinDistance(15);
tracker.setHarrisFreeParameter(0.04);
tracker.setBlockSize(9);
tracker.setUseHarris(1);
tracker.setPyramidLevels(3);
// Initialise the tracking
tracker.initTracking(cvI);
while (!reader.end()) {
reader.acquire(I);
std::cout << "Process image " << reader.getFrameIndex() << std::endl;
// Restart the initialization to detect new keypoints
if (reader.getFrameIndex() == 25) {
std::cout << "Re initialize the tracker" << std::endl;
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
// Save of previous features
std::vector<cv::Point2f> prev_features = tracker.getFeatures();
// Start a new feature detection
tracker.initTracking(cvI);
std::vector<cv::Point2f> new_features = tracker.getFeatures();
// Add previous features if they are not to close to detected one
double distance, minDistance_ = tracker.getMinDistance();
for (size_t i = 0; i < prev_features.size(); i++) {
// Test if a previous feature is not redundant with one of the newly
// detected
bool is_redundant = false;
for (size_t j = 0; j < new_features.size(); j++) {
distance = sqrt(vpMath::sqr(new_features[j].x - prev_features[i].x) +
vpMath::sqr(new_features[j].y - prev_features[i].y));
if (distance < minDistance_) {
is_redundant = true;
break;
}
}
if (is_redundant) {
continue;
}
// std::cout << "Add previous feature with index " << i <<
// std::endl;
tracker.addFeature(prev_features[i]);
}
#else
// Save of previous features
int prev_nfeatures = tracker.getNbFeatures();
float x, y;
long id;
int j = 0;
CvPoint2D32f *prev_features = (CvPoint2D32f *)cvAlloc(prev_nfeatures * sizeof(CvPoint2D32f));
for (int i = 0; i < prev_nfeatures; i++) {
tracker.getFeature(i, id, x, y);
prev_features[i].x = x;
prev_features[i].y = y;
// printf("prev feature %d: id %d coord: %g %g\n", i, id, x, y);
}
// Start a new feature detection
tracker.initTracking(cvI);
std::cout << "Detection of " << tracker.getNbFeatures() << " new features" << std::endl;
// Add previous features if they are not to close to detected one
double distance, minDistance_ = tracker.getMinDistance();
for (int i = tracker.getNbFeatures(); j < prev_nfeatures && i < tracker.getMaxFeatures(); j++) {
// Test if a previous feature is not redundant with new the one that
// are newly detected
bool is_redundant = false;
for (int k = 0; k < tracker.getNbFeatures(); k++) {
tracker.getFeature(k, id, x, y);
// printf("curr feature %d: id %d coord: %g %g\n", k, id, x, y);
distance = sqrt(vpMath::sqr(x - prev_features[j].x) + vpMath::sqr(y - prev_features[j].y));
if (distance < minDistance_) {
is_redundant = true;
break;
}
}
if (is_redundant) {
continue;
}
// std::cout << "Add previous feature with index " << i <<
// std::endl;
tracker.addFeature(i, prev_features[j].x, prev_features[j].y);
i++;
}
cvFree(&prev_features);
#endif
}
// Track the features
tracker.track(cvI);
std::cout << "tracking of " << tracker.getNbFeatures() << " features" << std::endl;
tracker.display(I, vpColor::red);
}
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
cvReleaseImage(&cvI);
#endif
return 0;
} catch (const vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
}
vpKltOpencv::addFeature
void addFeature(const float &x, const float &y)
Definition: vpKltOpencv.cpp:516
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:113
vpKltOpencv::setMinDistance
void setMinDistance(double minDistance)
Definition: vpKltOpencv.cpp:380
vpKltOpencv::getNbFeatures
int getNbFeatures() const
Get the number of current features.
Definition: vpKltOpencv.h:119
vpKltOpencv::track
void track(const cv::Mat &I)
Definition: vpKltOpencv.cpp:146
vpImageConvert::convert
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Definition: vpImageConvert.cpp:78
vpKltOpencv::getFeatures
std::vector< cv::Point2f > getFeatures() const
Get the list of current features.
Definition: vpKltOpencv.h:104
vpKltOpencv::display
void display(const vpImage< unsigned char > &I, const vpColor &color=vpColor::red, unsigned int thickness=1)
Definition: vpKltOpencv.cpp:216
vpKltOpencv::setQuality
void setQuality(double qualityLevel)
Definition: vpKltOpencv.cpp:355
vpVideoReader::end
bool end()
Definition: vpVideoReader.h:259
vpKltOpencv::setHarrisFreeParameter
void setHarrisFreeParameter(double harris_k)
Definition: vpKltOpencv.cpp:363
vpKltOpencv::getMaxFeatures
int getMaxFeatures() const
Get the list of lost feature.
Definition: vpKltOpencv.h:114
vpKltOpencv::initTracking
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
Definition: vpKltOpencv.cpp:117
vpVideoReader::getFrameIndex
long getFrameIndex() const
Definition: vpVideoReader.h:294
vpDisplayOpenCV
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Definition: vpDisplayOpenCV.h:140
vpKltOpencv
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:77
vpVideoReader
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
Definition: vpVideoReader.h:170
vpVideoReader::setFileName
void setFileName(const std::string &filename)
Definition: vpVideoReader.cpp:91
vpDisplay::display
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:739
vpKltOpencv::setWindowSize
void setWindowSize(int winSize)
Definition: vpKltOpencv.cpp:342
vpKltOpencv::setPyramidLevels
void setPyramidLevels(int pyrMaxLevel)
Definition: vpKltOpencv.cpp:408
vpKltOpencv::setUseHarris
void setUseHarris(int useHarrisDetector)
Definition: vpKltOpencv.cpp:371
vpKltOpencv::getMinDistance
double getMinDistance() const
Definition: vpKltOpencv.h:117
vpKltOpencv::getFeature
void getFeature(const int &index, long &id, float &x, float &y) const
Definition: vpKltOpencv.cpp:198
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:715
vpImage< unsigned char >
vpDisplay::getClick
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Definition: vpDisplay_uchar.cpp:764
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:70
vpVideoReader::acquire
void acquire(vpImage< vpRGBa > &I)
Definition: vpVideoReader.cpp:243
vpColor::red
static const vpColor red
Definition: vpColor.h:178
vpKltOpencv::setBlockSize
void setBlockSize(int blockSize)
Definition: vpKltOpencv.cpp:398
vpKltOpencv::setMaxFeatures
void setMaxFeatures(int maxCount)
Definition: vpKltOpencv.cpp:333