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];
797 int rank = L.pseudoInverse(Lp);
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
unsigned int getCols() const
unsigned int size() const
Return the number of elements of the 2D array.
unsigned int getRows() const
Implementation of column vector and the associated operations.
static vpMatrix skew(const vpColVector &v)
error that can be emited by ViSP classes.
@ dimensionError
Bad dimension.
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
vpTranslationVector getTranslationVector() const
void extract(vpRotationMatrix &R) const
void insert(const vpRotationMatrix &R)
static double sinc(double x)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
void stack(const vpMatrix &A)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix t() const
static vpRotationMatrix mean(const std::vector< vpHomogeneousMatrix > &vec_M)
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
static vpTranslationVector mean(const std::vector< vpHomogeneousMatrix > &vec_M)