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