Visual Servoing Platform  version 3.4.0
tutorial-hand-eye-calibration.cpp
1 #include <visp3/vision/vpHandEyeCalibration.h>
3 
4 int main(int argc, char *argv[])
5 {
6  unsigned int ndata = 0;
7  for (int i = 1; i < argc; i++) {
8  if (std::string(argv[i]) == "--ndata" && i+1 < argc) {
9  ndata = atoi(argv[i+1]);
10  } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
11  std::cout << argv[0] << " [--ndata <number of data to process>] "
12  "[--help] [-h]" << std::endl;
13  return EXIT_SUCCESS;
14  }
15  }
16  if (ndata == 0) {
17  std::cout << "Number of data to process not specified" << std::endl;
18  std::cout << argv[0] << " --help" << std::endl;
19  return EXIT_SUCCESS;
20  }
21  std::vector<vpHomogeneousMatrix> cMo(ndata);
22  std::vector<vpHomogeneousMatrix> wMe(ndata);
24 
25  for (unsigned int i = 1; i <= ndata; i++) {
26  std::ostringstream ss_fPe, ss_cPo;
27  ss_fPe << "pose_fPe_" << i << ".yaml";
28  ss_cPo << "pose_cPo_" << i << ".yaml";
29  std::cout << "Use fPe=" << ss_fPe.str() << ", cPo=" << ss_cPo.str() << std::endl;
30 
31  vpPoseVector wPe;
32  if (wPe.loadYAML(ss_fPe.str(), wPe) == false) {
33  std::cout << "Unable to read data from: " << ss_fPe.str() << std::endl;
34  return EXIT_FAILURE;
35  }
36  wMe[i - 1] = vpHomogeneousMatrix(wPe);
37 
38  vpPoseVector cPo;
39  if (cPo.loadYAML(ss_cPo.str(), cPo) == false) {
40  std::cout << "Unable to read data from: " << ss_cPo.str() << std::endl;
41  return EXIT_FAILURE;
42  }
43  cMo[i-1] = vpHomogeneousMatrix(cPo);
44  }
45 
46  int ret = vpHandEyeCalibration::calibrate(cMo, wMe, eMc);
47 
48  if (ret == 0) {
49  std::cout << std::endl << "** Hand-eye calibration succeed" << std::endl;
50  std::cout << std::endl << "** Hand-eye (eMc) transformation estimated:" << std::endl;
51  std::cout << eMc << std::endl;
52  std::cout << "** Corresponding pose vector: " << vpPoseVector(eMc).t() << std::endl;
53 
55  std::cout << std::endl << "** Translation [m]: " << eMc[0][3] << " " << eMc[1][3] << " " << eMc[2][3] << std::endl;
56  std::cout << "** Rotation (theta-u representation) [rad]: " << erc.t() << std::endl;
57  std::cout << "** Rotation (theta-u representation) [deg]: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
58  vpQuaternionVector quaternion(eMc.getRotationMatrix());
59  std::cout << "** Rotation (quaternion representation) [rad]: " << quaternion.t() << std::endl;
60 
61  // save eMc
62  std::ofstream file_eMc("eMc.txt");
63  eMc.save(file_eMc);
64 
65  vpPoseVector pose_vec(eMc);
66  std::string output_filename("eMc.yaml");
67  std::cout << std::endl << "Save transformation matrix eMc as a vpPoseVector in " << output_filename << std::endl;
68  pose_vec.saveYAML(output_filename, pose_vec);
69  }
70  else {
71  std::cout << std::endl << "** Hand-eye calibration failed" << std::endl;
72  std::cout << std::endl << "Check your input data and ensure they are covering the half sphere over the chessboard." << std::endl;
73  std::cout << std::endl << "See https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-calibration-extrinsic.html" << std::endl;
74  }
75 
76  return EXIT_SUCCESS;
77 }
78 
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=NULL)
Definition: vpArray2D.h:652
Implementation of an homogeneous matrix and operations on such kind of matrices.
static int calibrate(const std::vector< vpHomogeneousMatrix > &cMo, const std::vector< vpHomogeneousMatrix > &rMe, vpHomogeneousMatrix &eMc)
vpRowVector t() const
vpRotationMatrix getRotationMatrix() const
Implementation of a rotation vector as quaternion angle minimal representation.
void save(std::ofstream &f) const
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
Implementation of a rotation vector as axis-angle minimal representation.
vpRowVector t() const