ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
calibrate2dGrid.cpp
1 /****************************************************************************
2  *
3  * $Id: calibrate2dGrid.cpp 4056 2013-01-05 13:04:42Z 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  * Camera calibration with any calibration grid.
36  *
37  * Authors:
38  * Anthony Saunier
39  *
40  *****************************************************************************/
41 
42 
63 #include <visp/vpDebug.h>
64 #include <visp/vpParseArgv.h>
65 #include <visp/vpIoTools.h>
66 
67 #include <stdio.h>
68 #include <stdlib.h>
69 #include <sstream>
70 #include <iomanip>
71 
72 #include <visp/vpImage.h>
73 #include <visp/vpImageIo.h>
74 #include <visp/vpCalibration.h>
75 #include <visp/vpDisplayX.h>
76 #include <visp/vpDisplayGDI.h>
77 #include <visp/vpDisplayGTK.h>
78 #include <visp/vpDisplayD3D.h>
79 #include <visp/vpMouseButton.h>
80 #include <visp/vpXmlParserCamera.h>
81 
82 #include <visp/vpPose.h>
83 #include <visp/vpDot.h>
84 #include <visp/vpDot2.h>
85 #include <visp/vpPixelMeterConversion.h>
86 #include <visp/vpMeterPixelConversion.h>
87 
88 #ifdef VISP_HAVE_OPENCV
89 # include <visp/vpOpenCVGrabber.h>
90 #elif defined(VISP_HAVE_V4L2)
91 # include <visp/vpV4l2Grabber.h>
92 #elif defined(VISP_HAVE_DIRECTSHOW)
93 # include <visp/vpDirectShowGrabber.h>
94 #elif defined(VISP_HAVE_DC1394_2)
95 # include <visp/vp1394TwoGrabber.h>
96 #endif
97 
98 // List of allowed command line options
99 #define GETOPTARGS "di:p:hf:g:n:s:l:cv:"
100 
116 void usage(const char *name,const char *badparam, std::string ipath, std::string ppath,
117  double gray, unsigned first, unsigned nimages, unsigned step, double lambda)
118 {
119  fprintf(stdout, "\n\
120  Read images of a calibration grid from the disk and \n\
121  calibrate the camera used for grabbing it.\n\
122  Each image corresponds to a PGM file.\n\
123  The calibration grid used here is available in : \n\
124  ViSP-images/calibration/grid2d.{fig,pdf} or \n\
125  ./example/calibration/grid2d.fig\n\
126  This is a 6*6 dots calibration grid where dots centers \n\
127  are spaced by 0.03 meter. You can obviously use another \n\
128  calibration grid changing its parameters in the program.\n\
129  Then you have to grab some images of this grid (you can use \n\
130  grab examples of ViSP to do it), save them as PGM files and\n\
131  precise their names with the -p option.\n\
132 \n\
133 SYNOPSIS\n\
134  %s [-i <test image path>] [-p <personal image path>]\n\
135  [-g <gray level precision>] [-f <first image>] \n\
136  [-n <number of images>] [-s <step>] [-l lambda] \n\
137  [-c] [-d] [-h]\n\
138  ", name);
139 
140  fprintf(stdout, "\n\
141  OPTIONS: Default\n\
142  -i <test image path> %s\n\
143  Set image input path.\n\
144  From this path read \"ViSP-images/calibration/grid36-%%02d.pgm\"\n\
145  images and the calibration grid data. \n\
146  These images come from ViSP-images-x.y.z.tar.gz\n\
147  available on the ViSP website.\n\
148  Setting the VISP_INPUT_IMAGE_PATH environment\n\
149  variable produces the same behaviour than using\n\
150  this option.\n\
151  \n\
152  -p <personal image path> %s\n\
153  Specify a personal sequence containing images \n\
154  to process.\n\
155  By image sequence, we mean one file per image.\n\
156  The following image file formats PNM (PGM P5, PPM P6)\n\
157  are supported. The format is selected by analysing \n\
158  the filename extension.\n\
159  Example : \"/Temp/ViSP-images/calibration/grid36-%%02d.pgm\"\n\
160  %%02d is for the image numbering.\n\
161  \n\
162  -g <gray level precision> %f\n\
163  Specify a gray level precision to detect dots.\n\
164  A number between 0 and 1.\n\
165  precision of the gray level of the dot. \n\
166  It is a double precision float witch \n\
167  value is in ]0,1]. 1 means full precision, \n\
168  whereas values close to 0 show a very bad \n\
169  precision.\n\
170  \n\
171  -f <first image> %u\n\
172  First image number of the sequence.\n\
173  \n\
174  -n <number of images> %u\n\
175  Number of images used to compute calibration.\n\
176  \n\
177  -s <step> %u\n\
178  Step between two images.\n\
179  \n\
180  -l <lambda> %f\n\
181  Gain of the virtual visual servoing.\n\
182  \n\
183  -d \n\
184  Disable the image display. This can be useful \n\
185  for automatic tests using crontab under Unix or \n\
186  using the task manager under Windows.\n\
187  \n\
188  -v <generic image name> \n\
189  Record a serie of images using a webcam. A framegrabber (either \n\
190  vpOpenCVGrabber, vpDirectShowGrabber, vp1394TwoGrabber or vpV4l2Grabber) is\n\
191  required. The images are recorded in the disk using the generic name in \n\
192  parameter (for example \"/tmp/img-%%03d.pgm\").\n\
193  \n\
194  -c\n\
195  Disable the mouse click.\n\
196  If the image display is disabled (using -d)\n\
197  this option is without effect.\n\
198 \n\
199  -h\n\
200  Print the help.\n\n",
201  ipath.c_str(),ppath.c_str(), gray ,first, nimages, step,lambda);
202 
203  if (badparam)
204  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
205 }
235 bool getOptions(int argc,const char **argv, std::string &ipath, std::string &ppath,
236  double &gray, unsigned &first, unsigned &nimages, unsigned &step,
237  double &lambda, bool &display, bool &click, bool& opt_video, std::string& opt_video_image_path)
238 {
239  const char *optarg;
240  int c;
241  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
242 
243  switch (c) {
244  case 'd': display = false; break;
245  case 'i': ipath = optarg; break;
246  case 'p': ppath = optarg; break;
247  case 'g': gray = atof(optarg);break;
248  case 'f': first = (unsigned) atoi(optarg); break;
249  case 'n': nimages = (unsigned) atoi(optarg); break;
250  case 's': step = (unsigned) atoi(optarg); break;
251  case 'l': lambda = atof(optarg); break;
252  case 'c': click = false; break;
253  case 'v': opt_video = true; opt_video_image_path = optarg; break;
254  case 'h': usage(argv[0], NULL, ipath, ppath,gray, first, nimages, step, lambda);
255  return false; break;
256  default:
257  usage(argv[0], optarg, ipath, ppath, gray,first, nimages, step, lambda);
258  return false; break;
259  }
260  }
261 
262  if ((c == 1) || (c == -1)) {
263  // standalone param or error
264  usage(argv[0], NULL, ipath, ppath,gray, first, nimages, step, lambda);
265  std::cerr << "ERROR: " << std::endl;
266  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
267  return false;
268  }
269 
270  return true;
271 }
272 
273 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9))
274 
275 #if defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DIRECTSHOW) || defined(VISP_HAVE_DC1394_2)
276 
287 unsigned int recordImageSequence(const std::string& out_path, const unsigned int opt_step, const unsigned int first_image);
288 #endif
289 
290 int main(int argc, const char ** argv)
291 {
293  //---------PARAMETERS--------------------
294 
295  // set the camera intrinsic parameters
296  // see more details about the model in vpCameraParameters
297  double px = 600 ;
298  double py = 600 ;
299  double u0 = 0;
300  double v0 = 0;
301  vpCameraParameters cam(px,py,u0,v0) ;
302  //set tracking dots parameters
303  double sizePrecision = 0.5 ;
304  //Calibration grid parameters////
305  double Lx = 0.03; //distance between points along x axis
306  double Ly = 0.03; //distance between points along y axis
307  unsigned int sizeX = 6; //size of the calibration grid along x axis
308  unsigned int sizeY = 6; //size of the calibration grid along y axis
309  unsigned int nbpt = sizeX*sizeY; //number of points in the calibration grid
310  //set the 3D coordinates of points used to compute the initial pose
311  const unsigned int nptPose = 4; //number of init dots by image
312  vpPoint P[nptPose];
313  //plan xOy
314  P[0].setWorldCoordinates(Lx,Ly, 0 ) ;
315  P[1].setWorldCoordinates(Lx,4*Ly, 0 ) ;
316  P[2].setWorldCoordinates(3*Lx,4*Ly, 0 ) ;
317  P[3].setWorldCoordinates(4*Lx,Ly, 0 ) ;
318 
319  // Calibration grid data
320  std::list<double> LoX,LoY,LoZ; //3D coordinates of the calibration dots
321  // char gridname[FILENAME_MAX] = "./grid2d.dat";
322  // if(vpCalibration::readGrid(gridname,nbpt,LoX,LoY,LoZ)!=0){
323  // std::cout << "Can't read : " << gridname << std::endl;
324  // std::cout << "Calibration grid to use : " ;
325  // std::cin >> gridname ;
326  // if(vpCalibration::readGrid(gridname,nbpt,LoX,LoY,LoZ)!=0){
327  // vpCERROR << "Can't read " << gridname << std::endl;
328  // exit(-1);
329  // }
330  // }
331 
332  for (unsigned int i=0 ; i < sizeX ; i++){
333  for(unsigned int j=0 ; j < sizeY ; j++){
334  LoX.push_back(i*Lx) ;
335  LoY.push_back(j*Ly) ;
336  LoZ.push_back(0) ;
337  }
338  }
339 
340  //---------------------------------------------------
342  std::string env_ipath;
343  std::string opt_ipath;
344  std::string ipath;
345  std::string opt_ppath;
346  std::string dirname;
347  std::string filename;
348  std::string filename_out;
349  char comment[FILENAME_MAX];
350  double opt_gray = 0.7;
351  unsigned opt_first = 1;
352  unsigned opt_nimages = 4;
353  unsigned opt_step = 1;
354  double opt_lambda = 0.5;
355  bool opt_display = true;
356  bool opt_click = true;
357  bool save = false;
358  bool opt_video = false;
359  std::string opt_video_image_path;
360 
361  double dotSize;
362  // Get the VISP_IMAGE_PATH environment variable value
363  char *ptenv = getenv("VISP_INPUT_IMAGE_PATH");
364  if (ptenv != NULL)
365  env_ipath = ptenv;
366 
367  // Set the default input path
368  if (! env_ipath.empty())
369  ipath = env_ipath;
370 
371  // Read the command line options
372  if (getOptions(argc, argv, opt_ipath, opt_ppath,opt_gray,opt_first, opt_nimages,
373  opt_step, opt_lambda, opt_display, opt_click, opt_video, opt_video_image_path) == false) {
374  return (-1);
375  }
376 
377  if(opt_video){
378 #if (defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DIRECTSHOW) || defined(VISP_HAVE_DC1394_2))
379  if(!opt_display){
380  std::cerr << std::endl
381  << "ERROR:" << std::endl;
382  std::cerr << "Incompatible options -v and -d." << std::endl;
383  return -1;
384  }
385  if(!opt_click){
386  std::cerr << std::endl
387  << "ERROR:" << std::endl;
388  std::cerr << "Incompatible options -v and -c." << std::endl;
389  return -1;
390  }
391  if(!opt_ipath.empty()){
392  std::cerr << std::endl
393  << "ERROR:" << std::endl;
394  std::cerr << "Incompatible options -v and -i." << std::endl;
395  return -1;
396  }
397  if(!opt_ppath.empty()){
398  std::cerr << std::endl
399  << "ERROR:" << std::endl;
400  std::cerr << "Incompatible options -v and -p." << std::endl;
401  return -1;
402  }
403  if(opt_video_image_path.empty()){
404  std::cerr << std::endl
405  << "ERROR:" << std::endl;
406  std::cerr << "output image path empty." << std::endl;
407  return -1;
408  }
409  try{
410  opt_nimages = recordImageSequence(opt_video_image_path, opt_step, opt_first);
411  }
412  catch(...){
413  // no need to write the problem as it has already been writen.
414  return -1;
415  }
416  opt_ipath = opt_video_image_path;
417  opt_ppath = opt_video_image_path;
418 #else
419  {
420  std::cerr << std::endl
421  << "ERROR:" << std::endl;
422  std::cerr << "No framegrabber installed with ViSP. Cannot record images from video stream." << std::endl;
423  return -1;
424  }
425 #endif
426  }
427 
428  if (!opt_display)
429  opt_click = false; // turn off the waiting
430 
431  // Get the option values
432  if (!opt_ipath.empty())
433  ipath = opt_ipath;
434 
435  // Compare ipath and env_ipath. If they differ, we take into account
436  // the input path comming from the command line option
437  if (opt_ipath.empty() && opt_ppath.empty()) {
438  if (ipath != env_ipath) {
439  std::cout << std::endl
440  << "WARNING: " << std::endl;
441  std::cout << " Since -i <visp image path=" << ipath << "> "
442  << " is different from VISP_INPUT_IMAGE_PATH=" << env_ipath << std::endl
443  << " we skip the environment variable." << std::endl;
444  }
445  }
446 
447  // Test if an input path is set
448  if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty() ){
449  usage(argv[0], NULL, ipath, opt_ppath, opt_gray, opt_first, opt_nimages,
450  opt_step, opt_lambda);
451  std::cerr << std::endl
452  << "ERROR:" << std::endl;
453  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
454  << std::endl
455  << " environment variable to specify the location of the " << std::endl
456  << " image path where test images are located." << std::endl
457  << " Use -p <personal image path> option if you want to "<<std::endl
458  << " use personal images." << std::endl
459  << std::endl;
460 
461  return(-1);
462  }
463 
464  // Declare an image, this is a gray level image (unsigned char)
465  // it size is not defined yet, it will be defined when the image will
466  // read on the disk
468 
469  unsigned iter = opt_first;
470  std::ostringstream s;
471  char cfilename[FILENAME_MAX];
472 
473  if (opt_ppath.empty()){
474 
475 
476  // Warning :
477  // the image sequence is not provided with the ViSP package
478  // therefore the program will return you an error :
479  // !! vpImageIoPnm.cpp: readPGM(#210) :couldn't read file
480  // ViSP-images/calibration/grid36-01.pgm
481  // !! vpDotExample.cpp: main(#95) :Error while reading the image
482  // terminate called after throwing an instance of 'vpImageException'
483  //
484  // The sequence is available on the visp www site
485  // http://www.irisa.fr/lagadic/visp/visp.html
486  // in the download section. It is named "ViSP-images.tar.gz"
487 
488  // Set the path location of the image sequence
489  dirname = ipath + vpIoTools::path("/ViSP-images/calibration/");
490 
491  // Build the name of the image file
492 
493  s.setf(std::ios::right, std::ios::adjustfield);
494  s << "grid36-" << std::setw(2) << std::setfill('0') << iter << ".pgm";
495  filename = dirname + s.str();
496  }
497  else {
498 
499  sprintf(cfilename,opt_ppath.c_str(), iter) ;
500  filename = cfilename;
501  }
502  // Read the PGM image named "filename" on the disk, and put the
503  // bitmap into the image structure I. I is initialized to the
504  // correct size
505  //
506  // exception readPGM may throw various exception if, for example,
507  // the file does not exist, or if the memory cannot be allocated
508  try{
509  vpImageIo::readPGM(I, filename) ;
510  }
511  catch(...)
512  {
513  // an exception is throwned if an exception from readPGM has been catched
514  // here this will result in the end of the program
515  // Note that another error message has been printed from readPGM
516  // to give more information about the error
517  std::cerr << std::endl
518  << "ERROR:" << std::endl;
519  std::cerr << " Cannot read " << filename << std::endl;
520  std::cerr << " Check your -i " << ipath << " option, " << std::endl
521  << " or your -p " << opt_ppath << " option " <<std::endl
522  << " or VISP_INPUT_IMAGE_PATH environment variable"
523  << std::endl;
524  return(-1);
525  }
526 
527 
528  // We determine and store the calibration parameters for each image.
529  vpCalibration* table_cal;
530  table_cal = new vpCalibration[opt_nimages];
531  unsigned int niter = 0;
532  char title[100];
533 
534 
535 
536 #if defined VISP_HAVE_GDI
537  vpDisplayGDI display;
538 #elif defined VISP_HAVE_GTK
539  vpDisplayGTK display;
540 #elif defined VISP_HAVE_X11
541  vpDisplayX display;
542 #elif defined VISP_HAVE_D3D9
543  vpDisplayD3D display;
544 #endif
545 
546  if (opt_display) {
547  // Display size is automatically defined by the image (I) size
548  sprintf(title, "Calibration initialization on image %s", (s.str()).c_str());
549  display.init(I, 100, 100, title) ;
550  }
551 
552  while (iter < opt_first + opt_nimages*opt_step) {
553  try {
554  // set the new image name
555 
556  if (opt_ppath.empty()){
557  s.str("");
558  s << "grid36-" << std::setw(2) << std::setfill('0') << iter<< ".pgm";
559  filename = dirname + s.str();
560  }
561  else {
562  sprintf(cfilename, opt_ppath.c_str(), iter) ;
563  filename = cfilename;
564  }
565  filename_out = filename + ".txt";
566 
567  std::cout << "read : " << filename << std::endl;
568  // read the image
569  vpImageIo::readPGM(I, filename);
570 
571  double px = cam.get_px();
572  double py = cam.get_px();
573  double u0 = I.getWidth()/2;
574  double v0 = I.getHeight()/2;
575  cam.initPersProjWithoutDistortion(px, py, u0, v0);
576 
577 
578  if (opt_display) {
579  // Display the image
580 
581  try{
582  sprintf(title, "Calibration initialization on image %s", (s.str()).c_str());
583  vpDisplay::setTitle(I,title);
584  // Display the image
585  // The image class has a member that specify a pointer toward
586  // the display that has been initialized in the display declaration
587  // therefore is is no longuer necessary to make a reference to the
588  // display variable.
589  vpDisplay::display(I) ;
590  vpDisplay::flush(I) ;
591  }
592  catch(...){
593  vpERROR_TRACE("Error while displaying the image") ;
594  delete [] table_cal;
595  return(-1);
596  }
597  }
598 
599 
600  // here we track dots on the calibration grid
601  vpDot2 d[nptPose] ;
602  vpImagePoint ip_click[nptPose];
603 
604  try{
605  for(unsigned int i=0;i<nptPose;i++) {
606  // by using setGraphics, we request to see the edges of the dot
607  // in red on the screen.
608  // It uses the overlay image plane.
609  // The default of this setting is that it is time consumming
610 
611  d[i].setGraphics(true) ;
612  d[i].setGrayLevelPrecision(opt_gray);
613  d[i].setSizePrecision(sizePrecision);
614 
615  // tracking is initalized
616  // if no other parameters are given to the iniTracking(..) method
617  // a right mouse click on the dot is expected
618  // dot location can also be specified explicitely in the initTracking
619  // method : d.initTracking(I,u,v) where u is the column index and v is
620  // the row index
621  if (opt_click) {
623  std::printf("click in the dot %d of coordinates\nx=%f y=%f z=%f \n",
624  i+1 ,P[i].get_oX(),P[i].get_oY(),P[i].get_oZ());
625  std::sprintf(comment,"Click in the dot %d",i+1 );
626  vpImagePoint ip;
627  ip.set_i( 15 );
628  ip.set_j( 10 );
629 
630  vpDisplay::displayCharString(I, ip, &comment[0], vpColor::blue);
631  for(unsigned int j = 0;j<i;j++)
632  d[j].display(I) ;
633  // flush the display buffer
634  vpDisplay::flush(I);
635  try{
636  d[i].initTracking(I) ;
637  }
638  catch(...){
639  }
640  }
641  else{
642  d[i].initTracking(I, ip_click[i]);
643  }
644  // an expcetion is thrown by the track method if
645  // - dot is lost
646  // - the number of pixel is too small
647  // - too many pixels are detected (this is usual when a "big" specularity
648  // occurs. The threshold can be modified using the
649  // setNbMaxPoint(int) method
650  if (opt_display) {
651  d[i].display(I) ;
652  // flush the display buffer
653  vpDisplay::flush(I) ;
654  }
655  }
656  }
657  catch(vpException e){
658  vpERROR_TRACE("Error while tracking dots") ;
659  vpCTRACE << e;
660  delete [] table_cal;
661  return(-1);
662  }
663 
664  // --------------------------------------------------------
665  // Now will compute the pose
666  //
667  // The pose will be contained in an homogeneous matrix cMo
668  vpHomogeneousMatrix cMo ;
669 
670  // We need a structure that content both the 3D coordinates of the point
671  // in the object frame and the 2D coordinates of the point expressed in meter
672  // the vpPoint class is ok for that
673 
674  //The vpCalibration class mainly contents a list of points (X,Y,Z,u,v)
675  vpCalibration calib;
676  calib.clearPoint();
677 
678  // The vpPose class mainly contents a list of vpPoint (that is (X,Y,Z, x, y) )
679  vpPose pose ;
680  // the list of point is cleared (if that's not done before)
681  pose.clearPoint() ;
682  // we set the 3D points coordinates (in meter !) in the object/world frame
683 
684 
685  // pixel-> meter conversion
686  for (unsigned int i=0 ; i < nptPose ; i++){
687  // conversion in meter is achieved using
688  // x = (u-u0)/px
689  // y = (v-v0)/py
690  // where px, py, u0, v0 are the intrinsic camera parameters
691  double x=0, y=0;
692  vpImagePoint cog = d[i].getCog();
693  vpPixelMeterConversion::convertPoint(cam, cog, x, y);
694  P[i].set_x(x) ;
695  P[i].set_y(y) ;
696  }
697 
698  // The pose structure is build, we put in the point list the set of point
699  // here both 2D and 3D world coordinates are known
700  for (unsigned int i=0 ; i < nptPose ; i++){
701  vpImagePoint cog = d[i].getCog();
702  pose.addPoint(P[i]) ; // and added to the pose computation point list
703 
704  //and added to the local calibration points list
705  calib.addPoint(P[i].get_oX(),P[i].get_oY(),P[i].get_oZ(), cog);
706 
707  }
708  // compute the initial pose using Lagrange method followed by a non linear
709  // minimisation method
710 
711  // Pose by Lagrange it provides an initialization of the pose
712  pose.computePose(vpPose::LAGRANGE, cMo) ;
713 
714  // the pose is now refined using the virtual visual servoing approach
715  // Warning: cMo needs to be initialized otherwise it may diverge
716  pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
717  //pose.display(I,cMo,cam, 0.05, vpColor::blue) ;
718  vpHomogeneousMatrix cMoTmp = cMo;
719  vpCameraParameters camTmp = cam;
720  //compute local calibration to match the calibration grid with the image
721  try{
722  calib.computeCalibration(vpCalibration::CALIB_VIRTUAL_VS,cMoTmp,camTmp,false);
723  }
724  catch(...){
725  if(opt_click){
726  vpImagePoint ip;
728  ip.set_i( 15 );
729  ip.set_j( 10 );
730  vpDisplay::displayCharString(I, ip, "Pose computation failed",
731  vpColor::red);
732  ip.set_i( 30 );
733  ip.set_j( 10 );
735  "A left click to define other dots.",
736  vpColor::blue);
737  ip.set_i( 45 );
738  ip.set_j( 10 );
740  "A middle click to don't care of this pose.",
741  vpColor::blue);
742  vpDisplay::flush(I) ;
743  std::cout << "\nPose computation failed." << std::endl;
744  std::cout << "A left click to define other dots." << std::endl;
745  std::cout << "A middle click to don't care of this pose." << std::endl;
747  vpDisplay::getClick(I, ip, button) ;
748  switch(button){
749  case 1 :
750  std::cout << "Left click has been pressed." << std::endl;
751  continue;
752  case 3 :
753  std::cout << "Right click has been pressed." << std::endl;
754  continue;
755  case 2 :
756  std::cout << "Middle click has been pressed." << std::endl;
757  iter += opt_step ;
758  niter++;
759  continue;
760  }
761  }
762  else{
763  iter += opt_step ;
764  niter++;
765  continue;
766  }
767  }
768  if (opt_display) {
769  // display the computed pose
770  vpDisplay::display(I) ;
771  for(unsigned int j = 0;j<nptPose;j++)
772  d[j].display(I) ;
773  pose.display(I,cMoTmp,camTmp, 0.05, vpColor::red) ;
774  vpDisplay::flush(I) ;
775  if(opt_click){
776  vpImagePoint ip;
777  ip.set_i( 15 );
778  ip.set_j( 10 );
780  "A left click to display grid.",
781  vpColor::blue);
782  ip.set_i( 30 );
783  ip.set_j( 10 );
785  "A right click to define other dots.",
786  vpColor::blue);
787  vpDisplay::flush(I) ;
788  std::cout << "\nA a left click to display grid." << std::endl;
789  std::cout << "A right click to define other dots." << std::endl;
791  vpDisplay::getClick(I, ip, button) ;
792  switch(button){
793  case 1 :
794  std::cout << "Left click has been pressed." << std::endl;
795  break;
796  case 2 :
797  std::cout << "Middle click has been pressed." << std::endl;
798  continue;
799  case 3 :
800  std::cout << "Right click has been pressed." << std::endl;
801  continue;
802  }
803  }
804  vpDisplay::display(I) ;
805  vpDisplay::flush(I) ;
806  }
807  dotSize = 0;
808  for(unsigned i =0 ; i<nptPose ;i++){
809  dotSize += d[i].getWidth()+d[i].getHeight();
810  }
811  dotSize /= nptPose;
812 
813  //now we detect all dots of the grid
814  vpDot2* md = new vpDot2[nbpt];
815  for(unsigned int i=0;i<nbpt;i++){
816 
817  // by using setGraphics, we request to see the contour of the dot
818  // in red on the screen.
819  md[i].setGraphics(false);
820  md[i].setSizePrecision(sizePrecision);
821  md[i].setGrayLevelPrecision(opt_gray);
822  }
823 
824  vpDisplay::display(I) ;
825 
826  // --------------------------------------------------------
827  // Now we will compute the calibration
828  //
829 
830  // We need a structure that content both the 3D coordinates of the point
831  // in the object frame and the 2D coordinates of the point expressed in meter
832  // the vpPoint class is ok for that
833  vpPoint* mP=new vpPoint[nbpt] ;
834 
835  // The vpPose class mainly contents a list of vpPoint (that is (X,Y,Z, x, y) )
836  // the list of point is cleared (if that's not done before)
837  table_cal[niter].clearPoint() ;
838 
839  // we set the 3D points coordinates (in meter !) in the object/world frame
840  //xOy plan
841  std::list<double>::const_iterator it_LoX = LoX.begin();
842  std::list<double>::const_iterator it_LoY = LoY.begin();
843  std::list<double>::const_iterator it_LoZ = LoZ.begin();
844 
845  for(unsigned int i = 0 ; i < nbpt ; i++){
846  mP[i].setWorldCoordinates(*it_LoX, *it_LoY, *it_LoZ) ; // (X,Y,Z)
847  ++it_LoX;
848  ++it_LoY;
849  ++it_LoZ;
850  }
851  // pixel-> meter conversion
852  vpImagePoint ip;
853  vpImagePoint cog;
854  bool* valid = new bool[nbpt];
855  for (unsigned int i=0 ; i < nbpt ; i++){
856  vpColVector _cP, _p ;
857  valid[i] = true;
858  mP[i].changeFrame(cMoTmp,_cP) ;
859  mP[i].projection(_cP,_p) ;
860  vpMeterPixelConversion::convertPoint(camTmp,_p[0],_p[1], ip);
861  if (10 < ip.get_u() && ip.get_u() < I.getWidth()-10 &&
862  10 < ip.get_v() && ip.get_v() < I.getHeight()-10) {
863  try {
864  md[i].initTracking(I, ip, (unsigned int)dotSize);
865  vpRect bbox = md[i].getBBox();
866  cog = md[i].getCog();
867  if(bbox.getLeft()<5 || bbox.getRight()>(double)I.getWidth()-5 ||
868  bbox.getTop()<5 || bbox.getBottom()>(double)I.getHeight()-5||
869  vpMath::abs(ip.get_u() - cog.get_u()) > 10 ||
870  vpMath::abs(ip.get_v() - cog.get_v()) > 10)
871  valid[i] = false;
872  // u[i]. v[i] are expressed in pixel
873  // conversion in meter
874  double x=0, y=0;
875  vpPixelMeterConversion::convertPoint(camTmp, cog, x, y) ;
876  mP[i].set_x(x) ;
877  mP[i].set_y(y) ;
878  if (opt_display) {
879  if(valid[i]){
880  md[i].display(I,vpColor::red, 2);
881  mP[i].display(I,cMoTmp,camTmp) ;
882  }
883  }
884  }
885  catch(...){
886  valid[i] = false;
887  }
888  }
889  else {valid[i] = false;}
890  }
891 
892  // The calibration structure is build, we put in the point list the set of point
893  // here both 2D and 3D world coordinates are known
894  // and added to the calibration computation point list.
895 
896 
897  //we put the pose matrix in the current calibration structure
898  // table_cal[niter].cMo = cMo ; //.setIdentity();//
899  if(save == true) {
900  table_cal[niter].writeData(filename_out.c_str());
901  }
902  if (opt_click) {
903  sprintf(title, "Extracted 2D data from image %s", (s.str()).c_str());
904  vpDisplay::setTitle(I, title);
905  vpImagePoint ip;
906  ip.set_i( 15 );
907  ip.set_j( 10 );
909  "A left click to validate this pose.",
910  vpColor::blue);
911  ip.set_i( 30 );
912  ip.set_j( 10 );
914  "A right click to retry.",
915  vpColor::blue);
916  ip.set_i( 45 );
917  ip.set_j( 10 );
919  "A middle click to don't care of this pose.",
920  vpColor::blue);
921  vpDisplay::flush(I) ;
922 
923  std::cout << "\nA left click to validate this pose." << std::endl;
924  std::cout << "A right click to retry." << std::endl;
925  std::cout << "A middle click to don't care of this pose." << std::endl;
927  vpDisplay::getClick(I, ip, button) ;
928  switch(button){
929  case 1 : //left
930  std::cout << "\nLeft click has been pressed." << std::endl;
931  break;
932  case 2 : //middle
933  std::cout << "Middle click has been pressed." << std::endl;
934  for (unsigned int i=0 ; i < nbpt ; i++)
935  valid[i]=false;
936  break;
937  case 3 : //right
938  std::cout << "Right click has been pressed." << std::endl;
939  continue;
940  }
941  }
942  //Add valid points in the calibration structure
943  for (unsigned int i=0 ; i < nbpt ; i++){
944  if(valid[i]){
945  vpImagePoint cog = md[i].getCog();
946 
947  table_cal[niter].addPoint(mP[i].get_oX(),mP[i].get_oY(),mP[i].get_oZ(), cog) ;
948  }
949  }
950 
951  //we free the memory
952  delete [] mP;
953  delete [] md;
954  delete [] valid;
955 
956  niter++ ;
957  }
958  catch(...) {
959  return(-1) ;
960  }
961  iter += opt_step ;
962  }
963  vpCalibration::setLambda(opt_lambda);
964 
965  // Calibrate by a non linear method based on virtual visual servoing
966  vpCameraParameters cam2;
967  int resultCalib = vpCalibration::computeCalibrationMulti(vpCalibration::CALIB_VIRTUAL_VS,opt_nimages,table_cal,cam2,false) ;
968  if(resultCalib == 0)
969  std::cout << cam2 << std::endl;
970  else
971  std::cout << "Calibration without distortion failed." << std::endl;
972 
973  int resultCalibDist = vpCalibration::computeCalibrationMulti(vpCalibration::CALIB_VIRTUAL_VS_DIST,opt_nimages,table_cal,cam,false) ;
974  if(resultCalibDist == 0)
975  std::cout << cam << std::endl;
976  else
977  std::cout << "Calibration with distortion failed." << std::endl;
978 
979 
980  // Compute Tsai calibration for extrinsic parameters estimation
981 
982  iter = opt_first;
983  niter = 0;
984  //Print calibration results for each image
985  while (iter < opt_first + opt_nimages*opt_step) {
986  try {
987  // set the new image name
988 
989  if (opt_ppath.empty()){
990  s.str("");
991  s << "grid36-" << std::setw(2) << std::setfill('0') << iter<< ".pgm";
992  filename = dirname + s.str();
993  }
994  else {
995  sprintf(cfilename, opt_ppath.c_str(), iter) ;
996  filename = cfilename;
997  }
998 
999  std::cout << "read : " << filename << std::endl;
1000  // read the image
1001  vpImageIo::readPGM(I, filename);
1002  if(table_cal[niter].get_npt()!=0){
1003  std::cout << "\nCompute standard deviation for pose " << niter <<std::endl;
1004  double deviation, deviation_dist ;
1005  table_cal[niter].computeStdDeviation(deviation,deviation_dist);
1006  std::cout << "deviation for model without distortion : "
1007  << deviation << std::endl;
1008  std::cout << "deviation for model with distortion : "
1009  << deviation_dist << std::endl;
1010  //Display results
1011  }
1012  else{
1013  std::cout << "This image has not been used!" << std::endl;
1014  }
1015 
1016  if (opt_display) {
1017  // Display the image
1018 
1019  try{
1020  // Display size is automatically defined by the image (I) size
1021  sprintf(title, "Calibration results for image %s", (s.str()).c_str());
1022  vpDisplay::setTitle(I,title);
1023  // Display the image
1024  // The image class has a member that specify a pointer toward
1025  // the display that has been initialized in the display declaration
1026  // therefore is is no longuer necessary to make a reference to the
1027  // display variable.
1028  vpDisplay::display(I) ;
1029  }
1030  catch(...){
1031  vpERROR_TRACE("Error while displaying the image") ;
1032  delete [] table_cal;
1033  return(-1);
1034  }
1035  //Display the data of the calibration (center of the dots)
1036  table_cal[niter].displayData(I, vpColor::red, 3) ;
1037  //Display grid : estimated center of dots using camera parameters
1038  table_cal[niter].displayGrid(I, vpColor::yellow, 3) ;
1039  vpDisplay::flush(I) ;
1040  if(opt_click){
1041  vpImagePoint ip;
1042  ip.set_i( 15 );
1043  ip.set_j( 10 );
1044  vpDisplay::displayCharString(I, ip, "A click to continue...",
1045  vpColor::blue);
1046  vpDisplay::flush(I) ;
1047  std::cout << "\nA click to continue..." << std::endl;
1048  vpDisplay::getClick(I) ;
1049  }
1050  }
1051  niter++;
1052  }
1053  catch(...) {
1054  delete [] table_cal;
1055  return(-1) ;
1056  }
1057  iter += opt_step ;
1058  }
1059 
1060 #ifdef VISP_HAVE_XML2
1061  vpXmlParserCamera xml;
1062 
1063  if(resultCalib == 0){
1064  int resultSaving = xml.save(cam2, "calibrate2dGrid.xml", "Camera", I.getWidth(), I.getHeight());
1065  if(resultSaving == vpXmlParserCamera::SEQUENCE_OK)
1066  std::cout << "Camera parameters without distortion successfully saved in calibrate2dGrid.xml" << std::endl;
1067  else
1068  std::cout << "Failed to save the camera parameters without distortion in calibrate2dGrid.xml" << std::endl;
1069  }
1070 
1071  if(resultCalibDist == 0){
1072  int resultSavingDist = xml.save(cam, "calibrate2dGrid.xml", "Camera", I.getWidth(), I.getHeight());
1073 
1074 
1075  if(resultSavingDist == vpXmlParserCamera::SEQUENCE_OK )
1076  std::cout << "Camera parameters with distortion successfully saved in calibrate2dGrid.xml" << std::endl;
1077  else
1078  std::cout << "Failed to save the camera parameters with distortion in calibrate2dGrid.xml" << std::endl;
1079  }
1080 
1082 #endif
1083 
1084  delete [] table_cal;
1085  return(0);
1086 }
1087 
1088 
1089 #if defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DIRECTSHOW) || defined(VISP_HAVE_DC1394_2)
1090 unsigned int recordImageSequence(const std::string& out_path, const unsigned int opt_step, const unsigned int first_image)
1091 {
1092  unsigned int nbImg = first_image;
1093  unsigned int index = 0;
1094 
1095 #ifdef VISP_HAVE_OPENCV
1096  vpOpenCVGrabber g;
1097 #elif defined(VISP_HAVE_V4L2)
1098  vpV4l2Grabber g;
1099 #elif defined(VISP_HAVE_DIRECTSHOW)
1101 #elif defined(VISP_HAVE_DC1394_2)
1102  vp1394TwoGrabber g;
1103 #endif
1104 
1105 
1107 
1108  g.open(I);
1109  g.acquire(I);
1110 
1111 #if defined VISP_HAVE_GDI
1112  vpDisplayGDI display;
1113 #elif defined VISP_HAVE_GTK
1114  vpDisplayGTK display;
1115 #elif defined VISP_HAVE_X11
1116  vpDisplayX display;
1117 #elif defined VISP_HAVE_D3D9
1118  vpDisplayD3D display;
1119 #endif
1120  display.init(I, 100, 100, "record sequence for the calibration.");
1121 
1122  bool isOver = false;
1123  std::cout << "Left click to record the current image." << std::endl;
1124  std::cout << "Right click to stop the acquisition." << std::endl;
1125 
1126  while(!isOver){
1127  g.acquire(I);
1128  vpDisplay::display(I);
1130  "Left click to record the current image.", vpColor::blue);
1132  "Right click to stop the acquisition.", vpColor::blue);
1133  vpDisplay::flush(I);
1134 
1135  vpImagePoint ip;
1137  if(vpDisplay::getClick(I, ip, button, false)){
1138  if(button == vpMouseButton::button1){
1139  char curImgName[FILENAME_MAX];
1140  sprintf(curImgName, out_path.c_str(), nbImg);
1141  nbImg += opt_step;
1142  index++;
1143  try{
1144  std::cout << "write image : " << curImgName << std::endl;
1145  vpImageIo::writePGM(I, curImgName);
1146  }
1147  catch(...){
1148  std::cerr << std::endl
1149  << "ERROR." << std::endl
1150  << "Cannot record the image : " << curImgName << std::endl
1151  << "Check the path and the permissions." << std::endl;
1152  throw vpException(vpException::ioError, "Cannot record image");
1153  }
1154  }
1155  else if(button == vpMouseButton::button3){
1156  isOver = true;
1157  }
1158  }
1159  }
1160 
1161  display.close(I);
1162 
1163  g.close();
1164 
1165  return index;
1166 }
1167 #endif
1168 
1169 #else // (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)...)
1170 
1171 int
1172 main()
1173 {
1174  vpTRACE("X11 or GTK or GDI or D3D functionnality is not available...") ;
1175 }
1176 #endif // (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)...)