51 #include <visp3/core/vpDebug.h> 52 #include <visp3/core/vpExponentialMap.h> 53 #include <visp3/core/vpIoTools.h> 54 #include <visp3/io/vpParseArgv.h> 55 #include <visp3/vision/vpHandEyeCalibration.h> 59 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV)) 63 const unsigned int N = 6;
65 std::vector<vpHomogeneousMatrix> cMo(N);
69 std::vector<vpHomogeneousMatrix> wMe(N);
82 std::cout <<
"Simulated hand-eye transformation: eMc " << std::endl;
83 std::cout << eMc << std::endl;
88 for (
unsigned int i = 0; i < N; i++) {
92 cMo[0].buildFrom(0, 0, 0.5, 0, 0, 0);
93 wMe[0].buildFrom(0, 0, 0, 0, 0, 0);
111 cMo[i] = cMc.
inverse() * cMo[i - 1];
112 wMe[i] = wMe[i - 1] * eMc * cMc * eMc.
inverse();
118 for (
unsigned int i = 0; i < N; i++) {
120 wMo = wMe[i] * eMc * cMo[i];
121 std::cout << std::endl <<
"wMo[" << i <<
"] " << std::endl;
122 std::cout << wMo << std::endl;
123 std::cout <<
"cMo[" << i <<
"] " << std::endl;
124 std::cout << cMo[i] << std::endl;
125 std::cout <<
"wMe[" << i <<
"] " << std::endl;
126 std::cout << wMe[i] << std::endl;
140 std::cout << std::endl <<
"** Hand-eye calibration succeed" << std::endl;
141 std::cout << std::endl <<
"** Hand-eye (eMc) transformation estimated:" << std::endl;
142 std::cout << eMc << std::endl;
143 std::cout <<
"** Corresponding pose vector: " <<
vpPoseVector(eMc).
t() << std::endl;
145 std::cout << std::endl <<
"** Translation [m]: " << eMc[0][3] <<
" " << eMc[1][3] <<
" " << eMc[2][3] << std::endl;
146 std::cout <<
"** Rotation (theta-u representation) [rad]: " << erc.
t() << std::endl;
149 std::cout <<
"** Rotation (quaternion representation) [rad]: " << quaternion.
t() << std::endl;
152 std::cout << std::endl <<
"** Hand-eye calibration failed" << std::endl;
153 std::cout << std::endl <<
"Check your input data and ensure they are covering the half sphere over the chessboard." << std::endl;
154 std::cout << std::endl <<
"See https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-calibration-extrinsic.html" << std::endl;
159 std::cout <<
"Catch an exception: " << e << std::endl;
163 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
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)
error that can be emited by ViSP classes.
vpRotationMatrix getRotationMatrix() const
void extract(vpRotationMatrix &R) const
Implementation of a rotation vector as quaternion angle minimal representation.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double rad(double deg)
static double deg(double rad)
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
vpHomogeneousMatrix inverse() const
static vpHomogeneousMatrix direct(const vpColVector &v)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.