43 #include <visp3/vision/vpHandEyeCalibration.h> 45 #define DEBUG_LEVEL1 0 46 #define DEBUG_LEVEL2 0 58 void vpHandEyeCalibration::calibrationVerifrMo(
const std::vector<vpHomogeneousMatrix> &cMo,
59 const std::vector<vpHomogeneousMatrix> &rMe,
62 unsigned int nbPose = (
unsigned int)cMo.size();
63 std::vector<vpTranslationVector> rTo(nbPose);
64 std::vector<vpRotationMatrix> rRo(nbPose);
66 for (
unsigned int i = 0; i < nbPose; i++) {
76 std::cout <<
"Mean " << std::endl;
77 std::cout <<
"Translation: " << meanTrans.
t() << std::endl;
79 std::cout <<
"Rotation : theta (deg) = " <<
vpMath::deg(sqrt(P.sumSquare())) <<
" Matrice : " << std::endl
80 << meanRot << std::endl;
88 for (
unsigned int i = 0; i < nbPose; i++) {
92 double theta = sqrt(P.sumSquare());
93 std::cout <<
"Distance theta between rMo(" << i <<
") and mean (deg) = " <<
vpMath::deg(theta) << std::endl;
96 resRot += theta * theta;
98 resRot = sqrt(resRot / nbPose);
99 std::cout <<
"Mean residual rMo(" << nbPose <<
") - rotation (deg) = " <<
vpMath::deg(resRot) << std::endl;
101 double resTrans = 0.0;
102 for (
unsigned int i = 0; i < nbPose; i++) {
105 std::cout <<
"Distance d between rMo(" << i <<
") and mean (m) = " << sqrt(errTrans.
sumSquare()) << std::endl;
107 resTrans = sqrt(resTrans / nbPose);
108 std::cout <<
"Mean residual rMo(" << nbPose <<
") - translation (m) = " << resTrans << std::endl;
109 double resPos = (resRot * resRot + resTrans * resTrans) * nbPose;
110 resPos = sqrt(resPos / (2 * nbPose));
111 std::cout <<
"Mean residual rMo(" << nbPose <<
") - global = " << resPos << std::endl;
125 int vpHandEyeCalibration::calibrationRotationProcrustes(
const std::vector<vpHomogeneousMatrix> &cMo,
126 const std::vector<vpHomogeneousMatrix> &rMe,
136 unsigned int nbPose = (
unsigned int)cMo.
size();
139 for (
unsigned int i = 0; i < nbPose; i++) {
141 rMe[i].extract(rRei);
142 cMo[i].extract(ciRo);
145 for (
unsigned int j = 0; j < nbPose; j++) {
149 rMe[j].extract(rRej);
150 cMo[j].extract(cjRo);
188 std::cout <<
"Rotation from Procrustes method " << std::endl;
193 residual = A * Ct.
t() - Et.
t();
196 printf(
"Mean residual (rotation) = %lf\n", res);
214 int vpHandEyeCalibration::calibrationRotationTsai(
const std::vector<vpHomogeneousMatrix> &cMo,
219 unsigned int nbPose = (
unsigned int)cMo.
size();
222 for (
unsigned int i = 0; i < nbPose; i++) {
224 rMe[i].extract(rRei);
225 cMo[i].extract(ciRo);
228 for (
unsigned int j = 0; j < nbPose; j++) {
232 rMe[j].extract(rRej);
233 cMo[j].extract(cjRo);
263 std::cout <<
"Tsai method: system A X = B " << std::endl;
264 std::cout <<
"A " << std::endl << A << std::endl;
265 std::cout <<
"B " << std::endl << B << std::endl;
282 double c = 1 / sqrt(1 + norm);
283 double alpha = acos(c);
285 for (
unsigned int i = 0; i < 3; i++)
294 std::cout <<
"Rotation from Tsai method" << std::endl;
298 for (
unsigned int i = 0; i < 3; i++)
301 residual = A * x - B;
304 printf(
"Mean residual (rotation) = %lf\n", res);
322 int vpHandEyeCalibration::calibrationRotationTsaiOld(
const std::vector<vpHomogeneousMatrix> &cMo,
325 unsigned int nbPose = (
unsigned int)cMo.size();
331 for (
unsigned int i = 0; i < nbPose; i++) {
333 rMe[i].extract(rRei);
334 cMo[i].extract(ciRo);
337 for (
unsigned int j = 0; j < nbPose; j++) {
340 rMe[j].extract(rRej);
341 cMo[j].extract(cjRo);
350 double theta = sqrt(rPeij[0] * rPeij[0] + rPeij[1] * rPeij[1] + rPeij[2] * rPeij[2]);
357 for (
unsigned int m = 0; m < 3; m++) {
362 theta = sqrt(cijPo[0] * cijPo[0] + cijPo[1] * cijPo[1] + cijPo[2] * cijPo[2]);
363 for (
unsigned int m = 0; m < 3; m++) {
420 for (
unsigned int i = 0; i < 3; i++)
421 x[i] = 2 * x[i] / sqrt(1 + d);
423 theta = 2 * asin(theta);
425 if (std::fabs(theta) > std::numeric_limits<double>::epsilon()) {
426 for (
unsigned int i = 0; i < 3; i++)
427 x[i] *= theta / (2 * sin(theta / 2));
437 std::cout <<
"Rotation from Old Tsai method" << std::endl;
442 residual = A * x2 - B;
445 printf(
"Mean residual (rotation) = %lf\n", res);
464 int vpHandEyeCalibration::calibrationTranslation(
const std::vector<vpHomogeneousMatrix> &cMo,
471 unsigned int nbPose = (
unsigned int)cMo.size();
476 for (
unsigned int i = 0; i < nbPose; i++) {
477 for (
unsigned int j = 0; j < nbPose; j++) {
518 printf(
"New Hand-eye calibration : ");
519 std::cout <<
"Translation: " << eTc[0] <<
" " << eTc[1] <<
" " << eTc[2] << std::endl;
522 residual = A * x - B;
525 printf(
"Mean residual (translation) = %lf\n", res);
544 int vpHandEyeCalibration::calibrationTranslationOld(
const std::vector<vpHomogeneousMatrix> &cMo,
555 unsigned int nbPose = (
unsigned int)cMo.size();
557 for (
unsigned int i = 0; i < nbPose; i++) {
560 rMe[i].extract(rRei);
561 cMo[i].extract(ciRo);
562 rMe[i].extract(rTei);
563 cMo[i].extract(ciTo);
565 for (
unsigned int j = 0; j < nbPose; j++) {
570 rMe[j].extract(rRej);
571 cMo[j].extract(cjRo);
574 rMe[j].extract(rTej);
575 cMo[j].extract(cjTo);
581 rTeij = rRej.
t() * rTeij;
586 b = eRc * cjTo - rReij * eRc * ciTo + rTeij;
609 AeTc = Ap * A.
t() * B;
614 printf(
"Old Hand-eye calibration : ");
615 std::cout <<
"Translation: " << eTc[0] <<
" " << eTc[1] <<
" " << eTc[2] << std::endl;
619 residual = A * AeTc - B;
622 for (
unsigned int i = 0; i < residual.
getRows(); i++)
623 res += residual[i] * residual[i];
624 res = sqrt(res / residual.
getRows());
625 printf(
"Mean residual (translation) = %lf\n", res);
643 double vpHandEyeCalibration::calibrationErrVVS(
const std::vector<vpHomogeneousMatrix> &cMo,
644 const std::vector<vpHomogeneousMatrix> &rMe,
647 unsigned int nbPose = (
unsigned int)cMo.size();
656 for (
unsigned int i = 0; i < nbPose; i++) {
657 for (
unsigned int j = 0; j < nbPose; j++) {
684 s = (
vpMatrix(ejRei) - I3) * eTc - eRc * cjTci + ejTei;
690 double resRot, resTrans, resPos;
691 resRot = resTrans = resPos = 0.0;
692 for (
unsigned int i = 0; i < (
unsigned int)errVVS.
size(); i += 6) {
693 resRot += errVVS[i] * errVVS[i];
694 resRot += errVVS[i + 1] * errVVS[i + 1];
695 resRot += errVVS[i + 2] * errVVS[i + 2];
696 resTrans += errVVS[i + 3] * errVVS[i + 3];
697 resTrans += errVVS[i + 4] * errVVS[i + 4];
698 resTrans += errVVS[i + 5] * errVVS[i + 5];
700 resPos = resRot + resTrans;
701 resRot = sqrt(resRot * 2 / errVVS.
size());
702 resTrans = sqrt(resTrans * 2 / errVVS.
size());
703 resPos = sqrt(resPos / errVVS.
size());
706 printf(
"Mean VVS residual - rotation (deg) = %lf\n",
vpMath::deg(resRot));
707 printf(
"Mean VVS residual - translation = %lf\n", resTrans);
708 printf(
"Mean VVS residual - global = %lf\n", resPos);
724 #define NB_ITER_MAX 30 726 int vpHandEyeCalibration::calibrationVVS(
const std::vector<vpHomogeneousMatrix> &cMo,
731 unsigned int nbPose = (
unsigned int)cMo.size();
746 while ((res > 1e-7) && (it < NB_ITER_MAX)) {
748 vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, err);
751 for (
unsigned int i = 0; i < nbPose; i++) {
752 for (
unsigned int j = 0; j < nbPose; j++) {
755 vpMatrix Ls(3, 6), Lv(3, 3), Lw(3, 3);
771 for (
unsigned int m = 0; m < 3; m++)
772 for (
unsigned int n = 0; n < 3; n++) {
774 Ls[m][n + 3] = Lw[m][n];
785 for (
unsigned int m = 0; m < 3; m++)
786 for (
unsigned int n = 0; n < 3; n++) {
788 Ls[m][n + 3] = Lw[m][n];
812 printf(
" Iteration number for NL hand-eye minimisation : %d\n", it);
816 std::cout <<
"Translation: " << eTc[0] <<
" " << eTc[1] <<
" " << eTc[2] << std::endl;
819 res = sqrt(res / err.getRows());
820 printf(
"Mean residual (rotation+translation) = %lf\n", res);
823 if (it == NB_ITER_MAX)
832 #define HE_TSAI_OROT 1 833 #define HE_TSAI_ORNT 2 834 #define HE_TSAI_NROT 3 835 #define HE_TSAI_NRNT 4 836 #define HE_PROCRUSTES_OT 5 837 #define HE_PROCRUSTES_NT 6 856 if (cMo.size() != rMe.size())
866 resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
867 double vmin = resPos;
869 int He_method = HE_I;
873 int err = vpHandEyeCalibration::calibrationRotationTsaiOld(cMo, rMe, eRc);
875 printf(
"\n Problem in solving Hand-Eye Rotation by Old Tsai method \n");
878 err = vpHandEyeCalibration::calibrationTranslationOld(cMo, rMe, eRc, eTc);
880 printf(
"\n Problem in solving Hand-Eye Translation by Old Tsai method after Old Tsai method for Rotation\n");
885 printf(
"\nRotation by (old) Tsai, old implementation for translation\n");
886 vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
889 resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
894 He_method = HE_TSAI_OROT;
898 err = vpHandEyeCalibration::calibrationTranslation(cMo, rMe, eRc, eTc);
900 printf(
"\n Problem in solving Hand-Eye Translation after Old Tsai method for Rotation\n");
905 printf(
"\nRotation by (old) Tsai, new implementation for translation\n");
906 vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
909 resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
914 He_method = HE_TSAI_ORNT;
920 err = vpHandEyeCalibration::calibrationRotationTsai(cMo, rMe, eRc);
922 printf(
"\n Problem in solving Hand-Eye Rotation by Tsai method \n");
925 err = vpHandEyeCalibration::calibrationTranslationOld(cMo, rMe, eRc, eTc);
927 printf(
"\n Problem in solving Hand-Eye Translation by Old Tsai method after Tsai method for Rotation\n");
932 printf(
"\nRotation by Tsai, old implementation for translation\n");
933 vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
936 resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
941 He_method = HE_TSAI_NROT;
945 err = vpHandEyeCalibration::calibrationTranslation(cMo, rMe, eRc, eTc);
947 printf(
"\n Problem in solving Hand-Eye Translation after Tsai method for Rotation \n");
952 printf(
"\nRotation by Tsai, new implementation for translation\n");
953 vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
956 resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
961 He_method = HE_TSAI_NRNT;
967 err = vpHandEyeCalibration::calibrationRotationProcrustes(cMo, rMe, eRc);
969 printf(
"\n Problem in solving Hand-Eye Rotation by Procrustes method \n");
972 err = vpHandEyeCalibration::calibrationTranslationOld(cMo, rMe, eRc, eTc);
974 printf(
"\n Problem in solving Hand-Eye Translation by Old Tsai method after Procrustes method for Rotation\n");
979 printf(
"\nRotation by Procrustes, old implementation for translation\n");
980 vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
983 resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
988 He_method = HE_PROCRUSTES_OT;
992 err = vpHandEyeCalibration::calibrationTranslation(cMo, rMe, eRc, eTc);
994 printf(
"\n Problem in solving Hand-Eye Translation after Procrustes method for Rotation\n");
999 printf(
"\nRotation by Procrustes, new implementation for translation\n");
1000 vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
1003 resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
1004 if (resPos < vmin) {
1008 He_method = HE_PROCRUSTES_NT;
1018 if (He_method == HE_I)
1019 printf(
"Best method : I !!!, vmin = %lf\n", vmin);
1020 if (He_method == HE_TSAI_OROT)
1021 printf(
"Best method : TSAI_OROT, vmin = %lf\n", vmin);
1022 if (He_method == HE_TSAI_ORNT)
1023 printf(
"Best method : TSAI_ORNT, vmin = %lf\n", vmin);
1024 if (He_method == HE_TSAI_NROT)
1025 printf(
"Best method : TSAI_NROT, vmin = %lf\n", vmin);
1026 if (He_method == HE_TSAI_NRNT)
1027 printf(
"Best method : TSAI_NRNT, vmin = %lf\n", vmin);
1028 if (He_method == HE_PROCRUSTES_OT)
1029 printf(
"Best method : PROCRUSTES_OT, vmin = %lf\n", vmin);
1030 if (He_method == HE_PROCRUSTES_NT)
1031 printf(
"Best method : PROCRUSTES_NT, vmin = %lf\n", vmin);
1035 std::cout <<
"Translation: " << eMc[0][3] <<
" " << eMc[1][3] <<
" " << eMc[2][3] << std::endl;
1040 err = vpHandEyeCalibration::calibrationVVS(cMo, rMe, eMc);
1043 printf(
"\n Problem in solving Hand-Eye Calibration by VVS \n");
1045 printf(
"\nRotation and translation after VVS\n");
1046 vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
1056 #undef HE_PROCRUSTES_OT 1057 #undef HE_PROCRUSTES_NT Implementation of a matrix and operations on matrices.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
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 stack(const vpMatrix &A)
error that can be emited by ViSP classes.
unsigned int getRows() const
vpHomogeneousMatrix inverse() const
unsigned int size() const
Return the number of elements of the 2D array.
void extract(vpRotationMatrix &R) const
vpRotationMatrix t() const
static double sinc(double x)
Implementation of a rotation matrix and operations on such kind of matrices.
unsigned int getCols() const
void insert(const vpRotationMatrix &R)
static vpRotationMatrix mean(const std::vector< vpHomogeneousMatrix > &vec_M)
static vpTranslationVector mean(const std::vector< vpHomogeneousMatrix > &vec_M)
vpTranslationVector getTranslationVector() const
static double deg(double rad)
Implementation of column vector and the associated operations.
static vpHomogeneousMatrix direct(const vpColVector &v)
static vpMatrix skew(const vpColVector &v)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
vpRotationMatrix getRotationMatrix() const