ViSP
mbtKltTracking.cpp
1 /****************************************************************************
2  *
3  * $Id: mbtTracking.cpp 3957 2012-11-07 15:22:30Z fnovotny $
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 MBT KLT Tracking.
36  *
37  * Authors:
38  * Aurelien Yol
39  *
40  *****************************************************************************/
41 
48 #include <visp/vpConfig.h>
49 #include <visp/vpDebug.h>
50 #include <visp/vpDisplayD3D.h>
51 #include <visp/vpDisplayGTK.h>
52 #include <visp/vpDisplayGDI.h>
53 #include <visp/vpDisplayOpenCV.h>
54 #include <visp/vpDisplayX.h>
55 #include <visp/vpHomogeneousMatrix.h>
56 #include <visp/vpImageIo.h>
57 #include <visp/vpIoTools.h>
58 #include <visp/vpMath.h>
59 #include <visp/vpMbKltTracker.h>
60 #include <visp/vpVideoReader.h>
61 #include <visp/vpParseArgv.h>
62 
63 #if defined (VISP_HAVE_OPENCV) && defined (VISP_HAVE_DISPLAY) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
64 
65 
66 #define GETOPTARGS "x:m:i:n:dchtfo"
67 
68 void usage(const char *name, const char *badparam);
69 bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile,
70  std::string &initFile, bool &displayKltPoints, bool &click_allowed, bool &display,
71  bool& cao3DModel, bool &useOgre);
72 
73 void usage(const char *name, const char *badparam)
74 {
75  fprintf(stdout, "\n\
76 Example of tracking based on the 3D model.\n\
77 \n\
78 SYNOPSIS\n\
79  %s [-i <test image path>] [-x <config file>]\n\
80  [-m <model name>] [-n <initialisation file base name>]\n\
81  [-t] [-c] [-d] [-h] [-f]",
82  name );
83 
84  fprintf(stdout, "\n\
85 OPTIONS: \n\
86  -i <input image path> \n\
87  Set image input path.\n\
88  From this path read images \n\
89  \"ViSP-images/mbt/cube/image%%04d.ppm\". These \n\
90  images come from ViSP-images-x.y.z.tar.gz available \n\
91  on the ViSP website.\n\
92  Setting the VISP_INPUT_IMAGE_PATH environment\n\
93  variable produces the same behaviour than using\n\
94  this option.\n\
95 \n\
96  -x <config file> \n\
97  Set the config file (the xml file) to use.\n\
98  The config file is used to specify the parameters of the tracker.\n\
99 \n\
100  -m <model name> \n\
101  Specify the name of the file of the model\n\
102  The model can either be a vrml model (.wrl) or a .cao file.\n\
103 \n\
104  -f \n\
105  Do not use the vrml model, use the .cao one. These two models are \n\
106  equivalent and comes from ViSP-images-x.y.z.tar.gz available on the ViSP\n\
107  website. However, the .cao model allows to use the 3d model based tracker \n\
108  without Coin.\n\
109 \n\
110  -n <initialisation file base name> \n\
111  Base name of the initialisation file. The file will be 'base_name'.init .\n\
112  This base name is also used for the optionnal picture specifying where to \n\
113  click (a .ppm picture).\
114 \n\
115  -t \n\
116  Turn off the display of the the klt points. \n\
117 \n\
118  -d \n\
119  Turn off the display.\n\
120 \n\
121  -c\n\
122  Disable the mouse click. Useful to automaze the \n\
123  execution of this program without humain intervention.\n\
124 \n\
125  -o\n\
126  Use Ogre3D for visibility tests\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,
137  std::string &initFile, bool &displayKltPoints, bool &click_allowed, bool &display,
138  bool& cao3DModel, 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': displayKltPoints = false; break;
150  case 'f': cao3DModel = true; break;
151  case 'c': click_allowed = false; break;
152  case 'd': display = false; break;
153  case 'o': useOgre = true; break;
154  case 'h': usage(argv[0], NULL); return false; break;
155 
156  default:
157  usage(argv[0], optarg_);
158  return false; break;
159  }
160  }
161 
162  if ((c == 1) || (c == -1)) {
163  // standalone param or error
164  usage(argv[0], NULL);
165  std::cerr << "ERROR: " << std::endl;
166  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
167  return false;
168  }
169 
170  return true;
171 }
172 
173 int
174 main(int argc, const char ** argv)
175 {
176  try {
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 displayKltPoints = true;
187  bool opt_click_allowed = true;
188  bool opt_display = true;
189  bool cao3DModel = false;
190  bool useOgre = false;
191  bool quit = false;
192 
193  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
194  env_ipath = vpIoTools::getViSPImagesDataPath();
195 
196  // Set the default input path
197  if (! env_ipath.empty())
198  ipath = env_ipath;
199 
200  // Read the command line options
201  if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, displayKltPoints, opt_click_allowed, opt_display, cao3DModel, useOgre)) {
202  return (-1);
203  }
204 
205  // Test if an input path is set
206  if (opt_ipath.empty() && env_ipath.empty() ){
207  usage(argv[0], NULL);
208  std::cerr << std::endl
209  << "ERROR:" << std::endl;
210  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
211  << std::endl
212  << " environment variable to specify the location of the " << std::endl
213  << " image path where test images are located." << std::endl
214  << std::endl;
215 
216  return (-1);
217  }
218 
219  // Get the option values
220  if (!opt_ipath.empty())
221  ipath = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube/image%04d.pgm");
222  else
223  ipath = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube/image%04d.pgm");
224 
225  if (!opt_configFile.empty())
226  configFile = opt_configFile;
227  else if (!opt_ipath.empty())
228  configFile = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube.xml");
229  else
230  configFile = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.xml");
231 
232  if (!opt_modelFile.empty()){
233  modelFile = opt_modelFile;
234  }else{
235  std::string modelFileCao = "ViSP-images/mbt/cube.cao";
236  std::string modelFileWrl = "ViSP-images/mbt/cube.wrl";
237 
238  if(!opt_ipath.empty()){
239  if(cao3DModel){
240  modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
241  }
242  else{
243 #ifdef VISP_HAVE_COIN
244  modelFile = vpIoTools::createFilePath(opt_ipath, modelFileWrl);
245 #else
246  std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
247  modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
248 #endif
249  }
250  }
251  else{
252  if(cao3DModel){
253  modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
254  }
255  else{
256 #ifdef VISP_HAVE_COIN
257  modelFile = vpIoTools::createFilePath(env_ipath, modelFileWrl);
258 #else
259  std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
260  modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
261 #endif
262  }
263  }
264  }
265 
266  if (!opt_initFile.empty())
267  initFile = opt_initFile;
268  else if (!opt_ipath.empty())
269  initFile = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube");
270  else
271  initFile = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
272 
274  vpVideoReader reader;
275 
276  reader.setFileName(ipath);
277  try{
278  reader.open(I);
279  }catch(...){
280  std::cout << "Cannot open sequence: " << ipath << std::endl;
281  return -1;
282  }
283 
284  reader.acquire(I);
285 
286  // initialise a display
287 #if defined VISP_HAVE_X11
288  vpDisplayX display;
289 #elif defined VISP_HAVE_GDI
290  vpDisplayGDI display;
291 #elif defined VISP_HAVE_OPENCV
292  vpDisplayOpenCV display;
293 #elif defined VISP_HAVE_D3D9
294  vpDisplayD3D display;
295 #elif defined VISP_HAVE_GTK
296  vpDisplayGTK display;
297 #else
298  opt_display = false;
299 #endif
300  if (opt_display)
301  {
302 #if (defined VISP_HAVE_DISPLAY)
303  display.init(I, 100, 100, "Test tracking") ;
304 #endif
305  vpDisplay::display(I) ;
306  vpDisplay::flush(I);
307  }
308 
309  vpMbKltTracker tracker;
311 
312  // Load tracker config file (camera parameters and moving edge settings)
313  vpCameraParameters cam;
314 #if defined (VISP_HAVE_XML2)
315  // From the xml file
316  tracker.loadConfigFile(configFile);
317 #else
318  // By setting the parameters:
319  cam.initPersProjWithoutDistortion(547, 542, 338, 234);
320 
321  vpKltOpencv klt;
322  klt.setMaxFeatures(10000);
323  klt.setWindowSize(5);
324  klt.setQuality(0.01);
325  klt.setMinDistance(5);
326  klt.setHarrisFreeParameter(0.01);
327  klt.setBlockSize(3);
328  klt.setPyramidLevels(3);
329 
330  tracker.setCameraParameters(cam);
331  tracker.setKltOpencv(klt);
332  tracker.setAngleAppear( vpMath::rad(65) );
333  tracker.setAngleDisappear( vpMath::rad(75) );
334  tracker.setMaskBorder(5);
335 
336  // Specify the clipping to use
337  tracker.setNearClippingDistance(0.01);
338  tracker.setFarClippingDistance(0.90);
340  // tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
341 #endif
342 
343  // Display the klt points
344  tracker.setDisplayFeatures(displayKltPoints);
345 
346  // Tells if the tracker has to use Ogre3D for visibility tests
347  tracker.setOgreVisibilityTest(useOgre);
348 
349  // Retrieve the camera parameters from the tracker
350  tracker.getCameraParameters(cam);
351 
352  // Loop to position the cube
353  if (opt_display && opt_click_allowed)
354  {
355  while(!vpDisplay::getClick(I,false)){
357  vpDisplay::displayText(I, 15, 10, "click after positioning the object", vpColor::red);
358  vpDisplay::flush(I) ;
359  }
360  }
361 
362  // Load the 3D model (either a vrml file or a .cao file)
363  tracker.loadModel(modelFile);
364 
365  // Initialise the tracker by clicking on the image
366  // This function looks for
367  // - a ./cube/cube.init file that defines the 3d coordinates (in meter, in the object basis) of the points used for the initialisation
368  // - a ./cube/cube.ppm file to display where the user have to click (optionnal, set by the third parameter)
369  if (opt_display && opt_click_allowed)
370  {
371  tracker.initClick(I, initFile, true);
372  tracker.getPose(cMo);
373  // display the 3D model at the given pose
374  tracker.display(I,cMo, cam, vpColor::red);
375  }
376  else
377  {
378  vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
379  tracker.initFromPose(I, cMoi);
380  }
381 
382  //track the model
383  tracker.track(I);
384  tracker.getPose(cMo);
385 
386  if (opt_display)
387  vpDisplay::flush(I);
388 
389  // Uncomment if you want to compute the covariance matrix.
390  // tracker.setCovarianceComputation(true); //Important if you want tracker.getCovarianceMatrix() to work.
391 
392  while (!reader.end())
393  {
394  // acquire a new image
395  reader.acquire(I);
396  // display the image
397  if (opt_display)
399 
400  // Test to reset the tracker
401  if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 10) {
402  vpTRACE("Test reset tracker");
403  if (opt_display)
405  tracker.resetTracker();
406 #if defined (VISP_HAVE_XML2)
407  tracker.loadConfigFile(configFile);
408 #else
409  // By setting the parameters:
410  cam.initPersProjWithoutDistortion(547, 542, 338, 234);
411 
412  vpKltOpencv klt;
413  klt.setMaxFeatures(10000);
414  klt.setWindowSize(5);
415  klt.setQuality(0.01);
416  klt.setMinDistance(5);
417  klt.setHarrisFreeParameter(0.01);
418  klt.setBlockSize(3);
419  klt.setPyramidLevels(3);
420 
421  tracker.setCameraParameters(cam);
422  tracker.setKltOpencv(klt);
423  tracker.setAngleAppear( vpMath::rad(65) );
424  tracker.setAngleDisappear( vpMath::rad(75) );
425  tracker.setMaskBorder(5);
426 
427  // Specify the clipping to use
428  tracker.setNearClippingDistance(0.01);
429  tracker.setFarClippingDistance(0.90);
431  // tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
432 #endif
433  tracker.loadModel(modelFile);
434  tracker.setCameraParameters(cam);
435  tracker.setOgreVisibilityTest(useOgre);
436  tracker.initFromPose(I, cMo);
437  }
438 
439  // Test to set an initial pose
440  if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 50) {
441  cMo.buildFrom(0.04371844921, 0.08438820979, 0.5382029442, 2.200417277, 0.873535825, -0.3479076844);
442  vpTRACE("Test set pose");
443  tracker.setPose(I, cMo);
444  if (opt_display) {
445  // display the 3D model
446  tracker.display(I, cMo, cam, vpColor::darkRed);
447  // display the frame
448  vpDisplay::displayFrame (I, cMo, cam, 0.05);
449 // if (opt_click_allowed) {
450 // vpDisplay::flush(I);
451 // vpDisplay::getClick(I);
452 // }
453  }
454  }
455 
456  // track the object: stop tracking from frame 40 to 50
457  if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 || reader.getFrameIndex() > reader.getFirstFrameIndex() + 50) {
458  tracker.track(I);
459  tracker.getPose(cMo);
460  if (opt_display) {
461  // display the 3D model
462  tracker.display(I, cMo, cam, vpColor::darkRed);
463  // display the frame
464  vpDisplay::displayFrame (I, cMo, cam, 0.05);
465  }
466  }
467 
468  if (opt_click_allowed) {
469  vpDisplay::displayText(I, 10, 10, "Click to quit", vpColor::red);
470  if (vpDisplay::getClick(I, false)) {
471  quit = true;
472  break;
473  }
474  }
475 
476  // Uncomment if you want to print the covariance matrix.
477  // Make sure tracker.setCovarianceComputation(true) has been called (uncomment below).
478  // std::cout << tracker.getCovarianceMatrix() << std::endl << std::endl;
479 
480  vpDisplay::flush(I);
481  }
482  if (opt_click_allowed && !quit) {
484  }
485 
486  reader.close();
487 
488 #if defined (VISP_HAVE_XML2)
489  // Cleanup memory allocated by xml library used to parse the xml config file in vpMbKltTracker::loadConfigFile()
491 #endif
492 
493 #if defined(VISP_HAVE_COIN) && (COIN_MAJOR_VERSION == 3)
494  // Cleanup memory allocated by Coin library used to load a vrml model in vpMbKltTracker::loadModel()
495  // We clean only if Coin was used.
496  if(! cao3DModel)
497  SoDB::finish();
498 #endif
499 
500  return 0;
501  }
502  catch(vpException e) {
503  std::cout << "Catch an exception: " << e << std::endl;
504  return 1;
505  }
506 }
507 
508 #else
509 
510 int main()
511 {
512  std::cout << "OpenCV and display are required." << std::endl;
513  return 0;
514 
515 }
516 
517 #endif
void setKltOpencv(const vpKltOpencv &t)
virtual void track(const vpImage< unsigned char > &I)
long getFrameIndex() const
long getFirstFrameIndex() const
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1071
void setHarrisFreeParameter(double harris_k)
virtual unsigned int getClipping() const
Definition: vpMbTracker.h:220
virtual void setAngleDisappear(const double &a)
Definition: vpMbTracker.h:400
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpTRACE
Definition: vpDebug.h:418
static const vpColor darkRed
Definition: vpColor.h:168
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
void setMaxFeatures(const int maxCount)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Definition: vpDisplay.cpp:887
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...
void setMinDistance(double minDistance)
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)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2232
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:80
static const vpColor red
Definition: vpColor.h:167
virtual void loadConfigFile(const std::string &configFile)
void setQuality(double qualityLevel)
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
void open(vpImage< vpRGBa > &I)
Display for windows using Direct3D.
Definition: vpDisplayD3D.h:109
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
void setFileName(const char *filename)
virtual void setAngleAppear(const double &a)
Definition: vpMbTracker.h:389
Model based tracker using only KLT.
virtual void initClick(const vpImage< unsigned char > &I, const std::string &initFile, const bool displayHelp=false)
void setPyramidLevels(const int pyrMaxLevel)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
static double rad(double deg)
Definition: vpMath.h:100
static void cleanup()
Definition: vpXmlParser.h:220
void setWindowSize(const int winSize)
void setMaskBorder(const unsigned int &e)
virtual void loadModel(const char *modelFile, const bool verbose=false)
void setCameraParameters(const vpCameraParameters &cam)
virtual void setOgreVisibilityTest(const bool &v)
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV.
Definition: vpKltOpencv.h:79
void setBlockSize(const int blockSize)
virtual void setClipping(const unsigned int &flags)
virtual bool getClick(bool blocking=true)=0
void setDisplayFeatures(const bool displayF)
Definition: vpMbTracker.h:431
virtual void setFarClippingDistance(const double &dist)
virtual 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 void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void setNearClippingDistance(const double &dist)