Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
trackKltOpencv.cpp
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  * Example of dot tracking.
33  *
34  * Authors:
35  * Eric Marchand
36  * Fabien Spindler
37  *
38  *****************************************************************************/
45 #include <visp3/core/vpConfig.h>
46 #include <visp3/core/vpDebug.h>
47 
48 #include <iomanip>
49 #include <sstream>
50 #include <stdio.h>
51 #include <vector>
52 
53 #if defined(VISP_HAVE_MODULE_KLT) && \
54  (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
55 
56 #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
57 
58 #include <visp3/core/vpImage.h>
59 #include <visp3/core/vpIoTools.h>
60 #include <visp3/gui/vpDisplayGDI.h>
61 #include <visp3/gui/vpDisplayGTK.h>
62 #include <visp3/gui/vpDisplayOpenCV.h>
63 #include <visp3/gui/vpDisplayX.h>
64 #include <visp3/io/vpImageIo.h>
65 #include <visp3/io/vpParseArgv.h>
66 #include <visp3/klt/vpKltOpencv.h>
67 
68 // List of allowed command line options
69 #define GETOPTARGS "cdf:i:n:p:s:h"
70 
71 void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first,
72  unsigned nimages, unsigned step);
73 bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &nimages,
74  unsigned &step, bool &click_allowed, bool &display);
96 void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first,
97  unsigned nimages, unsigned step)
98 {
99  fprintf(stdout, "\n\
100 Example of KLT tracking using OpenCV library.\n\
101 \n\
102 SYNOPSIS\n\
103  %s [-i <test image path>] [-p <personal image path>]\n\
104  [-f <first image>] [-n <number of images>] [-s <step>]\n\
105  [-c] [-d] [-h]\n", name);
106 
107  fprintf(stdout, "\n\
108 OPTIONS: Default\n\
109  -i <input image path> %s\n\
110  Set image input path.\n\
111  From this path read images \n\
112  \"mire-2/image.%%04d.pgm\". These \n\
113  images come from ViSP-images-x.y.z.tar.gz available \n\
114  on the ViSP website.\n\
115  Setting the VISP_INPUT_IMAGE_PATH environment\n\
116  variable produces the same behaviour than using\n\
117  this option.\n\
118  \n\
119  -p <personal image path> %s\n\
120  Specify a personal sequence containing images \n\
121  to process.\n\
122  By image sequence, we mean one file per image.\n\
123  The following image file formats PNM (PGM P5, PPM P6)\n\
124  are supported. The format is selected by analysing \n\
125  the filename extension.\n\
126  Example : \"/Temp/ViSP-images/cube/image.%%04d.pgm\"\n\
127  %%04d is for the image numbering.\n\
128  \n\
129  -f <first image> %u\n\
130  First image number of the sequence.\n\
131  \n\
132  -n <number of images> %u\n\
133  Number of images to load from the sequence.\n\
134  \n\
135  -s <step> %u\n\
136  Step between two images.\n\
137 \n\
138  -c\n\
139  Disable the mouse click. Useful to automaze the \n\
140  execution of this program without humain intervention.\n\
141 \n\
142  -d \n\
143  Turn off the display.\n\
144 \n\
145  -h\n\
146  Print the help.\n", ipath.c_str(), ppath.c_str(), first, nimages, step);
147 
148  if (badparam)
149  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
150 }
168 bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &nimages,
169  unsigned &step, bool &click_allowed, bool &display)
170 {
171  const char *optarg_;
172  int c;
173  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
174 
175  switch (c) {
176  case 'c':
177  click_allowed = false;
178  break;
179  case 'd':
180  display = false;
181  break;
182  case 'i':
183  ipath = optarg_;
184  break;
185  case 'p':
186  ppath = optarg_;
187  break;
188  case 'f':
189  first = (unsigned)atoi(optarg_);
190  break;
191  case 'n':
192  nimages = (unsigned)atoi(optarg_);
193  break;
194  case 's':
195  step = (unsigned)atoi(optarg_);
196  break;
197  case 'h':
198  usage(argv[0], NULL, ipath, ppath, first, nimages, step);
199  return false;
200  break;
201 
202  default:
203  usage(argv[0], optarg_, ipath, ppath, first, nimages, step);
204  return false;
205  break;
206  }
207  }
208 
209  if ((c == 1) || (c == -1)) {
210  // standalone param or error
211  usage(argv[0], NULL, ipath, ppath, first, nimages, step);
212  std::cerr << "ERROR: " << std::endl;
213  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
214  return false;
215  }
216 
217  return true;
218 }
219 
220 int main(int argc, const char **argv)
221 {
222  try {
223  std::string env_ipath;
224  std::string opt_ipath;
225  std::string ipath;
226  std::string opt_ppath;
227  std::string dirname;
228  std::string filename;
229  unsigned opt_first = 1;
230  unsigned opt_nimages = 500;
231  unsigned opt_step = 1;
232  bool opt_click_allowed = true;
233  bool opt_display = true;
234 
235  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
236  // environment variable value
237  env_ipath = vpIoTools::getViSPImagesDataPath();
238 
239  // Set the default input path
240  if (!env_ipath.empty())
241  ipath = env_ipath;
242 
243  // Read the command line options
244  if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_click_allowed,
245  opt_display) == false) {
246  exit(-1);
247  }
248 
249  // Get the option values
250  if (!opt_ipath.empty())
251  ipath = opt_ipath;
252 
253  // Compare ipath and env_ipath. If they differ, we take into account
254  // the input path comming from the command line option
255  if (!opt_ipath.empty() && !env_ipath.empty() && opt_ppath.empty()) {
256  if (ipath != env_ipath) {
257  std::cout << std::endl << "WARNING: " << std::endl;
258  std::cout << " Since -i <visp image path=" << ipath << "> "
259  << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
260  << " we skip the environment variable." << std::endl;
261  }
262  }
263 
264  // Test if an input path is set
265  if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty()) {
266  usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step);
267  std::cerr << std::endl << "ERROR:" << std::endl;
268  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
269  << " environment variable to specify the location of the " << std::endl
270  << " image path where test images are located." << std::endl
271  << " Use -p <personal image path> option if you want to " << std::endl
272  << " use personal images." << std::endl
273  << std::endl;
274 
275  exit(-1);
276  }
277 
278  // Declare an image, this is a gray level image (unsigned char)
279  // it size is not defined yet, it will be defined when the image will
280  // read on the disk
281  vpImage<unsigned char> vpI; // This is a ViSP image used for display only
282 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
283  IplImage *cvI = NULL; // This is an OpenCV IPL image used by the tracker
284 #else
285  cv::Mat cvI;
286 #endif
287 
288  unsigned iter = opt_first;
289  std::ostringstream s;
290  char cfilename[FILENAME_MAX];
291 
292  if (opt_ppath.empty()) {
293 
294  // Warning :
295  // the image sequence is not provided with the ViSP package
296  // therefore the program will return you an error :
297  // !! vpImageIoPnm.cpp: readPGM(#210) :couldn't read file
298  // ViSP-images/mire-2/image.0001.pgm
299  // !! vpDotExample.cpp: main(#95) :Error while reading the image
300  // terminate called after throwing an instance of 'vpImageException'
301  //
302  // The sequence is available on the visp www site
303  // https://visp.inria.fr/download/
304  // in the download section. It is named "ViSP-images.tar.gz"
305 
306  // Set the path location of the image sequence
307  dirname = vpIoTools::createFilePath(ipath, "mire-2");
308 
309  // Build the name of the image file
310 
311  s.setf(std::ios::right, std::ios::adjustfield);
312  s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
313  filename = vpIoTools::createFilePath(dirname, s.str());
314  } else {
315  sprintf(cfilename, opt_ppath.c_str(), iter);
316  filename = cfilename;
317  }
318 
319  // Read the PGM image named "filename" on the disk, and put the
320  // bitmap into the image structure I. I is initialized to the
321  // correct size
322  //
323  // exception readPGM may throw various exception if, for example,
324  // the file does not exist, or if the memory cannot be allocated
325  try {
326  std::cout << "Load: " << filename << std::endl;
327 
328  // Load a ViSP image used for the display
329  vpImageIo::read(vpI, filename);
330  vpImageConvert::convert(vpI, cvI);
331  } catch (...) {
332  // an exception is throwned if an exception from readPGM has been
333  // catched here this will result in the end of the program Note that
334  // another error message has been printed from readPGM to give more
335  // information about the error
336  std::cerr << std::endl << "ERROR:" << std::endl;
337  std::cerr << " Cannot read " << filename << std::endl;
338  std::cerr << " Check your -i " << ipath << " option " << std::endl
339  << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl;
340  exit(-1);
341  }
342 
343 // We open a window using either X11, GTK or GDI.
344 #if defined VISP_HAVE_X11
345  vpDisplayX display;
346 #elif defined VISP_HAVE_GTK
347  vpDisplayGTK display;
348 #elif defined VISP_HAVE_GDI
349  vpDisplayGDI display;
350 #elif defined VISP_HAVE_OPENCV
351  vpDisplayOpenCV display;
352 #endif
353 
354  if (opt_display) {
355  // Display size is automatically defined by the image (I) size
356  display.init(vpI, 100, 100, "Display...");
357  // Display the image
358  // The image class has a member that specify a pointer toward
359  // the display that has been initialized in the display declaration
360  // therefore is is no longuer necessary to make a reference to the
361  // display variable.
362  vpDisplay::display(vpI);
363  vpDisplay::flush(vpI);
364  }
365 
366  // KLT tracker
367  vpKltOpencv tracker;
368 
369  // Event manager
370  // tracker.setOnNewFeature(&newFeature);
371  // tracker.setOnFeatureLost(&lostFeature);
372  // tracker.setIsFeatureValid(&isValid);
373 
374  // Tracker parameters
375  tracker.setTrackerId(1);
376  // tracker.setOnMeasureFeature(&modifyFeature);
377  tracker.setMaxFeatures(200);
378  tracker.setWindowSize(10);
379  tracker.setQuality(0.01);
380  tracker.setMinDistance(15);
381  tracker.setHarrisFreeParameter(0.04);
382  tracker.setBlockSize(9);
383  tracker.setUseHarris(1);
384  tracker.setPyramidLevels(3);
385 
386  // Point detection using Harris. In input we have an OpenCV IPL image
387  tracker.initTracking(cvI);
388 
389  if (opt_display) {
390  // Plot the Harris points on ViSP image
391  tracker.display(vpI, vpColor::red);
392  }
393 
394  // tracking is now initialized. We can start the tracker.
395  while (iter < opt_first + opt_nimages * opt_step) {
396  // set the new image name
397  if (opt_ppath.empty()) {
398  s.str("");
399  s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
400  filename = vpIoTools::createFilePath(dirname, s.str());
401  } else {
402  sprintf(cfilename, opt_ppath.c_str(), iter);
403  filename = cfilename;
404  }
405  // read the image
406  vpImageIo::read(vpI, filename);
407  vpImageConvert::convert(vpI, cvI);
408 
409  // track the dot and returns its coordinates in the image
410  // results are given in float since many many are usually considered
411  //
412  // an expcetion is thrown by the track method if
413  // - dot is lost
414 
415  if (opt_display) {
416  // Display the image
417  vpDisplay::display(vpI);
418  }
419 
420  std::cout << "Tracking on image: " << filename << std::endl;
421  double time = vpTime::measureTimeMs();
422  // Tracking of the detected points
423  tracker.track(cvI);
424  std::cout << "Tracking performed in " << vpTime::measureTimeMs() - time << " ms" << std::endl;
425 
426  if (opt_display) {
427  // Display the tracked points
428  tracker.display(vpI, vpColor::red);
429 
430  vpDisplay::flush(vpI);
431  }
432  iter += opt_step;
433  }
434  if (opt_display && opt_click_allowed) {
435  std::cout << "\nA click to exit..." << std::endl;
436  // Wait for a blocking mouse click
437  vpDisplay::getClick(vpI);
438  }
439  return EXIT_SUCCESS;
440  } catch (const vpException &e) {
441  std::cout << "Catch an exception: " << e << std::endl;
442  return EXIT_FAILURE;
443  }
444 }
445 #else
446 int main()
447 {
448  std::cout << "You do not have OpenCV functionalities to display images..." << std::endl;
449  std::cout << "Tip:" << std::endl;
450  std::cout << "- Install OpenCV, configure again ViSP using cmake and build again this example" << std::endl;
451  return EXIT_SUCCESS;
452 }
453 #endif
454 #else
455 #include <iostream>
456 
457 int main()
458 {
459  std::cout << "visp_klt module or X11, GTK, GDI or OpenCV display "
460  "functionalities are required..."
461  << std::endl;
462 }
463 
464 #endif
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:149
void setMaxFeatures(int maxCount)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1365
void setHarrisFreeParameter(double harris_k)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:134
void setMinDistance(double minDistance)
error that can be emited by ViSP classes.
Definition: vpException.h:71
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:126
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
static const vpColor red
Definition: vpColor.h:217
void display(const vpImage< unsigned char > &I, const vpColor &color=vpColor::red, unsigned int thickness=1)
void setQuality(double qualityLevel)
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1670
void setPyramidLevels(int pyrMaxLevel)
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 setTrackerId(int tid)
Definition: vpKltOpencv.h:155
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:134
void setWindowSize(int winSize)
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:78
void setUseHarris(int useHarrisDetector)
void setBlockSize(int blockSize)
void track(const cv::Mat &I)