Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpPoseLagrange.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
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 http://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  * Authors:
34  * Eric Marchand
35  * Francois Chaumette
36  *
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
53 void
54 calculTranslation (vpMatrix &a, vpMatrix &b, unsigned int nl, unsigned int nc1,
55  unsigned int nc3, vpColVector &x1, vpColVector &x2)
56 {
57 
58  try
59  {
60  unsigned int i,j;
61 
62  vpMatrix ct(3,nl) ;
63  for (i=0 ; i < 3 ; i++)
64  {
65  for (j=0 ; j < nl ; j++)
66  ct[i][j] = b[j][i+nc3] ;
67  }
68 
69  vpMatrix c ;
70  c = ct.t() ;
71 
72  vpMatrix ctc ;
73  ctc = ct*c ;
74 
75  vpMatrix ctc1 ; // (C^T C)^(-1)
76  ctc1 = ctc.inverseByLU() ;
77 
78  vpMatrix cta ;
79  vpMatrix ctb ;
80  cta = ct*a ; /* C^T A */
81  ctb = ct*b ; /* C^T B */
82 
83 #if (DEBUG_LEVEL2)
84  {
85  std::cout <<"ctc " << std::endl << ctc ;
86  std::cout <<"cta " << std::endl << cta ;
87  std::cout <<"ctb " << std::endl << ctb ;
88  }
89 #endif
90 
91  vpColVector X2(nc3) ;
92  vpMatrix CTB(nc1,nc3) ;
93  for (i=0 ; i < nc1 ; i++)
94  {
95  for (j=0 ; j < nc3 ; j++)
96  CTB[i][j] = ctb[i][j] ;
97  }
98 
99  for (j=0 ; j < nc3 ; j++)
100  X2[j] = x2[j] ;
101 
102  vpColVector sv ; // C^T A X1 + C^T B X2)
103  sv = cta*x1 + CTB*X2 ;// C^T A X1 + C^T B X2)
104 
105 #if (DEBUG_LEVEL2)
106  std::cout << "sv " << sv.t() ;
107 #endif
108 
109  vpColVector X3 ; /* X3 = - (C^T C )^{-1} C^T (A X1 + B X2) */
110  X3 = -ctc1*sv ;
111 
112 #if (DEBUG_LEVEL2)
113  std::cout << "x3 " << X3.t() ;
114 #endif
115 
116  for (i=0 ; i < nc1 ; i++)
117  x2[i+nc3] = X3[i] ;
118  }
119  catch(...)
120  {
121 
122  // en fait il y a des dizaines de raisons qui font que cette fonction
123  // rende une erreur (matrice pas inversible, pb de memoire etc...)
124  vpERROR_TRACE(" ") ;
125  throw ;
126  }
127 
128 
129 }
130 
131 
132 //*********************************************************************
133 // FONCTION LAGRANGE :
134 // -------------------
135 // Resolution d'un systeme lineaire de la forme A x1 + B x2 = 0
136 // sous la contrainte || x1 || = 1
137 // ou A est de dimension nl x nc1 et B nl x nc2
138 //*********************************************************************
139 
140 //#define EPS 1.e-5
141 
142 static
143 void
144 lagrange (vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2)
145 {
146 #if (DEBUG_LEVEL1)
147  std::cout << "begin (CLagrange.cc)Lagrange(...) " << std::endl;
148 #endif
149 
150  try{
151  unsigned int i,imin;
152 
153  vpMatrix ata ; // A^T A
154  ata = a.t()*a ;
155  vpMatrix btb ; // B^T B
156  btb = b.t()*b ;
157 
158  vpMatrix bta ; // B^T A
159  bta = b.t()*a ;
160 
161  vpMatrix btb1 ; // (B^T B)^(-1)
162 
163  if (b.getRows() >= b.getCols()) btb1 = btb.inverseByLU() ;
164  else btb1 = btb.pseudoInverse();
165 
166 #if (DEBUG_LEVEL1)
167  {
168  std::cout << " BTB1 * BTB : " << std::endl << btb1*btb << std::endl;
169  std::cout << " BTB * BTB1 : " << std::endl << btb*btb1 << std::endl;
170  }
171 #endif
172 
173  vpMatrix r ; // (B^T B)^(-1) B^T A
174  r = btb1*bta ;
175 
176  vpMatrix e ; // - A^T B (B^T B)^(-1) B^T A
177  e = - (a.t()*b) *r ;
178 
179  e += ata ; // calcul E = A^T A - A^T B (B^T B)^(-1) B^T A
180 
181 #if (DEBUG_LEVEL1)
182  {
183  std::cout << " E :" << std::endl << e << std::endl;
184  }
185 #endif
186 
187  // vpColVector sv ;
188  // vpMatrix v ;
189  e.svd(x1,ata) ;// destructif sur e
190  // calcul du vecteur propre de E correspondant a la valeur propre min.
191 
192  /* calcul de SVmax */
193  imin = 0;
194  // FC : Pourquoi calculer SVmax ??????
195  // double svm = 0.0;
196  // for (i=0;i<x1.getRows();i++)
197  // {
198  // if (x1[i] > svm) { svm = x1[i]; imin = i; }
199  // }
200  // svm *= EPS; /* pour le rang */
201 
202  for (i=0;i<x1.getRows();i++)
203  if (x1[i] < x1[imin]) imin = i;
204 
205 #if (DEBUG_LEVEL1)
206  {
207  printf("SV(E) : %.15lf %.15lf %.15lf\n",x1[0],x1[1],x1[2]);
208  std::cout << " i_min " << imin << std::endl;
209  }
210 #endif
211  for (i=0;i<x1.getRows();i++)
212  x1[i] = ata[i][imin];
213 
214  x2 = - (r*x1) ; // X_2 = - (B^T B)^(-1) B^T A X_1
215 
216 #if (DEBUG_LEVEL1)
217  {
218  std::cout << " X1 : " << x1.t() << std::endl;
219  std::cout << " V : " << std::endl << ata << std::endl;
220  }
221 #endif
222  }
223  catch(...)
224  {
225  vpERROR_TRACE(" ") ;
226  throw ;
227  }
228 #if (DEBUG_LEVEL1)
229  std::cout << "end (CLagrange.cc)Lagrange(...) " << std::endl;
230 #endif
231 }
232 
233 //#undef EPS
234 
245 void
246 vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type)
247 {
248 
249 #if (DEBUG_LEVEL1)
250  std::cout << "begin vpPose::PoseLagrange(...) " << std::endl ;
251 #endif
252  try
253  {
254  double s;
255  unsigned int i;
256 
257  unsigned int k=0;
258  unsigned int nl=npt*2;
259 
260 
261  vpMatrix a(nl,3) ;
262  vpMatrix b(nl,6);
263  vpPoint P ;
264  i=0 ;
265 
266  if (coplanar_plane_type == 1) { // plane ax=d
267  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
268  {
269  P = *it ;
270  a[k][0] = -P.get_oY();
271  a[k][1] = 0.0;
272  a[k][2] = P.get_oY()*P.get_x();
273 
274  a[k+1][0] = 0.0;
275  a[k+1][1] = -P.get_oY();
276  a[k+1][2] = P.get_oY()*P.get_y();
277 
278  b[k][0] = -P.get_oZ();
279  b[k][1] = 0.0;
280  b[k][2] = P.get_oZ()*P.get_x();
281  b[k][3] = -1.0;
282  b[k][4] = 0.0;
283  b[k][5] = P.get_x();
284 
285  b[k+1][0] = 0.0;
286  b[k+1][1] = -P.get_oZ();
287  b[k+1][2] = P.get_oZ()*P.get_y();
288  b[k+1][3] = 0.0;
289  b[k+1][4] = -1.0;
290  b[k+1][5] = P.get_y();
291 
292  k += 2;
293  }
294 
295  }
296  else if (coplanar_plane_type == 2) { // plane by=d
297  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
298  {
299  P = *it ;
300  a[k][0] = -P.get_oX();
301  a[k][1] = 0.0;
302  a[k][2] = P.get_oX()*P.get_x();
303 
304  a[k+1][0] = 0.0;
305  a[k+1][1] = -P.get_oX();
306  a[k+1][2] = P.get_oX()*P.get_y();
307 
308  b[k][0] = -P.get_oZ();
309  b[k][1] = 0.0;
310  b[k][2] = P.get_oZ()*P.get_x();
311  b[k][3] = -1.0;
312  b[k][4] = 0.0;
313  b[k][5] = P.get_x();
314 
315  b[k+1][0] = 0.0;
316  b[k+1][1] = -P.get_oZ();
317  b[k+1][2] = P.get_oZ()*P.get_y();
318  b[k+1][3] = 0.0;
319  b[k+1][4] = -1.0;
320  b[k+1][5] = P.get_y();
321 
322  k += 2;
323  }
324 
325  }
326  else { // plane cz=d or any other
327 
328  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
329  {
330  P = *it ;
331  a[k][0] = -P.get_oX();
332  a[k][1] = 0.0;
333  a[k][2] = P.get_oX()*P.get_x();
334 
335  a[k+1][0] = 0.0;
336  a[k+1][1] = -P.get_oX();
337  a[k+1][2] = P.get_oX()*P.get_y();
338 
339  b[k][0] = -P.get_oY();
340  b[k][1] = 0.0;
341  b[k][2] = P.get_oY()*P.get_x();
342  b[k][3] = -1.0;
343  b[k][4] = 0.0;
344  b[k][5] = P.get_x();
345 
346  b[k+1][0] = 0.0;
347  b[k+1][1] = -P.get_oY();
348  b[k+1][2] = P.get_oY()*P.get_y();
349  b[k+1][3] = 0.0;
350  b[k+1][4] = -1.0;
351  b[k+1][5] = P.get_y();
352 
353  k += 2;
354  }
355  }
356  vpColVector X1(3) ;
357  vpColVector X2(6) ;
358 
359 #if (DEBUG_LEVEL2)
360  {
361  std::cout <<"a " << a << std::endl ;
362  std::cout <<"b " << b << std::endl ;
363  }
364 #endif
365 
366  lagrange(a,b,X1,X2);
367 
368 #if (DEBUG_LEVEL2)
369  {
370  std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
371  std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
372  }
373 #endif
374 
375  if (X2[5] < 0.0)
376  { /* car Zo > 0 */
377  for (i=0;i<3;i++) X1[i] = -X1[i];
378  for (i=0;i<6;i++) X2[i] = -X2[i];
379  }
380  s = 0.0;
381  for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
382  for (i=0;i<3;i++) {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0 */
383 
384  //s = 0.0;
385  //for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
386  s = X2[0]*X2[0] + X2[1]*X2[1] + X2[2]*X2[2]; // To avoid a Coverity copy/past error
387 
388  if (s<1e-10)
389  {
390 // std::cout << "Points that produce an error: " << std::endl;
391 // for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
392 // {
393 // std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() << " "
394 // << (*it).get_oX() << " " << (*it).get_oY() << " " << (*it).get_oZ() << std::endl;
395 // }
397  "Division by zero in Lagrange pose computation (planar plane case)")) ;
398  }
399 
400  s = 1.0/sqrt(s);
401  for (i=0;i<3;i++) {X2[i] *= s;} /* X2^T X2 = 1 */
402 
403 
404  calculTranslation (a, b, nl, 3, 3, X1, X2) ;
405 
406  // if (err != OK)
407  {
408  // std::cout << "in (vpCalculPose_plan.cc)CalculTranslation returns " ;
409  // PrintError(err) ;
410  // return err ;
411  }
412 
413  if (coplanar_plane_type == 1) { // plane ax=d
414  cMo[0][0] = (X1[1]*X2[2])-(X1[2]*X2[1]);
415  cMo[1][0] = (X1[2]*X2[0])-(X1[0]*X2[2]);
416  cMo[2][0] = (X1[0]*X2[1])-(X1[1]*X2[0]);
417 
418  for (i=0;i<3;i++)
419  { /* calcul de la matrice de passage */
420  cMo[i][1] = X1[i];
421  cMo[i][2] = X2[i];
422  cMo[i][3] = X2[i+3];
423  }
424 
425  }
426  else if (coplanar_plane_type == 2) { // plane by=d
427  cMo[0][1] = (X1[1]*X2[2])-(X1[2]*X2[1]);
428  cMo[1][1] = (X1[2]*X2[0])-(X1[0]*X2[2]);
429  cMo[2][1] = (X1[0]*X2[1])-(X1[1]*X2[0]);
430 
431  for (i=0;i<3;i++)
432  { /* calcul de la matrice de passage */
433  cMo[i][0] = X1[i];
434  cMo[i][2] = X2[i];
435  cMo[i][3] = X2[i+3];
436  }
437  }
438  else { // plane cz=d or any other
439 
440  cMo[0][2] = (X1[1]*X2[2])-(X1[2]*X2[1]);
441  cMo[1][2] = (X1[2]*X2[0])-(X1[0]*X2[2]);
442  cMo[2][2] = (X1[0]*X2[1])-(X1[1]*X2[0]);
443 
444  for (i=0;i<3;i++)
445  { /* calcul de la matrice de passage */
446  cMo[i][0] = X1[i];
447  cMo[i][1] = X2[i];
448  cMo[i][3] = X2[i+3];
449  }
450  }
451  }
452  catch(...)
453  {
454  throw; // throw the original exception
455  }
456 
457 #if (DEBUG_LEVEL1)
458  std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
459 #endif
460  // return(OK);
461 }
462 
463 
464 void
466 {
467 
468 #if (DEBUG_LEVEL1)
469  std::cout << "begin CPose::PoseLagrange(...) " << std::endl ;
470 #endif
471  try{
472  double s;
473  unsigned int i;
474 
475  unsigned int k=0;
476  unsigned int nl=npt*2;
477 
478  vpMatrix a(nl,3) ;
479  vpMatrix b(nl,9);
480  b =0 ;
481 
482  vpPoint P ;
483  i=0 ;
484  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
485  {
486  P = *it;
487  a[k][0] = -P.get_oX();
488  a[k][1] = 0.0;
489  a[k][2] = P.get_oX()*P.get_x();
490 
491  a[k+1][0] = 0.0;
492  a[k+1][1] = -P.get_oX();
493  a[k+1][2] = P.get_oX()*P.get_y();
494 
495  b[k][0] = -P.get_oY();
496  b[k][1] = 0.0;
497  b[k][2] = P.get_oY()*P.get_x();
498 
499  b[k][3] = -P.get_oZ();
500  b[k][4] = 0.0;
501  b[k][5] = P.get_oZ()*P.get_x();
502 
503  b[k][6] = -1.0;
504  b[k][7] = 0.0;
505  b[k][8] = P.get_x();
506 
507  b[k+1][0] = 0.0;
508  b[k+1][1] = -P.get_oY();
509  b[k+1][2] = P.get_oY()*P.get_y();
510 
511  b[k+1][3] = 0.0;
512  b[k+1][4] = -P.get_oZ();
513  b[k+1][5] = P.get_oZ()*P.get_y();
514 
515  b[k+1][6] = 0.0;
516  b[k+1][7] = -1.0;
517  b[k+1][8] = P.get_y();
518 
519  k += 2;
520  }
521  vpColVector X1(3) ;
522  vpColVector X2(9) ;
523 
524 #if (DEBUG_LEVEL2)
525  {
526  std::cout <<"a " << a << std::endl ;
527  std::cout <<"b " << b << std::endl ;
528  }
529 #endif
530 
531  lagrange(a,b,X1,X2);
532  // if (err != OK)
533  {
534  // std::cout << "in (CLagrange.cc)Lagrange returns " ;
535  // PrintError(err) ;
536  // return err ;
537  }
538 
539 
540 #if (DEBUG_LEVEL2)
541  {
542  std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
543  std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
544  }
545 #endif
546 
547  if (X2[8] < 0.0)
548  { /* car Zo > 0 */
549  X1 *= -1 ;
550  X2 *= -1 ;
551  }
552  s = 0.0;
553  for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
554  for (i=0;i<3;i++) {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0 */
555 
556  //s = 0.0;
557  //for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
558  s = X2[0]*X2[0] + X2[1]*X2[1] + X2[2]*X2[2]; // To avoid a Coverity copy/past error
559 
560  if (s<1e-10)
561  {
562 // std::cout << "Points that produce an error: " << std::endl;
563 // for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
564 // {
565 // std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() << " "
566 // << (*it).get_oX() << " " << (*it).get_oY() << " " << (*it).get_oZ() << std::endl;
567 // }
568  //vpERROR_TRACE(" division par zero " ) ;
570  "Division by zero in Lagrange pose computation (non planar plane case)")) ;
571  }
572 
573  s = 1.0/sqrt(s);
574  for (i=0;i<3;i++) {X2[i] *= s;} /* X2^T X2 = 1 */
575 
576  X2[3] = (X1[1]*X2[2])-(X1[2]*X2[1]);
577  X2[4] = (X1[2]*X2[0])-(X1[0]*X2[2]);
578  X2[5] = (X1[0]*X2[1])-(X1[1]*X2[0]);
579 
580  calculTranslation (a, b, nl, 3, 6, X1, X2) ;
581 
582  for (i=0 ; i<3 ; i++)
583  {
584  cMo[i][0] = X1[i];
585  cMo[i][1] = X2[i];
586  cMo[i][2] = X2[i+3];
587  cMo[i][3] = X2[i+6];
588  }
589 
590  }
591  catch(...)
592  {
593  throw; // throw the original exception
594  }
595 
596 #if (DEBUG_LEVEL1)
597  std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
598 #endif
599 }
600 
601 
602 #undef DEBUG_LEVEL1
603 #undef DEBUG_LEVEL2
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
void poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type=0)
Compute the pose of a planar object using Lagrange approach.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
#define vpERROR_TRACE
Definition: vpDebug.h:391
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:449
error that can be emited by ViSP classes.
Definition: vpException.h:73
unsigned int getCols() const
Return the number of columns of the 2D array.
Definition: vpArray2D.h:154
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:458
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:101
Class that defines what is a point.
Definition: vpPoint.h:59
void svd(vpColVector &w, vpMatrix &v)
Definition: vpMatrix.cpp:1631
vpRowVector t() const
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:451
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:456
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:100
double sumSquare() const
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:447
vpMatrix t() const
Definition: vpMatrix.cpp:207
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpMatrix inverseByLU() const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1741