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