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