ViSP
mbtEdgeTracking.cpp
1 /****************************************************************************
2  *
3  * $Id: mbtEdgeTracking.cpp 5156 2015-01-13 07:07:08Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Example of model based tracking.
36  *
37  * Authors:
38  * Nicolas Melchior
39  * Romain Tallonneau
40  * Aurelien Yol
41  *
42  *****************************************************************************/
43 
50 #include <visp/vpConfig.h>
51 #include <visp/vpDebug.h>
52 #include <visp/vpDisplayD3D.h>
53 #include <visp/vpDisplayGTK.h>
54 #include <visp/vpDisplayGDI.h>
55 #include <visp/vpDisplayOpenCV.h>
56 #include <visp/vpDisplayX.h>
57 #include <visp/vpHomogeneousMatrix.h>
58 #include <visp/vpImageIo.h>
59 #include <visp/vpIoTools.h>
60 #include <visp/vpMath.h>
61 #include <visp/vpMbEdgeTracker.h>
62 #include <visp/vpVideoReader.h>
63 #include <visp/vpParseArgv.h>
64 
65 #if defined (VISP_HAVE_DISPLAY)
66 
67 #define GETOPTARGS "x:m:i:n:dchtfCo"
68 
69 void usage(const char *name, const char *badparam);
70 bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile,
71  std::string &initFile, bool &displayFeatures, bool &click_allowed, bool &display,
72  bool& cao3DModel, bool& trackCylinder, bool &useOgre);
73 
74 void usage(const char *name, const char *badparam)
75 {
76  fprintf(stdout, "\n\
77 Example of tracking based on the 3D model.\n\
78 \n\
79 SYNOPSIS\n\
80  %s [-i <test image path>] [-x <config file>]\n\
81  [-m <model name>] [-n <initialisation file base name>]\n\
82  [-t] [-c] [-d] [-h] [-f] [-C]",
83  name );
84 
85  fprintf(stdout, "\n\
86 OPTIONS: \n\
87  -i <input image path> \n\
88  Set image input path.\n\
89  From this path read images \n\
90  \"ViSP-images/mbt/cube/image%%04d.ppm\". These \n\
91  images come from ViSP-images-x.y.z.tar.gz available \n\
92  on the ViSP website.\n\
93  Setting the VISP_INPUT_IMAGE_PATH environment\n\
94  variable produces the same behaviour than using\n\
95  this option.\n\
96 \n\
97  -x <config file> \n\
98  Set the config file (the xml file) to use.\n\
99  The config file is used to specify the parameters of the tracker.\n\
100 \n\
101  -m <model name> \n\
102  Specify the name of the file of the model\n\
103  The model can either be a vrml model (.wrl) or a .cao file.\n\
104 \n\
105  -f \n\
106  Do not use the vrml model, use the .cao one. These two models are \n\
107  equivalent and comes from ViSP-images-x.y.z.tar.gz available on the ViSP\n\
108  website. However, the .cao model allows to use the 3d model based tracker \n\
109  without Coin.\n\
110 \n\
111  -C \n\
112  Track only the cube (not the cylinder). In this case the models files are\n\
113  cube.cao or cube.wrl instead of cube_and_cylinder.cao and \n\
114  cube_and_cylinder.wrl.\n\
115 \n\
116  -n <initialisation file base name> \n\
117  Base name of the initialisation file. The file will be 'base_name'.init .\n\
118  This base name is also used for the optionnal picture specifying where to \n\
119  click (a .ppm picture).\
120 \n\
121  -t \n\
122  Turn off the display of the the moving edges. \n\
123 \n\
124  -d \n\
125  Turn off the display.\n\
126 \n\
127  -c\n\
128  Disable the mouse click. Useful to automaze the \n\
129  execution of this program without humain intervention.\n\
130 \n\
131  -o\n\
132  Use Ogre3D for visibility tests\n\
133 \n\
134  -h \n\
135  Print the help.\n\n");
136 
137  if (badparam)
138  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
139 }
140 
141 
142 bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile,
143  std::string &initFile, bool &displayFeatures, bool &click_allowed, bool &display,
144  bool& cao3DModel, bool& trackCylinder, bool &useOgre)
145 {
146  const char *optarg_;
147  int c;
148  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
149 
150  switch (c) {
151  case 'i': ipath = optarg_; break;
152  case 'x': configFile = optarg_; break;
153  case 'm': modelFile = optarg_; break;
154  case 'n': initFile = optarg_; break;
155  case 't': displayFeatures = false; break;
156  case 'f': cao3DModel = true; break;
157  case 'c': click_allowed = false; break;
158  case 'd': display = false; break;
159  case 'C': trackCylinder = false; break;
160  case 'o': useOgre = true; break;
161  case 'h': usage(argv[0], NULL); return false; break;
162 
163  default:
164  usage(argv[0], optarg_);
165  return false; break;
166  }
167  }
168 
169  if ((c == 1) || (c == -1)) {
170  // standalone param or error
171  usage(argv[0], NULL);
172  std::cerr << "ERROR: " << std::endl;
173  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
174  return false;
175  }
176 
177  return true;
178 }
179 
180 int
181 main(int argc, const char ** argv)
182 {
183  try {
184  std::string env_ipath;
185  std::string opt_ipath;
186  std::string ipath;
187  std::string opt_configFile;
188  std::string configFile;
189  std::string opt_modelFile;
190  std::string modelFile;
191  std::string opt_initFile;
192  std::string initFile;
193  bool displayFeatures = true;
194  bool opt_click_allowed = true;
195  bool opt_display = true;
196  bool cao3DModel = false;
197  bool trackCylinder = true;
198  bool useOgre = false;
199  bool quit = false;
200 
201  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
202  env_ipath = vpIoTools::getViSPImagesDataPath();
203 
204  // Set the default input path
205  if (! env_ipath.empty())
206  ipath = env_ipath;
207 
208 
209  // Read the command line options
210  if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, displayFeatures, opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre)) {
211  return (-1);
212  }
213 
214  // Test if an input path is set
215  if (opt_ipath.empty() && env_ipath.empty() ){
216  usage(argv[0], NULL);
217  std::cerr << std::endl
218  << "ERROR:" << std::endl;
219  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
220  << std::endl
221  << " environment variable to specify the location of the " << std::endl
222  << " image path where test images are located." << std::endl
223  << std::endl;
224 
225  return (-1);
226  }
227 
228  // Get the option values
229  if (!opt_ipath.empty())
230  ipath = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube/image%04d.pgm");
231  else
232  ipath = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube/image%04d.pgm");
233 
234  if (!opt_configFile.empty())
235  configFile = opt_configFile;
236  else if (!opt_ipath.empty())
237  configFile = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube.xml");
238  else
239  configFile = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.xml");
240 
241  if (!opt_modelFile.empty()){
242  modelFile = opt_modelFile;
243  }else{
244  std::string modelFileCao;
245  std::string modelFileWrl;
246  if(trackCylinder){
247  modelFileCao = "ViSP-images/mbt/cube_and_cylinder.cao";
248  modelFileWrl = "ViSP-images/mbt/cube_and_cylinder.wrl";
249  }else{
250  modelFileCao = "ViSP-images/mbt/cube.cao";
251  modelFileWrl = "ViSP-images/mbt/cube.wrl";
252  }
253 
254  if(!opt_ipath.empty()){
255  if(cao3DModel){
256  modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
257  }
258  else{
259 #ifdef VISP_HAVE_COIN
260  modelFile = vpIoTools::createFilePath(opt_ipath, modelFileWrl);
261 #else
262  std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
263  modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
264 #endif
265  }
266  }
267  else{
268  if(cao3DModel){
269  modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
270  }
271  else{
272 #ifdef VISP_HAVE_COIN
273  modelFile = vpIoTools::createFilePath(env_ipath, modelFileWrl);
274 #else
275  std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
276  modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
277 #endif
278  }
279  }
280  }
281 
282  if (!opt_initFile.empty())
283  initFile = opt_initFile;
284  else if (!opt_ipath.empty())
285  initFile = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube");
286  else
287  initFile = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
288 
290  vpVideoReader reader;
291 
292  reader.setFileName(ipath);
293  try{
294  reader.open(I);
295  }catch(...){
296  std::cout << "Cannot open sequence: " << ipath << std::endl;
297  return -1;
298  }
299 
300  reader.acquire(I);
301 
302  // initialise a display
303 #if defined VISP_HAVE_X11
304  vpDisplayX display;
305 #elif defined VISP_HAVE_GDI
306  vpDisplayGDI display;
307 #elif defined VISP_HAVE_OPENCV
308  vpDisplayOpenCV display;
309 #elif defined VISP_HAVE_D3D9
310  vpDisplayD3D display;
311 #elif defined VISP_HAVE_GTK
312  vpDisplayGTK display;
313 #else
314  opt_display = false;
315 #endif
316  if (opt_display)
317  {
318 #if (defined VISP_HAVE_DISPLAY)
319  display.init(I, 100, 100, "Test tracking") ;
320 #endif
321  vpDisplay::display(I) ;
322  vpDisplay::flush(I);
323  }
324 
325  vpMbEdgeTracker tracker;
327 
328  // Initialise the tracker: camera parameters, moving edge and KLT settings
329  vpCameraParameters cam;
330 #if defined (VISP_HAVE_XML2)
331  // From the xml file
332  tracker.loadConfigFile(configFile);
333 #else
334  // By setting the parameters:
335  cam.initPersProjWithoutDistortion(547, 542, 338, 234);
336 
337  vpMe me;
338  me.setMaskSize(5);
339  me.setMaskNumber(180);
340  me.setRange(7);
341  me.setThreshold(5000);
342  me.setMu1(0.5);
343  me.setMu2(0.5);
344  me.setSampleStep(4);
345  me.setNbTotalSample(250);
346 
347  tracker.setCameraParameters(cam);
348  tracker.setMovingEdge(me);
349 
350  // Specify the clipping to use
351  tracker.setNearClippingDistance(0.01);
352  tracker.setFarClippingDistance(0.90);
354  // tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
355 #endif
356 
357  // Display the moving edges, see documentation for the signification of the colours
358  tracker.setDisplayFeatures(displayFeatures);
359 
360  // Tells if the tracker has to use Ogre3D for visibility tests
361  tracker.setOgreVisibilityTest(useOgre);
362 
363  // Retrieve the camera parameters from the tracker
364  tracker.getCameraParameters(cam);
365 
366  // Loop to position the cube
367  if (opt_display && opt_click_allowed)
368  {
369  while(!vpDisplay::getClick(I,false)){
371  vpDisplay::displayText(I, 15, 10, "click after positioning the object", vpColor::red);
372  vpDisplay::flush(I) ;
373  }
374  }
375 
376  // Load the 3D model (either a vrml file or a .cao file)
377  tracker.loadModel(modelFile);
378 
379  // Initialise the tracker by clicking on the image
380  // This function looks for
381  // - a ./cube/cube.init file that defines the 3d coordinates (in meter, in the object basis) of the points used for the initialisation
382  // - a ./cube/cube.ppm file to display where the user have to click (optionnal, set by the third parameter)
383  if (opt_display && opt_click_allowed)
384  {
385  tracker.initClick(I, initFile, true);
386  tracker.getPose(cMo);
387  // display the 3D model at the given pose
388  tracker.display(I,cMo, cam, vpColor::red);
389  }
390  else
391  {
392  vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
393  tracker.initFromPose(I, cMoi);
394  }
395 
396  //track the model
397  tracker.track(I);
398  tracker.getPose(cMo);
399 
400  if (opt_display)
401  vpDisplay::flush(I);
402 
403  // Uncomment if you want to compute the covariance matrix.
404  // tracker.setCovarianceComputation(true); //Important if you want tracker.getCovarianceMatrix() to work.
405 
406  while (!reader.end())
407  {
408  // acquire a new image
409  reader.acquire(I);
410  // display the image
411  if (opt_display)
413 
414  // Test to reset the tracker
415  if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 10) {
416  vpTRACE("Test reset tracker");
417  if (opt_display)
419  tracker.resetTracker();
420 #if defined (VISP_HAVE_XML2)
421  tracker.loadConfigFile(configFile);
422 #else
423  // By setting the parameters:
424  cam.initPersProjWithoutDistortion(547, 542, 338, 234);
425 
426  vpMe me;
427  me.setMaskSize(5);
428  me.setMaskNumber(180);
429  me.setRange(7);
430  me.setThreshold(5000);
431  me.setMu1(0.5);
432  me.setMu2(0.5);
433  me.setSampleStep(4);
434  me.setNbTotalSample(250);
435 
436  tracker.setCameraParameters(cam);
437  tracker.setMovingEdge(me);
438 
439  // Specify the clipping to use
440  tracker.setNearClippingDistance(0.01);
441  tracker.setFarClippingDistance(0.90);
443  // tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
444 #endif
445  tracker.loadModel(modelFile);
446  tracker.setCameraParameters(cam);
447  tracker.setOgreVisibilityTest(useOgre);
448  tracker.initFromPose(I, cMo);
449  }
450 
451  // Test to set an initial pose
452  if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 50) {
453  cMo.buildFrom(0.04371844921, 0.08438820979, 0.5382029442, 2.200417277, 0.873535825, -0.3479076844);
454  vpTRACE("Test set pose");
455  tracker.setPose(I, cMo);
456  if (opt_display) {
457  // display the 3D model
458  tracker.display(I, cMo, cam, vpColor::darkRed);
459  // display the frame
460  vpDisplay::displayFrame (I, cMo, cam, 0.05);
461 // if (opt_click_allowed) {
462 // vpDisplay::flush(I);
463 // vpDisplay::getClick(I);
464 // }
465  }
466  }
467 
468  // track the object: stop tracking from frame 40 to 50
469  if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 || reader.getFrameIndex() > reader.getFirstFrameIndex() + 50) {
470  tracker.track(I);
471  tracker.getPose(cMo);
472  if (opt_display) {
473  // display the 3D model
474  tracker.display(I, cMo, cam, vpColor::darkRed);
475  // display the frame
476  vpDisplay::displayFrame (I, cMo, cam, 0.05);
477  }
478  }
479 
480  if (opt_click_allowed) {
481  vpDisplay::displayText(I, 10, 10, "Click to quit", vpColor::red);
482  if (vpDisplay::getClick(I, false)) {
483  quit = true;
484  break;
485  }
486  }
487 
488  // Uncomment if you want to print the covariance matrix.
489  // Make sure tracker.setCovarianceComputation(true) has been called (uncomment below).
490  // std::cout << tracker.getCovarianceMatrix() << std::endl << std::endl;
491 
492  vpDisplay::flush(I) ;
493  }
494 
495  if (opt_click_allowed && !quit) {
497  }
498  reader.close();
499 
500 #if defined (VISP_HAVE_XML2)
501  // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile()
503 #endif
504 
505 #if defined(VISP_HAVE_COIN) && (COIN_MAJOR_VERSION == 3)
506  // Cleanup memory allocated by Coin library used to load a vrml model in vpMbEdgeTracker::loadModel()
507  // We clean only if Coin was used.
508  if(! cao3DModel)
509  SoDB::finish();
510 #endif
511 
512  return 0;
513  }
514  catch(vpException e) {
515  std::cout << "Catch an exception: " << e << std::endl;
516  return 1;
517  }
518 }
519 
520 #else
521 
522 int main()
523 {
524  std::cout << "Display is required to run this example." << std::endl;
525  return 0;
526 }
527 
528 #endif
529 
void setMovingEdge(const vpMe &me)
long getFrameIndex() const
long getFirstFrameIndex() const
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1071
virtual unsigned int getClipping() const
Definition: vpMbTracker.h:220
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void track(const vpImage< unsigned char > &I)
#define vpTRACE
Definition: vpDebug.h:418
void setMaskNumber(const unsigned int &a)
Definition: vpMe.cpp:480
static const vpColor darkRed
Definition: vpColor.h:168
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
void setSampleStep(const double &s)
Definition: vpMe.h:271
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Definition: vpDisplay.cpp:887
void setNbTotalSample(const int &nb)
Definition: vpMe.h:199
Define the X11 console to display images.
Definition: vpDisplayX.h:152
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:76
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
Contains predetermined masks for sites and holds moving edges tracking parameters.
Definition: vpMe.h:70
Make the complete tracking of an object by using its CAD model.
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
void loadConfigFile(const std::string &configFile)
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2232
void setMu1(const double &mu_1)
Definition: vpMe.h:171
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:80
static const vpColor red
Definition: vpColor.h:167
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
virtual void setNearClippingDistance(const double &dist)
virtual void setOgreVisibilityTest(const bool &v)
void open(vpImage< vpRGBa > &I)
Display for windows using Direct3D.
Definition: vpDisplayD3D.h:109
void setMaskSize(const unsigned int &a)
Definition: vpMe.cpp:488
void getPose(vpHomogeneousMatrix &cMo_) const
Definition: vpMbTracker.h:328
static std::string createFilePath(const std::string &parent, const std::string child)
Definition: vpIoTools.cpp:1245
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:210
The vpDisplayOpenCV allows to display image using the opencv library.
virtual void getCameraParameters(vpCameraParameters &camera) const
Definition: vpMbTracker.h:213
Generic class defining intrinsic camera parameters.
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Definition: vpDisplayGTK.h:145
void acquire(vpImage< vpRGBa > &I)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1)
Definition: vpDisplay.cpp:375
virtual void setFarClippingDistance(const double &dist)
void setFileName(const char *filename)
virtual void initClick(const vpImage< unsigned char > &I, const std::string &initFile, const bool displayHelp=false)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
static void cleanup()
Definition: vpXmlParser.h:220
void setMu2(const double &mu_2)
Definition: vpMe.h:185
virtual void loadModel(const char *modelFile, const bool verbose=false)
void setThreshold(const double &t)
Definition: vpMe.h:299
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
virtual bool getClick(bool blocking=true)=0
void setDisplayFeatures(const bool displayF)
Definition: vpMbTracker.h:431
void setRange(const unsigned int &r)
Definition: vpMe.h:229
virtual void setClipping(const unsigned int &flags)