63 #include <visp/vpDebug.h>
64 #include <visp/vpParseArgv.h>
65 #include <visp/vpIoTools.h>
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>
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>
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>
99 #define GETOPTARGS "di:p:hf:g:n:s:l:cv:"
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)
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\
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\
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\
152 -p <personal image path> %s\n\
153 Specify a personal sequence containing images \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\
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\
171 -f <first image> %u\n\
172 First image number of the sequence.\n\
174 -n <number of images> %u\n\
175 Number of images used to compute calibration.\n\
178 Step between two images.\n\
181 Gain of the virtual visual servoing.\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\
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\
195 Disable the mouse click.\n\
196 If the image display is disabled (using -d)\n\
197 this option is without effect.\n\
200 Print the help.\n\n",
201 ipath.c_str(),ppath.c_str(), gray ,first, nimages, step,lambda);
204 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
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)
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);
257 usage(argv[0], optarg, ipath, ppath, gray,first, nimages, step, lambda);
262 if ((c == 1) || (c == -1)) {
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;
273 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9))
275 #if defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DIRECTSHOW) || defined(VISP_HAVE_DC1394_2)
287 unsigned int recordImageSequence(
const std::string& out_path,
const unsigned int opt_step,
const unsigned int first_image);
290 int main(
int argc,
const char ** argv)
303 double sizePrecision = 0.5 ;
307 unsigned int sizeX = 6;
308 unsigned int sizeY = 6;
309 unsigned int nbpt = sizeX*sizeY;
311 const unsigned int nptPose = 4;
320 std::list<double> LoX,LoY,LoZ;
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) ;
342 std::string env_ipath;
343 std::string opt_ipath;
345 std::string opt_ppath;
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;
358 bool opt_video =
false;
359 std::string opt_video_image_path;
363 char *ptenv = getenv(
"VISP_INPUT_IMAGE_PATH");
368 if (! env_ipath.empty())
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) {
378 #if (defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DIRECTSHOW) || defined(VISP_HAVE_DC1394_2))
380 std::cerr << std::endl
381 <<
"ERROR:" << std::endl;
382 std::cerr <<
"Incompatible options -v and -d." << std::endl;
386 std::cerr << std::endl
387 <<
"ERROR:" << std::endl;
388 std::cerr <<
"Incompatible options -v and -c." << std::endl;
391 if(!opt_ipath.empty()){
392 std::cerr << std::endl
393 <<
"ERROR:" << std::endl;
394 std::cerr <<
"Incompatible options -v and -i." << std::endl;
397 if(!opt_ppath.empty()){
398 std::cerr << std::endl
399 <<
"ERROR:" << std::endl;
400 std::cerr <<
"Incompatible options -v and -p." << std::endl;
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;
410 opt_nimages = recordImageSequence(opt_video_image_path, opt_step, opt_first);
416 opt_ipath = opt_video_image_path;
417 opt_ppath = opt_video_image_path;
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;
432 if (!opt_ipath.empty())
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;
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 "
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
469 unsigned iter = opt_first;
470 std::ostringstream s;
471 char cfilename[FILENAME_MAX];
473 if (opt_ppath.empty()){
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();
499 sprintf(cfilename,opt_ppath.c_str(), iter) ;
500 filename = cfilename;
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"
531 unsigned int niter = 0;
536 #if defined VISP_HAVE_GDI
538 #elif defined VISP_HAVE_GTK
540 #elif defined VISP_HAVE_X11
542 #elif defined VISP_HAVE_D3D9
548 sprintf(title,
"Calibration initialization on image %s", (s.str()).c_str());
549 display.
init(I, 100, 100, title) ;
552 while (iter < opt_first + opt_nimages*opt_step) {
556 if (opt_ppath.empty()){
558 s <<
"grid36-" << std::setw(2) << std::setfill(
'0') << iter<<
".pgm";
559 filename = dirname + s.str();
562 sprintf(cfilename, opt_ppath.c_str(), iter) ;
563 filename = cfilename;
565 filename_out = filename +
".txt";
567 std::cout <<
"read : " << filename << std::endl;
582 sprintf(title,
"Calibration initialization on image %s", (s.str()).c_str());
605 for(
unsigned int i=0;i<nptPose;i++) {
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 );
631 for(
unsigned int j = 0;j<i;j++)
686 for (
unsigned int i=0 ; i < nptPose ; i++){
700 for (
unsigned int i=0 ; i < nptPose ; i++){
705 calib.
addPoint(P[i].get_oX(),P[i].get_oY(),P[i].get_oZ(), cog);
735 "A left click to define other dots.",
740 "A middle click to don't care of this pose.",
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;
750 std::cout <<
"Left click has been pressed." << std::endl;
753 std::cout <<
"Right click has been pressed." << std::endl;
756 std::cout <<
"Middle click has been pressed." << std::endl;
771 for(
unsigned int j = 0;j<nptPose;j++)
780 "A left click to display grid.",
785 "A right click to define other dots.",
788 std::cout <<
"\nA a left click to display grid." << std::endl;
789 std::cout <<
"A right click to define other dots." << std::endl;
794 std::cout <<
"Left click has been pressed." << std::endl;
797 std::cout <<
"Middle click has been pressed." << std::endl;
800 std::cout <<
"Right click has been pressed." << std::endl;
808 for(
unsigned i =0 ; i<nptPose ;i++){
815 for(
unsigned int i=0;i<nbpt;i++){
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();
845 for(
unsigned int i = 0 ; i < nbpt ; i++){
854 bool* valid =
new bool[nbpt];
855 for (
unsigned int i=0 ; i < nbpt ; i++){
881 mP[i].
display(I,cMoTmp,camTmp) ;
889 else {valid[i] =
false;}
900 table_cal[niter].
writeData(filename_out.c_str());
903 sprintf(title,
"Extracted 2D data from image %s", (s.str()).c_str());
909 "A left click to validate this pose.",
914 "A right click to retry.",
919 "A middle click to don't care of this pose.",
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;
930 std::cout <<
"\nLeft click has been pressed." << std::endl;
933 std::cout <<
"Middle click has been pressed." << std::endl;
934 for (
unsigned int i=0 ; i < nbpt ; i++)
938 std::cout <<
"Right click has been pressed." << std::endl;
943 for (
unsigned int i=0 ; i < nbpt ; i++){
947 table_cal[niter].
addPoint(mP[i].get_oX(),mP[i].get_oY(),mP[i].get_oZ(), cog) ;
969 std::cout << cam2 << std::endl;
971 std::cout <<
"Calibration without distortion failed." << std::endl;
974 if(resultCalibDist == 0)
975 std::cout << cam << std::endl;
977 std::cout <<
"Calibration with distortion failed." << std::endl;
985 while (iter < opt_first + opt_nimages*opt_step) {
989 if (opt_ppath.empty()){
991 s <<
"grid36-" << std::setw(2) << std::setfill(
'0') << iter<<
".pgm";
992 filename = dirname + s.str();
995 sprintf(cfilename, opt_ppath.c_str(), iter) ;
996 filename = cfilename;
999 std::cout <<
"read : " << filename << std::endl;
1002 if(table_cal[niter].get_npt()!=0){
1003 std::cout <<
"\nCompute standard deviation for pose " << niter <<std::endl;
1004 double 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;
1013 std::cout <<
"This image has not been used!" << std::endl;
1021 sprintf(title,
"Calibration results for image %s", (s.str()).c_str());
1032 delete [] table_cal;
1047 std::cout <<
"\nA click to continue..." << std::endl;
1054 delete [] table_cal;
1060 #ifdef VISP_HAVE_XML2
1063 if(resultCalib == 0){
1066 std::cout <<
"Camera parameters without distortion successfully saved in calibrate2dGrid.xml" << std::endl;
1068 std::cout <<
"Failed to save the camera parameters without distortion in calibrate2dGrid.xml" << std::endl;
1071 if(resultCalibDist == 0){
1076 std::cout <<
"Camera parameters with distortion successfully saved in calibrate2dGrid.xml" << std::endl;
1078 std::cout <<
"Failed to save the camera parameters with distortion in calibrate2dGrid.xml" << std::endl;
1084 delete [] table_cal;
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)
1092 unsigned int nbImg = first_image;
1093 unsigned int index = 0;
1095 #ifdef VISP_HAVE_OPENCV
1097 #elif defined(VISP_HAVE_V4L2)
1099 #elif defined(VISP_HAVE_DIRECTSHOW)
1101 #elif defined(VISP_HAVE_DC1394_2)
1111 #if defined VISP_HAVE_GDI
1113 #elif defined VISP_HAVE_GTK
1115 #elif defined VISP_HAVE_X11
1117 #elif defined VISP_HAVE_D3D9
1120 display.
init(I, 100, 100,
"record sequence for the calibration.");
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;
1139 char curImgName[FILENAME_MAX];
1140 sprintf(curImgName, out_path.c_str(), nbImg);
1144 std::cout <<
"write image : " << curImgName << std::endl;
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;
1169 #else // (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)...)
1174 vpTRACE(
"X11 or GTK or GDI or D3D functionnality is not available...") ;
1176 #endif // (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)...)