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