ViSP  2.8.0
vpPoseLagrange.cpp
1 /****************************************************************************
2 *
3 * $Id: vpPoseLagrange.cpp 4056 2013-01-05 13:04:42Z fspindle $
4 *
5 * This file is part of the ViSP software.
6 * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7 *
8 * This software is free software; you can redistribute it and/or
9 * modify it under the terms of the GNU General Public License
10 * ("GPL") version 2 as published by the Free Software Foundation.
11 * See the file LICENSE.txt at the root directory of this source
12 * distribution for additional information about the GNU GPL.
13 *
14 * For using ViSP with software that can not be combined with the GNU
15 * GPL, please contact INRIA about acquiring a ViSP Professional
16 * Edition License.
17 *
18 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19 *
20 * This software was developed at:
21 * INRIA Rennes - Bretagne Atlantique
22 * Campus Universitaire de Beaulieu
23 * 35042 Rennes Cedex
24 * France
25 * http://www.irisa.fr/lagadic
26 *
27 * If you have questions regarding the use of this file, please contact
28 * INRIA at visp@inria.fr
29 *
30 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32 *
33 *
34 * Description:
35 * Pose computation.
36 *
37 * Authors:
38 * Eric Marchand
39 * Francois Chaumette
40 *
41 *****************************************************************************/
42 
43 
44 #include <visp/vpPose.h>
45 
46 #define DEBUG_LEVEL1 0
47 #define DEBUG_LEVEL2 0
48 
49 /**********************************************************************/
50 /* FONCTION : CalculTranslation */
51 /* ROLE : Calcul de la translation entre la */
52 /* camera et l'outil connaissant la */
53 /* rotation */
54 /**********************************************************************/
55 
56 static
57 void
58 calculTranslation (vpMatrix &a, vpMatrix &b, unsigned int nl, unsigned int nc1,
59  unsigned int nc3, vpColVector &x1, vpColVector &x2)
60 {
61 
62  try
63  {
64  unsigned int i,j;
65 
66  vpMatrix ct(3,nl) ;
67  for (i=0 ; i < 3 ; i++)
68  {
69  for (j=0 ; j < nl ; j++)
70  ct[i][j] = b[j][i+nc3] ;
71  }
72 
73  vpMatrix c ;
74  c = ct.t() ;
75 
76  vpMatrix ctc ;
77  ctc = ct*c ;
78 
79  vpMatrix ctc1 ; // (C^T C)^(-1)
80  ctc1 = ctc.inverseByLU() ;
81 
82  vpMatrix cta ;
83  vpMatrix ctb ;
84  cta = ct*a ; /* C^T A */
85  ctb = ct*b ; /* C^T B */
86 
87 #if (DEBUG_LEVEL2)
88  {
89  std::cout <<"ctc " << std::endl << ctc ;
90  std::cout <<"cta " << std::endl << cta ;
91  std::cout <<"ctb " << std::endl << ctb ;
92  }
93 #endif
94 
95  vpColVector X2(nc3) ;
96  vpMatrix CTB(nc1,nc3) ;
97  for (i=0 ; i < nc1 ; i++)
98  {
99  for (j=0 ; j < nc3 ; j++)
100  CTB[i][j] = ctb[i][j] ;
101  }
102 
103  for (j=0 ; j < nc3 ; j++)
104  X2[j] = x2[j] ;
105 
106  vpColVector sv ; // C^T A X1 + C^T B X2)
107  sv = cta*x1 + CTB*X2 ;// C^T A X1 + C^T B X2)
108 
109 #if (DEBUG_LEVEL2)
110  std::cout << "sv " << sv.t() ;
111 #endif
112 
113  vpColVector X3 ; /* X3 = - (C^T C )^{-1} C^T (A X1 + B X2) */
114  X3 = -ctc1*sv ;
115 
116 #if (DEBUG_LEVEL2)
117  std::cout << "x3 " << X3.t() ;
118 #endif
119 
120  for (i=0 ; i < nc1 ; i++)
121  x2[i+nc3] = X3[i] ;
122  }
123  catch(...)
124  {
125 
126  // en fait il y a des dizaines de raisons qui font que cette fonction
127  // rende une erreur (matrice pas inversible, pb de memoire etc...)
128  vpERROR_TRACE(" ") ;
129  throw ;
130  }
131 
132 
133 }
134 
135 
136 //*********************************************************************
137 // FONCTION LAGRANGE :
138 // -------------------
139 // Resolution d'un systeme lineaire de la forme A x1 + B x2 = 0
140 // sous la contrainte || x1 || = 1
141 // ou A est de dimension nl x nc1 et B nl x nc2
142 //*********************************************************************
143 
144 //#define EPS 1.e-5
145 
146 static
147 void
148 lagrange (vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2)
149 {
150 #if (DEBUG_LEVEL1)
151  std::cout << "begin (CLagrange.cc)Lagrange(...) " << std::endl;
152 #endif
153 
154  try{
155  unsigned int i,imin;
156 
157  vpMatrix ata ; // A^T A
158  ata = a.t()*a ;
159  vpMatrix btb ; // B^T B
160  btb = b.t()*b ;
161 
162  vpMatrix bta ; // B^T A
163  bta = b.t()*a ;
164 
165  vpMatrix btb1 ; // (B^T B)^(-1)
166 
167  if (b.getRows() >= b.getCols()) btb1 = btb.inverseByLU() ;
168  else btb1 = btb.pseudoInverse();
169 
170 #if (DEBUG_LEVEL1)
171  {
172  std::cout << " BTB1 * BTB : " << std::endl << btb1*btb << std::endl;
173  std::cout << " BTB * BTB1 : " << std::endl << btb*btb1 << std::endl;
174  }
175 #endif
176 
177  vpMatrix r ; // (B^T B)^(-1) B^T A
178  r = btb1*bta ;
179 
180  vpMatrix e ; // - A^T B (B^T B)^(-1) B^T A
181  e = - (a.t()*b) *r ;
182 
183  e += ata ; // calcul E = A^T A - A^T B (B^T B)^(-1) B^T A
184 
185 #if (DEBUG_LEVEL1)
186  {
187  std::cout << " E :" << std::endl << e << std::endl;
188  }
189 #endif
190 
191  // vpColVector sv ;
192  // vpMatrix v ;
193  e.svd(x1,ata) ;// destructif sur e
194  // calcul du vecteur propre de E correspondant a la valeur propre min.
195 
196  /* calcul de SVmax */
197  imin = 0;
198  // FC : Pourquoi calculer SVmax ??????
199  // double svm = 0.0;
200  // for (i=0;i<x1.getRows();i++)
201  // {
202  // if (x1[i] > svm) { svm = x1[i]; imin = i; }
203  // }
204  // svm *= EPS; /* pour le rang */
205 
206  for (i=0;i<x1.getRows();i++)
207  if (x1[i] < x1[imin]) imin = i;
208 
209 #if (DEBUG_LEVEL1)
210  {
211  printf("SV(E) : %.15lf %.15lf %.15lf\n",x1[0],x1[1],x1[2]);
212  std::cout << " i_min " << imin << std::endl;
213  }
214 #endif
215  for (i=0;i<x1.getRows();i++)
216  x1[i] = ata[i][imin];
217 
218  x2 = - (r*x1) ; // X_2 = - (B^T B)^(-1) B^T A X_1
219 
220 #if (DEBUG_LEVEL1)
221  {
222  std::cout << " X1 : " << x1.t() << std::endl;
223  std::cout << " V : " << std::endl << ata << std::endl;
224  }
225 #endif
226  }
227  catch(...)
228  {
229  vpERROR_TRACE(" ") ;
230  throw ;
231  }
232 #if (DEBUG_LEVEL1)
233  std::cout << "end (CLagrange.cc)Lagrange(...) " << std::endl;
234 #endif
235 }
236 
237 //#undef EPS
238 
249 void
250 vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type)
251 {
252 
253 #if (DEBUG_LEVEL1)
254  std::cout << "begin vpPose::PoseLagrange(...) " << std::endl ;
255 #endif
256  try
257  {
258  double s;
259  unsigned int i;
260 
261  unsigned int k=0;
262  unsigned int nl=npt*2;
263 
264 
265  vpMatrix a(nl,3) ;
266  vpMatrix b(nl,6);
267  vpPoint P ;
268  i=0 ;
269 
270  if (coplanar_plane_type == 1) { // plane ax=d
271  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
272  {
273  P = *it ;
274  a[k][0] = -P.get_oY();
275  a[k][1] = 0.0;
276  a[k][2] = P.get_oY()*P.get_x();
277 
278  a[k+1][0] = 0.0;
279  a[k+1][1] = -P.get_oY();
280  a[k+1][2] = P.get_oY()*P.get_y();
281 
282  b[k][0] = -P.get_oZ();
283  b[k][1] = 0.0;
284  b[k][2] = P.get_oZ()*P.get_x();
285  b[k][3] = -1.0;
286  b[k][4] = 0.0;
287  b[k][5] = P.get_x();
288 
289  b[k+1][0] = 0.0;
290  b[k+1][1] = -P.get_oZ();
291  b[k+1][2] = P.get_oZ()*P.get_y();
292  b[k+1][3] = 0.0;
293  b[k+1][4] = -1.0;
294  b[k+1][5] = P.get_y();
295 
296  k += 2;
297  }
298 
299  }
300  else if (coplanar_plane_type == 2) { // plane by=d
301  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
302  {
303  P = *it ;
304  a[k][0] = -P.get_oX();
305  a[k][1] = 0.0;
306  a[k][2] = P.get_oX()*P.get_x();
307 
308  a[k+1][0] = 0.0;
309  a[k+1][1] = -P.get_oX();
310  a[k+1][2] = P.get_oX()*P.get_y();
311 
312  b[k][0] = -P.get_oZ();
313  b[k][1] = 0.0;
314  b[k][2] = P.get_oZ()*P.get_x();
315  b[k][3] = -1.0;
316  b[k][4] = 0.0;
317  b[k][5] = P.get_x();
318 
319  b[k+1][0] = 0.0;
320  b[k+1][1] = -P.get_oZ();
321  b[k+1][2] = P.get_oZ()*P.get_y();
322  b[k+1][3] = 0.0;
323  b[k+1][4] = -1.0;
324  b[k+1][5] = P.get_y();
325 
326  k += 2;
327  }
328 
329  }
330  else { // plane cz=d or any other
331 
332  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
333  {
334  P = *it ;
335  a[k][0] = -P.get_oX();
336  a[k][1] = 0.0;
337  a[k][2] = P.get_oX()*P.get_x();
338 
339  a[k+1][0] = 0.0;
340  a[k+1][1] = -P.get_oX();
341  a[k+1][2] = P.get_oX()*P.get_y();
342 
343  b[k][0] = -P.get_oY();
344  b[k][1] = 0.0;
345  b[k][2] = P.get_oY()*P.get_x();
346  b[k][3] = -1.0;
347  b[k][4] = 0.0;
348  b[k][5] = P.get_x();
349 
350  b[k+1][0] = 0.0;
351  b[k+1][1] = -P.get_oY();
352  b[k+1][2] = P.get_oY()*P.get_y();
353  b[k+1][3] = 0.0;
354  b[k+1][4] = -1.0;
355  b[k+1][5] = P.get_y();
356 
357  k += 2;
358  }
359  }
360  vpColVector X1(3) ;
361  vpColVector X2(6) ;
362 
363 #if (DEBUG_LEVEL2)
364  {
365  std::cout <<"a " << a << std::endl ;
366  std::cout <<"b " << b << std::endl ;
367  }
368 #endif
369 
370  lagrange(a,b,X1,X2);
371 
372 #if (DEBUG_LEVEL2)
373  {
374  std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
375  std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
376  }
377 #endif
378 
379  if (X2[5] < 0.0)
380  { /* car Zo > 0 */
381  for (i=0;i<3;i++) X1[i] = -X1[i];
382  for (i=0;i<6;i++) X2[i] = -X2[i];
383  }
384  s = 0.0;
385  for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
386  for (i=0;i<3;i++) {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0 */
387 
388  s = 0.0;
389  for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
390 
391  if (s<1e-10)
392  {
393  std::cout << "Points that produce an error: " << std::endl;
394  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
395  {
396  std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() << " "
397  << (*it).get_oX() << " " << (*it).get_oY() << " " << (*it).get_oZ() << std::endl;
398  }
399  vpERROR_TRACE( "division par zero ") ;
401  "division by zero ")) ;
402  }
403 
404  s = 1.0/sqrt(s);
405  for (i=0;i<3;i++) {X2[i] *= s;} /* X2^T X2 = 1 */
406 
407 
408  calculTranslation (a, b, nl, 3, 3, X1, X2) ;
409 
410  // if (err != OK)
411  {
412  // std::cout << "in (vpCalculPose_plan.cc)CalculTranslation returns " ;
413  // PrintError(err) ;
414  // return err ;
415  }
416 
417  if (coplanar_plane_type == 1) { // plane ax=d
418  cMo[0][0] = (X1[1]*X2[2])-(X1[2]*X2[1]);
419  cMo[1][0] = (X1[2]*X2[0])-(X1[0]*X2[2]);
420  cMo[2][0] = (X1[0]*X2[1])-(X1[1]*X2[0]);
421 
422  for (i=0;i<3;i++)
423  { /* calcul de la matrice de passage */
424  cMo[i][1] = X1[i];
425  cMo[i][2] = X2[i];
426  cMo[i][3] = X2[i+3];
427  }
428 
429  }
430  else if (coplanar_plane_type == 2) { // plane by=d
431  cMo[0][1] = (X1[1]*X2[2])-(X1[2]*X2[1]);
432  cMo[1][1] = (X1[2]*X2[0])-(X1[0]*X2[2]);
433  cMo[2][1] = (X1[0]*X2[1])-(X1[1]*X2[0]);
434 
435  for (i=0;i<3;i++)
436  { /* calcul de la matrice de passage */
437  cMo[i][0] = X1[i];
438  cMo[i][2] = X2[i];
439  cMo[i][3] = X2[i+3];
440  }
441  }
442  else { // plane cz=d or any other
443 
444  cMo[0][2] = (X1[1]*X2[2])-(X1[2]*X2[1]);
445  cMo[1][2] = (X1[2]*X2[0])-(X1[0]*X2[2]);
446  cMo[2][2] = (X1[0]*X2[1])-(X1[1]*X2[0]);
447 
448  for (i=0;i<3;i++)
449  { /* calcul de la matrice de passage */
450  cMo[i][0] = X1[i];
451  cMo[i][1] = X2[i];
452  cMo[i][3] = X2[i+3];
453  }
454  }
455  }
456  catch(...)
457  {
458  vpERROR_TRACE(" ") ;
459  throw ;
460  }
461 
462 
463 #if (DEBUG_LEVEL1)
464  std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
465 #endif
466  // return(OK);
467 }
468 
469 
470 void
472 {
473 
474 #if (DEBUG_LEVEL1)
475  std::cout << "begin CPose::PoseLagrange(...) " << std::endl ;
476 #endif
477  try{
478  double s;
479  unsigned int i;
480 
481  unsigned int k=0;
482  unsigned int nl=npt*2;
483 
484  vpMatrix a(nl,3) ;
485  vpMatrix b(nl,9);
486  b =0 ;
487 
488  vpPoint P ;
489  i=0 ;
490  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
491  {
492  P = *it;
493  a[k][0] = -P.get_oX();
494  a[k][1] = 0.0;
495  a[k][2] = P.get_oX()*P.get_x();
496 
497  a[k+1][0] = 0.0;
498  a[k+1][1] = -P.get_oX();
499  a[k+1][2] = P.get_oX()*P.get_y();
500 
501  b[k][0] = -P.get_oY();
502  b[k][1] = 0.0;
503  b[k][2] = P.get_oY()*P.get_x();
504 
505  b[k][3] = -P.get_oZ();
506  b[k][4] = 0.0;
507  b[k][5] = P.get_oZ()*P.get_x();
508 
509  b[k][6] = -1.0;
510  b[k][7] = 0.0;
511  b[k][8] = P.get_x();
512 
513  b[k+1][0] = 0.0;
514  b[k+1][1] = -P.get_oY();
515  b[k+1][2] = P.get_oY()*P.get_y();
516 
517  b[k+1][3] = 0.0;
518  b[k+1][4] = -P.get_oZ();
519  b[k+1][5] = P.get_oZ()*P.get_y();
520 
521  b[k+1][6] = 0.0;
522  b[k+1][7] = -1.0;
523  b[k+1][8] = P.get_y();
524 
525  k += 2;
526  }
527  vpColVector X1(3) ;
528  vpColVector X2(9) ;
529 
530 #if (DEBUG_LEVEL2)
531  {
532  std::cout <<"a " << a << std::endl ;
533  std::cout <<"b " << b << std::endl ;
534  }
535 #endif
536 
537  lagrange(a,b,X1,X2);
538  // if (err != OK)
539  {
540  // std::cout << "in (CLagrange.cc)Lagrange returns " ;
541  // PrintError(err) ;
542  // return err ;
543  }
544 
545 
546 #if (DEBUG_LEVEL2)
547  {
548  std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
549  std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
550  }
551 #endif
552 
553  if (X2[8] < 0.0)
554  { /* car Zo > 0 */
555  X1 *= -1 ;
556  X2 *= -1 ;
557  }
558  s = 0.0;
559  for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
560  for (i=0;i<3;i++) {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0 */
561 
562  s = 0.0;
563  for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
564 
565  if (s<1e-10)
566  {
567  std::cout << "Points that produce an error: " << std::endl;
568  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
569  {
570  std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() << " "
571  << (*it).get_oX() << " " << (*it).get_oY() << " " << (*it).get_oZ() << std::endl;
572  }
573  vpERROR_TRACE(" division par zero " ) ;
575  "division by zero ")) ;
576 
577  }
578 
579  s = 1.0/sqrt(s);
580  for (i=0;i<3;i++) {X2[i] *= s;} /* X2^T X2 = 1 */
581 
582  X2[3] = (X1[1]*X2[2])-(X1[2]*X2[1]);
583  X2[4] = (X1[2]*X2[0])-(X1[0]*X2[2]);
584  X2[5] = (X1[0]*X2[1])-(X1[1]*X2[0]);
585 
586  calculTranslation (a, b, nl, 3, 6, X1, X2) ;
587 
588 
589  for (i=0 ; i<3 ; i++)
590  {
591  cMo[i][0] = X1[i];
592  cMo[i][1] = X2[i];
593  cMo[i][2] = X2[i+3];
594  cMo[i][3] = X2[i+6];
595  }
596 
597  }
598  catch(...)
599  {
600  vpERROR_TRACE(" ") ;
601  throw ;
602  }
603 
604 #if (DEBUG_LEVEL1)
605  std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
606 #endif
607 }
608 
609 
610 #undef DEBUG_LEVEL1
611 #undef DEBUG_LEVEL2
612 
613 
614 
615 /*
616 * Local variables:
617 * c-basic-offset: 2
618 * End:
619 */
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
void poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type=0)
compute the pose using Lagrange approach (planar object)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:379
void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
compute the pose using Lagrange approach (non planar object)
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.h:129
error that can be emited by ViSP classes.
Definition: vpException.h:75
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.h:138
double sumSquare() const
return sum of the Aij^2 (for all i, for all j)
Definition: vpMatrix.cpp:760
std::list< vpPoint > listP
array of point (use here class vpPoint)
Definition: vpPose.h:95
Class that defines what is a point.
Definition: vpPoint.h:65
void svd(vpColVector &w, vpMatrix &v)
Definition: vpMatrix.cpp:1702
vpRowVector t() const
transpose of Vector
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.h:131
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.h:136
unsigned int npt
number of point used in pose computation
Definition: vpPose.h:94
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.h:127
vpMatrix t() const
Definition: vpMatrix.cpp:1176
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
vpMatrix inverseByLU() const
unsigned int getCols() const
Return the number of columns of the matrix.
Definition: vpMatrix.h:159
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1812
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:157