ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
mbtEdgeTracking.cpp
1 /****************************************************************************
2  *
3  * $Id: mbtEdgeTracking.cpp 4121 2013-02-08 10:32:20Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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 
70 void usage(const char *name, const char *badparam)
71 {
72  fprintf(stdout, "\n\
73 Example of tracking based on the 3D model.\n\
74 \n\
75 SYNOPSIS\n\
76  %s [-i <test image path>] [-x <config file>]\n\
77  [-m <model name>] [-n <initialisation file base name>]\n\
78  [-t] [-c] [-d] [-h] [-f] [-C]",
79  name );
80 
81  fprintf(stdout, "\n\
82 OPTIONS: \n\
83  -i <input image path> \n\
84  Set image input path.\n\
85  From this path read images \n\
86  \"ViSP-images/mbt/cube/image%%04d.ppm\". These \n\
87  images come from ViSP-images-x.y.z.tar.gz available \n\
88  on the ViSP website.\n\
89  Setting the VISP_INPUT_IMAGE_PATH environment\n\
90  variable produces the same behaviour than using\n\
91  this option.\n\
92 \n\
93  -x <config file> \n\
94  Set the config file (the xml file) to use.\n\
95  The config file is used to specify the parameters of the tracker.\n\
96 \n\
97  -m <model name> \n\
98  Specify the name of the file of the model\n\
99  The model can either be a vrml model (.wrl) or a .cao file.\n\
100 \n\
101  -f \n\
102  Do not use the vrml model, use the .cao one. These two models are \n\
103  equivalent and comes from ViSP-images-x.y.z.tar.gz available on the ViSP\n\
104  website. However, the .cao model allows to use the 3d model based tracker \n\
105  without Coin.\n\
106 \n\
107  -C \n\
108  Track only the cube (not the cylinder). In this case the models files are\n\
109  cube.cao or cube.wrl instead of cube_and_cylinder.cao and \n\
110  cube_and_cylinder.wrl.\n\
111 \n\
112  -n <initialisation file base name> \n\
113  Base name of the initialisation file. The file will be 'base_name'.init .\n\
114  This base name is also used for the optionnal picture specifying where to \n\
115  click (a .ppm picture).\
116 \n\
117  -t \n\
118  Turn off the display of the the moving edges. \n\
119 \n\
120  -d \n\
121  Turn off the display.\n\
122 \n\
123  -c\n\
124  Disable the mouse click. Useful to automaze the \n\
125  execution of this program without humain intervention.\n\
126 \n\
127  -o\n\
128  Use Ogre3D for visibility tests\n\
129 \n\
130  -h \n\
131  Print the help.\n\n");
132 
133  if (badparam)
134  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
135 }
136 
137 
138 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, bool &useOgre)
139 {
140  const char *optarg;
141  int c;
142  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
143 
144  switch (c) {
145  case 'i': ipath = optarg; break;
146  case 'x': configFile = optarg; break;
147  case 'm': modelFile = optarg; break;
148  case 'n': initFile = optarg; break;
149  case 't': displayMovingEdge = false; break;
150  case 'f': cao3DModel = true; break;
151  case 'c': click_allowed = false; break;
152  case 'd': display = false; break;
153  case 'C': trackCylinder = false; break;
154  case 'o' : useOgre = true; break;
155  case 'h': usage(argv[0], NULL); return false; break;
156 
157  default:
158  usage(argv[0], optarg);
159  return false; break;
160  }
161  }
162 
163  if ((c == 1) || (c == -1)) {
164  // standalone param or error
165  usage(argv[0], NULL);
166  std::cerr << "ERROR: " << std::endl;
167  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
168  return false;
169  }
170 
171  return true;
172 }
173 
174 int
175 main(int argc, const char ** argv)
176 {
177  std::string env_ipath;
178  std::string opt_ipath;
179  std::string ipath;
180  std::string opt_configFile;
181  std::string configFile;
182  std::string opt_modelFile;
183  std::string modelFile;
184  std::string opt_initFile;
185  std::string initFile;
186  bool displayMovingEdge = true;
187  bool opt_click_allowed = true;
188  bool opt_display = true;
189  bool cao3DModel = false;
190  bool trackCylinder = true;
191  bool useOgre = false;
192 
193  // Get the VISP_IMAGE_PATH environment variable value
194  char *ptenv = getenv("VISP_INPUT_IMAGE_PATH");
195  if (ptenv != NULL)
196  env_ipath = ptenv;
197 
198  // Set the default input path
199  if (! env_ipath.empty())
200  ipath = env_ipath;
201 
202 
203  // Read the command line options
204  if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, displayMovingEdge, opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre)) {
205  return (-1);
206  }
207 
208  // Test if an input path is set
209  if (opt_ipath.empty() && env_ipath.empty() ){
210  usage(argv[0], NULL);
211  std::cerr << std::endl
212  << "ERROR:" << std::endl;
213  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
214  << std::endl
215  << " environment variable to specify the location of the " << std::endl
216  << " image path where test images are located." << std::endl
217  << std::endl;
218 
219  return (-1);
220  }
221 
222  // Get the option values
223  if (!opt_ipath.empty())
224  ipath = opt_ipath + vpIoTools::path("/ViSP-images/mbt/cube/image%04d.pgm");
225  else
226  ipath = env_ipath + vpIoTools::path("/ViSP-images/mbt/cube/image%04d.pgm");
227 
228  if (!opt_configFile.empty())
229  configFile = opt_configFile;
230  else if (!opt_ipath.empty())
231  configFile = opt_ipath + vpIoTools::path("/ViSP-images/mbt/cube.xml");
232  else
233  configFile = env_ipath + vpIoTools::path("/ViSP-images/mbt/cube.xml");
234 
235  if (!opt_modelFile.empty()){
236  modelFile = opt_modelFile;
237  }else{
238  std::string modelFileCao;
239  std::string modelFileWrl;
240  if(trackCylinder){
241  modelFileCao = "/ViSP-images/mbt/cube_and_cylinder.cao";
242  modelFileWrl = "/ViSP-images/mbt/cube_and_cylinder.wrl";
243  }else{
244  modelFileCao = "/ViSP-images/mbt/cube.cao";
245  modelFileWrl = "/ViSP-images/mbt/cube.wrl";
246  }
247 
248  if(!opt_ipath.empty()){
249  if(cao3DModel){
250  modelFile = opt_ipath + vpIoTools::path(modelFileCao);
251  }
252  else{
253 #ifdef VISP_HAVE_COIN
254  modelFile = opt_ipath + vpIoTools::path(modelFileWrl);
255 #else
256  std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
257  modelFile = opt_ipath + vpIoTools::path(modelFileCao);
258 #endif
259  }
260  }
261  else{
262  if(cao3DModel){
263  modelFile = env_ipath + vpIoTools::path(modelFileCao);
264  }
265  else{
266 #ifdef VISP_HAVE_COIN
267  modelFile = env_ipath + vpIoTools::path(modelFileWrl);
268 #else
269  std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
270  modelFile = env_ipath + vpIoTools::path(modelFileCao);
271 #endif
272  }
273  }
274  }
275 
276  if (!opt_initFile.empty())
277  initFile = opt_initFile;
278  else if (!opt_ipath.empty())
279  initFile = opt_ipath + vpIoTools::path("/ViSP-images/mbt/cube");
280  else
281  initFile = env_ipath + vpIoTools::path("/ViSP-images/mbt/cube");
282 
284  vpVideoReader reader;
285 
286  reader.setFileName(ipath.c_str());
287  try{
288  reader.open(I);
289  }catch(...){
290  std::cout << "Cannot open sequence: " << ipath << std::endl;
291  return -1;
292  }
293 
294  reader.acquire(I);
295 
296  // initialise a display
297 #if defined VISP_HAVE_X11
298  vpDisplayX display;
299 #elif defined VISP_HAVE_GDI
300  vpDisplayGDI display;
301 #elif defined VISP_HAVE_OPENCV
302  vpDisplayOpenCV display;
303 #elif defined VISP_HAVE_D3D9
304  vpDisplayD3D display;
305 #elif defined VISP_HAVE_GTK
306  vpDisplayGTK display;
307 #else
308  opt_display = false;
309 #endif
310  if (opt_display)
311  {
312 #if (defined VISP_HAVE_DISPLAY)
313  display.init(I, 100, 100, "Test tracking") ;
314 #endif
315  vpDisplay::display(I) ;
316  vpDisplay::flush(I);
317  }
318 
319  vpMbEdgeTracker tracker;
321 
322  // Initialise the tracker: camera parameters, moving edge and KLT settings
323  vpCameraParameters cam;
324 #if defined (VISP_HAVE_XML2)
325  // From the xml file
326  tracker.loadConfigFile(configFile.c_str());
327 #else
328  // By setting the parameters:
329  cam.initPersProjWithoutDistortion(547, 542, 338, 234);
330 
331  vpMe me;
332  me.setMaskSize(5);
333  me.setMaskNumber(180);
334  me.setRange(7);
335  me.setThreshold(5000);
336  me.setMu1(0.5);
337  me.setMu2(0.5);
338  me.setMinSampleStep(4);
339  me.setNbTotalSample(250);
340 
341  tracker.setCameraParameters(cam);
342  tracker.setMovingEdge(me);
343 #endif
344 
345  // Display the moving edges, see documentation for the significations of the colour
346  tracker.setDisplayMovingEdges(displayMovingEdge);
347 
348  // Tells if the tracker has to use Ogre3D for visibility tests
349  tracker.setOgreVisibilityTest(useOgre);
350 
351  // Retrieve the camera parameters from the tracker
352  tracker.getCameraParameters(cam);
353 
354  // Loop to position the cube
355  if (opt_display && opt_click_allowed)
356  {
357  while(!vpDisplay::getClick(I,false)){
360  "click after positioning the object",
361  vpColor::red);
362  vpDisplay::flush(I) ;
363  }
364  }
365 
366  // Load the 3D model (either a vrml file or a .cao file)
367  try{
368  tracker.loadModel(modelFile.c_str());
369  }
370  catch(...)
371  {
372  return 0;
373  }
374  // Initialise the tracker by clicking on the image
375  // This function looks for
376  // - a ./cube/cube.init file that defines the 3d coordinates (in meter, in the object basis) of the points used for the initialisation
377  // - a ./cube/cube.ppm file to display where the user have to click (optionnal, set by the third parameter)
378  if (opt_display && opt_click_allowed)
379  {
380  tracker.initClick(I, initFile.c_str(), true);
381  tracker.getPose(cMo);
382  // display the 3D model at the given pose
383  tracker.display(I,cMo, cam, vpColor::red);
384  }
385  else
386  {
387  vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
388  tracker.initFromPose(I, cMoi);
389  }
390 
391  //track the model
392  tracker.track(I);
393  tracker.getPose(cMo);
394 
395  if (opt_display)
396  vpDisplay::flush(I);
397 
398  // Uncomment if you want to compute the covariance matrix.
399  // tracker.setCovarianceComputation(true); //Important if you want tracker.getCovarianceMatrix() to work.
400 
401  int iter = 0;
402  while (iter < 200)
403  {
404  try
405  {
406  // acquire a new image
407  reader.acquire(I);
408  // display the image
409  if (opt_display)
411  // track the object
412  tracker.track(I);
413  tracker.getPose(cMo);
414  // display the 3D model
415  if (opt_display)
416  {
417  tracker.display(I, cMo, cam, vpColor::darkRed);
418  // display the frame
419  vpDisplay::displayFrame (I, cMo, cam, 0.05, vpColor::blue);
420  }
421 
422  // Uncomment if you want to print the covariance matrix.
423  // Make sure tracker.setCovarianceComputation(true) has been called (uncomment below).
424  // std::cout << tracker.getCovarianceMatrix() << std::endl << std::endl;
425 
426  }
427  catch(...)
428  {
429  std::cout << "error caught" << std::endl;
430  break;
431  }
432  vpDisplay::flush(I) ;
433  iter++;
434  }
435  reader.close();
436 
437  // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile()
438 #if defined (VISP_HAVE_XML2)
440 #endif
441 
442  return 0;
443 }
444 
445 #else
446 
447 int main()
448 {
449  std::cout << "Display is required to run this example." << std::endl;
450  return 0;
451 }
452 
453 #endif
454