Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpPoseLagrange.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 * Pose computation.
32 */
33
34#include <visp3/vision/vpPose.h>
35
36#define DEBUG_LEVEL1 0
37#define DEBUG_LEVEL2 0
38
39/**********************************************************************/
40/* FONCTION : CalculTranslation */
41/* ROLE : Calcul de la translation entre la */
42/* camera et l'outil connaissant la */
43/* rotation */
44/**********************************************************************/
45
46static void calculTranslation(vpMatrix &a, vpMatrix &b, unsigned int nl, unsigned int nc1, unsigned int nc3,
47 vpColVector &x1, vpColVector &x2)
48{
49
50 try {
51 unsigned int i, j;
52
53 vpMatrix ct(3, nl);
54 for (i = 0; i < 3; i++) {
55 for (j = 0; j < nl; j++)
56 ct[i][j] = b[j][i + nc3];
57 }
58
59 vpMatrix c;
60 c = ct.t();
61
62 vpMatrix ctc;
63 ctc = ct * c;
64
65 vpMatrix ctc1; // (C^T C)^(-1)
66 ctc1 = ctc.inverseByLU();
67
68 vpMatrix cta;
69 vpMatrix ctb;
70 cta = ct * a; /* C^T A */
71 ctb = ct * b; /* C^T B */
72
73#if (DEBUG_LEVEL2)
74 {
75 std::cout << "ctc " << std::endl << ctc;
76 std::cout << "cta " << std::endl << cta;
77 std::cout << "ctb " << std::endl << ctb;
78 }
79#endif
80
81 vpColVector X2(nc3);
82 vpMatrix CTB(nc1, nc3);
83 for (i = 0; i < nc1; i++) {
84 for (j = 0; j < nc3; j++)
85 CTB[i][j] = ctb[i][j];
86 }
87
88 for (j = 0; j < nc3; j++)
89 X2[j] = x2[j];
90
91 vpColVector sv; // C^T A X1 + C^T B X2)
92 sv = cta * x1 + CTB * X2; // C^T A X1 + C^T B X2)
93
94#if (DEBUG_LEVEL2)
95 std::cout << "sv " << sv.t();
96#endif
97
98 vpColVector X3; /* X3 = - (C^T C )^{-1} C^T (A X1 + B X2) */
99 X3 = -ctc1 * sv;
100
101#if (DEBUG_LEVEL2)
102 std::cout << "x3 " << X3.t();
103#endif
104
105 for (i = 0; i < nc1; i++)
106 x2[i + nc3] = X3[i];
107 } catch (...) {
108
109 // en fait il y a des dizaines de raisons qui font que cette fonction
110 // rende une erreur (matrice pas inversible, pb de memoire etc...)
111 vpERROR_TRACE(" ");
112 throw;
113 }
114}
115
116//*********************************************************************
117// FONCTION LAGRANGE :
118// -------------------
119// Resolution d'un systeme lineaire de la forme A x1 + B x2 = 0
120// sous la contrainte || x1 || = 1
121// ou A est de dimension nl x nc1 et B nl x nc2
122//*********************************************************************
123
124//#define EPS 1.e-5
125
126static void lagrange(vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2)
127{
128#if (DEBUG_LEVEL1)
129 std::cout << "begin (CLagrange.cc)Lagrange(...) " << std::endl;
130#endif
131
132 try {
133 unsigned int i, imin;
134
135 vpMatrix ata; // A^T A
136 ata = a.t() * a;
137 vpMatrix btb; // B^T B
138 btb = b.t() * b;
139
140 vpMatrix bta; // B^T A
141 bta = b.t() * a;
142
143 vpMatrix btb1; // (B^T B)^(-1)
144
145 /* Warning:
146 when using btb.inverseByLU() that call cv::inv(cv::DECOMP_LU) with
147 OpenCV 3.1.0 and 3.2.0 we notice that OpenCV is not able to compute the
148 inverse of the following matrix:
149
150 btb[9,9]=
151 0.015925 0.0 0.0030866 0.00035 0.0 0.000041 0.105
152 0.0 0.0346242 0.0 0.015925 -0.0050979 0.0 0.00035
153 -0.000063 0.0 0.105 -0.0637464 0.0030866 -0.0050979 0.0032301
154 0.000041 -0.000063 0.000016 0.0346242 -0.0637464 0.0311185 0.00035
155 0.0 0.000041 0.0001 0.0 0.000012 0.01 0.0
156 0.0011594 0.0 0.00035 -0.000063 0.0 0.0001 -0.000018
157 0.0 0.01 -0.0018040 0.000041 -0.000063 0.000016 0.000012
158 -0.000018 0.000005 0.0011594 -0.0018040 0.0004599 0.105 0.0
159 0.0346242 0.01 0.0 0.0011594 5.0 0.0 0.13287
160 0.0 0.105 -0.0637464 0.0 0.01 -0.0018040 0.0
161 5.0 -0.731499 0.0346242 -0.0637464 0.0311185 0.0011594 -0.0018040
162 0.0004599 0.13287 -0.731499 0.454006
163
164 That's why instead of using inverseByLU() we are now using pseudoInverse()
165 */
166#if 0
167 if (b.getRows() >= b.getCols()) {
168 btb1 = btb.inverseByLU();
169 }
170 else {
171 btb1 = btb.pseudoInverse();
172 }
173#else
174 btb1 = btb.pseudoInverse();
175#endif
176
177#if (DEBUG_LEVEL1)
178 {
179 std::cout << " BTB1 * BTB : " << std::endl << btb1 * btb << std::endl;
180 std::cout << " BTB * BTB1 : " << std::endl << btb * btb1 << std::endl;
181 }
182#endif
183
184 vpMatrix r; // (B^T B)^(-1) B^T A
185 r = btb1 * bta;
186
187 vpMatrix e; // - A^T B (B^T B)^(-1) B^T A
188 e = -(a.t() * b) * r;
189
190 e += ata; // calcul E = A^T A - A^T B (B^T B)^(-1) B^T A
191
192#if (DEBUG_LEVEL1)
193 {
194 std::cout << " E :" << std::endl << e << std::endl;
195 }
196#endif
197
198 // vpColVector sv ;
199 // vpMatrix v ;
200 e.svd(x1, ata); // destructif sur e
201 // calcul du vecteur propre de E correspondant a la valeur propre min.
202 /* calcul de SVmax */
203 imin = 0;
204 // FC : Pourquoi calculer SVmax ??????
205 // double svm = 0.0;
206 // for (i=0;i<x1.getRows();i++)
207 // {
208 // if (x1[i] > svm) { svm = x1[i]; imin = i; }
209 // }
210 // svm *= EPS; /* pour le rang */
211
212 for (i = 0; i < x1.getRows(); i++)
213 if (x1[i] < x1[imin])
214 imin = i;
215
216#if (DEBUG_LEVEL1)
217 {
218 printf("SV(E) : %.15lf %.15lf %.15lf\n", x1[0], x1[1], x1[2]);
219 std::cout << " i_min " << imin << std::endl;
220 }
221#endif
222 for (i = 0; i < x1.getRows(); i++)
223 x1[i] = ata[i][imin];
224
225 x2 = -(r * x1); // X_2 = - (B^T B)^(-1) B^T A X_1
226
227#if (DEBUG_LEVEL1)
228 {
229 std::cout << " X1 : " << x1.t() << std::endl;
230 std::cout << " V : " << std::endl << ata << std::endl;
231 }
232#endif
233 } catch (...) {
234 vpERROR_TRACE(" ");
235 throw;
236 }
237#if (DEBUG_LEVEL1)
238 std::cout << "end (CLagrange.cc)Lagrange(...) " << std::endl;
239#endif
240}
241
242//#undef EPS
243
255void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double *p_a, double *p_b, double *p_c, double *p_d)
256{
257#if (DEBUG_LEVEL1)
258 std::cout << "begin vpPose::PoseLagrangePlan(...) " << std::endl;
259#endif
260 // determination of the plane equation a X + b Y + c Z + d = 0
261 double a, b, c, d;
262
263 // Checking if coplanar has already been called and if the plan coefficients have been given
264 if((p_isPlan != NULL) && (p_a != NULL) && (p_b != NULL) && (p_c != NULL) && (p_d != NULL))
265 {
266 if(*p_isPlan)
267 {
268 // All the pointers towards the plan coefficients are different from NULL => using them in the rest of the method
269 a = *p_a;
270 b = *p_b;
271 c = *p_c;
272 d = *p_d;
273 }
274 else
275 {
276 // The call to coplanar that was performed outside vpPose::poseLagrangePlan indicated that the points are not coplanar.
277 throw vpException(vpException::fatalError, "Called vpPose::poseLagrangePlan but the call to vpPose::coplanar done outside the method indicated that the points are not coplanar");
278 }
279 }
280 else
281 {
282 // At least one of the coefficient is a NULL pointer => calling coplanar by ourselves
283 int coplanarType;
284 bool areCoplanar = coplanar(coplanarType, &a, &b, &c, &d);
285 if(!areCoplanar)
286 {
287 throw vpException(vpException::fatalError, "Called vpPose::poseLagrangePlan but call to vpPose::coplanar indicates that the points are not coplanar");
288 }
289 }
290
291
292 if (c < 0.0) { // imposing c >= 0
293 a = -a;
294 b = -b;
295 c = -c;
296 d = -d;
297 }
298 // to have (a,b,c) as a unit vector if it was not the case
299 double n = 1.0 / sqrt(a * a + b * b + c * c); // Not possible to have a NaN...
300 a *= n;
301 b *= n;
302 c *= n;
303 d *= n;
304 // printf("a = %lf, b = %lf, c = %lf, d = %lf\n",a,b,c,d);
305 // transformation to have object plane with equation Z = 0
306 vpColVector r1(3), r2(3), r3(3);
307
308 r3[0] = a;
309 r3[1] = b;
310 r3[2] = c;
311 // build r1 as a unit vector orthogonal to r3
312 double n1 = sqrt(1.0 - a * a);
313 double n2 = sqrt(1.0 - b * b);
314 if (n1 >= n2) {
315 r1[0] = n1;
316 r1[1] = -a * b / n1;
317 r1[2] = -a * c / n1;
318 } else {
319 r1[0] = -a * b / n2;
320 r1[1] = n2;
321 r1[2] = -b * c / n2;
322 }
323 // double norm = r1[0]*r1[0] + r1[1]*r1[1] + r1[2]*r1[2];
324 // double crossprod = r1[0]*r3[0] + r1[1]*r3[1] + r1[2]*r3[2];
325 // printf("r1 norm = 1 ? %lf, r1^T r3 = 0 ? %lf\n",norm, crossprod);
326 // r2 unit vector orthogonal to r3 and r1
327 r2 = vpColVector::crossProd(r3, r1);
328
330 for (unsigned int i = 0; i < 3; i++) {
331 fMo[0][i] = r1[i];
332 fMo[1][i] = r2[i];
333 fMo[2][i] = r3[i];
334 }
335 fMo[0][3] = fMo[1][3] = 0.0;
336 fMo[2][3] = d;
337
338 // std::cout << "fMo : " << std::endl << fMo << std::endl;
339 // Build and solve the system
340 unsigned int k = 0;
341 unsigned int nl = npt * 2;
342
343 vpMatrix A(nl, 3);
344 vpMatrix B(nl, 6);
345 vpPoint P;
346
347 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
348 P = *it;
349
350 // Transform each point in plane Z = 0
351 vpColVector Xf, X(4);
352 X[0] = P.get_oX();
353 X[1] = P.get_oY();
354 X[2] = P.get_oZ();
355 X[3] = 1.0;
356 Xf = fMo * X;
357 // printf("Z = 0 = %lf\n",Xf[2]);
358 // build the system
359 A[k][0] = -Xf[0];
360 A[k][1] = 0.0;
361 A[k][2] = Xf[0] * P.get_x();
362
363 A[k + 1][0] = 0.0;
364 A[k + 1][1] = -Xf[0];
365 A[k + 1][2] = Xf[0] * P.get_y();
366
367 B[k][0] = -Xf[1];
368 B[k][1] = 0.0;
369 B[k][2] = Xf[1] * P.get_x();
370 B[k][3] = -1.0;
371 B[k][4] = 0.0;
372 B[k][5] = P.get_x();
373
374 B[k + 1][0] = 0.0;
375 B[k + 1][1] = -Xf[1];
376 B[k + 1][2] = Xf[1] * P.get_y();
377 B[k + 1][3] = 0.0;
378 B[k + 1][4] = -1.0;
379 B[k + 1][5] = P.get_y();
380
381 k += 2;
382 }
383 vpColVector X1(3);
384 vpColVector X2(6);
385
386#if (DEBUG_LEVEL2)
387 {
388 std::cout << "A " << std::endl << A << std::endl;
389 std::cout << "B " << std::endl << B << std::endl;
390 }
391#endif
392
393 lagrange(A, B, X1, X2);
394
395#if (DEBUG_LEVEL2)
396 {
397 std::cout << "A X1+B X2 (should be 0): " << (A * X1 + B * X2).t() << std::endl;
398 std::cout << " X1 norm: " << X1.sumSquare() << std::endl;
399 }
400#endif
401
402 if (X2[5] < 0.0) { /* to obtain Zo > 0 */
403 for (unsigned int i = 0; i < 3; i++)
404 X1[i] = -X1[i];
405 for (unsigned int i = 0; i < 6; i++)
406 X2[i] = -X2[i];
407 }
408 double s = 0.0;
409 for (unsigned int i = 0; i < 3; i++) {
410 s += (X1[i] * X2[i]);
411 }
412 for (unsigned int i = 0; i < 3; i++) {
413 X2[i] -= (s * X1[i]);
414 } /* X1^T X2 = 0 */
415
416 // s = 0.0;
417 // for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
418 s = X2[0] * X2[0] + X2[1] * X2[1] + X2[2] * X2[2]; // To avoid a Coverity copy/past error
419
420 if (s < 1e-10) {
421 // std::cout << "Points that produce an error: " << std::endl;
422 // for (std::list<vpPoint>::const_iterator it = listP.begin(); it
423 // != listP.end(); ++it)
424 // {
425 // std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() <<
426 // " "
427 // << (*it).get_oX() << " " << (*it).get_oY() << " " <<
428 // (*it).get_oZ() << std::endl;
429 // }
430 throw(vpException(vpException::divideByZeroError, "Division by zero in Lagrange pose computation "
431 "(planar plane case)"));
432 }
433
434 s = 1.0 / sqrt(s);
435 for (unsigned int i = 0; i < 3; i++) {
436 X2[i] *= s;
437 } /* X2^T X2 = 1 */
438
439 calculTranslation(A, B, nl, 3, 3, X1, X2);
440
441 // if (err != OK)
442 {
443 // std::cout << "in (vpCalculPose_plan.cc)CalculTranslation returns " ;
444 // PrintError(err) ;
445 // return err ;
446 }
448 /* X1 x X2 */
449 cMf[0][2] = (X1[1] * X2[2]) - (X1[2] * X2[1]);
450 cMf[1][2] = (X1[2] * X2[0]) - (X1[0] * X2[2]);
451 cMf[2][2] = (X1[0] * X2[1]) - (X1[1] * X2[0]);
452 /* calcul de la matrice de passage */
453 for (unsigned int i = 0; i < 3; i++) {
454 cMf[i][0] = X1[i];
455 cMf[i][1] = X2[i];
456 cMf[i][3] = X2[i + 3];
457 }
458 // std::cout << "cMf : " << std::endl << cMf << std::endl;
459
460 // Apply the transform to go back to object frame
461 cMo = cMf * fMo;
462
463#if (DEBUG_LEVEL1)
464 std::cout << "end vpCalculPose::PoseLagrangePlan(...) " << std::endl;
465#endif
466 // return(OK);
467}
468
470{
471
472#if (DEBUG_LEVEL1)
473 std::cout << "begin CPose::PoseLagrangeNonPlan(...) " << std::endl;
474#endif
475 try {
476 double s;
477 unsigned int i;
478
479 unsigned int k = 0;
480 unsigned int nl = npt * 2;
481
482 if (npt < 6) {
484 "Lagrange, non planar case, insufficient number of points %d < 6\n", npt));
485 }
486
487 vpMatrix a(nl, 3);
488 vpMatrix b(nl, 9);
489 b = 0;
490
491 vpPoint P;
492 i = 0;
493 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
494 P = *it;
495 a[k][0] = -P.get_oX();
496 a[k][1] = 0.0;
497 a[k][2] = P.get_oX() * P.get_x();
498
499 a[k + 1][0] = 0.0;
500 a[k + 1][1] = -P.get_oX();
501 a[k + 1][2] = P.get_oX() * P.get_y();
502
503 b[k][0] = -P.get_oY();
504 b[k][1] = 0.0;
505 b[k][2] = P.get_oY() * P.get_x();
506
507 b[k][3] = -P.get_oZ();
508 b[k][4] = 0.0;
509 b[k][5] = P.get_oZ() * P.get_x();
510
511 b[k][6] = -1.0;
512 b[k][7] = 0.0;
513 b[k][8] = P.get_x();
514
515 b[k + 1][0] = 0.0;
516 b[k + 1][1] = -P.get_oY();
517 b[k + 1][2] = P.get_oY() * P.get_y();
518
519 b[k + 1][3] = 0.0;
520 b[k + 1][4] = -P.get_oZ();
521 b[k + 1][5] = P.get_oZ() * P.get_y();
522
523 b[k + 1][6] = 0.0;
524 b[k + 1][7] = -1.0;
525 b[k + 1][8] = P.get_y();
526
527 k += 2;
528 }
529 vpColVector X1(3);
530 vpColVector X2(9);
531
532#if (DEBUG_LEVEL2)
533 {
534 std::cout << "a " << a << std::endl;
535 std::cout << "b " << b << std::endl;
536 }
537#endif
538
539 lagrange(a, b, X1, X2);
540 // if (err != OK)
541 {
542 // std::cout << "in (CLagrange.cc)Lagrange returns " ;
543 // PrintError(err) ;
544 // return err ;
545 }
546
547#if (DEBUG_LEVEL2)
548 {
549 std::cout << "ax1+bx2 (devrait etre 0) " << (a * X1 + b * X2).t() << std::endl;
550 std::cout << "norme X1 " << X1.sumSquare() << std::endl;
551 }
552#endif
553
554 if (X2[8] < 0.0) { /* car Zo > 0 */
555 X1 *= -1;
556 X2 *= -1;
557 }
558 s = 0.0;
559 for (i = 0; i < 3; i++) {
560 s += (X1[i] * X2[i]);
561 }
562 for (i = 0; i < 3; i++) {
563 X2[i] -= (s * X1[i]);
564 } /* X1^T X2 = 0 */
565
566 // s = 0.0;
567 // for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
568 s = X2[0] * X2[0] + X2[1] * X2[1] + X2[2] * X2[2]; // To avoid a Coverity copy/past error
569
570 if (s < 1e-10) {
571 // std::cout << "Points that produce an error: " << std::endl;
572 // for (std::list<vpPoint>::const_iterator it = listP.begin(); it
573 // != listP.end(); ++it)
574 // {
575 // std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() <<
576 // " "
577 // << (*it).get_oX() << " " << (*it).get_oY() << " " <<
578 // (*it).get_oZ() << std::endl;
579 // }
580 // vpERROR_TRACE(" division par zero " ) ;
581 throw(vpException(vpException::divideByZeroError, "Division by zero in Lagrange pose computation (non "
582 "planar plane case)"));
583 }
584
585 s = 1.0 / sqrt(s);
586 for (i = 0; i < 3; i++) {
587 X2[i] *= s;
588 } /* X2^T X2 = 1 */
589
590 X2[3] = (X1[1] * X2[2]) - (X1[2] * X2[1]);
591 X2[4] = (X1[2] * X2[0]) - (X1[0] * X2[2]);
592 X2[5] = (X1[0] * X2[1]) - (X1[1] * X2[0]);
593
594 calculTranslation(a, b, nl, 3, 6, X1, X2);
595
596 for (i = 0; i < 3; i++) {
597 cMo[i][0] = X1[i];
598 cMo[i][1] = X2[i];
599 cMo[i][2] = X2[i + 3];
600 cMo[i][3] = X2[i + 6];
601 }
602
603 } catch (...) {
604 throw; // throw the original exception
605 }
606
607#if (DEBUG_LEVEL1)
608 std::cout << "end vpCalculPose::PoseLagrangeNonPlan(...) " << std::endl;
609#endif
610}
611
612#undef DEBUG_LEVEL1
613#undef DEBUG_LEVEL2
unsigned int getCols() const
Definition vpArray2D.h:280
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of column vector and the associated operations.
double sumSquare() const
vpRowVector t() const
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ dimensionError
Bad dimension.
Definition vpException.h:83
@ fatalError
Fatal error.
Definition vpException.h:84
@ divideByZeroError
Division by zero.
Definition vpException.h:82
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
vpMatrix inverseByLU() const
void svd(vpColVector &w, vpMatrix &V)
vpMatrix t() const
Definition vpMatrix.cpp:461
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
double get_oX() const
Get the point oX coordinate in the object frame.
Definition vpPoint.cpp:458
double get_y() const
Get the point y coordinate in the image plane.
Definition vpPoint.cpp:469
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition vpPoint.cpp:462
double get_x() const
Get the point x coordinate in the image plane.
Definition vpPoint.cpp:467
double get_oY() const
Get the point oY coordinate in the object frame.
Definition vpPoint.cpp:460
unsigned int npt
Number of point used in pose computation.
Definition vpPose.h:114
void poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan=NULL, double *p_a=NULL, double *p_b=NULL, double *p_c=NULL, double *p_d=NULL)
Compute the pose of a planar object using Lagrange approach.
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition vpPose.h:115
void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
bool coplanar(int &coplanar_plane_type, double *p_a=NULL, double *p_b=NULL, double *p_c=NULL, double *p_d=NULL)
Definition vpPose.cpp:187
#define vpERROR_TRACE
Definition vpDebug.h:388