ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
AROgreBasic.cpp
1 /****************************************************************************
2  *
3  * $Id: AROgreBasic.cpp 4111 2013-02-06 17:27:14Z 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  * Implementation of a simple augmented reality application using the vpAROgre
36  * class.
37  *
38  * Authors:
39  * Bertrand Delabarre
40  *
41  *****************************************************************************/
42 
50 #include <visp/vpConfig.h>
51 #include <iostream>
52 #if defined(VISP_HAVE_OGRE) && defined(VISP_HAVE_DISPLAY)
53 
54 #if defined(VISP_HAVE_X11) && ! defined(APPLE)
55 // produce an error on OSX: ‘typedef int Cursor’
56 // /usr/X11R6/include/X11/X.h:108: error: ‘Cursor’ has a previous
57 // declaration as ‘typedef XID Cursor’. That's why it should not be
58 // used on APPLE platforms
59 # include <visp/vpDisplayX.h>
60 #endif
61 #include <visp/vpDisplayGTK.h>
62 #include <visp/vpDisplayGDI.h>
63 #include <visp/vpDisplayOpenCV.h>
64 #include <visp/vpDisplayD3D.h>
65 #include <visp/vpPose.h>
66 #include <visp/vpPoint.h>
67 #include <visp/vpImagePoint.h>
68 #include <visp/vpDot2.h>
69 #include <visp/vpPixelMeterConversion.h>
70 #include <visp/vpVideoReader.h>
71 #include <visp/vpParseArgv.h>
72 #include <visp/vpIoTools.h>
73 #include <visp/vpDebug.h>
74 #include <visp/vpAROgre.h>
75 
76 // List of allowed command line options
77 #define GETOPTARGS "ci:p:h"
78 
89 void usage(const char *name, const char *badparam, std::string ipath, std::string ppath)
90 {
91  fprintf(stdout, "\n\
92 Test augmented reality using the vpAROgre class.\n\
93 \n\
94 SYNOPSIS\n\
95  %s [-i <test image path>] [-p <personal image path>]\n\
96  [-c] [-h]\n", name);
97 
98  fprintf(stdout, "\n\
99 OPTIONS: Default\n\
100  -i <input image path> %s\n\
101  Set image input path.\n\
102  From this path read images \n\
103  \"ViSP-images/mire-2/image.%%04d.pgm\". These \n\
104  images come from ViSP-images-x.y.z.tar.gz available \n\
105  on the ViSP website.\n\
106  Setting the VISP_INPUT_IMAGE_PATH environment\n\
107  variable produces the same behaviour than using\n\
108  this option.\n\
109  \n\
110  -p <personal image path> %s\n\
111  Specify a personal sequence containing images \n\
112  to process.\n\
113  By image sequence, we mean one file per image.\n\
114  The following image file formats PNM (PGM P5, PPM P6)\n\
115  are supported. The format is selected by analysing \n\
116  the filename extension.\n\
117  Example : \"/Temp/ViSP-images/cube/image.%%04d.pgm\"\n\
118  %%04d is for the image numbering.\n\
119 \n\
120  -c\n\
121  Disable the mouse click. Useful to automaze the \n\
122  execution of this program without humain intervention.\n\
123 \n\
124  -h\n\
125  Print the help.\n",
126  ipath.c_str(), ppath.c_str());
127 
128  if (badparam)
129  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
130 }
144 bool getOptions(int argc, const char **argv, std::string &ipath,
145  std::string &ppath, bool &click_allowed)
146 {
147  const char *optarg;
148  int c;
149  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
150 
151  switch (c) {
152  case 'c': click_allowed = false; break;
153  case 'i': ipath = optarg; break;
154  case 'p': ppath = optarg; break;
155  case 'h': usage(argv[0], NULL, ipath, ppath);
156  return false; break;
157 
158  default:
159  usage(argv[0], optarg, ipath, ppath);
160  return false; break;
161  }
162  }
163 
164  if ((c == 1) || (c == -1)) {
165  // standalone param or error
166  usage(argv[0], NULL, ipath, ppath);
167  std::cerr << "ERROR: " << std::endl;
168  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
169  return false;
170  }
171 
172  return true;
173 }
174 
179 void computeInitialPose(vpCameraParameters *mcam, vpImage<unsigned char> &I,
180  vpPose * mPose, vpDot2 *md, vpImagePoint *mcog,
181  vpHomogeneousMatrix *cmo, vpPoint *mP,
182  const bool &opt_click_allowed)
183 {
184  // ---------------------------------------------------
185  // Code inspired from ViSP example of camera pose
186  // ----------------------------------------------------
187  bool opt_display = true;
188 
189 #if defined(VISP_HAVE_X11) && ! defined(APPLE)
190  // produce an error on OSX: ‘typedef int Cursor’
191  // /usr/X11R6/include/X11/X.h:108: error: ‘Cursor’ has a previous
192  // declaration as ‘typedef XID Cursor’. That's why it should not be
193  // used on APPLE platforms
194  vpDisplayX display;
195 #elif defined VISP_HAVE_GTK
196  vpDisplayGTK display;
197 #elif defined VISP_HAVE_GDI
198  vpDisplayGDI display;
199 #elif defined VISP_HAVE_OPENCV
200  vpDisplayOpenCV display;
201 #elif defined VISP_HAVE_D3D9
202  vpDisplayD3D display;
203 #endif
204 
205  for (unsigned int i=0 ; i < 4 ; i++)
206  {
207  if (opt_display) {
208  md[i].setGraphics(true) ;
209  }
210  else {
211  md[i].setGraphics(false) ;
212  }
213  }
214 
215  if (opt_display) {
216  try{
217  // Display size is automatically defined by the image (I) size
218  display.init(I,100,100,"Preliminary Pose Calculation");
219  // display the image
220  // The image class has a member that specify a pointer toward
221  // the display that has been initialized in the display declaration
222  // therefore is is no longuer necessary to make a reference to the
223  // display variable.
224  vpDisplay::display(I) ;
225  //Flush the display
226  vpDisplay::flush(I) ;
227 
228  }
229  catch(...)
230  {
231  vpERROR_TRACE("Error while displaying the image") ;
232  return ;
233  }
234  }
235 
236  std::cout<<"************************************************************************************"<<std::endl;
237  std::cout<<"*************************** Preliminary Pose Calculation ***************************"<<std::endl;
238  std::cout<<"****************************** Click on the 4 dots *******************************"<<std::endl;
239  std::cout<<"********Dot1 : (-x,-y,0), Dot2 : (x,-y,0), Dot3 : (x,y,0), Dot4 : (-x,y,0)**********"<<std::endl;
240  std::cout<<"************************************************************************************"<<std::endl;
241 
242  try{
243  vpImagePoint ip[4];
244  if (! opt_click_allowed) {
245  ip[0].set_i( 265 );
246  ip[0].set_j( 93 );
247  ip[1].set_i( 248 );
248  ip[1].set_j( 242 );
249  ip[2].set_i( 166 );
250  ip[2].set_j( 215 );
251  ip[3].set_i( 178 );
252  ip[3].set_j( 85 );
253  }
254 
255  for(unsigned int i=0;i<4;i++) {
256  // by using setGraphics, we request to see the edges of the dot
257  // in red on the screen.
258  // It uses the overlay image plane.
259  // The default of this setting is that it is time consumming
260 
261  md[i].setGraphics(true) ;
262  md[i].setGrayLevelPrecision(0.7);
263  md[i].setSizePrecision(0.5);
264 
265  for(unsigned int j = 0;j<i;j++)
266  md[j].display(I) ;
267 
268  // flush the display buffer
269  vpDisplay::flush(I);
270  try{
271  if (opt_click_allowed) {
272  md[i].initTracking(I);
273  //std::cout << "click " << i << " " << md[i] << std::endl;
274  }
275  else {
276  md[i].initTracking(I, ip[i]);
277  }
278  }
279  catch(...){
280  }
281 
282  mcog[i] = md[i].getCog();
283  // an expcetion is thrown by the track method if
284  // - dot is lost
285  // - the number of pixel is too small
286  // - too many pixels are detected (this is usual when a "big" specularity
287  // occurs. The threshold can be modified using the
288  // setNbMaxPoint(int) method
289  if (opt_display) {
290  md[i].display(I) ;
291  // flush the display buffer
292  vpDisplay::flush(I) ;
293  }
294  }
295  }
296  catch(vpException e){
297  vpERROR_TRACE("Error while tracking dots") ;
298  vpCTRACE << e;
299  return;
300  }
301 
302  if (opt_display)
303  {
304  // display a red cross (size 10) in the image at the dot center
305  // of gravity location
306  //
307  // WARNING
308  // in the vpDisplay class member's when pixel coordinates
309  // are considered the first element is the row index and the second
310  // is the column index:
311  // vpDisplay::displayCross(Image, row index, column index, size, color)
312  // therefore u and v are inverted wrt to the vpDot specification
313  // Alternatively, to avoid this problem another set of member have
314  // been defined in the vpDisplay class.
315  // If the method name is postfixe with _uv the specification is :
316  // vpDisplay::displayCross_uv(Image, column index, row index, size, color)
317 
318  for (unsigned int i=0 ; i < 4 ; i++)
319  vpDisplay::displayCross(I, mcog[i], 10, vpColor::red) ;
320 
321  // flush the X11 buffer
322  vpDisplay::flush(I) ;
323  }
324 
325  // --------------------------------------------------------
326  // Now we will compute the pose
327  // --------------------------------------------------------
328 
329  // the list of point is cleared (if that's not done before)
330  mPose->clearPoint() ;
331 
332  // we set the 3D points coordinates (in meter !) in the object/world frame
333  double l=0.06 ;
334  double L=0.07 ;
335  mP[0].setWorldCoordinates(-L,-l, 0 ) ; // (X,Y,Z)
336  mP[1].setWorldCoordinates(L,-l, 0 ) ;
337  mP[2].setWorldCoordinates(L,l, 0 ) ;
338  mP[3].setWorldCoordinates(-L,l, 0 ) ;
339 
340  // pixel-> meter conversion
341  for (unsigned int i=0 ; i < 4 ; i++)
342  {
343  // u[i]. v[i] are expressed in pixel
344  // conversion in meter is achieved using
345  // x = (u-u0)/px
346  // y = (v-v0)/py
347  // where px, py, u0, v0 are the intrinsic camera parameters
348  double x=0, y=0;
349  vpPixelMeterConversion::convertPoint(*mcam, mcog[i], x,y) ;
350  mP[i].set_x(x) ;
351  mP[i].set_y(y) ;
352  }
353 
354 
355  // The pose structure is build, we put in the point list the set of point
356  // here both 2D and 3D world coordinates are known
357  for (unsigned int i=0 ; i < 4 ; i++)
358  {
359  mPose->addPoint(mP[i]) ; // and added to the pose computation point list
360  }
361 
362  // compute the initial pose using Dementhon method followed by a non linear
363  // minimisation method
364 
365  // Pose by Lagrange it provides an initialization of the pose
366  mPose->computePose(vpPose::LAGRANGE, *cmo) ;
367  // the pose is now refined using the virtual visual servoing approach
368  // Warning: cMo needs to be initialized otherwise it may diverge
369  mPose->computePose(vpPose::VIRTUAL_VS, *cmo) ;
370 
371  // Display breifly just to have a glimpse a the ViSP pose
372  // while(cpt<500){
373  if( opt_display ){
374  // Display the computed pose
375  mPose->display(I,*cmo,*mcam, 0.05, vpColor::red) ;
376  vpDisplay::flush(I) ;
377  vpTime::wait(1000);
378  }
379 }
380 
381 
382 int main(int argc, const char **argv)
383 {
384  std::string env_ipath;
385  std::string opt_ipath;
386  std::string ipath;
387  std::string opt_ppath;
388  std::string dirname;
389  std::string filename;
390  bool opt_click_allowed = true;
391 
392  // Get the VISP_IMAGE_PATH environment variable value
393  char *ptenv = getenv("VISP_INPUT_IMAGE_PATH");
394  if (ptenv != NULL)
395  env_ipath = ptenv;
396 
397  // Set the default input path
398  if (! env_ipath.empty())
399  ipath = env_ipath;
400 
401 
402  // Read the command line options
403  if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed) == false) {
404  exit (-1);
405  }
406 
407  // Get the option values
408  if (!opt_ipath.empty())
409  ipath = opt_ipath;
410 
411  // Compare ipath and env_ipath. If they differ, we take into account
412  // the input path comming from the command line option
413  if (!opt_ipath.empty() && !env_ipath.empty() && opt_ppath.empty()) {
414  if (ipath != env_ipath) {
415  std::cout << std::endl
416  << "WARNING: " << std::endl;
417  std::cout << " Since -i <visp image path=" << ipath << "> "
418  << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
419  << " we skip the environment variable." << std::endl;
420  }
421  }
422 
423  // Test if an input path is set
424  if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty() ){
425  usage(argv[0], NULL, ipath, opt_ppath);
426  std::cerr << std::endl
427  << "ERROR:" << std::endl;
428  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
429  << std::endl
430  << " environment variable to specify the location of the " << std::endl
431  << " image path where test images are located." << std::endl
432  << " Use -p <personal image path> option if you want to "<<std::endl
433  << " use personal images." << std::endl
434  << std::endl;
435 
436  exit(-1);
437  }
438 
439  // Declare an image, this is a gray level image (unsigned char)
440  // it size is not defined yet, it will be defined when the image will
441  // read on the disk
442  // vpImage<unsigned char> I ;
443 
444  // unsigned iter = 0;
445  std::ostringstream s;
446  // char cfilename[FILENAME_MAX];
447 
448  if (opt_ppath.empty()){
449  // Set the path location of the image sequence
450  dirname = ipath + vpIoTools::path("/ViSP-images/mire-2/");
451 
452  // Build the name of the image file
453 
454  s.setf(std::ios::right, std::ios::adjustfield);
455  s << "image.%04d.pgm";
456  filename = dirname + s.str();
457  }
458  else {
459  filename = opt_ppath;
460  }
461 
462  //We will read a sequence of images
463  vpVideoReader grabber;
464  grabber.setFirstFrameIndex(1);
465  grabber.setFileName(filename.c_str());
466  // Grey level image associated to a display in the initial pose computation
467  vpImage<unsigned char> Idisplay;
468  // Grey level image to track points
470  // RGBa image to get background
471  vpImage<vpRGBa> IC;
472  // Matrix representing camera parameters
474 
475  // Variables used for pose computation purposes
476  vpPose mPose;
477  vpDot2 md[4];
478  vpImagePoint mcog[4];
479  vpPoint mP[4];
480 
481  // CameraParameters we got from calibration
482  // Keep u0 and v0 as center of the screen
483  vpCameraParameters mcam;
484 
485  // Read the PGM image named "filename" on the disk, and put the
486  // bitmap into the image structure I. I is initialized to the
487  // correct size
488  //
489  // exception readPGM may throw various exception if, for example,
490  // the file does not exist, or if the memory cannot be allocated
491  try{
492  vpCTRACE << "Load: " << filename << std::endl;
493  grabber.open(Idisplay);
494  grabber.acquire(Idisplay);
495  vpCameraParameters mcamTmp(592,570,grabber.getWidth()/2,grabber.getHeight()/2);
496  // Compute the initial pose of the camera
497  computeInitialPose(&mcamTmp, Idisplay, &mPose, md, mcog, &cmo, mP,
498  opt_click_allowed);
499  // Close the framegrabber
500  grabber.close();
501 
502  // Associate the grabber to the RGBa image
503  grabber.open(IC);
504  mcam.init(mcamTmp);
505  }
506  catch(...)
507  {
508  // an exception is thrown if an exception from readPGM has been caught
509  // here this will result in the end of the program
510  // Note that another error message has been printed from readPGM
511  // to give more information about the error
512  std::cerr << std::endl
513  << "ERROR:" << std::endl;
514  std::cerr << " Cannot read " << filename << std::endl;
515  std::cerr << " Check your -i " << ipath << " option " << std::endl
516  << " or VISP_INPUT_IMAGE_PATH environment variable."
517  << std::endl;
518  exit(-1);
519  }
520 
521  // Create a vpRAOgre object with color background
522  vpAROgre ogre(mcam, (unsigned int)grabber.getWidth(), (unsigned int)grabber.getHeight());
523  // Initialize it
524  ogre.init(IC);
525  ogre.load("Robot", "robot.mesh");
526  ogre.setScale("Robot", 0.001f,0.001f,0.001f);
527  ogre.setRotation("Robot", vpRotationMatrix(vpRxyzVector(M_PI/2, -M_PI/2, 0)));
528 
529  try
530  {
531  // Rendering loop
532  while(ogre.continueRendering()){
533  // Acquire a frame
534  grabber.acquire(IC);
535 
536  // Convert it to a grey level image for tracking purpose
538 
539  // Update pose calculation
540  try{
541  // kill the point list
542  mPose.clearPoint() ;
543 
544  // track the dot
545  for (int i=0 ; i < 4 ; i++)
546  {
547  // track the point
548  md[i].track(I, mcog[i]) ;
549  md[i].setGrayLevelPrecision(0.90);
550  // pixel->meter conversion
551  {
552  double x=0, y=0;
553  vpPixelMeterConversion::convertPoint(mcam, mcog[i], x, y) ;
554  mP[i].set_x(x) ;
555  mP[i].set_y(y) ;
556  }
557 
558  // and added to the pose computation point list
559  mPose.addPoint(mP[i]) ;
560  }
561  // the pose structure has been updated
562 
563  // the pose is now updated using the virtual visual servoing approach
564  // Dementhon or lagrange is no longuer necessary, pose at the
565  // previous iteration is sufficient
566  mPose.computePose(vpPose::VIRTUAL_VS, cmo);
567  }
568  catch(...){
569  vpERROR_TRACE("Error in tracking loop") ;
570  return false;
571  }
572 
573  // Display with ogre
574  ogre.display(IC,cmo);
575 
576  // Wait so that the video does not go too fast
577  vpTime::wait(15);
578  }
579  // Close the grabber
580  grabber.close();
581  }
582  catch (Ogre::Exception& e)
583  {
584  std::cerr << "Exception:\n";
585  std::cerr << e.getFullDescription().c_str() << "\n";
586  return 1;
587  }
588  catch (...)
589  {
590  std::cerr << "Exception: " << "\n";
591  return 1;
592  }
593 
594  return EXIT_SUCCESS;
595 }
596 #else // VISP_HAVE_OGRE && VISP_HAVE_DISPLAY
597 int
598 main()
599 {
600  std::cout << "You should install Ogre3D to run this example..." << std::endl;
601 }
602 #endif