48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpExponentialMap.h>
50 #include <visp3/core/vpIoTools.h>
51 #include <visp3/io/vpParseArgv.h>
52 #include <visp3/vision/vpHandEyeCalibration.h>
56 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
60 const unsigned int N = 6;
62 std::vector<vpHomogeneousMatrix> cMo(N);
66 std::vector<vpHomogeneousMatrix> wMe(N);
79 std::cout <<
"Simulated hand-eye transformation: eMc " << std::endl;
80 std::cout << eMc << std::endl;
85 for (
unsigned int i = 0; i < N; i++) {
89 cMo[0].buildFrom(0, 0, 0.5, 0, 0, 0);
90 wMe[0].buildFrom(0, 0, 0, 0, 0, 0);
109 cMo[i] = cMc.
inverse() * cMo[i - 1];
110 wMe[i] = wMe[i - 1] * eMc * cMc * eMc.
inverse();
116 for (
unsigned int i = 0; i < N; i++) {
118 wMo = wMe[i] * eMc * cMo[i];
119 std::cout << std::endl <<
"wMo[" << i <<
"] " << std::endl;
120 std::cout << wMo << std::endl;
121 std::cout <<
"cMo[" << i <<
"] " << std::endl;
122 std::cout << cMo[i] << std::endl;
123 std::cout <<
"wMe[" << i <<
"] " << std::endl;
124 std::cout << wMe[i] << std::endl;
138 std::cout << std::endl <<
"** Hand-eye calibration succeed" << std::endl;
139 std::cout << std::endl <<
"** Hand-eye (eMc) transformation estimated:" << std::endl;
140 std::cout << eMc << std::endl;
141 std::cout <<
"** Corresponding pose vector: " <<
vpPoseVector(eMc).
t() << std::endl;
143 std::cout << std::endl
144 <<
"** Translation [m]: " << eMc[0][3] <<
" " << eMc[1][3] <<
" " << eMc[2][3] << std::endl;
145 std::cout <<
"** Rotation (theta-u representation) [rad]: " << erc.
t() << std::endl;
146 std::cout <<
"** Rotation (theta-u representation) [deg]: " <<
vpMath::deg(erc[0]) <<
" " <<
vpMath::deg(erc[1])
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
154 <<
"Check your input data and ensure they are covering the half sphere over the chessboard."
156 std::cout << std::endl
157 <<
"See https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-calibration-extrinsic.html" << std::endl;
163 std::cout <<
"Catch an exception: " << e << std::endl;
167 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
static vpHomogeneousMatrix direct(const vpColVector &v)
static int calibrate(const std::vector< vpHomogeneousMatrix > &cMo, const std::vector< vpHomogeneousMatrix > &rMe, vpHomogeneousMatrix &eMc)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
vpHomogeneousMatrix inverse() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void extract(vpRotationMatrix &R) const
static double rad(double deg)
static double deg(double rad)
Implementation of a pose vector and operations on poses.
Implementation of a rotation vector as quaternion angle minimal representation.
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.