Visual Servoing Platform  version 3.0.0
vpPoseDementhon.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 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 
41 #include <visp3/vision/vpPose.h>
42 #include <visp3/core/vpMath.h>
43 
44 #define DEBUG_LEVEL1 0
45 #define DEBUG_LEVEL2 0
46 #define DEBUG_LEVEL3 0
47 
48 /* FC
49 #ifndef DEG
50 #define DEG (180.0/M_PI)
51 #endif
52 */
53 
61 void
62 vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
63 {
64  double normI = 0., normJ = 0.;
65  double Z0 = 0.;
66  double seuil=1.0;
67  double f=1.;
68 
69  vpPoint p0 = listP.front() ;
70 
71  c3d.clear();
72  vpPoint P;
73  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
74  {
75  P = (*it);
76  P.set_oX(P.get_oX()-p0.get_oX()) ;
77  P.set_oY(P.get_oY()-p0.get_oY()) ;
78  P.set_oZ(P.get_oZ()-p0.get_oZ()) ;
79  c3d.push_back(P) ;
80  }
81 
82  vpMatrix a(npt,3) ;
83 
84  for (unsigned int i=0 ; i < npt ; i++)
85  {
86  a[i][0]=c3d[i].get_oX();
87  a[i][1]=c3d[i].get_oY();
88  a[i][2]=c3d[i].get_oZ();
89  }
90 
91  //std::cout << a << std::endl ;
92  // calcul a^T a
93  vpMatrix ata ;
94  ata = a.t()*a ;
95 
96  // calcul (a^T a)^-1 par decomposition LU
97  vpMatrix ata1 ;
98  ata1 = ata.pseudoInverse(1e-6) ; //InverseByLU() ;
99 
100  vpMatrix b ;
101  b = (a*ata1).t() ;
102 
103 #if (DEBUG_LEVEL2)
104  {
105  std::cout << "a" << std::endl <<a<<std::endl ;
106  std::cout << "ata" << std::endl <<ata<<std::endl ;
107  std::cout << "ata1" << std::endl <<ata1<<std::endl ;
108  std::cout<< " ata*ata1" << std::endl << ata*ata1 ;
109  std::cout<< " b" << std::endl << (a*ata1).t() ;
110 
111  }
112 #endif
113 
114  // calcul de la premiere solution
115 
116  vpColVector eps(npt) ;
117  eps =0 ;
118 
119  int cpt = 0 ;
120  vpColVector I, J, k ;
121  I.resize(3) ;
122  J.resize(3) ;
123  k.resize(3) ;
124 
125  while(cpt < 20)
126  {
127  I = 0 ;
128  J = 0 ;
129 
130  vpColVector xprim(npt) ;
131  vpColVector yprim(npt) ;
132  for (unsigned int i=0;i<npt;i++)
133  {
134  xprim[i]=(1+ eps[i])*c3d[i].get_x() - c3d[0].get_x();
135  yprim[i]=(1+ eps[i])*c3d[i].get_y() - c3d[0].get_y();
136  }
137  I = b*xprim ;
138  J = b*yprim ;
139  normI = sqrt(I.sumSquare()) ;
140  normJ = sqrt(J.sumSquare()) ;
141  I = I/normI ;
142  J = J/normJ ;
143 
144  if (normI+normJ < 1e-10)
145  {
146  //vpERROR_TRACE(" normI+normJ = 0, division par zero " ) ;
147  throw(vpException(vpException::divideByZeroError,
148  "Division by zero in Dementhon pose computation: normI+normJ = 0")) ;
149  }
150 
151  k = vpColVector::cross(I,J) ;
152  Z0=2*f/(normI+normJ);
153  cpt=cpt+1; seuil=0.0;
154  for (unsigned int i=0; i<npt; i++)
155  {
156  double epsi_1 = eps[i] ;
157  eps[i]=(c3d[i].get_oX()*k[0]+c3d[i].get_oY()*k[1]+c3d[i].get_oZ()*k[2])/Z0;
158  seuil+=fabs(eps[i]-epsi_1);
159  }
160  if (npt==0)
161  {
162  //vpERROR_TRACE( " npt = 0, division par zero ");
163  throw(vpException(vpException::divideByZeroError,
164  "Division by zero in Dementhon pose computation: no points")) ;
165  }
166  seuil/=npt;
167  }
168  k.normalize();
169  J = vpColVector::cross(k,I) ;
170  /*matrice de passage*/
171 
172  cMo[0][0]=I[0];
173  cMo[0][1]=I[1];
174  cMo[0][2]=I[2];
175  cMo[0][3]=c3d[0].get_x()*2/(normI+normJ);
176 
177  cMo[1][0]=J[0];
178  cMo[1][1]=J[1];
179  cMo[1][2]=J[2];
180  cMo[1][3]=c3d[0].get_y()*2/(normI+normJ);
181 
182  cMo[2][0]=k[0];
183  cMo[2][1]=k[1];
184  cMo[2][2]=k[2];
185  cMo[2][3]=Z0;
186 
187  cMo[0][3] -= (p0.get_oX()*cMo[0][0]+p0.get_oY()*cMo[0][1]+p0.get_oZ()*cMo[0][2]);
188  cMo[1][3] -= (p0.get_oX()*cMo[1][0]+p0.get_oY()*cMo[1][1]+p0.get_oZ()*cMo[1][2]);
189  cMo[2][3] -= (p0.get_oX()*cMo[2][0]+p0.get_oY()*cMo[2][1]+p0.get_oZ()*cMo[2][2]);
190 }
191 
192 
193 #define DMIN 0.01 /* distance min entre la cible et la camera */
194 #define EPS 0.0000001
195 #define EPS_DEM 0.001
196 
197 static void
198 calculRTheta(double s, double c, double &r, double &theta)
199 {
200  if ((fabs(c) > EPS_DEM) || (fabs(s) > EPS_DEM))
201  {
202  r = sqrt(sqrt(s*s+c*c));
203  theta = atan2(s,c)/2.0;
204  }
205  else
206  {
207  if (fabs(c) > fabs(s))
208  {
209  r = fabs(c);
210  if (c >= 0.0)
211  theta = M_PI/2;
212  else
213  theta = -M_PI/2;
214  }
215  else
216  {
217  r = fabs(s);
218  if (s >= 0.0)
219  theta = M_PI/4.0;
220  else
221  theta = -M_PI/4.0;
222  }
223  }
224 }
225 
226 static
227 void calculSolutionDementhon(double xi0, double yi0,
228  vpColVector &I, vpColVector &J,
229  vpHomogeneousMatrix &cMo )
230 {
231 
232 #if (DEBUG_LEVEL1)
233  std::cout << "begin (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
234 #endif
235 
236  double normI, normJ, normk, Z0;
237  vpColVector k(3);
238 
239  // normalisation de I et J
240  normI = sqrt(I.sumSquare()) ;
241  normJ = sqrt(J.sumSquare()) ;
242 
243  I/=normI;
244  J/=normJ;
245 
246 
247  k = vpColVector::cross(I,J) ; // k = I^I
248 
249  Z0=2.0/(normI+normJ);
250 
251  normk = sqrt(k.sumSquare()) ;
252  k /= normk ;
253 
254  J = vpColVector::cross(k,I) ;
255 
256  //calcul de la matrice de passage
257  cMo[0][0]=I[0];
258  cMo[0][1]=I[1];
259  cMo[0][2]=I[2];
260  cMo[0][3]=xi0*Z0;
261 
262  cMo[1][0]=J[0];
263  cMo[1][1]=J[1];
264  cMo[1][2]=J[2];
265  cMo[1][3]=yi0*Z0;
266 
267  cMo[2][0]=k[0];
268  cMo[2][1]=k[1];
269  cMo[2][2]=k[2];
270  cMo[2][3]=Z0;
271 
272 
273 #if (DEBUG_LEVEL1)
274  std::cout << "end (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
275 #endif
276 
277 }
278 
279 int
280 vpPose::calculArbreDementhon(vpMatrix &b, vpColVector &U,
281  vpHomogeneousMatrix &cMo)
282 {
283 
284 #if (DEBUG_LEVEL1)
285  std::cout << "begin vpPose::CalculArbreDementhon() " << std::endl;
286 #endif
287 
288  unsigned int i, k;
289  int erreur = 0;
290  unsigned int cpt;
291  double s,c,si,co;
292  double smin,smin_old, s1,s2;
293  double r, theta;
294  vpHomogeneousMatrix cMo1,cMo2,cMo_old;
295 
296  unsigned int iter_max = 20;
297  vpMatrix eps(iter_max+1,npt) ;
298 
299 
300  // on test si tous les points sont devant la camera
301  for(i = 0; i < npt; i++)
302  {
303  double z ;
304  z = cMo[2][0]*c3d[i].get_oX()+cMo[2][1]*c3d[i].get_oY()+cMo[2][2]*c3d[i].get_oZ() + cMo[2][3];
305  if (z <= 0.0) erreur = -1;
306  }
307 
308  smin = sqrt(computeResidualDementhon(cMo)/npt) ;
309 
310  vpColVector xi(npt) ;
311  vpColVector yi(npt) ;
312 
313  if (erreur==0)
314  {
315  k=0;
316  for(i = 0; i < npt; i++)
317  {
318  xi[k] = c3d[i].get_x();
319  yi[k] = c3d[i].get_y();
320 
321  if (k != 0)
322  { // On ne prend pas le 1er point
323  eps[0][k] = (cMo[2][0]*c3d[i].get_oX() +
324  cMo[2][1]*c3d[i].get_oY() +
325  cMo[2][2]*c3d[i].get_oZ())/cMo[2][3];
326  }
327  k++;
328  }
329 
330 
331  vpColVector I0(3) ;
332  vpColVector J0(3) ;
333  vpColVector I(3) ;
334  vpColVector J(3) ;
335 
336  smin_old = 2*smin ;
337 
338  cpt = 0;
339  while ((cpt<20) && (smin_old > 0.01) && (smin <= smin_old))
340  {
341 #if (DEBUG_LEVEL2)
342  {
343  std::cout << "cpt " << cpt << std::endl ;
344  std::cout << "smin_old " << smin_old << std::endl ;
345  std::cout << "smin " << smin << std::endl ;
346  }
347 #endif
348 
349  smin_old = smin;
350  cMo_old = cMo;
351 
352  I0 = 0 ;
353  J0 = 0 ;
354 
355  for (i=1;i<npt;i++)
356  {
357  s = (1.0+eps[cpt][i])*xi[i] - xi[0];
358  I0[0] += b[0][i-1] * s;
359  I0[1] += b[1][i-1] * s;
360  I0[2] += b[2][i-1] * s;
361  s = (1.0+eps[cpt][i])*yi[i] - yi[0];
362  J0[0] += b[0][i-1] * s;
363  J0[1] += b[1][i-1] * s;
364  J0[2] += b[2][i-1] * s;
365  }
366 
367  s = -2.0*(vpColVector::dotProd(I0,J0));
368  c = J0.sumSquare() - I0.sumSquare() ;
369 
370  calculRTheta(s,c,r,theta);
371  co = cos(theta);
372  si = sin(theta);
373 
374  /* 1ere branche */
375  I = I0 + U*r*co ;
376  J = J0 + U*r*si ;
377 
378 #if (DEBUG_LEVEL3)
379  {
380  std::cout << "I " << I.t() ;
381  std::cout << "J " << J.t() ;
382  }
383 #endif
384 
385  calculSolutionDementhon(xi[0],yi[0],I,J,cMo1);
386  s1 = sqrt(computeResidualDementhon(cMo1)/npt) ;
387 #if (DEBUG_LEVEL3)
388  std::cout << "cMo1 "<< std::endl << cMo1 << std::endl ;
389 #endif
390 
391  /* 2eme branche */
392  I = I0 - U*r*co ;
393  J = J0 - U*r*si ;
394 #if (DEBUG_LEVEL3)
395  {
396  std::cout << "I " << I.t() ;
397  std::cout << "J " << J.t() ;
398  }
399 #endif
400 
401  calculSolutionDementhon(xi[0],yi[0],I,J,cMo2);
402  s2 = sqrt(computeResidualDementhon(cMo2)/npt) ;
403 #if (DEBUG_LEVEL3)
404  std::cout << "cMo2 "<< std::endl << cMo2 << std::endl ;
405 #endif
406 
407  cpt ++;
408  if (s1 <= s2)
409  {
410  smin = s1;
411  k = 0;
412  for(i = 0; i < npt; i++)
413  {
414  if (k != 0) { // On ne prend pas le 1er point
415  eps[cpt][k] = (cMo1[2][0]*c3d[i].get_oX() + cMo1[2][1]*c3d[i].get_oY()
416  + cMo1[2][2]*c3d[i].get_oZ())/cMo1[2][3];
417  }
418  k++;
419  }
420  cMo = cMo1 ;
421  }
422  else
423  {
424  smin = s2;
425  k = 0;
426  for(i = 0; i < npt; i++)
427  {
428  if (k != 0) { // On ne prend pas le 1er point
429  eps[cpt][k] = (cMo2[2][0]*c3d[i].get_oX() + cMo2[2][1]*c3d[i].get_oY()
430  + cMo2[2][2]*c3d[i].get_oZ())/cMo2[2][3];
431  }
432  k++;
433  }
434  cMo = cMo2 ;
435  }
436 
437  if (smin > smin_old)
438  {
439 #if (DEBUG_LEVEL2)
440  std::cout << "Divergence " << std::endl ;
441 #endif
442 
443  cMo = cMo_old ;
444  }
445 #if (DEBUG_LEVEL2)
446  {
447  std::cout << "s1 = " << s1 << std::endl ;
448  std::cout << "s2 = " << s2 << std::endl ;
449  std::cout << "smin = " << smin << std::endl ;
450  std::cout << "smin_old = " << smin_old << std::endl ;
451  }
452 #endif
453  }
454  }
455 #if (DEBUG_LEVEL1)
456  std::cout << "end vpPose::CalculArbreDementhon() return "<< erreur << std::endl;
457 #endif
458 
459  return erreur ;
460 }
461 
470 void
471 vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo)
472 {
473 #if (DEBUG_LEVEL1)
474  std::cout << "begin CCalculPose::PoseDementhonPlan()" << std::endl ;
475 #endif
476 
477  unsigned int i,j,k ;
478 
479  vpPoint p0 = listP.front() ;
480 
481  vpPoint P ;
482  c3d.clear();
483  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
484  {
485  P = *it;
486  P.set_oX(P.get_oX()-p0.get_oX()) ;
487  P.set_oY(P.get_oY()-p0.get_oY()) ;
488  P.set_oZ(P.get_oZ()-p0.get_oZ()) ;
489  c3d.push_back(P);
490  }
491 
492  vpMatrix a ;
493  try
494  {
495  a.resize(npt-1,3) ;
496  }
497  catch(...)
498  {
499  vpERROR_TRACE(" ") ;
500  throw ;
501  }
502 
503 
504  for (i=1 ; i < npt ; i++)
505  {
506  a[i-1][0]=c3d[i].get_oX();
507  a[i-1][1]=c3d[i].get_oY();
508  a[i-1][2]=c3d[i].get_oZ();
509  }
510 
511  // calcul a^T a
512  vpMatrix ata ;
513  ata = a.t()*a ;
514 
515  /* essai FC pour debug SVD */
516  /*
517  vpMatrix ata_old ;
518  ata_old = a.t()*a ;
519 
520  vpMatrix ata((ata_old.getRows()-1),(ata_old.getCols()-1)) ;
521  for (i=0;i<ata.getRows();i++)
522  for (j=0;j<ata.getCols();j++) ata[i][j] = ata_old[i][j];
523  */
524  vpMatrix ata_sav;
525  ata_sav = ata;
526 
527 #if (DEBUG_LEVEL2)
528  {
529  std::cout << "a" << std::endl <<a<<std::endl ;
530  std::cout << "ata" << std::endl <<ata<<std::endl ;
531  }
532 #endif
533 
534  // calcul (a^T a)^-1
535  vpMatrix ata1(ata.getRows(),ata.getCols()) ;
536  vpMatrix v(ata.getRows(),ata.getCols());
537  vpColVector sv(ata.getRows());
538  // ata1 = ata.i() ;
539  unsigned int imin = 0;
540  double s = 0.0;
541 
542  //calcul de ata^-1
543  ata.svd(sv,v) ;
544 
545  unsigned int nc = sv.getRows() ;
546  for (i=0; i < nc ; i++)
547  if (sv[i] > s) s = sv[i];
548 
549  s *= 0.0002;
550  int irank = 0;
551  for (i=0;i<nc;i++)
552  if (sv[i] > s ) irank++;
553 
554  double svm = 100.0;
555  for (i = 0; i < nc; i++)
556  if (sv[i] < svm) { imin = i; svm = sv[i]; }
557 
558 #if (DEBUG_LEVEL2)
559  {
560  std::cout << "rang: " << irank << std::endl ;;
561  std::cout <<"imin = " << imin << std::endl ;
562  std::cout << "sv " << sv.t() << std::endl ;
563  }
564 #endif
565 
566  for (i=0 ; i < ata.getRows() ; i++)
567  for (j=0 ; j < ata.getCols() ; j++)
568  {
569  ata1[i][j] = 0.0;
570  for (k=0 ; k < nc ; k++)
571  if (sv[k] > s)
572  ata1[i][j] += ((v[i][k]*ata[j][k])/sv[k]);
573  }
574 
575 
576 
577  vpMatrix b ; // b=(at a)^-1*at
578  b = ata1*a.t() ;
579 
580  //calcul de U
581  vpColVector U(3) ;
582  U = ata.getCol(imin) ;
583 
584 #if (DEBUG_LEVEL2)
585  {
586  std::cout << "a" << std::endl <<a<<std::endl ;
587  std::cout << "ata" << std::endl <<ata_sav<<std::endl ;
588  std::cout << "ata1" << std::endl <<ata1<<std::endl ;
589  std::cout << "ata1*ata" << std::endl << ata1*ata_sav ;
590  std::cout << "b" << std::endl << b ;
591  std::cout << "U " << U.t() << std::endl ;
592  }
593 #endif
594 
595  vpColVector xi(npt) ;
596  vpColVector yi(npt) ;
597  //calcul de la premiere solution
598  for (i = 0; i < npt; i++)
599  {
600  xi[i] = c3d[i].get_x() ;
601  yi[i] = c3d[i].get_y() ;
602 
603  }
604 
605  vpColVector I0(3) ; I0 = 0 ;
606  vpColVector J0(3) ; J0 = 0 ;
607  vpColVector I(3) ;
608  vpColVector J(3) ;
609 
610  for (i=1;i<npt;i++)
611  {
612  I0[0] += b[0][i-1] * (xi[i]-xi[0]);
613  I0[1] += b[1][i-1] * (xi[i]-xi[0]);
614  I0[2] += b[2][i-1] * (xi[i]-xi[0]);
615 
616  J0[0] += b[0][i-1] * (yi[i]-yi[0]);
617  J0[1] += b[1][i-1] * (yi[i]-yi[0]);
618  J0[2] += b[2][i-1] * (yi[i]-yi[0]);
619  }
620 
621 
622 #if (DEBUG_LEVEL2)
623  {
624  std::cout << "I0 "<<I0.t() ;
625  std::cout << "J0 "<<J0.t() ;
626  }
627 #endif
628 
629  s = -2.0*vpColVector::dotProd(I0,J0);
630  double c = J0.sumSquare() - I0.sumSquare() ;
631 
632  double r,theta,si,co ;
633  calculRTheta(s, c, r, theta);
634  co = cos(theta);
635  si = sin(theta);
636 
637  // calcul de la premiere solution
638  I = I0 + U*r*co ;
639  J = J0 + U*r*si ;
640 
641  vpHomogeneousMatrix cMo1f ;
642  calculSolutionDementhon(xi[0], yi[0], I, J, cMo1f);
643 
644 
645  int erreur1 = calculArbreDementhon(b, U, cMo1f);
646 
647  // calcul de la deuxieme solution
648  I = I0 - U*r*co ;
649  J = J0 - U*r*si ;
650 
651  vpHomogeneousMatrix cMo2f;
652  calculSolutionDementhon(xi[0], yi[0], I, J, cMo2f);
653 
654  int erreur2 = calculArbreDementhon(b, U, cMo2f);
655 
656  if ((erreur1 == 0) && (erreur2 == -1)) cMo = cMo1f ;
657  if ((erreur1 == -1) && (erreur2 == 0)) cMo = cMo2f ;
658  if ((erreur1 == 0) && (erreur2 == 0))
659  {
660  double s1 = sqrt(computeResidualDementhon(cMo1f)/npt) ;
661  double s2 = sqrt(computeResidualDementhon(cMo2f)/npt) ;
662 
663  if (s1<=s2) cMo = cMo1f ; else cMo = cMo2f ;
664  }
665 
666  cMo[0][3] -= p0.get_oX()*cMo[0][0]+p0.get_oY()*cMo[0][1]+p0.get_oZ()*cMo[0][2];
667  cMo[1][3] -= p0.get_oX()*cMo[1][0]+p0.get_oY()*cMo[1][1]+p0.get_oZ()*cMo[1][2];
668  cMo[2][3] -= p0.get_oX()*cMo[2][0]+p0.get_oY()*cMo[2][1]+p0.get_oZ()*cMo[2][2];
669 
670 #if (DEBUG_LEVEL1)
671  std::cout << "end CCalculPose::PoseDementhonPlan()" << std::endl ;
672 #endif
673 }
674 
675 #undef DMIN
676 #undef EPS
677 #undef EPS_DEM
678 
679 
688 double vpPose::computeResidualDementhon(const vpHomogeneousMatrix &cMo)
689 {
690  double residual_ = 0 ;
691 
692  residual_ =0 ;
693  for (unsigned int i =0 ; i < npt ; i++)
694  {
695 
696  double X = c3d[i].get_oX()*cMo[0][0]+c3d[i].get_oY()*cMo[0][1]+c3d[i].get_oZ()*cMo[0][2] + cMo[0][3];
697  double Y = c3d[i].get_oX()*cMo[1][0]+c3d[i].get_oY()*cMo[1][1]+c3d[i].get_oZ()*cMo[1][2] + cMo[1][3];
698  double Z = c3d[i].get_oX()*cMo[2][0]+c3d[i].get_oY()*cMo[2][1]+c3d[i].get_oZ()*cMo[2][2] + cMo[2][3];
699 
700  double x = X/Z ;
701  double y = Y/Z ;
702 
703  residual_ += vpMath::sqr(x-c3d[i].get_x()) + vpMath::sqr(y-c3d[i].get_y()) ;
704  }
705  return residual_ ;
706 }
707 
708 
709 #undef DEBUG_LEVEL1
710 #undef DEBUG_LEVEL2
711 #undef DEBUG_LEVEL3
712 
vpPose::calculArbreDementhon
int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo)
Definition: vpPoseDementhon.cpp:280
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
vpPoint::set_oZ
void set_oZ(const double oZ)
Set the point Z coordinate in the object frame.
Definition: vpPoint.cpp:491
vpArray2D::resize
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
vpColVector::cross
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:257
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:93
vpERROR_TRACE
#define vpERROR_TRACE
Definition: vpDebug.h:391
vpPoint::get_oY
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:449
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:73
vpArray2D::getCols
unsigned int getCols() const
Return the number of columns of the 2D array.
Definition: vpArray2D.h:154
vpPose::listP
std::list< vpPoint > listP
array of point (use here class vpPoint)
Definition: vpPose.h:91
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:59
vpColVector::normalize
vpColVector & normalize()
Definition: vpColVector.cpp:725
vpMatrix::svd
void svd(vpColVector &w, vpMatrix &v)
Definition: vpMatrix.cpp:1646
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:110
vpColVector::t
vpRowVector t() const
Definition: vpColVector.cpp:633
vpMatrix::getCol
vpColVector getCol(const unsigned int j) const
Definition: vpMatrix.cpp:2250
vpPoint::get_oZ
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:451
vpPoint::set_oX
void set_oX(const double oX)
Set the point X coordinate in the object frame.
Definition: vpPoint.cpp:487
vpArray2D::getRows
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
vpPose::npt
unsigned int npt
number of point used in pose computation
Definition: vpPose.h:90
vpColVector::sumSquare
double sumSquare() const
Definition: vpColVector.cpp:1223
vpPoint::get_oX
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:447
vpMatrix::t
vpMatrix t() const
Definition: vpMatrix.cpp:221
vpPose::poseDementhonPlan
void poseDementhonPlan(vpHomogeneousMatrix &cMo)
compute the pose using Dementhon approach (planar object)
Definition: vpPoseDementhon.cpp:471
vpPose::poseDementhonNonPlan
void poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
compute the pose using Dementhon approach (non planar object)
Definition: vpPoseDementhon.cpp:62
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpColVector::dotProd
static double dotProd(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.cpp:677
vpPoint::set_oY
void set_oY(const double oY)
Set the point Y coordinate in the object frame.
Definition: vpPoint.cpp:489
vpMatrix::pseudoInverse
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1756
vpPose::computeResidualDementhon
double computeResidualDementhon(const vpHomogeneousMatrix &cMo)
Compute and return the residual expressed in meter for the pose matrix 'pose'.
Definition: vpPoseDementhon.cpp:688
vpColVector::resize
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:217