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