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