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