ViSP  2.6.2
mbtTracking.cpp
1 /****************************************************************************
2  *
3  * $Id: mbtTracking.cpp 3834 2012-07-03 10:36:30Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 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 
53 #include <visp/vpImageIo.h>
54 #include <visp/vpIoTools.h>
55 #include <visp/vpMath.h>
56 #include <visp/vpHomogeneousMatrix.h>
57 #include <visp/vpDisplayGTK.h>
58 #include <visp/vpDisplayX.h>
59 #include <visp/vpDisplayGDI.h>
60 
61 #include <visp/vpMbEdgeTracker.h>
62 
63 #include <visp/vpVideoReader.h>
64 #include <visp/vpParseArgv.h>
65 
66 #if (defined VISP_HAVE_XML2)
67 
68 #define GETOPTARGS "x:m:i:n:dchtfC"
69 
70 
71 void usage(const char *name, const char *badparam)
72 {
73  fprintf(stdout, "\n\
74 Example of tracking based on the 3D model.\n\
75 \n\
76 SYNOPSIS\n\
77  %s [-i <test image path>] [-x <config file>]\n\
78  [-m <model name>] [-n <initialisation file base name>]\n\
79  [-t] [-c] [-d] [-h] [-f] [-C]",
80  name );
81 
82  fprintf(stdout, "\n\
83 OPTIONS: \n\
84  -i <input image path> \n\
85  Set image input path.\n\
86  From this path read images \n\
87  \"ViSP-images/mbt/cube/image%%04d.ppm\". These \n\
88  images come from ViSP-images-x.y.z.tar.gz available \n\
89  on the ViSP website.\n\
90  Setting the VISP_INPUT_IMAGE_PATH environment\n\
91  variable produces the same behaviour than using\n\
92  this option.\n\
93 \n\
94  -x <config file> \n\
95  Set the config file (the xml file) to use.\n\
96  The config file is used to specify the parameters of the tracker.\n\
97 \n\
98  -m <model name> \n\
99  Specify the name of the file of the model\n\
100  The model can either be a vrml model (.wrl) or a .cao file.\n\
101 \n\
102  -f \n\
103  Do not use the vrml model, use the .cao one. These two models are \n\
104  equivalent and comes from ViSP-images-x.y.z.tar.gz available on the ViSP\n\
105  website. However, the .cao model allows to use the 3d model based tracker \n\
106  without Coin.\n\
107 \n\
108  -C \n\
109  Track only the cube (not the cylinder). In this case the models files are\n\
110  cube.cao or cube.wrl instead of cube_and_cylinder.cao and \n\
111  cube_and_cylinder.wrl.\n\
112 \n\
113  -n <initialisation file base name> \n\
114  Base name of the initialisation file. The file will be 'base_name'.init .\n\
115  This base name is also used for the optionnal picture specifying where to \n\
116  click (a .ppm picture).\
117 \n\
118  -t \n\
119  Turn off the display of the the moving edges. \n\
120 \n\
121  -d \n\
122  Turn off the display.\n\
123 \n\
124  -c\n\
125  Disable the mouse click. Useful to automaze the \n\
126  execution of this program without humain intervention.\n\
127 \n\
128  -h \n\
129  Print the help.\n\n");
130 
131  if (badparam)
132  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
133 }
134 
135 
136 bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile, std::string &initFile, bool &displayMovingEdge, bool &click_allowed, bool &display, bool& cao3DModel, bool& trackCylinder)
137 {
138  const char *optarg;
139  int c;
140  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
141 
142  switch (c) {
143  case 'i': ipath = optarg; break;
144  case 'x': configFile = optarg; break;
145  case 'm': modelFile = optarg; break;
146  case 'n': initFile = optarg; break;
147  case 't': displayMovingEdge = false; break;
148  case 'f': cao3DModel = true; break;
149  case 'c': click_allowed = false; break;
150  case 'd': display = false; break;
151  case 'C': trackCylinder = false; break;
152  case 'h': usage(argv[0], NULL); return false; break;
153 
154  default:
155  usage(argv[0], optarg);
156  return false; break;
157  }
158  }
159 
160  if ((c == 1) || (c == -1)) {
161  // standalone param or error
162  usage(argv[0], NULL);
163  std::cerr << "ERROR: " << std::endl;
164  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
165  return false;
166  }
167 
168  return true;
169 }
170 
171 int
172 main(int argc, const char ** argv)
173 {
174  std::string env_ipath;
175  std::string opt_ipath;
176  std::string ipath;
177  std::string opt_configFile;
178  std::string configFile;
179  std::string opt_modelFile;
180  std::string modelFile;
181  std::string opt_initFile;
182  std::string initFile;
183  bool displayMovingEdge = true;
184  bool opt_click_allowed = true;
185  bool opt_display = true;
186  bool cao3DModel = false;
187  bool trackCylinder = true;
188 
189  // Get the VISP_IMAGE_PATH environment variable value
190  char *ptenv = getenv("VISP_INPUT_IMAGE_PATH");
191  if (ptenv != NULL)
192  env_ipath = ptenv;
193 
194  // Set the default input path
195  if (! env_ipath.empty())
196  ipath = env_ipath;
197 
198 
199  // Read the command line options
200  if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, displayMovingEdge, opt_click_allowed, opt_display, cao3DModel, trackCylinder)) {
201  return (-1);
202  }
203 
204  // Test if an input path is set
205  if (opt_ipath.empty() && env_ipath.empty() ){
206  usage(argv[0], NULL);
207  std::cerr << std::endl
208  << "ERROR:" << std::endl;
209  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
210  << std::endl
211  << " environment variable to specify the location of the " << std::endl
212  << " image path where test images are located." << std::endl
213  << std::endl;
214 
215  return (-1);
216  }
217 
218  // Get the option values
219  if (!opt_ipath.empty())
220  ipath = opt_ipath + vpIoTools::path("/ViSP-images/mbt/cube/image%04d.pgm");
221  else
222  ipath = env_ipath + vpIoTools::path("/ViSP-images/mbt/cube/image%04d.pgm");
223 
224  if (!opt_configFile.empty())
225  configFile = opt_configFile;
226  else if (!opt_ipath.empty())
227  configFile = opt_ipath + vpIoTools::path("/ViSP-images/mbt/cube.xml");
228  else
229  configFile = env_ipath + vpIoTools::path("/ViSP-images/mbt/cube.xml");
230 
231  if (!opt_modelFile.empty()){
232  modelFile = opt_modelFile;
233  }else{
234  std::string modelFileCao;
235  std::string modelFileWrl;
236  if(trackCylinder){
237  modelFileCao = "/ViSP-images/mbt/cube_and_cylinder.cao";
238  modelFileWrl = "/ViSP-images/mbt/cube_and_cylinder.wrl";
239  }else{
240  modelFileCao = "/ViSP-images/mbt/cube.cao";
241  modelFileWrl = "/ViSP-images/mbt/cube.wrl";
242  }
243 
244  if(!opt_ipath.empty()){
245  if(cao3DModel){
246  modelFile = opt_ipath + vpIoTools::path(modelFileCao);
247  }
248  else{
249 #ifdef VISP_HAVE_COIN
250  modelFile = opt_ipath + vpIoTools::path(modelFileWrl);
251 #else
252  std::cerr << "Coin is not detected in ViSP. USe the .cao model instead." << std::endl;
253  modelFile = opt_ipath + vpIoTools::path(modelFileCao);
254 #endif
255  }
256  }
257  else{
258  if(cao3DModel){
259  modelFile = env_ipath + vpIoTools::path(modelFileCao);
260  }
261  else{
262 #ifdef VISP_HAVE_COIN
263  modelFile = env_ipath + vpIoTools::path(modelFileWrl);
264 #else
265  std::cerr << "Coin is not detected in ViSP. USe the .cao model instead." << std::endl;
266  modelFile = env_ipath + vpIoTools::path(modelFileCao);
267 #endif
268  }
269  }
270  }
271 
272  if (!opt_initFile.empty())
273  initFile = opt_initFile;
274  else if (!opt_ipath.empty())
275  initFile = opt_ipath + vpIoTools::path("/ViSP-images/mbt/cube");
276  else
277  initFile = env_ipath + vpIoTools::path("/ViSP-images/mbt/cube");
278 
280  vpVideoReader reader;
281 
282  reader.setFileName(ipath.c_str());
283  try{
284  reader.open(I);
285  }catch(...){
286  std::cout << "Cannot open sequence: " << ipath << std::endl;
287  return -1;
288  }
289 
290  reader.acquire(I);
291 
292  // initialise a display
293 #if defined VISP_HAVE_X11
294  vpDisplayX display;
295 #elif defined VISP_HAVE_GDI
296  vpDisplayGDI display;
297 #elif defined VISP_HAVE_D3D
298  vpDisplayD3D display;
299 #else
300  opt_display = false;
301 #endif
302  if (opt_display)
303  {
304 #if (defined VISP_HAVE_X11) || (defined VISP_HAVE_GDI) || (defined VISP_HAVE_D3D)
305  display.init(I, 100, 100, "Test tracking") ;
306 #endif
307  vpDisplay::display(I) ;
308  vpDisplay::flush(I);
309  }
310 
311  vpMbEdgeTracker tracker;
313 
314  // Load tracker config file (camera parameters and moving edge settings)
315  tracker.loadConfigFile(configFile.c_str());
316 
317  // Display the moving edges, see documentation for the significations of the colour
318  tracker.setDisplayMovingEdges(displayMovingEdge);
319 
320  // initialise an instance of vpCameraParameters with the parameters from the tracker
321  vpCameraParameters cam;
322  tracker.getCameraParameters(cam);
323 
324 
325  // Loop to position the cube
326  if (opt_display && opt_click_allowed)
327  {
328  while(!vpDisplay::getClick(I,false)){
331  "click after positioning the object",
332  vpColor::red);
333  vpDisplay::flush(I) ;
334  }
335  }
336 
337  // Load the 3D model (either a vrml file or a .cao file)
338  try{
339  tracker.loadModel(modelFile.c_str());
340  }
341  catch(...)
342  {
343  return 0;
344  }
345  // Initialise the tracker by clicking on the image
346  // This function looks for
347  // - a ./cube/cube.init file that defines the 3d coordinates (in meter, in the object basis) of the points used for the initialisation
348  // - a ./cube/cube.ppm file to display where the user have to click (optionnal, set by the third parameter)
349  if (opt_display && opt_click_allowed)
350  {
351  tracker.initClick(I, initFile.c_str(), true);
352  tracker.getPose(cMo);
353  // display the 3D model at the given pose
354  tracker.display(I,cMo, cam, vpColor::red);
355  }
356  else
357  {
358  vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
359  tracker.initFromPose(I, cMoi);
360  }
361 
362  //track the model
363  tracker.track(I);
364  tracker.getPose(cMo);
365 
366  if (opt_display)
367  vpDisplay::flush(I);
368 
369  // Uncomment if you want to compute the covariance matrix.
370  // tracker.setCovarianceComputation(true); //Important if you want tracker.getCovarianceMatrix() to work.
371 
372  int iter = 0;
373  while (iter < 200)
374  {
375  try
376  {
377  // acquire a new image
378  reader.acquire(I);
379  // display the image
380  if (opt_display)
382  // track the object
383  tracker.track(I);
384  tracker.getPose(cMo);
385  // display the 3D model
386  if (opt_display)
387  {
388  tracker.display(I, cMo, cam, vpColor::darkRed, 1);
389  // display the frame
390  vpDisplay::displayFrame (I, cMo, cam, 0.05, vpColor::blue);
391  }
392 
393  // Uncomment if you want to print the covariance matrix.
394  // Make sure tracker.setCovarianceComputation(true) has been called (uncomment below).
395  // std::cout << tracker.getCovarianceMatrix() << std::endl << std::endl;
396 
397  }
398  catch(...)
399  {
400  std::cout << "error caught" << std::endl;
401  break;
402  }
403  vpDisplay::flush(I) ;
404  iter++;
405  }
406  reader.close();
407 
408  // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile()
410 
411  return 0;
412 }
413 
414 #else
415 
416 int main()
417 {
418  std::cout << "Libxml2 is required to read the configuration of the tracker." << std::endl;
419  return 0;
420 }
421 
422 #endif
423 
void setDisplayMovingEdges(const bool displayMe)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void loadConfigFile(const std::string &filename)
void track(const vpImage< unsigned char > &I)
static const vpColor darkRed
Definition: vpColor.h:166
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
virtual void initFromPose(const vpImage< unsigned char > &_I, const std::string &_initFile)
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...
static std::string path(const char *pathname)
Definition: vpIoTools.cpp:669
Make the complete tracking of an object by using its CAD model.
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:1964
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:79
static const vpColor red
Definition: vpColor.h:165
virtual void getCameraParameters(vpCameraParameters &_cam) const
Definition: vpMbTracker.h:203
virtual void initClick(const vpImage< unsigned char > &_I, const std::string &_initFile, const bool _displayHelp=false)
void open(vpImage< vpRGBa > &I)
Display for windows using Direct3D.
Definition: vpDisplayD3D.h:108
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:186
Generic class defining intrinsic camera parameters.
void acquire(vpImage< vpRGBa > &I)
void setFileName(const char *filename)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness=1)
Definition: vpDisplay.cpp:346
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
static void cleanup()
Definition: vpXmlParser.h:220
virtual void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color=vpColor::green)=0
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int l=1, const bool displayFullModel=false)
void getPose(vpHomogeneousMatrix &_cMo) const
Definition: vpMbTracker.h:222
virtual bool getClick(bool blocking=true)=0
void loadModel(const std::string &cad_name)
static const vpColor blue
Definition: vpColor.h:171