Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
templateTracker.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 template tracking.
32  *
33  * Authors:
34  * Amaury Dame
35  * Aurelien Yol
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
46 #include <iostream>
47 #include <visp3/core/vpConfig.h>
48 
49 #if defined(VISP_HAVE_MODULE_TT) && defined (VISP_HAVE_DISPLAY)
50 
51 #include <visp3/core/vpDebug.h>
52 #include <visp3/gui/vpDisplayD3D.h>
53 #include <visp3/gui/vpDisplayGTK.h>
54 #include <visp3/gui/vpDisplayGDI.h>
55 #include <visp3/gui/vpDisplayOpenCV.h>
56 #include <visp3/gui/vpDisplayX.h>
57 #include <visp3/core/vpHomogeneousMatrix.h>
58 #include <visp3/io/vpImageIo.h>
59 #include <visp3/core/vpIoTools.h>
60 #include <visp3/core/vpMath.h>
61 #include <visp3/io/vpVideoReader.h>
62 #include <visp3/io/vpParseArgv.h>
63 
64 #include <visp3/tt/vpTemplateTrackerSSD.h>
65 #include <visp3/tt/vpTemplateTrackerSSDForwardAdditional.h>
66 #include <visp3/tt/vpTemplateTrackerSSDForwardCompositional.h>
67 #include <visp3/tt/vpTemplateTrackerSSDInverseCompositional.h>
68 #include <visp3/tt/vpTemplateTrackerSSDESM.h>
69 #include <visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h>
70 #include <visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h>
71 
72 #include <visp3/tt/vpTemplateTrackerWarpAffine.h>
73 #include <visp3/tt/vpTemplateTrackerWarpHomography.h>
74 #include <visp3/tt/vpTemplateTrackerWarpHomographySL3.h>
75 #include <visp3/tt/vpTemplateTrackerWarpSRT.h>
76 #include <visp3/tt/vpTemplateTrackerWarpTranslation.h>
77 #include <visp3/tt/vpTemplateTrackerWarpRT.h>
78 
79 #ifdef VISP_HAVE_MODULE_TT_MI
80 # include <visp3/tt_mi/vpTemplateTrackerMIESM.h>
81 # include <visp3/tt_mi/vpTemplateTrackerMIForwardAdditional.h>
82 # include <visp3/tt_mi/vpTemplateTrackerMIForwardCompositional.h>
83 # include <visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h>
84 #endif
85 
86 #define GETOPTARGS "cdhi:l:pt:w:"
87 
88 #ifndef DOXYGEN_SHOULD_SKIP_THIS
89 typedef enum {
90  WARP_AFFINE,
91  WARP_HOMOGRAPHY,
92  WARP_HOMOGRAPHY_SL3,
93  WARP_SRT,
94  WARP_TRANSLATION,
95 #ifdef VISP_HAVE_MODULE_TT_MI
96  WARP_RT,
97 #endif
98  WARP_LAST
99 } WarpType;
100 
101 typedef enum {
102  TRACKER_SSD_ESM,
103  TRACKER_SSD_FORWARD_ADDITIONAL,
104  TRACKER_SSD_FORWARD_COMPOSITIONAL,
105  TRACKER_SSD_INVERSE_COMPOSITIONAL, // The most efficient
106  TRACKER_ZNCC_FORWARD_ADDITIONEL,
107  TRACKER_ZNCC_INVERSE_COMPOSITIONAL,
108 #ifdef VISP_HAVE_MODULE_TT_MI
109  TRACKER_MI_ESM,
110  TRACKER_MI_FORWARD_ADDITIONAL,
111  TRACKER_MI_FORWARD_COMPOSITIONAL,
112  TRACKER_MI_INVERSE_COMPOSITIONAL, // The most efficient
113 #endif
114  TRACKER_LAST
115 } TrackerType;
116 
117 #endif
118 
119 void usage(const char *name, const char *badparam, const WarpType &warp_type,
120  TrackerType &tracker_type, const long &last_frame);
121 bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display,
122  bool &pyramidal, WarpType &warp_type, TrackerType &tracker_type, long &last_frame);
123 
124 void usage(const char *name, const char *badparam, const WarpType &warp_type,
125  TrackerType &tracker_type, const long &last_frame)
126 {
127  fprintf(stdout, "\n\
128 Example of template tracking.\n\
129 \n\
130 SYNOPSIS\n\
131  %s [-i <test image path>] [-c] [-d] [-p] \n\
132  [-w <warp type>] [-t <tracker type>] \n\
133  [-l <last frame number>] [-h]\n", name );
134 
135  fprintf(stdout, "\n\
136 OPTIONS: Default\n\
137  -i <input image path> \n\
138  Set image input path.\n\
139  From this path read images \n\
140  \"ViSP-images/cube/image%%04d.pgm\". These \n\
141  images come from ViSP-images-x.y.z.tar.gz available \n\
142  on the ViSP website.\n\
143  Setting the VISP_INPUT_IMAGE_PATH environment\n\
144  variable produces the same behaviour than using\n\
145  this option.\n\
146  \n\
147  -l <last frame number> %ld\n\
148  Last frame number to consider.\n\
149  \n\
150  -d \n\
151  Turn off the display.\n\
152  \n\
153  -c\n\
154  Disable the mouse click. Useful to automaze the \n\
155  execution of this program without humain intervention.\n\
156  \n", last_frame);
157 
158 #ifdef VISP_HAVE_MODULE_TT_MI
159  fprintf(stdout, "\n\
160  -w <warp type=[0,1,2,3,4,5]> %d\n\
161  Set the model used to warp the template. \n\
162  Authorized values are:\n\
163  %d : Affine\n\
164  %d : Homography\n\
165  %d : Homography in SL3\n\
166  %d : SRT (scale, rotation, translation)\n\
167  %d : RT (rotation, translation)\n\
168  %d : Translation\n\n",
169  (int)warp_type, (int)WARP_AFFINE, (int)WARP_HOMOGRAPHY, (int)WARP_HOMOGRAPHY_SL3, (int)WARP_SRT, (int)WARP_TRANSLATION, (int)WARP_RT);
170 #else
171  fprintf(stdout, "\n\
172  -w <warp type=[0,1,2,3,4]> %d\n\
173  Set the model used to warp the template. \n\
174  Authorized values are:\n\
175  %d : Affine\n\
176  %d : Homography\n\
177  %d : Homography in SL3\n\
178  %d : SRT (scale, rotation, translation)\n\
179  %d : Translation\n\n",
180  (int)warp_type, (int)WARP_AFFINE, (int)WARP_HOMOGRAPHY, (int)WARP_HOMOGRAPHY_SL3, (int)WARP_SRT, (int)WARP_TRANSLATION);
181 #endif
182 
183 #ifdef VISP_HAVE_MODULE_TT_MI
184  fprintf(stdout, "\n\
185  -t <tracker type=[0,1,2,3,4,5,6,7,8,9]> %d\n\
186  Set the tracker used to track the template. \n\
187  Authorized values are:\n\
188  %d : SSD ESM\n\
189  %d : SSD forward additional\n\
190  %d : SSD forward compositional\n\
191  %d : SSD inverse compositional\n\
192  %d : ZNCC forward additional\n\
193  %d : ZNCC inverse compositional\n\
194  %d : MI ESM\n\
195  %d : MI forward additional\n\
196  %d : MI forward compositional\n\
197  %d : MI inverse compositional\n\n",
198  (int)tracker_type,
199  (int)TRACKER_SSD_ESM, (int)TRACKER_SSD_FORWARD_ADDITIONAL, (int)TRACKER_SSD_FORWARD_COMPOSITIONAL,
200  (int)TRACKER_SSD_INVERSE_COMPOSITIONAL, (int)TRACKER_ZNCC_FORWARD_ADDITIONEL,
201  (int)TRACKER_ZNCC_INVERSE_COMPOSITIONAL,
202  (int)TRACKER_MI_ESM, (int)TRACKER_MI_FORWARD_ADDITIONAL, (int)TRACKER_MI_FORWARD_COMPOSITIONAL,
203  (int)TRACKER_MI_INVERSE_COMPOSITIONAL);
204 #else
205  fprintf(stdout, "\n\
206  -t <tracker type=[0,1,2,3,4,5]> %d\n\
207  Set the tracker used to track the template. \n\
208  Authorized values are:\n\
209  %d : SSD ESM\n\
210  %d : SSD forward additional\n\
211  %d : SSD forward compositional\n\
212  %d : SSD inverse compositional\n\
213  %d : ZNCC forward additional\n\
214  %d : ZNCC inverse compositional\n\n",
215  (int)tracker_type,
216  (int)TRACKER_SSD_ESM, (int)TRACKER_SSD_FORWARD_ADDITIONAL, (int)TRACKER_SSD_FORWARD_COMPOSITIONAL,
217  (int)TRACKER_SSD_INVERSE_COMPOSITIONAL, (int)TRACKER_ZNCC_FORWARD_ADDITIONEL,
218  (int)TRACKER_ZNCC_INVERSE_COMPOSITIONAL);
219 
220 #endif
221  fprintf(stdout, "\n\
222  -p\n\
223  Enable pyramidal tracking.\n\
224  \n\
225  -h \n\
226  Print the help.\n\n");
227 
228  if (badparam)
229  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
230 }
231 
232 
233 bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display,
234  bool &pyramidal, WarpType &warp_type, TrackerType &tracker_type, long &last_frame)
235 {
236  const char *optarg_;
237  int c;
238  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
239 
240  switch (c) {
241  case 'c': click_allowed = false; break;
242  case 'd': display = false; break;
243  case 'h': usage(argv[0], NULL, warp_type, tracker_type, last_frame); return false; break;
244  case 'i': ipath = optarg_; break;
245  case 'l': last_frame = (long)atoi(optarg_); break;
246  case 'p': pyramidal = true; break;
247  case 't': tracker_type = (TrackerType)atoi(optarg_); break;
248  case 'w': warp_type = (WarpType)atoi(optarg_); break;
249 
250  default:
251  usage(argv[0], optarg_, warp_type, tracker_type, last_frame);
252  return false; break;
253  }
254  }
255 
256  if (warp_type >= WARP_LAST) {
257  usage(argv[0], NULL, warp_type, tracker_type, last_frame);
258  std::cerr << "ERROR: " << std::endl;
259  std::cerr << " Bad argument -w <warp type> with \"warp type\"=" << (int)warp_type << std::endl << std::endl;
260  return false;
261  }
262  if (tracker_type >= TRACKER_LAST) {
263  usage(argv[0], NULL, warp_type, tracker_type, last_frame);
264  std::cerr << "ERROR: " << std::endl;
265  std::cerr << " Bad argument -t <tracker type> with \"tracker type\"=" << (int)tracker_type << std::endl << std::endl;
266  return false;
267  }
268  if ((c == 1) || (c == -1)) {
269  // standalone param or error
270  usage(argv[0], NULL, warp_type, tracker_type, last_frame);
271  std::cerr << "ERROR: " << std::endl;
272  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
273  return false;
274  }
275 
276  return true;
277 }
278 
279 int
280 main(int argc, const char ** argv)
281 {
282  try {
283  std::string env_ipath;
284  std::string opt_ipath;
285  std::string ipath;
286  bool opt_click_allowed = true;
287  bool opt_display = true;
288  bool opt_pyramidal = false;
289  TrackerType opt_tracker_type = TRACKER_SSD_INVERSE_COMPOSITIONAL;
290  WarpType opt_warp_type = WARP_AFFINE;
291  long opt_last_frame = 30;
292 
293  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
294  env_ipath = vpIoTools::getViSPImagesDataPath();
295 
296  // Set the default input path
297  if (! env_ipath.empty())
298  ipath = env_ipath;
299 
300  // Read the command line options
301  if (!getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, opt_pyramidal,
302  opt_warp_type, opt_tracker_type, opt_last_frame)) {
303  return (-1);
304  }
305 
306  // Test if an input path is set
307  if (opt_ipath.empty() && env_ipath.empty() ){
308  usage(argv[0], NULL, opt_warp_type, opt_tracker_type, opt_last_frame);
309  std::cerr << std::endl
310  << "ERROR:" << std::endl;
311  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
312  << std::endl
313  << " environment variable to specify the location of the " << std::endl
314  << " image path where test images are located." << std::endl
315  << std::endl;
316 
317  return (-1);
318  }
319 
320  // Get the option values
321  if (!opt_ipath.empty())
322  ipath = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mire-2/image.%04d.pgm");
323  else
324  ipath = vpIoTools::createFilePath(env_ipath, "ViSP-images/mire-2/image.%04d.pgm");
325 
327  vpVideoReader reader;
328 
329  reader.setFileName(ipath.c_str());
330  reader.setFirstFrameIndex(1);
331  reader.setLastFrameIndex(opt_last_frame);
332  try{
333  reader.open(I);
334  }catch(...){
335  std::cout << "Cannot open sequence: " << ipath << std::endl;
336  return -1;
337  }
338  reader.acquire(I);
339 
340  vpDisplay *display = NULL;
341  if (opt_display)
342  {
343  // initialise a display
344 #if defined VISP_HAVE_X11
345  display = new vpDisplayX;
346 #elif defined VISP_HAVE_GDI
347  display = new vpDisplayGDI;
348 #elif defined VISP_HAVE_OPENCV
349  display = new vpDisplayOpenCV;
350 #elif defined VISP_HAVE_D3D9
351  display = new vpDisplayD3D;
352 #elif defined VISP_HAVE_GTK
353  display = new vpDisplayGTK;
354 #else
355  opt_display = false;
356 #endif
357 #if (defined VISP_HAVE_DISPLAY)
358  display->init(I, 100, 100, "Test tracking") ;
359 #endif
360  vpDisplay::display(I) ;
361  vpDisplay::flush(I);
362  }
363 
364  vpTemplateTrackerWarp *warp = NULL;
365  switch(opt_warp_type) {
366  case WARP_AFFINE: warp = new vpTemplateTrackerWarpAffine; break;
367  case WARP_HOMOGRAPHY: warp = new vpTemplateTrackerWarpHomography; break;
368  case WARP_HOMOGRAPHY_SL3: warp = new vpTemplateTrackerWarpHomographySL3; break;
369  case WARP_SRT: warp = new vpTemplateTrackerWarpSRT; break;
370  case WARP_TRANSLATION: warp = new vpTemplateTrackerWarpTranslation; break;
371 #ifdef VISP_HAVE_MODULE_TT_MI
372  case WARP_RT: warp = new vpTemplateTrackerWarpRT; break;
373 #endif
374  default: return 0;
375  }
376 
377  vpTemplateTracker *tracker = NULL;
378  switch(opt_tracker_type) {
379  case TRACKER_SSD_ESM: tracker = new vpTemplateTrackerSSDESM(warp); break;
380  case TRACKER_SSD_FORWARD_ADDITIONAL: tracker = new vpTemplateTrackerSSDForwardAdditional(warp); break;
381  case TRACKER_SSD_FORWARD_COMPOSITIONAL: tracker = new vpTemplateTrackerSSDForwardCompositional(warp); break;
382  case TRACKER_SSD_INVERSE_COMPOSITIONAL: tracker = new vpTemplateTrackerSSDInverseCompositional(warp); break;
383  case TRACKER_ZNCC_FORWARD_ADDITIONEL: tracker = new vpTemplateTrackerZNCCForwardAdditional(warp); break;
384  case TRACKER_ZNCC_INVERSE_COMPOSITIONAL: tracker = new vpTemplateTrackerZNCCInverseCompositional(warp); break;
385 #ifdef VISP_HAVE_MODULE_TT_MI
386  case TRACKER_MI_ESM: tracker = new vpTemplateTrackerMIESM(warp); break;
387  case TRACKER_MI_FORWARD_ADDITIONAL: tracker = new vpTemplateTrackerMIForwardAdditional(warp); break;
388  case TRACKER_MI_FORWARD_COMPOSITIONAL: tracker = new vpTemplateTrackerMIForwardCompositional(warp); break;
389  case TRACKER_MI_INVERSE_COMPOSITIONAL: tracker = new vpTemplateTrackerMIInverseCompositional(warp); break;
390 #endif
391  default: return 0;
392  }
393 
394  tracker->setSampling(2,2);
395  tracker->setLambda(0.001);
396  tracker->setThresholdGradient(60.);
397  tracker->setIterationMax(800);
398  if (opt_pyramidal) {
399  tracker->setPyramidal(2, 1);
400  }
401  bool delaunay = false;
402  if (opt_display && opt_click_allowed)
403  tracker->initClick(I, delaunay);
404  else {
405  std::vector<vpImagePoint> v_ip;
406  vpImagePoint ip;
407  ip.set_ij(166, 54); v_ip.push_back(ip);
408  ip.set_ij(284, 55); v_ip.push_back(ip);
409  ip.set_ij(259, 284); v_ip.push_back(ip); // ends the first triangle
410  ip.set_ij(259, 284); v_ip.push_back(ip); // start the second triangle
411  ip.set_ij(149, 240); v_ip.push_back(ip);
412  ip.set_ij(167, 58); v_ip.push_back(ip);
413 
414  tracker->initFromPoints(I, v_ip, false);
415  }
416 
417  while (! reader.end())
418  {
419  std::cout << "Process image number " << reader.getFrameIndex() << std::endl;
420  // Acquire a new image
421  reader.acquire(I);
422  // Display the image
424  // Track the template
425  tracker->track(I);
426 
427  // Simulate a re-init
428  if (reader.getFrameIndex() == 10){
429  std::cout << "re-init simulation" << std::endl;
430  if (opt_click_allowed)
432 
433  tracker->resetTracker();
434 
435  if (opt_display && opt_click_allowed) {
436  vpDisplay::displayText(I, 10, 10, "Re-init simulation", vpColor::red);
437  vpDisplay::flush(I);
438  tracker->initClick(I, delaunay);
439  }
440  else {
441  std::vector<vpImagePoint> v_ip;
442  vpImagePoint ip;
443  ip.set_ij(146, 60); v_ip.push_back(ip);
444  ip.set_ij(254, 74); v_ip.push_back(ip);
445  ip.set_ij(228, 288); v_ip.push_back(ip); // ends the first triangle
446  ip.set_ij(228, 288); v_ip.push_back(ip); // start the second triangle
447  ip.set_ij(126, 242); v_ip.push_back(ip);
448  ip.set_ij(146, 60); v_ip.push_back(ip);
449 
450  tracker->initFromPoints(I, v_ip, false);
451  }
452  }
453 
454  // Display the template
455 #if 1
456  tracker->display(I, vpColor::red, 3);
457 #else
458  vpTemplateTrackerZone zoneWarped_, zoneRef_ = tracker->getZoneRef();
459  vpTemplateTrackerWarp *warp_ = tracker->getWarp();
460  vpColVector p_= tracker->getp();
461  warp_->warpZone(zoneRef_, p_, zoneWarped_);
462  zoneWarped_.display(I, vpColor::red, 3);
463  zoneRef_.display(I, vpColor::green, 3);
464 #endif
465 
466  vpDisplay::flush(I) ;
467  }
468  if (opt_click_allowed) {
469  vpDisplay::displayText(I, 10,10, "A click to exit...", vpColor::red);
470  vpDisplay::flush(I) ;
472  }
473  reader.close();
474  if (display)
475  delete display;
476 
477  delete warp;
478  delete tracker;
479 
480  return 0;
481  }
482  catch(vpException &e)
483  {
484  std::cout << "Catch an exception: " << e << std::endl;
485  return -1;
486  }
487 }
488 
489 #else
490 
491 int main()
492 {
493  std::cout << "visp_tt module or display not available." << std::endl;
494  return 0;
495 }
496 
497 #endif
long getFrameIndex() const
void setSampling(int sample_i, int sample_j)
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:169
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1157
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:153
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
error that can be emited by ViSP classes.
Definition: vpException.h:73
void display(const vpImage< unsigned char > &I, const vpColor &col=vpColor::green, const unsigned int thickness=3)
void setIterationMax(const unsigned int &n)
static const vpColor green
Definition: vpColor.h:166
static void flush(const vpImage< unsigned char > &I)
vpColVector getp() const
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 open(vpImage< vpRGBa > &I)
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed...
Definition: vpDisplayD3D.h:107
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 initClick(const vpImage< unsigned char > &I, bool delaunay=false)
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:138
void acquire(vpImage< vpRGBa > &I)
void setFileName(const char *filename)
void initFromPoints(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &v_ip, bool delaunay=false)
void warpZone(const vpTemplateTrackerZone &in, const vpColVector &p, vpTemplateTrackerZone &out)
void setLastFrameIndex(const long last_frame)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void setLambda(double l)
void track(const vpImage< unsigned char > &I)
vpTemplateTrackerZone getZoneRef() const
void setFirstFrameIndex(const long first_frame)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void set_ij(const double ii, const double jj)
Definition: vpImagePoint.h:185
void setPyramidal(unsigned int nlevels=2, unsigned int level_to_stop=1)
vpTemplateTrackerWarp * getWarp() const
void setThresholdGradient(double threshold)
void display(const vpImage< unsigned char > &I, const vpColor &col=vpColor::green, const unsigned int thickness=3)