Visual Servoing Platform  version 3.2.1 under development (2019-05-20)
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 
52 static 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 
132 static 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  }
617 #endif
618 
619  if (X2[8] < 0.0) { /* car Zo > 0 */
620  X1 *= -1;
621  X2 *= -1;
622  }
623  s = 0.0;
624  for (i = 0; i < 3; i++) {
625  s += (X1[i] * X2[i]);
626  }
627  for (i = 0; i < 3; i++) {
628  X2[i] -= (s * X1[i]);
629  } /* X1^T X2 = 0 */
630 
631  // s = 0.0;
632  // for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
633  s = X2[0] * X2[0] + X2[1] * X2[1] + X2[2] * X2[2]; // To avoid a Coverity copy/past error
634 
635  if (s < 1e-10) {
636  // std::cout << "Points that produce an error: " << std::endl;
637  // for (std::list<vpPoint>::const_iterator it = listP.begin(); it
638  // != listP.end(); ++it)
639  // {
640  // std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() <<
641  // " "
642  // << (*it).get_oX() << " " << (*it).get_oY() << " " <<
643  // (*it).get_oZ() << std::endl;
644  // }
645  // vpERROR_TRACE(" division par zero " ) ;
646  throw(vpException(vpException::divideByZeroError, "Division by zero in Lagrange pose computation (non "
647  "planar plane case)"));
648  }
649 
650  s = 1.0 / sqrt(s);
651  for (i = 0; i < 3; i++) {
652  X2[i] *= s;
653  } /* X2^T X2 = 1 */
654 
655  X2[3] = (X1[1] * X2[2]) - (X1[2] * X2[1]);
656  X2[4] = (X1[2] * X2[0]) - (X1[0] * X2[2]);
657  X2[5] = (X1[0] * X2[1]) - (X1[1] * X2[0]);
658 
659  calculTranslation(a, b, nl, 3, 6, X1, X2);
660 
661  for (i = 0; i < 3; i++) {
662  cMo[i][0] = X1[i];
663  cMo[i][1] = X2[i];
664  cMo[i][2] = X2[i + 3];
665  cMo[i][3] = X2[i + 6];
666  }
667 
668  } catch (...) {
669  throw; // throw the original exception
670  }
671 
672 #if (DEBUG_LEVEL1)
673  std::cout << "end vpCalculPose::PoseLagrangeNonPlan(...) " << std::endl;
674 #endif
675 }
676 
677 #undef DEBUG_LEVEL1
678 #undef DEBUG_LEVEL2
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:1881
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
Implementation of an homogeneous matrix and operations on such kind of matrices.
void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
#define vpERROR_TRACE
Definition: vpDebug.h:393
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:422
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getCols() const
Definition: vpArray2D.h:279
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:431
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:108
Class that defines what is a point.
Definition: vpPoint.h:58
vpRowVector t() const
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:424
void poseLagrangePlan(vpHomogeneousMatrix &cMo)
Compute the pose of a planar object using Lagrange approach.
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:429
unsigned int getRows() const
Definition: vpArray2D.h:289
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:107
double sumSquare() const
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:420
vpMatrix t() const
Definition: vpMatrix.cpp:375
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpMatrix inverseByLU() const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2021
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)