Visual Servoing Platform  version 3.3.1 under development (2020-08-10)
vpCalibration.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Camera calibration.
33  *
34  * Authors:
35  * Eric Marchand
36  * Francois Chaumette
37  * Anthony Saunier
38  *
39  *****************************************************************************/
40 
46 #include <visp3/core/vpDebug.h>
47 #include <visp3/core/vpPixelMeterConversion.h>
48 #include <visp3/vision/vpCalibration.h>
49 #include <visp3/vision/vpPose.h>
50 
51 double vpCalibration::threshold = 1e-10f;
52 unsigned int vpCalibration::nbIterMax = 4000;
53 double vpCalibration::gain = 0.25;
58 {
59  npt = 0;
60 
61  residual = residual_dist = 1000.;
62 
63  LoX.clear();
64  LoY.clear();
65  LoZ.clear();
66  Lip.clear();
67 
68  return 0;
69 }
70 
75  : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(), npt(0), LoX(), LoY(), LoZ(), Lip(), residual(1000.),
76  residual_dist(1000.)
77 {
78  init();
79 }
84  : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(), npt(0), LoX(), LoY(), LoZ(), Lip(), residual(1000.),
85  residual_dist(1000.)
86 {
87  (*this) = c;
88 }
89 
94 
101 {
102  npt = twinCalibration.npt;
103  LoX = twinCalibration.LoX;
104  LoY = twinCalibration.LoY;
105  LoZ = twinCalibration.LoZ;
106  Lip = twinCalibration.Lip;
107 
108  residual = twinCalibration.residual;
109  cMo = twinCalibration.cMo;
110  residual_dist = twinCalibration.residual_dist;
111  cMo_dist = twinCalibration.cMo_dist;
112 
113  cam = twinCalibration.cam;
114  cam_dist = twinCalibration.cam_dist;
115 
116  rMe = twinCalibration.rMe;
117 
118  eMc = twinCalibration.eMc;
119  eMc_dist = twinCalibration.eMc_dist;
120 
121  return (*this);
122 }
123 
128 {
129  LoX.clear();
130  LoY.clear();
131  LoZ.clear();
132  Lip.clear();
133  npt = 0;
134 
135  return 0;
136 }
137 
144 int vpCalibration::addPoint(double X, double Y, double Z, vpImagePoint &ip)
145 {
146  LoX.push_back(X);
147  LoY.push_back(Y);
148  LoZ.push_back(Z);
149 
150  Lip.push_back(ip);
151 
152  npt++;
153 
154  return 0;
155 }
156 
162 void vpCalibration::computePose(const vpCameraParameters &camera, vpHomogeneousMatrix &cMo_est)
163 {
164  // The vpPose class mainly contents a list of vpPoint (that is (X,Y,Z, x, y)
165  // )
166  vpPose pose;
167  // the list of point is cleared (if that's not done before)
168  pose.clearPoint();
169  // we set the 3D points coordinates (in meter !) in the object/world frame
170  std::list<double>::const_iterator it_LoX = LoX.begin();
171  std::list<double>::const_iterator it_LoY = LoY.begin();
172  std::list<double>::const_iterator it_LoZ = LoZ.begin();
173  std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
174 
175  for (unsigned int i = 0; i < npt; i++) {
176  vpPoint P(*it_LoX, *it_LoY, *it_LoZ);
177  double x = 0, y = 0;
178  vpPixelMeterConversion::convertPoint(camera, *it_Lip, x, y);
179  P.set_x(x);
180  P.set_y(y);
181 
182  pose.addPoint(P);
183  ++it_LoX;
184  ++it_LoY;
185  ++it_LoZ;
186  ++it_Lip;
187  }
188  vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon
189  vpHomogeneousMatrix cMo_lagrange; // computed pose with dementhon
190 
191  // compute the initial pose using Lagrange method followed by a non linear
192  // minimisation method
193  // Pose by Lagrange it provides an initialization of the pose
194  pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
195  double residual_lagrange = pose.computeResidual(cMo_lagrange);
196 
197  // compute the initial pose using Dementhon method followed by a non linear
198  // minimisation method
199  // Pose by Dementhon it provides an initialization of the pose
200  pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
201  double residual_dementhon = pose.computeResidual(cMo_dementhon);
202 
203  // we keep the better initialization
204  if (residual_lagrange < residual_dementhon)
205  cMo_est = cMo_lagrange;
206  else
207  cMo_est = cMo_dementhon;
208 
209  // the pose is now refined using the virtual visual servoing approach
210  // Warning: cMo needs to be initialized otherwise it may diverge
211  pose.computePose(vpPose::VIRTUAL_VS, cMo_est);
212 }
213 
222 {
223  double residual_ = 0;
224 
225  std::list<double>::const_iterator it_LoX = LoX.begin();
226  std::list<double>::const_iterator it_LoY = LoY.begin();
227  std::list<double>::const_iterator it_LoZ = LoZ.begin();
228  std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
229 
230  double u0 = camera.get_u0();
231  double v0 = camera.get_v0();
232  double px = camera.get_px();
233  double py = camera.get_py();
234  vpImagePoint ip;
235 
236  for (unsigned int i = 0; i < npt; i++) {
237  double oX = *it_LoX;
238  double oY = *it_LoY;
239  double oZ = *it_LoZ;
240 
241  double cX = oX * cMo_est[0][0] + oY * cMo_est[0][1] + oZ * cMo_est[0][2] + cMo_est[0][3];
242  double cY = oX * cMo_est[1][0] + oY * cMo_est[1][1] + oZ * cMo_est[1][2] + cMo_est[1][3];
243  double cZ = oX * cMo_est[2][0] + oY * cMo_est[2][1] + oZ * cMo_est[2][2] + cMo_est[2][3];
244 
245  double x = cX / cZ;
246  double y = cY / cZ;
247 
248  ip = *it_Lip;
249  double u = ip.get_u();
250  double v = ip.get_v();
251 
252  double xp = u0 + x * px;
253  double yp = v0 + y * py;
254 
255  residual_ += (vpMath::sqr(xp - u) + vpMath::sqr(yp - v));
256 
257  ++it_LoX;
258  ++it_LoY;
259  ++it_LoZ;
260  ++it_Lip;
261  }
262  this->residual = residual_;
263  return sqrt(residual_ / npt);
264 }
273 {
274  double residual_ = 0;
275 
276  std::list<double>::const_iterator it_LoX = LoX.begin();
277  std::list<double>::const_iterator it_LoY = LoY.begin();
278  std::list<double>::const_iterator it_LoZ = LoZ.begin();
279  std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
280 
281  double u0 = camera.get_u0();
282  double v0 = camera.get_v0();
283  double px = camera.get_px();
284  double py = camera.get_py();
285  double kud = camera.get_kud();
286  double kdu = camera.get_kdu();
287 
288  double inv_px = 1 / px;
289  double inv_py = 1 / px;
290  vpImagePoint ip;
291 
292  for (unsigned int i = 0; i < npt; i++) {
293  double oX = *it_LoX;
294  double oY = *it_LoY;
295  double oZ = *it_LoZ;
296 
297  double cX = oX * cMo_est[0][0] + oY * cMo_est[0][1] + oZ * cMo_est[0][2] + cMo_est[0][3];
298  double cY = oX * cMo_est[1][0] + oY * cMo_est[1][1] + oZ * cMo_est[1][2] + cMo_est[1][3];
299  double cZ = oX * cMo_est[2][0] + oY * cMo_est[2][1] + oZ * cMo_est[2][2] + cMo_est[2][3];
300 
301  double x = cX / cZ;
302  double y = cY / cZ;
303 
304  ip = *it_Lip;
305  double u = ip.get_u();
306  double v = ip.get_v();
307 
308  double r2ud = 1 + kud * (vpMath::sqr(x) + vpMath::sqr(y));
309 
310  double xp = u0 + x * px * r2ud;
311  double yp = v0 + y * py * r2ud;
312 
313  residual_ += (vpMath::sqr(xp - u) + vpMath::sqr(yp - v));
314 
315  double r2du = (vpMath::sqr((u - u0) * inv_px) + vpMath::sqr((v - v0) * inv_py));
316 
317  xp = u0 + x * px - kdu * (u - u0) * r2du;
318  yp = v0 + y * py - kdu * (v - v0) * r2du;
319 
320  residual_ += (vpMath::sqr(xp - u) + vpMath::sqr(yp - v));
321  ++it_LoX;
322  ++it_LoY;
323  ++it_LoZ;
324  ++it_Lip;
325  }
326  residual_ /= 2;
327 
328  this->residual_dist = residual_;
329  return sqrt(residual_ / npt);
330 }
331 
339 void vpCalibration::computeStdDeviation(double &deviation, double &deviation_dist)
340 {
341  deviation = computeStdDeviation(cMo, cam);
342  deviation_dist = computeStdDeviation_dist(cMo_dist, cam_dist);
343 }
344 
357  vpCameraParameters &cam_est, bool verbose)
358 {
359  try {
360  computePose(cam_est, cMo_est);
361  switch (method) {
362  case CALIB_LAGRANGE:
364  calibLagrange(cam_est, cMo_est);
365  } break;
366  case CALIB_VIRTUAL_VS:
369  default:
370  break;
371  }
372 
373  switch (method) {
374  case CALIB_VIRTUAL_VS:
378  if (verbose) {
379  std::cout << "start calibration without distortion" << std::endl;
380  }
381  calibVVS(cam_est, cMo_est, verbose);
382  } break;
383  case CALIB_LAGRANGE:
384  default:
385  break;
386  }
387  this->cMo = cMo_est;
388  this->cMo_dist = cMo_est;
389 
390  // Print camera parameters
391  if (verbose) {
392  // std::cout << "Camera parameters without distortion :" <<
393  // std::endl;
394  cam_est.printParameters();
395  }
396 
397  this->cam = cam_est;
398 
399  switch (method) {
402  if (verbose) {
403  std::cout << "start calibration with distortion" << std::endl;
404  }
405  calibVVSWithDistortion(cam_est, cMo_est, verbose);
406  } break;
407  case CALIB_LAGRANGE:
408  case CALIB_VIRTUAL_VS:
410  default:
411  break;
412  }
413  // Print camera parameters
414  if (verbose) {
415  // std::cout << "Camera parameters without distortion :" <<
416  // std::endl;
417  this->cam.printParameters();
418  // std::cout << "Camera parameters with distortion :" <<
419  // std::endl;
420  cam_est.printParameters();
421  }
422 
423  this->cam_dist = cam_est;
424 
425  this->cMo_dist = cMo_est;
426 
427  if (cam_est.get_px() < 0 || cam_est.get_py() < 0 || cam_est.get_u0() < 0 || cam_est.get_v0() < 0) {
428  if (verbose) {
429  std::cout << "Unable to calibrate the camera. Estimated parameters are negative." << std::endl;
430  }
431  return EXIT_FAILURE;
432  }
433 
434  return EXIT_SUCCESS;
435  } catch (...) {
436  throw;
437  }
438 }
439 
454 int vpCalibration::computeCalibrationMulti(vpCalibrationMethodType method, std::vector<vpCalibration> &table_cal,
455  vpCameraParameters &cam_est, double &globalReprojectionError, bool verbose)
456 {
457  try {
458  unsigned int nbPose = (unsigned int)table_cal.size();
459  for (unsigned int i = 0; i < nbPose; i++) {
460  if (table_cal[i].get_npt() > 3)
461  table_cal[i].computePose(cam_est, table_cal[i].cMo);
462  }
463  switch (method) {
464  case CALIB_LAGRANGE: {
465  if (nbPose > 1) {
466  std::cout << "this calibration method is not available in" << std::endl
467  << "vpCalibration::computeCalibrationMulti()" << std::endl;
468  return -1;
469  } else {
470  table_cal[0].calibLagrange(cam_est, table_cal[0].cMo);
471  table_cal[0].cam = cam_est;
472  table_cal[0].cam_dist = cam_est;
473  table_cal[0].cMo_dist = table_cal[0].cMo;
474  }
475  break;
476  }
479  if (nbPose > 1) {
480  std::cout << "this calibration method is not available in" << std::endl
481  << "vpCalibration::computeCalibrationMulti()" << std::endl
482  << "with several images." << std::endl;
483  return -1;
484  } else {
485  table_cal[0].calibLagrange(cam_est, table_cal[0].cMo);
486  table_cal[0].cam = cam_est;
487  table_cal[0].cam_dist = cam_est;
488  table_cal[0].cMo_dist = table_cal[0].cMo;
489  }
490  calibVVSMulti(table_cal, cam_est, globalReprojectionError, verbose);
491  break;
492  }
493  case CALIB_VIRTUAL_VS:
494  case CALIB_VIRTUAL_VS_DIST: {
495  calibVVSMulti(table_cal, cam_est, globalReprojectionError, verbose);
496  break;
497  }
498  }
499  // Print camera parameters
500  if (verbose) {
501  // std::cout << "Camera parameters without distortion :" <<
502  // std::endl;
503  cam_est.printParameters();
504  }
505 
506  switch (method) {
507  case CALIB_LAGRANGE:
509  case CALIB_VIRTUAL_VS:
510  verbose = false;
511  break;
513  case CALIB_VIRTUAL_VS_DIST: {
514  if (verbose)
515  std::cout << "Compute camera parameters with distortion" << std::endl;
516 
517  calibVVSWithDistortionMulti(table_cal, cam_est, globalReprojectionError, verbose);
518  } break;
519  }
520  // Print camera parameters
521  if (verbose) {
522  // std::cout << "Camera parameters without distortion :" <<
523  // std::endl;
524  table_cal[0].cam.printParameters();
525  // std::cout << "Camera parameters with distortion:" << std::endl;
526  cam_est.printParameters();
527  std::cout << std::endl;
528  }
529 
530  if (cam_est.get_px() < 0 || cam_est.get_py() < 0 || cam_est.get_u0() < 0 || cam_est.get_v0() < 0) {
531  if (verbose) {
532  std::cout << "Unable to calibrate the camera. Estimated parameters are negative." << std::endl;
533  }
534  return EXIT_FAILURE;
535  }
536 
537  return EXIT_SUCCESS;
538  } catch (...) {
539  throw;
540  }
541 }
542 
550 int vpCalibration::writeData(const char *filename)
551 {
552  std::ofstream f(filename);
553  vpImagePoint ip;
554 
555  std::list<double>::const_iterator it_LoX = LoX.begin();
556  std::list<double>::const_iterator it_LoY = LoY.begin();
557  std::list<double>::const_iterator it_LoZ = LoZ.begin();
558  std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
559 
560  f.precision(10);
561  f.setf(std::ios::fixed, std::ios::floatfield);
562  f << LoX.size() << std::endl;
563 
564  for (unsigned int i = 0; i < LoX.size(); i++) {
565 
566  double oX = *it_LoX;
567  double oY = *it_LoY;
568  double oZ = *it_LoZ;
569 
570  ip = *it_Lip;
571  double u = ip.get_u();
572  double v = ip.get_v();
573 
574  f << oX << " " << oY << " " << oZ << " " << u << " " << v << std::endl;
575 
576  ++it_LoX;
577  ++it_LoY;
578  ++it_LoZ;
579  ++it_Lip;
580  }
581 
582  f.close();
583  return 0;
584 }
585 
592 int vpCalibration::readData(const char *filename)
593 {
594  vpImagePoint ip;
595  std::ifstream f;
596  f.open(filename);
597  if (!f.fail()) {
598  unsigned int n;
599  f >> n;
600  std::cout << "There are " << n << " point on the calibration grid " << std::endl;
601 
602  clearPoint();
603 
604  if (n > 100000)
605  throw(vpException(vpException::badValue, "Bad number of point in the calibration grid"));
606 
607  for (unsigned int i = 0; i < n; i++) {
608  double x, y, z, u, v;
609  f >> x >> y >> z >> u >> v;
610  std::cout << x << " " << y << " " << z << " " << u << " " << v << std::endl;
611  ip.set_u(u);
612  ip.set_v(v);
613  addPoint(x, y, z, ip);
614  }
615 
616  f.close();
617  return 0;
618  } else {
619  return -1;
620  }
621 }
638 int vpCalibration::readGrid(const char *filename, unsigned int &n, std::list<double> &oX, std::list<double> &oY,
639  std::list<double> &oZ, bool verbose)
640 {
641  try {
642  std::ifstream f;
643  f.open(filename);
644  if (!f.fail()) {
645 
646  f >> n;
647  if (verbose)
648  std::cout << "There are " << n << " points on the calibration grid " << std::endl;
649  int no_pt;
650  double x, y, z;
651 
652  // clear the list
653  oX.clear();
654  oY.clear();
655  oZ.clear();
656 
657  if (n > 100000)
658  throw(vpException(vpException::badValue, "Bad number of point in the calibration grid"));
659 
660  for (unsigned int i = 0; i < n; i++) {
661  f >> no_pt >> x >> y >> z;
662  if (verbose) {
663  std::cout << no_pt << std::endl;
664  std::cout << x << " " << y << " " << z << std::endl;
665  }
666  oX.push_back(x);
667  oY.push_back(y);
668  oZ.push_back(z);
669  }
670 
671  f.close();
672  } else {
673  return -1;
674  }
675  } catch (...) {
676  return -1;
677  }
678  return 0;
679 }
680 
691 int vpCalibration::displayData(vpImage<unsigned char> &I, vpColor color, unsigned int thickness, int subsampling_factor)
692 {
693 
694  for (std::list<vpImagePoint>::const_iterator it = Lip.begin(); it != Lip.end(); ++it) {
695  vpImagePoint ip = *it;
696  if (subsampling_factor > 1.) {
697  ip.set_u(ip.get_u() / subsampling_factor);
698  ip.set_v(ip.get_v() / subsampling_factor);
699  }
700  vpDisplay::displayCross(I, ip, 12, color, thickness);
701  }
702  return 0;
703 }
704 
715 int vpCalibration::displayGrid(vpImage<unsigned char> &I, vpColor color, unsigned int thickness, int subsampling_factor)
716 {
717  double u0_dist = cam_dist.get_u0() / subsampling_factor;
718  double v0_dist = cam_dist.get_v0() / subsampling_factor;
719  double px_dist = cam_dist.get_px() / subsampling_factor;
720  double py_dist = cam_dist.get_py() / subsampling_factor;
721  double kud_dist = cam_dist.get_kud();
722  // double kdu_dist = cam_dist.get_kdu() ;
723 
724  // double u0 = cam.get_u0() ;
725  // double v0 = cam.get_v0() ;
726  // double px = cam.get_px() ;
727  // double py = cam.get_py() ;
728 
729  std::list<double>::const_iterator it_LoX = LoX.begin();
730  std::list<double>::const_iterator it_LoY = LoY.begin();
731  std::list<double>::const_iterator it_LoZ = LoZ.begin();
732 
733  for (unsigned int i = 0; i < npt; i++) {
734  double oX = *it_LoX;
735  double oY = *it_LoY;
736  double oZ = *it_LoZ;
737 
738  // double cX = oX*cMo[0][0]+oY*cMo[0][1]+oZ*cMo[0][2] + cMo[0][3];
739  // double cY = oX*cMo[1][0]+oY*cMo[1][1]+oZ*cMo[1][2] + cMo[1][3];
740  // double cZ = oX*cMo[2][0]+oY*cMo[2][1]+oZ*cMo[2][2] + cMo[2][3];
741 
742  // double x = cX/cZ ;
743  // double y = cY/cZ ;
744 
745  // double xp = u0 + x*px ;
746  // double yp = v0 + y*py ;
747 
748  // vpDisplay::displayCross(I,(int)vpMath::round(yp),
749  // (int)vpMath::round(xp),
750  // 5,col) ;
751 
752  double cX = oX * cMo_dist[0][0] + oY * cMo_dist[0][1] + oZ * cMo_dist[0][2] + cMo_dist[0][3];
753  double cY = oX * cMo_dist[1][0] + oY * cMo_dist[1][1] + oZ * cMo_dist[1][2] + cMo_dist[1][3];
754  double cZ = oX * cMo_dist[2][0] + oY * cMo_dist[2][1] + oZ * cMo_dist[2][2] + cMo_dist[2][3];
755 
756  double x = cX / cZ;
757  double y = cY / cZ;
758 
759  double r2 = 1 + kud_dist * (vpMath::sqr(x) + vpMath::sqr(y));
760 
761  vpImagePoint ip;
762  ip.set_u(u0_dist + x * px_dist * r2);
763  ip.set_v(v0_dist + y * py_dist * r2);
764 
765  vpDisplay::displayCross(I, ip, 6, color, thickness);
767 
768  // std::cout << oX << " " << oY << " " <<oZ << std::endl ;
769  // I.getClick() ;
770  ++it_LoX;
771  ++it_LoY;
772  ++it_LoZ;
773  }
774  return 0;
775 }
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:374
double get_kdu() const
int computeCalibration(vpCalibrationMethodType method, vpHomogeneousMatrix &cMo_est, vpCameraParameters &cam_est, bool verbose=false)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void set_u(double u)
Definition: vpImagePoint.h:225
int readData(const char *filename)
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
error that can be emited by ViSP classes.
Definition: vpException.h:71
int addPoint(double X, double Y, double Z, vpImagePoint &ip)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Tools for perspective camera calibration.
Definition: vpCalibration.h:71
virtual ~vpCalibration()
vpHomogeneousMatrix cMo
(as a 3x4 matrix [R T])
Definition: vpCalibration.h:93
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:497
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:499
vpCalibration & operator=(const vpCalibration &twinCalibration)
double get_u() const
Definition: vpImagePoint.h:262
int clearPoint()
Suppress all the point in the array of point.
static double sqr(double x)
Definition: vpMath.h:116
int displayGrid(vpImage< unsigned char > &I, vpColor color=vpColor::yellow, unsigned int thickness=1, int subsampling_factor=1)
vpHomogeneousMatrix cMo_dist
Definition: vpCalibration.h:95
unsigned int get_npt() const
get the number of points
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:80
Generic class defining intrinsic camera parameters.
vpHomogeneousMatrix eMc_dist
Position of the camera in end-effector frame using camera parameters with distorsion.
vpHomogeneousMatrix rMe
reference coordinates (manipulator base coordinates)
double computeStdDeviation_dist(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
void set_v(double v)
Definition: vpImagePoint.h:236
vpCameraParameters cam_dist
projection model with distortion
int writeData(const char *filename)
vpHomogeneousMatrix eMc
Position of the camera in end-effector frame using camera parameters without distorsion.
double get_kud() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
void computeStdDeviation(double &deviation, double &deviation_dist)
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo...
Definition: vpPose.cpp:336
vpCameraParameters cam
projection model without distortion
Definition: vpCalibration.h:98
static int computeCalibrationMulti(vpCalibrationMethodType method, std::vector< vpCalibration > &table_cal, vpCameraParameters &cam, double &globalReprojectionError, bool verbose=false)
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:149
int displayData(vpImage< unsigned char > &I, vpColor color=vpColor::red, unsigned int thickness=1, int subsampling_factor=1)
double get_v() const
Definition: vpImagePoint.h:273
static int readGrid(const char *filename, unsigned int &n, std::list< double > &oX, std::list< double > &oY, std::list< double > &oZ, bool verbose=false)
void clearPoint()
Definition: vpPose.cpp:134