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