Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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") {
11  std::cout << argv[0] << " [--ndata <number of data to process>] "
12  "[--help]" << 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  vpHandEyeCalibration::calibrate(cMo, wMe, eMc);
47 
48  // save eMc
49  std::ofstream file_eMc("eMc.txt");
50  eMc.save(file_eMc);
51 
52  vpPoseVector pose_vec(eMc);
53  std::cout << "\nSave eMc.yaml" << std::endl;
54  pose_vec.saveYAML("eMc.yaml", pose_vec);
55 
56  std::cout << "\nOutput: Hand-eye calibration result: eMc estimated " << std::endl;
57  std::cout << eMc << std::endl;
58  vpThetaUVector ePc(eMc);
59  std::cout << "theta U (deg): " << vpMath::deg(ePc[0]) << " " << vpMath::deg(ePc[1]) << " " << vpMath::deg(ePc[2]) << std::endl;
60 
61  return EXIT_SUCCESS;
62 }
63 
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=NULL)
Definition: vpArray2D.h:434
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)
void save(std::ofstream &f) const
static double deg(double rad)
Definition: vpMath.h:95
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:92
Implementation of a rotation vector as axis-angle minimal representation.