Visual Servoing Platform version 3.7.0
Loading...
Searching...
No Matches
vpHandEyeCalibration.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2025 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
42BEGIN_VISP_NAMESPACE
43
56 void vpHandEyeCalibration::calibrationVerifrMo(const std::vector<vpHomogeneousMatrix> &cMo,
57 const std::vector<vpHomogeneousMatrix> &rMe,
58 const vpHomogeneousMatrix &eMc, vpHomogeneousMatrix &mean_rMo)
59{
60 unsigned int nbPose = static_cast<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 }
69 vpRotationMatrix meanRot = vpRotationMatrix::mean(rRo);
70 vpTranslationVector meanTrans = vpTranslationVector::mean(rTo);
71
72 mean_rMo.buildFrom(meanTrans, meanRot);
73
74#if DEBUG_LEVEL2
75 {
76 std::cout << "Mean rMo " << 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())) << " Matrix : " << 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/rMc(" << 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/rMc(" << 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/rMc(" << i << ") and mean (m) = " << sqrt(errTrans.sumSquare()) << std::endl;
106 }
107 resTrans = sqrt(resTrans / nbPose);
108 std::cout << "Mean residual rMo/rMc(" << 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/rMc(" << nbPose << ") - global = " << resPos << std::endl;
112}
113
124void vpHandEyeCalibration::calibrationVerifrMo(const std::vector<vpHomogeneousMatrix> &cMo,
125 const std::vector<vpHomogeneousMatrix> &rMe,
126 const vpHomogeneousMatrix &eMc)
127{
128 vpHomogeneousMatrix rMo;
129 rMo.eye();
130 vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc, rMo);
131}
143int vpHandEyeCalibration::calibrationRotationProcrustes(const std::vector<vpHomogeneousMatrix> &cMo,
144 const std::vector<vpHomogeneousMatrix> &rMe,
145 vpRotationMatrix &eRc)
146{
147 // Method by solving the orthogonal Procrustes problem
148 // [... (theta u)_e ...] = eRc [ ... (theta u)_c ...]
149 // similar to E^T = eRc C^T below
150
151 vpMatrix Et, Ct;
152 vpMatrix A;
153 unsigned int k = 0;
154 unsigned int nbPose = static_cast<unsigned int>(cMo.size());
155
156 // for all couples ij
157 for (unsigned int i = 0; i < nbPose; ++i) {
158 vpRotationMatrix rRei, ciRo;
159 rMe[i].extract(rRei);
160 cMo[i].extract(ciRo);
161 // std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
162
163 for (unsigned int j = 0; j < nbPose; ++j) {
164 if (j > i) // we don't use two times same couples...
165 {
166 vpRotationMatrix rRej, cjRo;
167 rMe[j].extract(rRej);
168 cMo[j].extract(cjRo);
169 // std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
170
171 vpRotationMatrix ejRei = rRej.t() * rRei;
172 vpThetaUVector ejPei(ejRei);
173 vpColVector xe = vpColVector(ejPei);
174
175 vpRotationMatrix cjRci = cjRo * ciRo.t();
176 vpThetaUVector cjPci(cjRci);
177 vpColVector xc = vpColVector(cjPci);
178
179 if (k == 0) {
180 Et = xe.t();
181 Ct = xc.t();
182 }
183 else {
184 Et.stack(xe.t());
185 Ct.stack(xc.t());
186 }
187 k++;
188 }
189 }
190 }
191 // std::cout << "Et " << std::endl << Et << std::endl;
192 // std::cout << "Ct " << std::endl << Ct << std::endl;
193
194 // R obtained from the SVD of (E C^T) with all singular values equal to 1
195 A = Et.t() * Ct;
196 vpMatrix M, U, V;
197 vpColVector sv;
198 unsigned int rank = A.pseudoInverse(M, sv, 1e-6, U, V);
199 if (rank != 3) {
200 return -1;
201 }
202 A = U * V.t();
203 eRc = vpRotationMatrix(A);
204
205#if DEBUG_LEVEL2
206 {
207 vpThetaUVector ePc(eRc);
208 std::cout << "Rotation from Procrustes method " << std::endl;
209 std::cout << "theta U (deg): " << vpMath::deg(ePc[0]) << " " << vpMath::deg(ePc[1]) << " " << vpMath::deg(ePc[2])
210 << std::endl;
211 // Residual
212 vpMatrix residual;
213 residual = A * Ct.t() - Et.t();
214 // std::cout << "Residual: " << std::endl << residual << std::endl;
215 double res = sqrt(residual.sumSquare() / (residual.getRows() * residual.getCols()));
216 printf("Mean residual (rotation) = %lf\n", res);
217 }
218#endif
219 return 0;
220}
221
234int vpHandEyeCalibration::calibrationRotationTsai(const std::vector<vpHomogeneousMatrix> &cMo,
235 const std::vector<vpHomogeneousMatrix> &rMe, vpRotationMatrix &eRc)
236{
237 vpMatrix A;
238 vpColVector B;
239 unsigned int nbPose = static_cast<unsigned int>(cMo.size());
240 unsigned int k = 0;
241 // for all couples ij
242 for (unsigned int i = 0; i < nbPose; ++i) {
243 vpRotationMatrix rRei, ciRo;
244 rMe[i].extract(rRei);
245 cMo[i].extract(ciRo);
246 // std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
247
248 for (unsigned int j = 0; j < nbPose; ++j) {
249 if (j > i) { // we don't use two times same couples...
250 vpRotationMatrix rRej, cjRo;
251 rMe[j].extract(rRej);
252 cMo[j].extract(cjRo);
253 // std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
254
255 vpRotationMatrix ejRei = rRej.t() * rRei;
256 vpThetaUVector ejPei(ejRei);
257
258 vpRotationMatrix cjRci = cjRo * ciRo.t();
259 vpThetaUVector cjPci(cjRci);
260 // std::cout << "theta U (camera) " << cjPci.t() << std::endl;
261
262 vpMatrix As;
263 vpColVector b(3);
264
265 As = vpColVector::skew(vpColVector(ejPei) + vpColVector(cjPci));
266
267 b = static_cast<vpColVector>(cjPci) - static_cast<vpColVector>(ejPei); // A.40
268
269 if (k == 0) {
270 A = As;
271 B = b;
272 }
273 else {
274 A = vpMatrix::stack(A, As);
275 B = vpColVector::stack(B, b);
276 }
277 k++;
278 }
279 }
280 }
281#if DEBUG_LEVEL2
282 {
283 std::cout << "Tsai method: system A X = B " << std::endl;
284 std::cout << "A " << std::endl << A << std::endl;
285 std::cout << "B " << std::endl << B << std::endl;
286 }
287#endif
288 vpMatrix Ap;
289 // the linear system A x = B is solved
290 // using x = A^+ B
291
292 unsigned int rank = A.pseudoInverse(Ap);
293 if (rank != 3) {
294 return -1;
295 }
296
297 vpColVector x = Ap * B;
298
299 // extraction of theta U
300
301 // x = tan(theta/2) U
302 double norm = x.sumSquare();
303 double c = 1 / sqrt(1 + norm); // cos(theta/2)
304 double alpha = acos(c); // theta/2
305 norm = 2.0 * c / vpMath::sinc(alpha); // theta / tan(theta/2)
306 for (unsigned int i = 0; i < 3; ++i) {
307 x[i] *= norm;
308 }
309
310 // Building of the rotation matrix eRc
311 vpThetaUVector xP(x[0], x[1], x[2]);
312 eRc = vpRotationMatrix(xP);
313
314#if DEBUG_LEVEL2
315 {
316 std::cout << "Rotation from Tsai method" << std::endl;
317 std::cout << "theta U (deg): " << vpMath::deg(x[0]) << " " << vpMath::deg(x[1]) << " " << vpMath::deg(x[2])
318 << std::endl;
319 // Residual
320 for (unsigned int i = 0; i < 3; ++i) {
321 x[i] /= norm; /* original x */
322 }
323 vpColVector residual;
324 residual = A * x - B;
325 // std::cout << "Residual: " << std::endl << residual << std::endl;
326 double res = sqrt(residual.sumSquare() / residual.getRows());
327 printf("Mean residual (rotation) = %lf\n", res);
328 }
329#endif
330 return 0;
331}
332
345int vpHandEyeCalibration::calibrationRotationTsaiOld(const std::vector<vpHomogeneousMatrix> &cMo,
346 const std::vector<vpHomogeneousMatrix> &rMe, vpRotationMatrix &eRc)
347{
348 unsigned int nbPose = static_cast<unsigned int>(cMo.size());
349 vpMatrix A;
350 vpColVector B;
351 vpColVector x;
352 unsigned int k = 0;
353 // for all couples ij
354 for (unsigned int i = 0; i < nbPose; ++i) {
355 vpRotationMatrix rRei, ciRo;
356 rMe[i].extract(rRei);
357 cMo[i].extract(ciRo);
358 // std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
359
360 for (unsigned int j = 0; j < nbPose; ++j) {
361 if (j > i) { // we don't use two times same couples...
362 vpRotationMatrix rRej, cjRo;
363 rMe[j].extract(rRej);
364 cMo[j].extract(cjRo);
365 // std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
366
367 vpRotationMatrix rReij = rRej.t() * rRei;
368
369 vpRotationMatrix cijRo = cjRo * ciRo.t();
370
371 vpThetaUVector rPeij(rReij);
372
373 double theta = sqrt(rPeij[0] * rPeij[0] + rPeij[1] * rPeij[1] + rPeij[2] * rPeij[2]);
374
375 // std::cout << i << " " << j << " " << "ejRei: " << std::endl << rReij << std::endl;
376 // std::cout << "theta (robot) " << theta << std::endl;
377 // std::cout << "theta U (robot) " << rPeij << std::endl;
378 // std::cout << "cjRci: " << std::endl << cijRo.t() << std::endl;
379
380 for (unsigned int m = 0; m < 3; m++) {
381 rPeij[m] = rPeij[m] * vpMath::sinc(theta / 2);
382 }
383
384 vpThetaUVector cijPo(cijRo);
385 theta = sqrt(cijPo[0] * cijPo[0] + cijPo[1] * cijPo[1] + cijPo[2] * cijPo[2]);
386 for (unsigned int m = 0; m < 3; m++) {
387 cijPo[m] = cijPo[m] * vpMath::sinc(theta / 2);
388 }
389
390 // std::cout << "theta (camera) " << theta << std::endl;
391 // std::cout << "theta U (camera) " << cijPo.t() << std::endl;
392
393 vpMatrix As;
394 vpColVector b(3);
395
396 As = vpColVector::skew(vpColVector(rPeij) + vpColVector(cijPo));
397
398 b = static_cast<vpColVector>(cijPo) - static_cast<vpColVector>(rPeij); // A.40
399
400 if (k == 0) {
401 A = As;
402 B = b;
403 }
404 else {
405 A = vpMatrix::stack(A, As);
406 B = vpColVector::stack(B, b);
407 }
408 k++;
409 }
410 }
411 }
412
413 // std::cout << "A " << std::endl << A << std::endl;
414 // std::cout << "B " << std::endl << B << std::endl;
415
416 // the linear system is defined
417 // x = AtA^-1AtB is solved
418 vpMatrix AtA = A.AtA();
419
420 vpMatrix Ap;
421 unsigned int rank = AtA.pseudoInverse(Ap, 1e-6);
422 if (rank != 3) {
423 return -1;
424 }
425
426 x = Ap * A.t() * B;
427 vpColVector x2 = x; // For residual computation
428
429 // extraction of theta and U
430 double theta;
431 double d = x.sumSquare();
432 for (unsigned int i = 0; i < 3; ++i) {
433 x[i] = 2 * x[i] / sqrt(1 + d);
434 }
435 theta = sqrt(x.sumSquare()) / 2;
436 theta = 2 * asin(theta);
437 // if (theta !=0)
438 if (std::fabs(theta) > std::numeric_limits<double>::epsilon()) {
439 for (unsigned int i = 0; i < 3; ++i) {
440 x[i] *= theta / (2 * sin(theta / 2));
441 }
442 }
443 else {
444 x = 0;
445 }
446
447 // Building of the rotation matrix eRc
448 vpThetaUVector xP(x[0], x[1], x[2]);
449 eRc = vpRotationMatrix(xP);
450
451#if DEBUG_LEVEL2
452 {
453 std::cout << "Rotation from Old Tsai method" << std::endl;
454 std::cout << "theta U (deg): " << vpMath::deg(x[0]) << " " << vpMath::deg(x[1]) << " " << vpMath::deg(x[2])
455 << std::endl;
456 // Residual
457 vpColVector residual;
458 residual = A * x2 - B;
459 // std::cout << "Residual: " << std::endl << residual << std::endl;
460 double res = sqrt(residual.sumSquare() / residual.getRows());
461 printf("Mean residual (rotation) = %lf\n", res);
462 }
463#endif
464 return 0;
465}
466
480int vpHandEyeCalibration::calibrationTranslation(const std::vector<vpHomogeneousMatrix> &cMo,
481 const std::vector<vpHomogeneousMatrix> &rMe, vpRotationMatrix &eRc,
482 vpTranslationVector &eTc)
483{
484 vpMatrix I3(3, 3);
485 I3.eye();
486 unsigned int k = 0;
487 unsigned int nbPose = static_cast<unsigned int>(cMo.size());
488 vpMatrix A(3 * nbPose, 3);
489 vpColVector B(3 * nbPose);
490 // Building of the system for the translation estimation
491 // for all couples ij
492 for (unsigned int i = 0; i < nbPose; ++i) {
493 for (unsigned int j = 0; j < nbPose; ++j) {
494 if (j > i) { // we don't use two times same couples...
495 vpHomogeneousMatrix ejMei = rMe[j].inverse() * rMe[i];
496 vpHomogeneousMatrix cjMci = cMo[j] * cMo[i].inverse();
497
498 vpRotationMatrix ejRei, cjRci;
499 vpTranslationVector ejTei, cjTci;
500
501 ejMei.extract(ejRei);
502 ejMei.extract(ejTei);
503
504 cjMci.extract(cjRci);
505 cjMci.extract(cjTci);
506
507 vpMatrix a = vpMatrix(ejRei) - I3;
508 vpTranslationVector b = eRc * cjTci - ejTei;
509
510 if (k == 0) {
511 A = a;
512 B = b;
513 }
514 else {
515 A = vpMatrix::stack(A, a);
516 B = vpColVector::stack(B, vpColVector(b));
517 }
518 k++;
519 }
520 }
521 }
522
523 // the linear system A x = B is solved
524 // using x = A^+ B
525 vpMatrix Ap;
526 unsigned int rank = A.pseudoInverse(Ap);
527 if (rank != 3) {
528 return -1;
529 }
530
531 vpColVector x = Ap * B;
532 eTc = static_cast<vpTranslationVector>(x);
533
534#if DEBUG_LEVEL2
535 {
536 printf("New Hand-eye calibration : ");
537 std::cout << "Translation: " << eTc[0] << " " << eTc[1] << " " << eTc[2] << std::endl;
538 // residual
539 vpColVector residual;
540 residual = A * x - B;
541 // std::cout << "Residual: " << std::endl << residual << std::endl;
542 double res = sqrt(residual.sumSquare() / residual.getRows());
543 printf("Mean residual (translation) = %lf\n", res);
544 }
545#endif
546 return 0;
547}
548
562int vpHandEyeCalibration::calibrationTranslationOld(const std::vector<vpHomogeneousMatrix> &cMo,
563 const std::vector<vpHomogeneousMatrix> &rMe, vpRotationMatrix &eRc,
564 vpTranslationVector &eTc)
565{
566 vpMatrix A;
567 vpColVector B;
568 // Building of the system for the translation estimation
569 // for all couples ij
570 vpRotationMatrix I3;
571 I3.eye();
572 int k = 0;
573 unsigned int nbPose = static_cast<unsigned int>(cMo.size());
574
575 for (unsigned int i = 0; i < nbPose; ++i) {
576 vpRotationMatrix rRei, ciRo;
577 vpTranslationVector rTei, ciTo;
578 rMe[i].extract(rRei);
579 cMo[i].extract(ciRo);
580 rMe[i].extract(rTei);
581 cMo[i].extract(ciTo);
582
583 for (unsigned int j = 0; j < nbPose; ++j) {
584 if (j > i) { // we don't use two times same couples...
585 vpRotationMatrix rRej, cjRo;
586 rMe[j].extract(rRej);
587 cMo[j].extract(cjRo);
588
589 vpTranslationVector rTej, cjTo;
590 rMe[j].extract(rTej);
591 cMo[j].extract(cjTo);
592
593 vpRotationMatrix rReij = rRej.t() * rRei;
594
595 vpTranslationVector rTeij = rTej + (-rTei);
596
597 rTeij = rRej.t() * rTeij;
598
599 vpMatrix a = vpMatrix(rReij) - vpMatrix(I3);
600
601 vpTranslationVector b;
602 b = eRc * cjTo - rReij * eRc * ciTo + rTeij;
603
604 if (k == 0) {
605 A = a;
606 B = b;
607 }
608 else {
609 A = vpMatrix::stack(A, a);
610 B = vpColVector::stack(B, vpColVector(b));
611 }
612 k++;
613 }
614 }
615 }
616
617 // the linear system is solved
618 // x = AtA^-1AtB is solved
619 vpMatrix AtA = A.AtA();
620 vpMatrix Ap;
621 vpColVector AeTc;
622 unsigned int rank = AtA.pseudoInverse(Ap, 1e-6);
623 if (rank != 3) {
624 return -1;
625 }
626
627 AeTc = Ap * A.t() * B;
628 eTc = static_cast<vpTranslationVector>(AeTc);
629
630#if DEBUG_LEVEL2
631 {
632 printf("Old Hand-eye calibration : ");
633 std::cout << "Translation: " << eTc[0] << " " << eTc[1] << " " << eTc[2] << std::endl;
634
635 // residual
636 vpColVector residual;
637 residual = A * AeTc - B;
638 // std::cout << "Residual: " << std::endl << residual << std::endl;
639 double res = 0;
640 for (unsigned int i = 0; i < residual.getRows(); ++i) {
641 res += residual[i] * residual[i];
642 }
643 res = sqrt(res / residual.getRows());
644 printf("Mean residual (translation) = %lf\n", res);
645 }
646#endif
647 return 0;
648}
649
661double vpHandEyeCalibration::calibrationErrVVS(const std::vector<vpHomogeneousMatrix> &cMo,
662 const std::vector<vpHomogeneousMatrix> &rMe,
663 const vpHomogeneousMatrix &eMc, vpColVector &errVVS)
664{
665 unsigned int nbPose = static_cast<unsigned int>(cMo.size());
666 vpMatrix I3(3, 3);
667 I3.eye();
668 vpRotationMatrix eRc;
669 vpTranslationVector eTc;
670 eMc.extract(eRc);
671 eMc.extract(eTc);
672
673 unsigned int k = 0;
674 for (unsigned int i = 0; i < nbPose; ++i) {
675 for (unsigned int j = 0; j < nbPose; ++j) {
676 if (j > i) { // we don't use two times same couples...
677 vpColVector s(3);
678
679 vpHomogeneousMatrix ejMei = rMe[j].inverse() * rMe[i];
680 vpHomogeneousMatrix cjMci = cMo[j] * cMo[i].inverse();
681
682 vpRotationMatrix ejRei, cjRci;
683 vpTranslationVector ejTei, cjTci;
684
685 ejMei.extract(ejRei);
686 vpThetaUVector ejPei(ejRei);
687 ejMei.extract(ejTei);
688
689 cjMci.extract(cjRci);
690 vpThetaUVector cjPci(cjRci);
691 cjMci.extract(cjTci);
692 // terms due to rotation
693 s = vpMatrix(eRc) * vpColVector(cjPci) - vpColVector(ejPei);
694 if (k == 0) {
695 errVVS = s;
696 }
697 else {
698 errVVS = vpColVector::stack(errVVS, s);
699 }
700 k++;
701 // terms due to translation
702 s = (vpMatrix(ejRei) - I3) * eTc - eRc * cjTci + ejTei;
703 errVVS = vpColVector::stack(errVVS, s);
704 } // enf if i > j
705 } // end for j
706 } // end for i
707
708 double resRot, resTrans, resPos;
709 resRot = resTrans = resPos = 0.0;
710 for (unsigned int i = 0; i <static_cast<unsigned int>(errVVS.size()); i += 6) {
711 resRot += errVVS[i] * errVVS[i];
712 resRot += errVVS[i + 1] * errVVS[i + 1];
713 resRot += errVVS[i + 2] * errVVS[i + 2];
714 resTrans += errVVS[i + 3] * errVVS[i + 3];
715 resTrans += errVVS[i + 4] * errVVS[i + 4];
716 resTrans += errVVS[i + 5] * errVVS[i + 5];
717 }
718 resPos = resRot + resTrans;
719 resRot = sqrt(resRot * 2 / errVVS.size());
720 resTrans = sqrt(resTrans * 2 / errVVS.size());
721 resPos = sqrt(resPos / errVVS.size());
722#if DEBUG_LEVEL1
723 {
724 printf("Mean VVS residual - rotation (deg) = %lf\n", vpMath::deg(resRot));
725 printf("Mean VVS residual - translation = %lf\n", resTrans);
726 printf("Mean VVS residual - global = %lf\n", resPos);
727 }
728#endif
729 return resPos;
730}
731
742int vpHandEyeCalibration::calibrationVVS(const std::vector<vpHomogeneousMatrix> &cMo,
743 const std::vector<vpHomogeneousMatrix> &rMe, vpHomogeneousMatrix &eMc)
744{
745 unsigned int it = 0;
746 unsigned int nb_iter_max = 30;
747 double res = 1.0;
748 unsigned int nbPose = static_cast<unsigned int>(cMo.size());
749 vpColVector err;
750 vpMatrix L;
751 vpMatrix I3(3, 3);
752 I3.eye();
753 vpRotationMatrix eRc;
754 vpTranslationVector eTc;
755 eMc.extract(eRc);
756 eMc.extract(eTc);
757
758 /* FC : on recalcule 2 fois tous les ejMei et cjMci a chaque iteration
759 alors qu'ils sont constants. Ce serait sans doute mieux de les
760 calculer une seule fois et de les stocker. Pourraient alors servir
761 dans les autres fonctions HandEye. A voir si vraiment interessant vu la
762 combinatoire. Idem pour les theta u */
763 while ((res > 1e-7) && (it < nb_iter_max)) {
764 /* compute s - s^* */
765 vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, err);
766 /* compute L_s */
767 unsigned int k = 0;
768 for (unsigned int i = 0; i < nbPose; ++i) {
769 for (unsigned int j = 0; j < nbPose; ++j) {
770 if (j > i) // we don't use two times same couples...
771 {
772 vpMatrix Ls(3, 6), Lv(3, 3), Lw(3, 3);
773
774 vpHomogeneousMatrix ejMei = rMe[j].inverse() * rMe[i];
775 vpHomogeneousMatrix cjMci = cMo[j] * cMo[i].inverse();
776
777 vpRotationMatrix ejRei;
778 ejMei.extract(ejRei);
779 vpThetaUVector cjPci(cjMci);
780
781 vpTranslationVector cjTci;
782
783 cjMci.extract(cjTci);
784 // terms due to rotation
785 // Lv.diag(0.0); //
786 Lv = 0.0;
787 Lw = -vpMatrix(eRc) * vpColVector::skew(vpColVector(cjPci));
788 for (unsigned int m = 0; m < 3; m++)
789 for (unsigned int n = 0; n < 3; n++) {
790 Ls[m][n] = Lv[m][n];
791 Ls[m][n + 3] = Lw[m][n];
792 }
793 if (k == 0) {
794 L = Ls;
795 }
796 else {
797 L = vpMatrix::stack(L, Ls);
798 }
799 k++;
800 // terms due to translation
801 Lv = (vpMatrix(ejRei) - I3) * vpMatrix(eRc);
802 Lw = vpMatrix(eRc) * vpColVector::skew(vpColVector(cjTci));
803 for (unsigned int m = 0; m < 3; m++)
804 for (unsigned int n = 0; n < 3; n++) {
805 Ls[m][n] = Lv[m][n];
806 Ls[m][n + 3] = Lw[m][n];
807 }
808 L = vpMatrix::stack(L, Ls);
809
810 } // enf if i > j
811 } // end for j
812 } // end for i
813 double lambda = 0.9;
814 vpMatrix Lp;
815 unsigned int rank = L.pseudoInverse(Lp);
816 if (rank != 6) {
817 return -1;
818 }
819
820 vpColVector e = Lp * err;
821 vpColVector v = -e * lambda;
822 // std::cout << "e: " << e.t() << std::endl;
823 eMc = eMc * vpExponentialMap::direct(v);
824 eMc.extract(eRc);
825 eMc.extract(eTc);
826 res = sqrt(v.sumSquare() / v.getRows());
827 it++;
828 } // end while
829#if DEBUG_LEVEL2
830 {
831 printf(" Iteration number for NL hand-eye minimization : %d\n", it);
832 vpThetaUVector ePc(eRc);
833 std::cout << "theta U (deg): " << vpMath::deg(ePc[0]) << " " << vpMath::deg(ePc[1]) << " " << vpMath::deg(ePc[2])
834 << std::endl;
835 std::cout << "Translation: " << eTc[0] << " " << eTc[1] << " " << eTc[2] << std::endl;
836 // Residual
837 double res = err.sumSquare();
838 res = sqrt(res / err.getRows());
839 printf("Mean residual (rotation+translation) = %lf\n", res);
840 }
841#endif
842 if (it == nb_iter_max) {
843 return 1; // VVS has not converged before nb_iter_max
844 }
845 else {
846 return 0;
847 }
848}
849
850
851int vpHandEyeCalibration::calibrate(const std::vector<vpHomogeneousMatrix> &cMo,
852 const std::vector<vpHomogeneousMatrix> &rMe, vpHomogeneousMatrix &eMc)
853{
854 int err;
855 vpHomogeneousMatrix rMo;
856 rMo.eye();
857 err = vpHandEyeCalibration::calibrate(cMo, rMe, eMc, rMo);
858 return err;
859}
860
861#if DEBUG_LEVEL1
862#define HE_I 0
863#define HE_TSAI_OROT 1
864#define HE_TSAI_ORNT 2
865#define HE_TSAI_NROT 3
866#define HE_TSAI_NRNT 4
867#define HE_PROCRUSTES_OT 5
868#define HE_PROCRUSTES_NT 6
869#endif
870
871int vpHandEyeCalibration::calibrate(const std::vector<vpHomogeneousMatrix> &cMo,
872 const std::vector<vpHomogeneousMatrix> &rMe,
873 vpHomogeneousMatrix &eMc, vpHomogeneousMatrix &rMo)
874{
875 if (cMo.size() != rMe.size()) {
876 throw vpException(vpException::dimensionError, "cMo and rMe have different sizes");
877 }
878
879 vpRotationMatrix eRc;
880 vpTranslationVector eTc;
881 vpColVector errVVS;
882 double resPos;
883
884 /* initialisation of eMc and rMo to I in case all other methods fail */
885 eMc.eye();
886 rMo.eye();
887 resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
888 double vmin = resPos; // will serve to determine the best method
889#if DEBUG_LEVEL1
890 int He_method = HE_I; // will serve to know which is the best method
891#endif
892 vpHomogeneousMatrix eMcMin = eMc; // best initial estimation for VSS
893 // Method using Old Tsai implementation
894 int err = vpHandEyeCalibration::calibrationRotationTsaiOld(cMo, rMe, eRc);
895 if (err != 0) {
896 std::cout << "\nProblem in solving Hand-Eye Rotation by Old Tsai method" << std::endl;
897 }
898 else {
899 eMc.insert(eRc);
900 err = vpHandEyeCalibration::calibrationTranslationOld(cMo, rMe, eRc, eTc);
901 if (err != 0) {
902 std::cout << "\nProblem in solving Hand-Eye Translation by Old Tsai method after Old Tsai method for rotation" << std::endl;
903 }
904 else {
905 eMc.insert(eTc);
906#if DEBUG_LEVEL1
907 {
908 std::cout << "\nRotation by (old) Tsai, old implementation for translation" << std::endl;
909 vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
910 }
911#endif
912 resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
913 if (resPos < vmin) {
914 vmin = resPos;
915 eMcMin = eMc;
916#if DEBUG_LEVEL1
917 He_method = HE_TSAI_OROT;
918#endif
919 }
920 }
921 err = vpHandEyeCalibration::calibrationTranslation(cMo, rMe, eRc, eTc);
922 if (err != 0) {
923 std::cout << "\n Problem in solving Hand-Eye Translation after Old Tsai method for rotation" << std::endl;
924 }
925 else {
926 eMc.insert(eTc);
927#if DEBUG_LEVEL1
928 {
929 std::cout << "\nRotation by (old) Tsai, new implementation for translation" << std::endl;
930 vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
931 }
932#endif
933 resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
934 if (resPos < vmin) {
935 vmin = resPos;
936 eMcMin = eMc;
937#if DEBUG_LEVEL1
938 He_method = HE_TSAI_ORNT;
939#endif
940 }
941 }
942 }
943 // First method using Tsai formulation
944 err = vpHandEyeCalibration::calibrationRotationTsai(cMo, rMe, eRc);
945 if (err != 0) {
946 std::cout << "\n Problem in solving Hand-Eye Rotation by Tsai method" << std::endl;
947 }
948 else {
949 eMc.insert(eRc);
950 err = vpHandEyeCalibration::calibrationTranslationOld(cMo, rMe, eRc, eTc);
951 if (err != 0) {
952 std::cout << "\n Problem in solving Hand-Eye Translation by Old Tsai method after Tsai method for rotation" << std::endl;
953 }
954 else {
955 eMc.insert(eTc);
956#if DEBUG_LEVEL1
957 {
958 std::cout << "\nRotation by Tsai, old implementation for translation" << std::endl;
959 vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
960 }
961#endif
962 resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
963 if (resPos < vmin) {
964 vmin = resPos;
965 eMcMin = eMc;
966#if DEBUG_LEVEL1
967 He_method = HE_TSAI_NROT;
968#endif
969 }
970 }
971 err = vpHandEyeCalibration::calibrationTranslation(cMo, rMe, eRc, eTc);
972 if (err != 0) {
973 std::cout << "\n Problem in solving Hand-Eye Translation after Tsai method for rotation" << std::endl;
974 }
975 else {
976 eMc.insert(eTc);
977#if DEBUG_LEVEL1
978 {
979 std::cout << "\nRotation by Tsai, new implementation for translation" << std::endl;
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_TSAI_NRNT;
989#endif
990 }
991 }
992 }
993 // Second method by solving the orthogonal Procrustes problem
994 err = vpHandEyeCalibration::calibrationRotationProcrustes(cMo, rMe, eRc);
995 if (err != 0)
996 std::cout << "\n Problem in solving Hand-Eye Rotation by Procrustes method" << std::endl;
997 else {
998 eMc.insert(eRc);
999 err = vpHandEyeCalibration::calibrationTranslationOld(cMo, rMe, eRc, eTc);
1000 if (err != 0) {
1001 std::cout << "\n Problem in solving Hand-Eye Translation by Old Tsai method after Procrustes method for rotation" << std::endl;
1002 }
1003 else {
1004 eMc.insert(eTc);
1005#if DEBUG_LEVEL1
1006 {
1007 std::cout << "\nRotation by Procrustes, old implementation for translation" << std::endl;
1008 vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
1009 }
1010#endif
1011 resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
1012 if (resPos < vmin) {
1013 vmin = resPos;
1014 eMcMin = eMc;
1015#if DEBUG_LEVEL1
1016 He_method = HE_PROCRUSTES_OT;
1017#endif
1018 }
1019 }
1020 err = vpHandEyeCalibration::calibrationTranslation(cMo, rMe, eRc, eTc);
1021 if (err != 0) {
1022 std::cout << "\n Problem in solving Hand-Eye Translation after Procrustes method for rotation" << std::endl;
1023 }
1024 else {
1025 eMc.insert(eTc);
1026#if DEBUG_LEVEL1
1027 {
1028 std::cout << "\nRotation by Procrustes, new implementation for translation" << std::endl;
1029 vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
1030 }
1031#endif
1032 resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
1033 if (resPos < vmin) {
1034 eMcMin = eMc;
1035#if DEBUG_LEVEL1
1036 vmin = resPos;
1037 He_method = HE_PROCRUSTES_NT;
1038#endif
1039 }
1040 }
1041 }
1042
1043 /* determination of the best method in case at least one succeeds */
1044 eMc = eMcMin;
1045#if DEBUG_LEVEL1
1046 {
1047 if (He_method == HE_I) {
1048 std::cout << "Best method : I !!!, vmin = " << vmin << std::endl;
1049 }
1050 if (He_method == HE_TSAI_OROT) {
1051 std::cout << "Best method : TSAI_OROT, vmin = " << vmin << std::endl;
1052 }
1053 if (He_method == HE_TSAI_ORNT) {
1054 std::cout << "Best method : TSAI_ORNT, vmin = " << vmin << std::endl;
1055 }
1056 if (He_method == HE_TSAI_NROT) {
1057 std::cout << "Best method : TSAI_NROT, vmin = " << vmin << std::endl;
1058 }
1059 if (He_method == HE_TSAI_NRNT) {
1060 std::cout << "Best method : TSAI_NRNT, vmin = " << vmin << std::endl;
1061 }
1062 if (He_method == HE_PROCRUSTES_OT) {
1063 std::cout << "Best method : PROCRUSTES_OT, vmin = " << vmin << std::endl;
1064 }
1065 if (He_method == HE_PROCRUSTES_NT) {
1066 std::cout << "Best method : PROCRUSTES_NT, vmin = " << vmin << std::endl;
1067 }
1068 vpThetaUVector ePc(eMc);
1069 std::cout << "theta U (deg): " << vpMath::deg(ePc[0]) << " " << vpMath::deg(ePc[1]) << " " << vpMath::deg(ePc[2])
1070 << std::endl;
1071 std::cout << "Translation: " << eMc[0][3] << " " << eMc[1][3] << " " << eMc[2][3] << std::endl;
1072 }
1073#endif
1074
1075 // Non linear iterative minimization to estimate simultaneouslty eRc and eTc
1076 err = vpHandEyeCalibration::calibrationVVS(cMo, rMe, eMc);
1077 // err : 0 si tout OK, -1 si pb de rang, 1 si pas convergence
1078 if (err != 0) {
1079 std::cout << "\n Problem in solving Hand-Eye Calibration by VVS" << std::endl;
1080 }
1081 else {
1082 // printf("\nRotation and translation after VVS\n");
1083 vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc, rMo);
1084 }
1085 return err;
1086}
1087
1088#undef HE_I
1089#undef HE_TSAI_OROT
1090#undef HE_TSAI_ORNT
1091#undef HE_TSAI_NROT
1092#undef HE_TSAI_NRNT
1093#undef HE_PROCRUSTES_OT
1094#undef HE_PROCRUSTES_NT
1095
1096#undef DEBUG_LEVEL1
1097#undef DEBUG_LEVEL2
1098
1099END_VISP_NAMESPACE
vpArray2D::getCols
unsigned int getCols() const
Definition vpArray2D.h:423
vpArray2D::size
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:435
vpArray2D::getRows
unsigned int getRows() const
Definition vpArray2D.h:433
vpColVector
Implementation of column vector and the associated operations.
Definition vpColVector.h:191
vpColVector::sumSquare
double sumSquare() const
Definition vpColVector.cpp:938
vpColVector::stack
void stack(double d)
Definition vpColVector.cpp:669
vpColVector::skew
static vpMatrix skew(const vpColVector &v)
Definition vpColVector.cpp:763
vpColVector::t
vpRowVector t() const
Definition vpColVector.cpp:558
vpException
error that can be emitted by ViSP classes.
Definition vpException.h:60
vpException::dimensionError
@ dimensionError
Bad dimension.
Definition vpException.h:71
vpExponentialMap::direct
static vpHomogeneousMatrix direct(const vpColVector &v)
Definition vpExponentialMap.cpp:54
vpHandEyeCalibration::calibrate
static int calibrate(const std::vector< vpHomogeneousMatrix > &cMo, const std::vector< vpHomogeneousMatrix > &rMe, vpHomogeneousMatrix &eMc, vpHomogeneousMatrix &rMo)
Definition vpHandEyeCalibration.cpp:871
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition vpHomogeneousMatrix.h:221
vpHomogeneousMatrix::getRotationMatrix
vpRotationMatrix getRotationMatrix() const
Definition vpHomogeneousMatrix.cpp:1177
vpHomogeneousMatrix::buildFrom
vpHomogeneousMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Definition vpHomogeneousMatrix.cpp:330
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition vpHomogeneousMatrix.cpp:1016
vpHomogeneousMatrix::getTranslationVector
vpTranslationVector getTranslationVector() const
Definition vpHomogeneousMatrix.cpp:1167
vpHomogeneousMatrix::extract
void extract(vpRotationMatrix &R) const
Definition vpHomogeneousMatrix.cpp:912
vpHomogeneousMatrix::insert
void insert(const vpRotationMatrix &R)
Definition vpHomogeneousMatrix.cpp:958
vpMath::sinc
static double sinc(double x)
Definition vpMath.cpp:289
vpMath::deg
static double deg(double rad)
Definition vpMath.h:119
vpMatrix::stack
void stack(const vpMatrix &A)
Definition vpMatrix_stack.cpp:239
vpMatrix::AtA
vpMatrix AtA() const
Definition vpMatrix_operations.cpp:785
vpMatrix::pseudoInverse
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition vpMatrix_pseudo_inverse.cpp:336
vpMatrix::sumSquare
double sumSquare() const
Definition vpMatrix.cpp:1984
vpMatrix::t
vpMatrix t() const
Definition vpMatrix_operations.cpp:46
vpRotationMatrix
Implementation of a rotation matrix and operations on such kind of matrices.
Definition vpRotationMatrix.h:130
vpRotationMatrix::t
vpRotationMatrix t() const
Definition vpRotationMatrix.cpp:625
vpRotationMatrix::mean
static vpRotationMatrix mean(const std::vector< vpHomogeneousMatrix > &vec_M)
Definition vpRotationMatrix.cpp:965
vpThetaUVector
Implementation of a rotation vector as axis-angle minimal representation.
Definition vpThetaUVector.h:172
vpTranslationVector
Class that consider the case of a translation vector.
Definition vpTranslationVector.h:119
vpTranslationVector::mean
static vpTranslationVector mean(const std::vector< vpHomogeneousMatrix > &vec_M)
Definition vpTranslationVector.cpp:817
BEGIN_VISP_NAMESPACE
Definition vpMbtDistanceCircle.cpp:55