45 #include <visp/vpPose.h>
46 #include <visp/vpMath.h>
48 #define DEBUG_LEVEL1 0
49 #define DEBUG_LEVEL2 0
50 #define DEBUG_LEVEL3 0
71 double normI = 0., normJ = 0.;
78 if (c3d !=NULL)
delete [] c3d ;
85 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it)
105 for (
unsigned int i=0 ; i <
npt ; i++)
126 std::cout <<
"a" << std::endl <<a<<std::endl ;
127 std::cout <<
"ata" << std::endl <<ata<<std::endl ;
128 std::cout <<
"ata1" << std::endl <<ata1<<std::endl ;
129 std::cout<<
" ata*ata1" << std::endl << ata*ata1 ;
130 std::cout<<
" b" << std::endl << (a*ata1).t() ;
175 for (
unsigned int i=0;i<
npt;i++)
177 xprim[i]=(1+ eps[i])*c3d[i].get_x() - c3d[0].
get_x();
178 yprim[i]=(1+ eps[i])*c3d[i].get_y() - c3d[0].
get_y();
187 if (normI+normJ < 1e-10)
191 "division by zero ")) ;
195 Z0=2*f/(normI+normJ);
196 cpt=cpt+1; seuil=0.0;
197 for (
unsigned int i=0; i<
npt; i++)
199 double epsi_1 = eps[i] ;
201 seuil+=fabs(eps[i]-epsi_1);
207 "division by zero ")) ;
218 cMo[0][3]=c3d[0].
get_x()*2/(normI+normJ);
223 cMo[1][3]=c3d[0].
get_y()*2/(normI+normJ);
234 delete [] c3d ; c3d = NULL ;
239 #define EPS 0.0000001
240 #define EPS_DEM 0.001
243 calculRTheta(
double s,
double c,
double &r,
double &theta)
245 if ((fabs(c) > EPS_DEM) || (fabs(s) > EPS_DEM))
247 r = sqrt(sqrt(s*s+c*c));
248 theta = atan2(s,c)/2.0;
252 if (fabs(c) > fabs(s))
272 void calculSolutionDementhon(
double xi0,
double yi0,
278 std::cout <<
"begin (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
281 double normI, normJ, normk, Z0;
294 Z0=2.0/(normI+normJ);
296 normk = sqrt(k.sumSquare()) ;
319 std::cout <<
"end (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
330 std::cout <<
"begin vpPose::CalculArbreDementhon() " << std::endl;
337 double smin,smin_old, s1,s2;
341 unsigned int iter_max = 20;
346 for(i = 0; i <
npt; i++)
349 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];
350 if (z <= 0.0) erreur = -1;
361 for(i = 0; i <
npt; i++)
363 xi[k] = c3d[i].
get_x();
364 yi[k] = c3d[i].
get_y();
368 eps[0][k] = (cMo[2][0]*c3d[i].
get_oX() +
369 cMo[2][1]*c3d[i].
get_oY() +
370 cMo[2][2]*c3d[i].
get_oZ())/cMo[2][3];
385 while ((cpt<20) && (smin_old > 0.01) && (smin <= smin_old))
389 std::cout <<
"cpt " << cpt << std::endl ;
390 std::cout <<
"smin_old " << smin_old << std::endl ;
391 std::cout <<
"smin " << smin << std::endl ;
403 s = (1.0+eps[cpt][i])*xi[i] - xi[0];
404 I0[0] += b[0][i-1] * s;
405 I0[1] += b[1][i-1] * s;
406 I0[2] += b[2][i-1] * s;
407 s = (1.0+eps[cpt][i])*yi[i] - yi[0];
408 J0[0] += b[0][i-1] * s;
409 J0[1] += b[1][i-1] * s;
410 J0[2] += b[2][i-1] * s;
416 calculRTheta(s,c,r,theta);
426 std::cout <<
"I " << I.
t() ;
427 std::cout <<
"J " << J.
t() ;
431 calculSolutionDementhon(xi[0],yi[0],I,J,cMo1);
434 std::cout <<
"cMo1 "<< std::endl << cMo1 << std::endl ;
442 std::cout <<
"I " << I.
t() ;
443 std::cout <<
"J " << J.
t() ;
447 calculSolutionDementhon(xi[0],yi[0],I,J,cMo2);
450 std::cout <<
"cMo2 "<< std::endl << cMo2 << std::endl ;
458 for(i = 0; i <
npt; i++)
461 eps[cpt][k] = (cMo1[2][0]*c3d[i].
get_oX() + cMo1[2][1]*c3d[i].
get_oY()
462 + cMo1[2][2]*c3d[i].
get_oZ())/cMo1[2][3];
472 for(i = 0; i <
npt; i++)
475 eps[cpt][k] = (cMo2[2][0]*c3d[i].
get_oX() + cMo2[2][1]*c3d[i].
get_oY()
476 + cMo2[2][2]*c3d[i].
get_oZ())/cMo2[2][3];
486 std::cout <<
"Divergence " << std::endl ;
493 std::cout <<
"s1 = " << s1 << std::endl ;
494 std::cout <<
"s2 = " << s2 << std::endl ;
495 std::cout <<
"smin = " << smin << std::endl ;
496 std::cout <<
"smin_old = " << smin_old << std::endl ;
502 std::cout <<
"end vpPose::CalculArbreDementhon() return "<< erreur << std::endl;
520 std::cout <<
"begin CCalculPose::PoseDementhonPlan()" << std::endl ;
525 if (c3d !=NULL)
delete []c3d ;
532 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it)
554 for (i=1 ; i <
npt ; i++)
556 a[i-1][0]=c3d[i].
get_oX();
557 a[i-1][1]=c3d[i].
get_oY();
558 a[i-1][2]=c3d[i].
get_oZ();
579 std::cout <<
"a" << std::endl <<a<<std::endl ;
580 std::cout <<
"ata" << std::endl <<ata<<std::endl ;
589 unsigned int imin = 0;
595 unsigned int nc = sv.getRows() ;
596 for (i=0; i < nc ; i++)
597 if (sv[i] > s) s = sv[i];
602 if (sv[i] > s ) irank++;
605 for (i = 0; i < nc; i++)
606 if (sv[i] < svm) { imin = i; svm = sv[i]; }
610 std::cout <<
"rang: " << irank << std::endl ;;
611 std::cout <<
"imin = " << imin << std::endl ;
612 std::cout <<
"sv " << sv.t() << std::endl ;
616 for (i=0 ; i < ata.
getRows() ; i++)
617 for (j=0 ; j < ata.
getCols() ; j++)
620 for (k=0 ; k < nc ; k++)
622 ata1[i][j] += ((v[i][k]*ata[j][k])/sv[k]);
636 std::cout <<
"a" << std::endl <<a<<std::endl ;
637 std::cout <<
"ata" << std::endl <<ata_sav<<std::endl ;
638 std::cout <<
"ata1" << std::endl <<ata1<<std::endl ;
639 std::cout <<
"ata1*ata" << std::endl << ata1*ata_sav ;
640 std::cout <<
"b" << std::endl << b ;
641 std::cout <<
"U " << U.
t() << std::endl ;
648 for (i = 0; i <
npt; i++)
650 xi[i] = c3d[i].
get_x() ;
651 yi[i] = c3d[i].
get_y() ;
662 I0[0] += b[0][i-1] * (xi[i]-xi[0]);
663 I0[1] += b[1][i-1] * (xi[i]-xi[0]);
664 I0[2] += b[2][i-1] * (xi[i]-xi[0]);
666 J0[0] += b[0][i-1] * (yi[i]-yi[0]);
667 J0[1] += b[1][i-1] * (yi[i]-yi[0]);
668 J0[2] += b[2][i-1] * (yi[i]-yi[0]);
674 std::cout <<
"I0 "<<I0.
t() ;
675 std::cout <<
"J0 "<<J0.
t() ;
682 double r,theta,si,co ;
683 calculRTheta(s, c, r, theta);
692 calculSolutionDementhon(xi[0], yi[0], I, J, cMo1f);
702 calculSolutionDementhon(xi[0], yi[0], I, J, cMo2f);
706 if ((erreur1 == 0) && (erreur2 == -1)) cMo = cMo1f ;
707 if ((erreur1 == -1) && (erreur2 == 0)) cMo = cMo2f ;
708 if ((erreur1 == 0) && (erreur2 == 0))
713 if (s1<=s2) cMo = cMo1f ;
else cMo = cMo2f ;
720 delete [] c3d ; c3d = NULL ;
722 std::cout <<
"end CCalculPose::PoseDementhonPlan()" << std::endl ;
746 for (i =0 ; i <
npt ; i++)
749 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];
750 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];
751 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];
int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo)
Definition of the vpMatrix class.
double residual
compute the residual in meter
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
static vpColVector cross(const vpColVector &a, const vpColVector &b)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
double get_oY() const
Get the point Y coordinate in the object frame.
double get_y() const
Get the point y coordinate in the image plane.
double sumSquare() const
return sum of the Aij^2 (for all i, for all j)
std::list< vpPoint > listP
array of point (use here class vpPoint)
void set_oX(const double X)
Set the point X coordinate in the object frame.
vpColVector column(const unsigned int j)
Column extraction.
Class that defines what is a point.
void set_oZ(const double Z)
Set the point Z coordinate in the object frame.
void svd(vpColVector &w, vpMatrix &v)
static double sqr(double x)
vpRowVector t() const
transpose of Vector
double get_oZ() const
Get the point Z coordinate in the object frame.
double get_x() const
Get the point x coordinate in the image plane.
unsigned int npt
number of point used in pose computation
double get_oX() const
Get the point X coordinate in the object frame.
double computeResidualDementhon(vpHomogeneousMatrix &cMo)
Compute and return the residual expressed in meter for the pose matrix 'pose'.
void poseDementhonPlan(vpHomogeneousMatrix &cMo)
compute the pose using Dementhon approach (planar object)
void poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
compute the pose using Dementhon approach (non planar object)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
unsigned int getCols() const
Return the number of columns of the matrix.
static double dotProd(const vpColVector &a, const vpColVector &b)
Dot Product.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
unsigned int getRows() const
Return the number of rows of the matrix.
vpColVector & normalize()
normalise the vector
void set_oY(const double Y)
Set the point Y coordinate in the object frame.
void resize(const unsigned int i, const bool flagNullify=true)