Visual Servoing Platform  version 3.6.1 under development (2024-12-13)
vpHandEyeCalibration.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Hand-eye calibration.
32  */
33 
34 #include <cmath> // std::fabs
35 #include <limits> // numeric_limits
36 
37 #include <visp3/vision/vpHandEyeCalibration.h>
38 
39 #define DEBUG_LEVEL1 0
40 #define DEBUG_LEVEL2 0
41 
42 BEGIN_VISP_NAMESPACE
43 
54 void vpHandEyeCalibration::calibrationVerifrMo(const std::vector<vpHomogeneousMatrix> &cMo,
55  const std::vector<vpHomogeneousMatrix> &rMe,
56  const vpHomogeneousMatrix &eMc)
57 {
58  unsigned int nbPose = (unsigned int)cMo.size();
59  std::vector<vpTranslationVector> rTo(nbPose);
60  std::vector<vpRotationMatrix> rRo(nbPose);
61 
62  for (unsigned int i = 0; i < nbPose; i++) {
63  vpHomogeneousMatrix rMo = rMe[i] * eMc * cMo[i];
64  rRo[i] = rMo.getRotationMatrix();
65  rTo[i] = rMo.getTranslationVector();
66  }
69 
70 #if DEBUG_LEVEL2
71  {
72  std::cout << "Mean " << std::endl;
73  std::cout << "Translation: " << meanTrans.t() << std::endl;
74  vpThetaUVector P(meanRot);
75  std::cout << "Rotation : theta (deg) = " << vpMath::deg(sqrt(P.sumSquare())) << " Matrice : " << std::endl
76  << meanRot << std::endl;
77  std::cout << "theta U (deg): " << vpMath::deg(P[0]) << " " << vpMath::deg(P[1]) << " " << vpMath::deg(P[2])
78  << std::endl;
79  }
80 #endif
81 
82  // standard deviation, rotational part
83  double resRot = 0.0;
84  for (unsigned int i = 0; i < nbPose; i++) {
85  vpRotationMatrix R = meanRot.t() * rRo[i]; // Rm^T Ri
86  vpThetaUVector P(R);
87  // theta = Riemannian distance d(Rm,Ri)
88  double theta = sqrt(P.sumSquare());
89  std::cout << "Distance theta between rMo(" << i << ") and mean (deg) = " << vpMath::deg(theta) << std::endl;
90  // Euclidean distance d(Rm,Ri) not used
91  // theta = 2.0*sqrt(2.0)*sin(theta/2.0);
92  resRot += theta * theta;
93  }
94  resRot = sqrt(resRot / nbPose);
95  std::cout << "Mean residual rMo(" << nbPose << ") - rotation (deg) = " << vpMath::deg(resRot) << std::endl;
96  // standard deviation, translational part
97  double resTrans = 0.0;
98  for (unsigned int i = 0; i < nbPose; i++) {
99  vpColVector errTrans = vpColVector(rTo[i] - meanTrans);
100  resTrans += errTrans.sumSquare();
101  std::cout << "Distance d between rMo(" << i << ") and mean (m) = " << sqrt(errTrans.sumSquare()) << std::endl;
102  }
103  resTrans = sqrt(resTrans / nbPose);
104  std::cout << "Mean residual rMo(" << nbPose << ") - translation (m) = " << resTrans << std::endl;
105  double resPos = (resRot * resRot + resTrans * resTrans) * nbPose;
106  resPos = sqrt(resPos / (2 * nbPose));
107  std::cout << "Mean residual rMo(" << nbPose << ") - global = " << resPos << std::endl;
108 }
109 
121 int vpHandEyeCalibration::calibrationRotationProcrustes(const std::vector<vpHomogeneousMatrix> &cMo,
122  const std::vector<vpHomogeneousMatrix> &rMe,
123  vpRotationMatrix &eRc)
124 {
125  // Method by solving the orthogonal Procrustes problem
126  // [... (theta u)_e ...] = eRc [ ... (theta u)_c ...]
127  // similar to E^T = eRc C^T below
128 
129  vpMatrix Et, Ct;
130  vpMatrix A;
131  unsigned int k = 0;
132  unsigned int nbPose = (unsigned int)cMo.size();
133 
134  // for all couples ij
135  for (unsigned int i = 0; i < nbPose; i++) {
136  vpRotationMatrix rRei, ciRo;
137  rMe[i].extract(rRei);
138  cMo[i].extract(ciRo);
139  // std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
140 
141  for (unsigned int j = 0; j < nbPose; j++) {
142  if (j > i) // we don't use two times same couples...
143  {
144  vpRotationMatrix rRej, cjRo;
145  rMe[j].extract(rRej);
146  cMo[j].extract(cjRo);
147  // std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
148 
149  vpRotationMatrix ejRei = rRej.t() * rRei;
150  vpThetaUVector ejPei(ejRei);
151  vpColVector xe = vpColVector(ejPei);
152 
153  vpRotationMatrix cjRci = cjRo * ciRo.t();
154  vpThetaUVector cjPci(cjRci);
155  vpColVector xc = vpColVector(cjPci);
156 
157  if (k == 0) {
158  Et = xe.t();
159  Ct = xc.t();
160  }
161  else {
162  Et.stack(xe.t());
163  Ct.stack(xc.t());
164  }
165  k++;
166  }
167  }
168  }
169  // std::cout << "Et " << std::endl << Et << std::endl;
170  // std::cout << "Ct " << std::endl << Ct << std::endl;
171 
172  // R obtained from the SVD of (E C^T) with all singular values equal to 1
173  A = Et.t() * Ct;
174  vpMatrix M, U, V;
175  vpColVector sv;
176  int rank = A.pseudoInverse(M, sv, 1e-6, U, V);
177  if (rank != 3)
178  return -1;
179  A = U * V.t();
180  eRc = vpRotationMatrix(A);
181 
182 #if DEBUG_LEVEL2
183  {
184  vpThetaUVector ePc(eRc);
185  std::cout << "Rotation from Procrustes method " << std::endl;
186  std::cout << "theta U (deg): " << vpMath::deg(ePc[0]) << " " << vpMath::deg(ePc[1]) << " " << vpMath::deg(ePc[2])
187  << std::endl;
188  // Residual
189  vpMatrix residual;
190  residual = A * Ct.t() - Et.t();
191  // std::cout << "Residual: " << std::endl << residual << std::endl;
192  double res = sqrt(residual.sumSquare() / (residual.getRows() * residual.getCols()));
193  printf("Mean residual (rotation) = %lf\n", res);
194  }
195 #endif
196  return 0;
197 }
198 
211 int vpHandEyeCalibration::calibrationRotationTsai(const std::vector<vpHomogeneousMatrix> &cMo,
212  const std::vector<vpHomogeneousMatrix> &rMe, vpRotationMatrix &eRc)
213 {
214  vpMatrix A;
215  vpColVector B;
216  unsigned int nbPose = (unsigned int)cMo.size();
217  unsigned int k = 0;
218  // for all couples ij
219  for (unsigned int i = 0; i < nbPose; i++) {
220  vpRotationMatrix rRei, ciRo;
221  rMe[i].extract(rRei);
222  cMo[i].extract(ciRo);
223  // std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
224 
225  for (unsigned int j = 0; j < nbPose; j++) {
226  if (j > i) // we don't use two times same couples...
227  {
228  vpRotationMatrix rRej, cjRo;
229  rMe[j].extract(rRej);
230  cMo[j].extract(cjRo);
231  // std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
232 
233  vpRotationMatrix ejRei = rRej.t() * rRei;
234  vpThetaUVector ejPei(ejRei);
235 
236  vpRotationMatrix cjRci = cjRo * ciRo.t();
237  vpThetaUVector cjPci(cjRci);
238  // std::cout << "theta U (camera) " << cjPci.t() << std::endl;
239 
240  vpMatrix As;
241  vpColVector b(3);
242 
243  As = vpColVector::skew(vpColVector(ejPei) + vpColVector(cjPci));
244 
245  b = (vpColVector)cjPci - (vpColVector)ejPei; // A.40
246 
247  if (k == 0) {
248  A = As;
249  B = b;
250  }
251  else {
252  A = vpMatrix::stack(A, As);
253  B = vpColVector::stack(B, b);
254  }
255  k++;
256  }
257  }
258  }
259 #if DEBUG_LEVEL2
260  {
261  std::cout << "Tsai method: system A X = B " << std::endl;
262  std::cout << "A " << std::endl << A << std::endl;
263  std::cout << "B " << std::endl << B << std::endl;
264  }
265 #endif
266  vpMatrix Ap;
267  // the linear system A x = B is solved
268  // using x = A^+ B
269 
270  int rank = A.pseudoInverse(Ap);
271  if (rank != 3)
272  return -1;
273 
274  vpColVector x = Ap * B;
275 
276  // extraction of theta U
277 
278  // x = tan(theta/2) U
279  double norm = x.sumSquare();
280  double c = 1 / sqrt(1 + norm); // cos(theta/2)
281  double alpha = acos(c); // theta/2
282  norm = 2.0 * c / vpMath::sinc(alpha); // theta / tan(theta/2)
283  for (unsigned int i = 0; i < 3; i++)
284  x[i] *= norm;
285 
286  // Building of the rotation matrix eRc
287  vpThetaUVector xP(x[0], x[1], x[2]);
288  eRc = vpRotationMatrix(xP);
289 
290 #if DEBUG_LEVEL2
291  {
292  std::cout << "Rotation from Tsai method" << std::endl;
293  std::cout << "theta U (deg): " << vpMath::deg(x[0]) << " " << vpMath::deg(x[1]) << " " << vpMath::deg(x[2])
294  << std::endl;
295  // Residual
296  for (unsigned int i = 0; i < 3; i++)
297  x[i] /= norm; /* original x */
298  vpColVector residual;
299  residual = A * x - B;
300  // std::cout << "Residual: " << std::endl << residual << std::endl;
301  double res = sqrt(residual.sumSquare() / residual.getRows());
302  printf("Mean residual (rotation) = %lf\n", res);
303  }
304 #endif
305  return 0;
306 }
307 
320 int vpHandEyeCalibration::calibrationRotationTsaiOld(const std::vector<vpHomogeneousMatrix> &cMo,
321  const std::vector<vpHomogeneousMatrix> &rMe, vpRotationMatrix &eRc)
322 {
323  unsigned int nbPose = (unsigned int)cMo.size();
324  vpMatrix A;
325  vpColVector B;
326  vpColVector x;
327  unsigned int k = 0;
328  // for all couples ij
329  for (unsigned int i = 0; i < nbPose; i++) {
330  vpRotationMatrix rRei, ciRo;
331  rMe[i].extract(rRei);
332  cMo[i].extract(ciRo);
333  // std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
334 
335  for (unsigned int j = 0; j < nbPose; j++) {
336  if (j > i) { // we don't use two times same couples...
337  vpRotationMatrix rRej, cjRo;
338  rMe[j].extract(rRej);
339  cMo[j].extract(cjRo);
340  // std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
341 
342  vpRotationMatrix rReij = rRej.t() * rRei;
343 
344  vpRotationMatrix cijRo = cjRo * ciRo.t();
345 
346  vpThetaUVector rPeij(rReij);
347 
348  double theta = sqrt(rPeij[0] * rPeij[0] + rPeij[1] * rPeij[1] + rPeij[2] * rPeij[2]);
349 
350  // std::cout << i << " " << j << " " << "ejRei: " << std::endl << rReij << std::endl;
351  // std::cout << "theta (robot) " << theta << std::endl;
352  // std::cout << "theta U (robot) " << rPeij << std::endl;
353  // std::cout << "cjRci: " << std::endl << cijRo.t() << std::endl;
354 
355  for (unsigned int m = 0; m < 3; m++) {
356  rPeij[m] = rPeij[m] * vpMath::sinc(theta / 2);
357  }
358 
359  vpThetaUVector cijPo(cijRo);
360  theta = sqrt(cijPo[0] * cijPo[0] + cijPo[1] * cijPo[1] + cijPo[2] * cijPo[2]);
361  for (unsigned int m = 0; m < 3; m++) {
362  cijPo[m] = cijPo[m] * vpMath::sinc(theta / 2);
363  }
364 
365  // std::cout << "theta (camera) " << theta << std::endl;
366  // std::cout << "theta U (camera) " << cijPo.t() << std::endl;
367 
368  vpMatrix As;
369  vpColVector b(3);
370 
371  As = vpColVector::skew(vpColVector(rPeij) + vpColVector(cijPo));
372 
373  b = (vpColVector)cijPo - (vpColVector)rPeij; // A.40
374 
375  if (k == 0) {
376  A = As;
377  B = b;
378  }
379  else {
380  A = vpMatrix::stack(A, As);
381  B = vpColVector::stack(B, b);
382  }
383  k++;
384  }
385  }
386  }
387 
388  // std::cout << "A " << std::endl << A << std::endl;
389  // std::cout << "B " << std::endl << B << std::endl;
390 
391  // the linear system is defined
392  // x = AtA^-1AtB is solved
393  vpMatrix AtA = A.AtA();
394 
395  vpMatrix Ap;
396  int rank = AtA.pseudoInverse(Ap, 1e-6);
397  if (rank != 3)
398  return -1;
399 
400  x = Ap * A.t() * B;
401  vpColVector x2 = x; /* pour calcul residu */
402 
403  // extraction of theta and U
404  double theta;
405  double d = x.sumSquare();
406  for (unsigned int i = 0; i < 3; i++)
407  x[i] = 2 * x[i] / sqrt(1 + d);
408  theta = sqrt(x.sumSquare()) / 2;
409  theta = 2 * asin(theta);
410  // if (theta !=0)
411  if (std::fabs(theta) > std::numeric_limits<double>::epsilon()) {
412  for (unsigned int i = 0; i < 3; i++)
413  x[i] *= theta / (2 * sin(theta / 2));
414  }
415  else
416  x = 0;
417 
418  // Building of the rotation matrix eRc
419  vpThetaUVector xP(x[0], x[1], x[2]);
420  eRc = vpRotationMatrix(xP);
421 
422 #if DEBUG_LEVEL2
423  {
424  std::cout << "Rotation from Old Tsai method" << std::endl;
425  std::cout << "theta U (deg): " << vpMath::deg(x[0]) << " " << vpMath::deg(x[1]) << " " << vpMath::deg(x[2])
426  << std::endl;
427  // Residual
428  vpColVector residual;
429  residual = A * x2 - B;
430  // std::cout << "Residual: " << std::endl << residual << std::endl;
431  double res = sqrt(residual.sumSquare() / residual.getRows());
432  printf("Mean residual (rotation) = %lf\n", res);
433  }
434 #endif
435  return 0;
436 }
437 
451 int vpHandEyeCalibration::calibrationTranslation(const std::vector<vpHomogeneousMatrix> &cMo,
452  const std::vector<vpHomogeneousMatrix> &rMe, vpRotationMatrix &eRc,
453  vpTranslationVector &eTc)
454 {
455  vpMatrix I3(3, 3);
456  I3.eye();
457  unsigned int k = 0;
458  unsigned int nbPose = (unsigned int)cMo.size();
459  vpMatrix A(3 * nbPose, 3);
460  vpColVector B(3 * nbPose);
461  // Building of the system for the translation estimation
462  // for all couples ij
463  for (unsigned int i = 0; i < nbPose; i++) {
464  for (unsigned int j = 0; j < nbPose; j++) {
465  if (j > i) { // we don't use two times same couples...
466  vpHomogeneousMatrix ejMei = rMe[j].inverse() * rMe[i];
467  vpHomogeneousMatrix cjMci = cMo[j] * cMo[i].inverse();
468 
469  vpRotationMatrix ejRei, cjRci;
470  vpTranslationVector ejTei, cjTci;
471 
472  ejMei.extract(ejRei);
473  ejMei.extract(ejTei);
474 
475  cjMci.extract(cjRci);
476  cjMci.extract(cjTci);
477 
478  vpMatrix a = vpMatrix(ejRei) - I3;
479  vpTranslationVector b = eRc * cjTci - ejTei;
480 
481  if (k == 0) {
482  A = a;
483  B = b;
484  }
485  else {
486  A = vpMatrix::stack(A, a);
487  B = vpColVector::stack(B, vpColVector(b));
488  }
489  k++;
490  }
491  }
492  }
493 
494  // the linear system A x = B is solved
495  // using x = A^+ B
496  vpMatrix Ap;
497  int rank = A.pseudoInverse(Ap);
498  if (rank != 3)
499  return -1;
500 
501  vpColVector x = Ap * B;
502  eTc = (vpTranslationVector)x;
503 
504 #if DEBUG_LEVEL2
505  {
506  printf("New Hand-eye calibration : ");
507  std::cout << "Translation: " << eTc[0] << " " << eTc[1] << " " << eTc[2] << std::endl;
508  // residual
509  vpColVector residual;
510  residual = A * x - B;
511  // std::cout << "Residual: " << std::endl << residual << std::endl;
512  double res = sqrt(residual.sumSquare() / residual.getRows());
513  printf("Mean residual (translation) = %lf\n", res);
514  }
515 #endif
516  return 0;
517 }
518 
532 int vpHandEyeCalibration::calibrationTranslationOld(const std::vector<vpHomogeneousMatrix> &cMo,
533  const std::vector<vpHomogeneousMatrix> &rMe, vpRotationMatrix &eRc,
534  vpTranslationVector &eTc)
535 {
536  vpMatrix A;
537  vpColVector B;
538  // Building of the system for the translation estimation
539  // for all couples ij
540  vpRotationMatrix I3;
541  I3.eye();
542  int k = 0;
543  unsigned int nbPose = (unsigned int)cMo.size();
544 
545  for (unsigned int i = 0; i < nbPose; i++) {
546  vpRotationMatrix rRei, ciRo;
547  vpTranslationVector rTei, ciTo;
548  rMe[i].extract(rRei);
549  cMo[i].extract(ciRo);
550  rMe[i].extract(rTei);
551  cMo[i].extract(ciTo);
552 
553  for (unsigned int j = 0; j < nbPose; j++) {
554  if (j > i) // we don't use two times same couples...
555  {
556 
557  vpRotationMatrix rRej, cjRo;
558  rMe[j].extract(rRej);
559  cMo[j].extract(cjRo);
560 
561  vpTranslationVector rTej, cjTo;
562  rMe[j].extract(rTej);
563  cMo[j].extract(cjTo);
564 
565  vpRotationMatrix rReij = rRej.t() * rRei;
566 
567  vpTranslationVector rTeij = rTej + (-rTei);
568 
569  rTeij = rRej.t() * rTeij;
570 
571  vpMatrix a = vpMatrix(rReij) - vpMatrix(I3);
572 
574  b = eRc * cjTo - rReij * eRc * ciTo + rTeij;
575 
576  if (k == 0) {
577  A = a;
578  B = b;
579  }
580  else {
581  A = vpMatrix::stack(A, a);
582  B = vpColVector::stack(B, vpColVector(b));
583  }
584  k++;
585  }
586  }
587  }
588 
589  // the linear system is solved
590  // x = AtA^-1AtB is solved
591  vpMatrix AtA = A.AtA();
592  vpMatrix Ap;
593  vpColVector AeTc;
594  int rank = AtA.pseudoInverse(Ap, 1e-6);
595  if (rank != 3)
596  return -1;
597 
598  AeTc = Ap * A.t() * B;
599  eTc = (vpTranslationVector)AeTc;
600 
601 #if DEBUG_LEVEL2
602  {
603  printf("Old Hand-eye calibration : ");
604  std::cout << "Translation: " << eTc[0] << " " << eTc[1] << " " << eTc[2] << std::endl;
605 
606  // residual
607  vpColVector residual;
608  residual = A * AeTc - B;
609  // std::cout << "Residual: " << std::endl << residual << std::endl;
610  double res = 0;
611  for (unsigned int i = 0; i < residual.getRows(); i++)
612  res += residual[i] * residual[i];
613  res = sqrt(res / residual.getRows());
614  printf("Mean residual (translation) = %lf\n", res);
615  }
616 #endif
617  return 0;
618 }
619 
632 double vpHandEyeCalibration::calibrationErrVVS(const std::vector<vpHomogeneousMatrix> &cMo,
633  const std::vector<vpHomogeneousMatrix> &rMe,
634  const vpHomogeneousMatrix &eMc, vpColVector &errVVS)
635 {
636  unsigned int nbPose = (unsigned int)cMo.size();
637  vpMatrix I3(3, 3);
638  I3.eye();
639  vpRotationMatrix eRc;
641  eMc.extract(eRc);
642  eMc.extract(eTc);
643 
644  unsigned int k = 0;
645  for (unsigned int i = 0; i < nbPose; i++) {
646  for (unsigned int j = 0; j < nbPose; j++) {
647  if (j > i) // we don't use two times same couples...
648  {
649  vpColVector s(3);
650 
651  vpHomogeneousMatrix ejMei = rMe[j].inverse() * rMe[i];
652  vpHomogeneousMatrix cjMci = cMo[j] * cMo[i].inverse();
653 
654  vpRotationMatrix ejRei, cjRci;
655  vpTranslationVector ejTei, cjTci;
656 
657  ejMei.extract(ejRei);
658  vpThetaUVector ejPei(ejRei);
659  ejMei.extract(ejTei);
660 
661  cjMci.extract(cjRci);
662  vpThetaUVector cjPci(cjRci);
663  cjMci.extract(cjTci);
664  // terms due to rotation
665  s = vpMatrix(eRc) * vpColVector(cjPci) - vpColVector(ejPei);
666  if (k == 0) {
667  errVVS = s;
668  }
669  else {
670  errVVS = vpColVector::stack(errVVS, s);
671  }
672  k++;
673  // terms due to translation
674  s = (vpMatrix(ejRei) - I3) * eTc - eRc * cjTci + ejTei;
675  errVVS = vpColVector::stack(errVVS, s);
676  } // enf if i > j
677  } // end for j
678  } // end for i
679 
680  double resRot, resTrans, resPos;
681  resRot = resTrans = resPos = 0.0;
682  for (unsigned int i = 0; i < (unsigned int)errVVS.size(); i += 6) {
683  resRot += errVVS[i] * errVVS[i];
684  resRot += errVVS[i + 1] * errVVS[i + 1];
685  resRot += errVVS[i + 2] * errVVS[i + 2];
686  resTrans += errVVS[i + 3] * errVVS[i + 3];
687  resTrans += errVVS[i + 4] * errVVS[i + 4];
688  resTrans += errVVS[i + 5] * errVVS[i + 5];
689  }
690  resPos = resRot + resTrans;
691  resRot = sqrt(resRot * 2 / errVVS.size());
692  resTrans = sqrt(resTrans * 2 / errVVS.size());
693  resPos = sqrt(resPos / errVVS.size());
694 #if DEBUG_LEVEL1
695  {
696  printf("Mean VVS residual - rotation (deg) = %lf\n", vpMath::deg(resRot));
697  printf("Mean VVS residual - translation = %lf\n", resTrans);
698  printf("Mean VVS residual - global = %lf\n", resPos);
699  }
700 #endif
701  return resPos;
702 }
703 
714 #define NB_ITER_MAX 30
715 
716 int vpHandEyeCalibration::calibrationVVS(const std::vector<vpHomogeneousMatrix> &cMo,
717  const std::vector<vpHomogeneousMatrix> &rMe, vpHomogeneousMatrix &eMc)
718 {
719  unsigned int it = 0;
720  double res = 1.0;
721  unsigned int nbPose = (unsigned int)cMo.size();
722  vpColVector err;
723  vpMatrix L;
724  vpMatrix I3(3, 3);
725  I3.eye();
726  vpRotationMatrix eRc;
728  eMc.extract(eRc);
729  eMc.extract(eTc);
730 
731  /* FC : on recalcule 2 fois tous les ejMei et cjMci a chaque iteration
732  alors qu'ils sont constants. Ce serait sans doute mieux de les
733  calculer une seule fois et de les stocker. Pourraient alors servir
734  dans les autres fonctions HandEye. A voir si vraiment interessant vu la
735  combinatoire. Idem pour les theta u */
736  while ((res > 1e-7) && (it < NB_ITER_MAX)) {
737  /* compute s - s^* */
738  vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, err);
739  /* compute L_s */
740  unsigned int k = 0;
741  for (unsigned int i = 0; i < nbPose; i++) {
742  for (unsigned int j = 0; j < nbPose; j++) {
743  if (j > i) // we don't use two times same couples...
744  {
745  vpMatrix Ls(3, 6), Lv(3, 3), Lw(3, 3);
746 
747  vpHomogeneousMatrix ejMei = rMe[j].inverse() * rMe[i];
748  vpHomogeneousMatrix cjMci = cMo[j] * cMo[i].inverse();
749 
750  vpRotationMatrix ejRei;
751  ejMei.extract(ejRei);
752  vpThetaUVector cjPci(cjMci);
753 
754  vpTranslationVector cjTci;
755 
756  cjMci.extract(cjTci);
757  // terms due to rotation
758  // Lv.diag(0.0); //
759  Lv = 0.0;
760  Lw = -vpMatrix(eRc) * vpColVector::skew(vpColVector(cjPci));
761  for (unsigned int m = 0; m < 3; m++)
762  for (unsigned int n = 0; n < 3; n++) {
763  Ls[m][n] = Lv[m][n];
764  Ls[m][n + 3] = Lw[m][n];
765  }
766  if (k == 0) {
767  L = Ls;
768  }
769  else {
770  L = vpMatrix::stack(L, Ls);
771  }
772  k++;
773  // terms due to translation
774  Lv = (vpMatrix(ejRei) - I3) * vpMatrix(eRc);
775  Lw = vpMatrix(eRc) * vpColVector::skew(vpColVector(cjTci));
776  for (unsigned int m = 0; m < 3; m++)
777  for (unsigned int n = 0; n < 3; n++) {
778  Ls[m][n] = Lv[m][n];
779  Ls[m][n + 3] = Lw[m][n];
780  }
781  L = vpMatrix::stack(L, Ls);
782 
783  } // enf if i > j
784  } // end for j
785  } // end for i
786  double lambda = 0.9;
787  vpMatrix Lp;
788  int rank = L.pseudoInverse(Lp);
789  if (rank != 6)
790  return -1;
791 
792  vpColVector e = Lp * err;
793  vpColVector v = -e * lambda;
794  // std::cout << "e: " << e.t() << std::endl;
795  eMc = eMc * vpExponentialMap::direct(v);
796  eMc.extract(eRc);
797  eMc.extract(eTc);
798  res = sqrt(v.sumSquare() / v.getRows());
799  it++;
800  } // end while
801 #if DEBUG_LEVEL2
802  {
803  printf(" Iteration number for NL hand-eye minimization : %d\n", it);
804  vpThetaUVector ePc(eRc);
805  std::cout << "theta U (deg): " << vpMath::deg(ePc[0]) << " " << vpMath::deg(ePc[1]) << " " << vpMath::deg(ePc[2])
806  << std::endl;
807  std::cout << "Translation: " << eTc[0] << " " << eTc[1] << " " << eTc[2] << std::endl;
808  // Residual
809  double res = err.sumSquare();
810  res = sqrt(res / err.getRows());
811  printf("Mean residual (rotation+translation) = %lf\n", res);
812  }
813 #endif
814  if (it == NB_ITER_MAX)
815  return 1; // VVS has not converged before NB_ITER_MAX
816  else
817  return 0;
818 }
819 
820 #undef NB_ITER_MAX
821 
822 #define HE_I 0
823 #define HE_TSAI_OROT 1
824 #define HE_TSAI_ORNT 2
825 #define HE_TSAI_NROT 3
826 #define HE_TSAI_NRNT 4
827 #define HE_PROCRUSTES_OT 5
828 #define HE_PROCRUSTES_NT 6
829 
830 int vpHandEyeCalibration::calibrate(const std::vector<vpHomogeneousMatrix> &cMo,
831  const std::vector<vpHomogeneousMatrix> &rMe, vpHomogeneousMatrix &eMc)
832 {
833  if (cMo.size() != rMe.size())
834  throw vpException(vpException::dimensionError, "cMo and rMe have different sizes");
835 
836  vpRotationMatrix eRc;
838  vpColVector errVVS;
839  double resPos;
840 
841  /* initialisation of eMc to I in case all other methods fail */
842  eMc.eye();
843  resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
844  double vmin = resPos; // will serve to determine the best method
845 #if DEBUG_LEVEL1
846  int He_method = HE_I; // will serve to know which is the best method
847 #endif
848  vpHomogeneousMatrix eMcMin = eMc; // best initial estimation for VSS
849  // Method using Old Tsai implementation
850  int err = vpHandEyeCalibration::calibrationRotationTsaiOld(cMo, rMe, eRc);
851  if (err != 0)
852  printf("\n Problem in solving Hand-Eye Rotation by Old Tsai method \n");
853  else {
854  eMc.insert(eRc);
855  err = vpHandEyeCalibration::calibrationTranslationOld(cMo, rMe, eRc, eTc);
856  if (err != 0)
857  printf("\n Problem in solving Hand-Eye Translation by Old Tsai method after Old Tsai method for Rotation\n");
858  else {
859  eMc.insert(eTc);
860 #if DEBUG_LEVEL1
861  {
862  printf("\nRotation by (old) Tsai, old implementation for translation\n");
863  vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
864  }
865 #endif
866  resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
867  if (resPos < vmin) {
868  vmin = resPos;
869  eMcMin = eMc;
870 #if DEBUG_LEVEL1
871  He_method = HE_TSAI_OROT;
872 #endif
873  }
874  }
875  err = vpHandEyeCalibration::calibrationTranslation(cMo, rMe, eRc, eTc);
876  if (err != 0)
877  printf("\n Problem in solving Hand-Eye Translation after Old Tsai method for Rotation\n");
878  else {
879  eMc.insert(eTc);
880 #if DEBUG_LEVEL1
881  {
882  printf("\nRotation by (old) Tsai, new implementation for translation\n");
883  vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
884  }
885 #endif
886  resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
887  if (resPos < vmin) {
888  vmin = resPos;
889  eMcMin = eMc;
890 #if DEBUG_LEVEL1
891  He_method = HE_TSAI_ORNT;
892 #endif
893  }
894  }
895  }
896  // First method using Tsai formulation
897  err = vpHandEyeCalibration::calibrationRotationTsai(cMo, rMe, eRc);
898  if (err != 0)
899  printf("\n Problem in solving Hand-Eye Rotation by Tsai method \n");
900  else {
901  eMc.insert(eRc);
902  err = vpHandEyeCalibration::calibrationTranslationOld(cMo, rMe, eRc, eTc);
903  if (err != 0)
904  printf("\n Problem in solving Hand-Eye Translation by Old Tsai method after Tsai method for Rotation\n");
905  else {
906  eMc.insert(eTc);
907 #if DEBUG_LEVEL1
908  {
909  printf("\nRotation by Tsai, old implementation for translation\n");
910  vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
911  }
912 #endif
913  resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
914  if (resPos < vmin) {
915  vmin = resPos;
916  eMcMin = eMc;
917 #if DEBUG_LEVEL1
918  He_method = HE_TSAI_NROT;
919 #endif
920  }
921  }
922  err = vpHandEyeCalibration::calibrationTranslation(cMo, rMe, eRc, eTc);
923  if (err != 0)
924  printf("\n Problem in solving Hand-Eye Translation after Tsai method for Rotation \n");
925  else {
926  eMc.insert(eTc);
927 #if DEBUG_LEVEL1
928  {
929  printf("\nRotation by Tsai, new implementation for translation\n");
930  vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
931  }
932 #endif
933  resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
934  if (resPos < vmin) {
935  vmin = resPos;
936  eMcMin = eMc;
937 #if DEBUG_LEVEL1
938  He_method = HE_TSAI_NRNT;
939 #endif
940  }
941  }
942  }
943  // Second method by solving the orthogonal Procrustes problem
944  err = vpHandEyeCalibration::calibrationRotationProcrustes(cMo, rMe, eRc);
945  if (err != 0)
946  printf("\n Problem in solving Hand-Eye Rotation by Procrustes method \n");
947  else {
948  eMc.insert(eRc);
949  err = vpHandEyeCalibration::calibrationTranslationOld(cMo, rMe, eRc, eTc);
950  if (err != 0)
951  printf("\n Problem in solving Hand-Eye Translation by Old Tsai method after Procrustes method for Rotation\n");
952  else {
953  eMc.insert(eTc);
954 #if DEBUG_LEVEL1
955  {
956  printf("\nRotation by Procrustes, old implementation for translation\n");
957  vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
958  }
959 #endif
960  resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
961  if (resPos < vmin) {
962  vmin = resPos;
963  eMcMin = eMc;
964 #if DEBUG_LEVEL1
965  He_method = HE_PROCRUSTES_OT;
966 #endif
967  }
968  }
969  err = vpHandEyeCalibration::calibrationTranslation(cMo, rMe, eRc, eTc);
970  if (err != 0)
971  printf("\n Problem in solving Hand-Eye Translation after Procrustes method for Rotation\n");
972  else {
973  eMc.insert(eTc);
974 #if DEBUG_LEVEL1
975  {
976  printf("\nRotation by Procrustes, new implementation for translation\n");
977  vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
978  }
979 #endif
980  resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
981  if (resPos < vmin) {
982  eMcMin = eMc;
983 #if DEBUG_LEVEL1
984  vmin = resPos;
985  He_method = HE_PROCRUSTES_NT;
986 #endif
987  }
988  }
989  }
990 
991  /* determination of the best method in case at least one succeeds */
992  eMc = eMcMin;
993 #if DEBUG_LEVEL1
994  {
995  if (He_method == HE_I)
996  printf("Best method : I !!!, vmin = %lf\n", vmin);
997  if (He_method == HE_TSAI_OROT)
998  printf("Best method : TSAI_OROT, vmin = %lf\n", vmin);
999  if (He_method == HE_TSAI_ORNT)
1000  printf("Best method : TSAI_ORNT, vmin = %lf\n", vmin);
1001  if (He_method == HE_TSAI_NROT)
1002  printf("Best method : TSAI_NROT, vmin = %lf\n", vmin);
1003  if (He_method == HE_TSAI_NRNT)
1004  printf("Best method : TSAI_NRNT, vmin = %lf\n", vmin);
1005  if (He_method == HE_PROCRUSTES_OT)
1006  printf("Best method : PROCRUSTES_OT, vmin = %lf\n", vmin);
1007  if (He_method == HE_PROCRUSTES_NT)
1008  printf("Best method : PROCRUSTES_NT, vmin = %lf\n", vmin);
1009  vpThetaUVector ePc(eMc);
1010  std::cout << "theta U (deg): " << vpMath::deg(ePc[0]) << " " << vpMath::deg(ePc[1]) << " " << vpMath::deg(ePc[2])
1011  << std::endl;
1012  std::cout << "Translation: " << eMc[0][3] << " " << eMc[1][3] << " " << eMc[2][3] << std::endl;
1013  }
1014 #endif
1015 
1016  // Non linear iterative minimization to estimate simultaneouslty eRc and eTc
1017  err = vpHandEyeCalibration::calibrationVVS(cMo, rMe, eMc);
1018  // FC : err : 0 si tout OK, -1 si pb de rang, 1 si pas convergence
1019  if (err != 0)
1020  printf("\n Problem in solving Hand-Eye Calibration by VVS \n");
1021  else {
1022  printf("\nRotation and translation after VVS\n");
1023  vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
1024  }
1025  return err;
1026 }
1027 
1028 END_VISP_NAMESPACE
1029 
1030 #undef HE_I
1031 #undef HE_TSAI_OROT
1032 #undef HE_TSAI_ORNT
1033 #undef HE_TSAI_NROT
1034 #undef HE_TSAI_NRNT
1035 #undef HE_PROCRUSTES_OT
1036 #undef HE_PROCRUSTES_NT
1037 
1038 #undef DEBUG_LEVEL1
1039 #undef DEBUG_LEVEL2
unsigned int getCols() const
Definition: vpArray2D.h:337
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
unsigned int getRows() const
Definition: vpArray2D.h:347
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
double sumSquare() const
void stack(double d)
static vpMatrix skew(const vpColVector &v)
vpRowVector t() const
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
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)
Definition: vpMath.cpp:268
static double deg(double rad)
Definition: vpMath.h:119
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
void stack(const vpMatrix &A)
vpMatrix AtA() const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
double sumSquare() const
Definition: vpMatrix.cpp:1971
vpMatrix t() 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)
vpRowVector t() const