41 #include <visp3/vision/vpPose.h> 42 #include <visp3/core/vpMath.h> 44 #define DEBUG_LEVEL1 0 45 #define DEBUG_LEVEL2 0 46 #define DEBUG_LEVEL3 0 64 double normI = 0., normJ = 0.;
73 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it)
84 for (
unsigned int i=0 ; i <
npt ; i++)
86 a[i][0]=c3d[i].get_oX();
87 a[i][1]=c3d[i].get_oY();
88 a[i][2]=c3d[i].get_oZ();
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() ;
132 for (
unsigned int i=0;i<
npt;i++)
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();
144 if (normI+normJ < 1e-10)
148 "Division by zero in Dementhon pose computation: normI+normJ = 0")) ;
152 Z0=2*f/(normI+normJ);
154 for (
unsigned int i=0; i<
npt; i++)
157 eps[i]=(c3d[i].get_oX()*k[0]+c3d[i].get_oY()*k[1]+c3d[i].get_oZ()*k[2])/Z0;
164 "Division by zero in Dementhon pose computation: no points")) ;
175 cMo[0][3]=c3d[0].get_x()*2/(normI+normJ);
180 cMo[1][3]=c3d[0].get_y()*2/(normI+normJ);
194 #define EPS 0.0000001 195 #define EPS_DEM 0.001 198 calculRTheta(
double s,
double c,
double &r,
double &theta)
200 if ((fabs(c) > EPS_DEM) || (fabs(s) > EPS_DEM))
202 r = sqrt(sqrt(s*s+c*c));
203 theta = atan2(s,c)/2.0;
207 if (fabs(c) > fabs(s))
227 void calculSolutionDementhon(
double xi0,
double yi0,
233 std::cout <<
"begin (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
236 double normI, normJ, normk, Z0;
249 Z0=2.0/(normI+normJ);
274 std::cout <<
"end (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
285 std::cout <<
"begin vpPose::CalculArbreDementhon() " << std::endl;
292 unsigned int iter_max = 20;
297 for(
unsigned int i = 0; i <
npt; i++)
300 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];
301 if (z <= 0.0) erreur = -1;
312 for(
unsigned int i = 0; i <
npt; i++)
314 xi[k] = c3d[i].get_x();
315 yi[k] = c3d[i].get_y();
319 eps[0][k] = (cMo[2][0]*c3d[i].get_oX() +
320 cMo[2][1]*c3d[i].get_oY() +
321 cMo[2][2]*c3d[i].get_oZ())/cMo[2][3];
332 double smin_old = 2*smin ;
334 unsigned int cpt = 0;
335 while ((cpt<20) && (smin_old > 0.01) && (smin <= smin_old))
337 double r, theta, s1, s2;
341 std::cout <<
"cpt " << cpt << std::endl ;
342 std::cout <<
"smin_old " << smin_old << std::endl ;
343 std::cout <<
"smin " << smin << std::endl ;
353 for (
unsigned int i=1;i<
npt;i++)
355 double s = (1.0+eps[cpt][i])*xi[i] - xi[0];
356 I0[0] += b[0][i-1] * s;
357 I0[1] += b[1][i-1] * s;
358 I0[2] += b[2][i-1] * s;
359 s = (1.0+eps[cpt][i])*yi[i] - yi[0];
360 J0[0] += b[0][i-1] * s;
361 J0[1] += b[1][i-1] * s;
362 J0[2] += b[2][i-1] * s;
368 calculRTheta(s,c,r,theta);
369 double co = cos(theta);
370 double si = sin(theta);
378 std::cout <<
"I " << I.
t() ;
379 std::cout <<
"J " << J.
t() ;
383 calculSolutionDementhon(xi[0],yi[0],I,J,cMo1);
386 std::cout <<
"cMo1 "<< std::endl << cMo1 << std::endl ;
394 std::cout <<
"I " << I.
t() ;
395 std::cout <<
"J " << J.
t() ;
399 calculSolutionDementhon(xi[0],yi[0],I,J,cMo2);
402 std::cout <<
"cMo2 "<< std::endl << cMo2 << std::endl ;
410 for(
unsigned int i = 0; i <
npt; i++)
413 eps[cpt][k] = (cMo1[2][0]*c3d[i].get_oX() + cMo1[2][1]*c3d[i].get_oY()
414 + cMo1[2][2]*c3d[i].get_oZ())/cMo1[2][3];
424 for(
unsigned int i = 0; i <
npt; i++)
427 eps[cpt][k] = (cMo2[2][0]*c3d[i].get_oX() + cMo2[2][1]*c3d[i].get_oY()
428 + cMo2[2][2]*c3d[i].get_oZ())/cMo2[2][3];
438 std::cout <<
"Divergence " << std::endl ;
445 std::cout <<
"s1 = " << s1 << std::endl ;
446 std::cout <<
"s2 = " << s2 << std::endl ;
447 std::cout <<
"smin = " << smin << std::endl ;
448 std::cout <<
"smin_old = " << smin_old << std::endl ;
454 std::cout <<
"end vpPose::CalculArbreDementhon() return "<< erreur << std::endl;
472 std::cout <<
"begin CCalculPose::PoseDementhonPlan()" << std::endl ;
481 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it)
502 for (i=1 ; i <
npt ; i++)
504 a[i-1][0]=c3d[i].get_oX();
505 a[i-1][1]=c3d[i].get_oY();
506 a[i-1][2]=c3d[i].get_oZ();
527 std::cout <<
"a" << std::endl <<a<<std::endl ;
528 std::cout <<
"ata" << std::endl <<ata<<std::endl ;
537 unsigned int imin = 0;
543 unsigned int nc = sv.getRows() ;
544 for (i=0; i < nc ; i++)
545 if (sv[i] > s) s = sv[i];
550 if (sv[i] > s ) irank++;
553 for (i = 0; i < nc; i++)
554 if (sv[i] < svm) { imin = i; svm = sv[i]; }
558 std::cout <<
"rang: " << irank << std::endl ;;
559 std::cout <<
"imin = " << imin << std::endl ;
560 std::cout <<
"sv " << sv.t() << std::endl ;
564 for (i=0 ; i < ata.
getRows() ; i++)
565 for (j=0 ; j < ata.
getCols() ; j++)
568 for (k=0 ; k < nc ; k++)
570 ata1[i][j] += ((v[i][k]*ata[j][k])/sv[k]);
584 std::cout <<
"a" << std::endl <<a<<std::endl ;
585 std::cout <<
"ata" << std::endl <<ata_sav<<std::endl ;
586 std::cout <<
"ata1" << std::endl <<ata1<<std::endl ;
587 std::cout <<
"ata1*ata" << std::endl << ata1*ata_sav ;
588 std::cout <<
"b" << std::endl << b ;
589 std::cout <<
"U " << U.
t() << std::endl ;
596 for (i = 0; i <
npt; i++)
598 xi[i] = c3d[i].get_x() ;
599 yi[i] = c3d[i].get_y() ;
610 I0[0] += b[0][i-1] * (xi[i]-xi[0]);
611 I0[1] += b[1][i-1] * (xi[i]-xi[0]);
612 I0[2] += b[2][i-1] * (xi[i]-xi[0]);
614 J0[0] += b[0][i-1] * (yi[i]-yi[0]);
615 J0[1] += b[1][i-1] * (yi[i]-yi[0]);
616 J0[2] += b[2][i-1] * (yi[i]-yi[0]);
622 std::cout <<
"I0 "<<I0.
t() ;
623 std::cout <<
"J0 "<<J0.
t() ;
630 double r,theta,si,co ;
631 calculRTheta(s, c, r, theta);
640 calculSolutionDementhon(xi[0], yi[0], I, J, cMo1f);
650 calculSolutionDementhon(xi[0], yi[0], I, J, cMo2f);
654 if ((erreur1 == 0) && (erreur2 == -1)) cMo = cMo1f ;
655 if ((erreur1 == -1) && (erreur2 == 0)) cMo = cMo2f ;
656 if ((erreur1 == 0) && (erreur2 == 0))
661 if (s1<=s2) cMo = cMo1f ;
else cMo = cMo2f ;
669 std::cout <<
"end CCalculPose::PoseDementhonPlan()" << std::endl ;
688 double residual_ = 0 ;
691 for (
unsigned int i =0 ; i <
npt ; i++)
694 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];
695 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];
696 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)
Implementation of a matrix and operations on matrices.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
void set_oZ(const double oZ)
Set the point Z coordinate in the object frame.
double get_oY() const
Get the point Y coordinate in the object frame.
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Implementation of an homogeneous matrix and operations on such kind of matrices.
error that can be emited by ViSP classes.
unsigned int getRows() const
Return the number of rows of the 2D array.
std::list< vpPoint > listP
Array of point (use here class vpPoint)
double get_oX() const
Get the point X coordinate in the object frame.
Class that defines what is a point.
unsigned int getCols() const
Return the number of columns of the 2D array.
vpColVector & normalize()
void svd(vpColVector &w, vpMatrix &v)
static double sqr(double x)
void set_oX(const double oX)
Set the point X coordinate in the object frame.
double get_oZ() const
Get the point Z coordinate in the object frame.
unsigned int npt
Number of point used in pose computation.
void poseDementhonPlan(vpHomogeneousMatrix &cMo)
Compute the pose using Dementhon approach for planar objects this is a direct implementation of the a...
void poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
Compute the pose using Dementhon approach for non planar objects this is a direct implementation of t...
Implementation of column vector and the associated operations.
static double dotProd(const vpColVector &a, const vpColVector &b)
void set_oY(const double oY)
Set the point Y coordinate in the object frame.
vpColVector getCol(const unsigned int j) const
double computeResidualDementhon(const vpHomogeneousMatrix &cMo)
Compute and return the residual expressed in meter for the pose matrix 'pose'.
void resize(const unsigned int i, const bool flagNullify=true)