44 #include <visp/vpPose.h>
46 #define DEBUG_LEVEL1 0
47 #define DEBUG_LEVEL2 0
58 calculTranslation (
vpMatrix &a,
vpMatrix &b,
unsigned int nl,
unsigned int nc1,
67 for (i=0 ; i < 3 ; i++)
69 for (j=0 ; j < nl ; j++)
70 ct[i][j] = b[j][i+nc3] ;
89 std::cout <<
"ctc " << std::endl << ctc ;
90 std::cout <<
"cta " << std::endl << cta ;
91 std::cout <<
"ctb " << std::endl << ctb ;
97 for (i=0 ; i < nc1 ; i++)
99 for (j=0 ; j < nc3 ; j++)
100 CTB[i][j] = ctb[i][j] ;
103 for (j=0 ; j < nc3 ; j++)
107 sv = cta*x1 + CTB*X2 ;
110 std::cout <<
"sv " << sv.
t() ;
117 std::cout <<
"x3 " << X3.
t() ;
120 for (i=0 ; i < nc1 ; i++)
151 std::cout <<
"begin (CLagrange.cc)Lagrange(...) " << std::endl;
172 std::cout <<
" BTB1 * BTB : " << std::endl << btb1*btb << std::endl;
173 std::cout <<
" BTB * BTB1 : " << std::endl << btb*btb1 << std::endl;
187 std::cout <<
" E :" << std::endl << e << std::endl;
207 if (x1[i] < x1[imin]) imin = i;
211 printf(
"SV(E) : %.15lf %.15lf %.15lf\n",x1[0],x1[1],x1[2]);
212 std::cout <<
" i_min " << imin << std::endl;
216 x1[i] = ata[i][imin];
222 std::cout <<
" X1 : " << x1.
t() << std::endl;
223 std::cout <<
" V : " << std::endl << ata << std::endl;
233 std::cout <<
"end (CLagrange.cc)Lagrange(...) " << std::endl;
247 std::cout <<
"begin vpPose::PoseLagrange(...) " << std::endl ;
255 unsigned int nl=
npt*2;
262 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it)
285 b[k+1][5] = P.
get_y();
294 std::cout <<
"a " << a << std::endl ;
295 std::cout <<
"b " << b << std::endl ;
303 std::cout <<
"ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
304 std::cout <<
"norme X1 " << X1.
sumSquare() << std::endl ;;
310 for (i=0;i<3;i++) X1[i] = -X1[i];
311 for (i=0;i<6;i++) X2[i] = -X2[i];
314 for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
315 for (i=0;i<3;i++) {X2[i] -= (s*X1[i]);}
318 for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
322 vpERROR_TRACE(
"in vpCalculPose::PosePlan(...) division par zero ") ;
324 "division by zero ")) ;
328 for (i=0;i<3;i++) {X2[i] *= s;}
331 calculTranslation (a, b, nl, 3, 3, X1, X2) ;
340 cMo[0][2] = (X1[1]*X2[2])-(X1[2]*X2[1]);
341 cMo[1][2] = (X1[2]*X2[0])-(X1[0]*X2[2]);
342 cMo[2][2] = (X1[0]*X2[1])-(X1[1]*X2[0]);
359 std::cout <<
"end vpCalculPose::PoseLagrange(...) " << std::endl ;
370 std::cout <<
"begin CPose::PoseLagrange(...) " << std::endl ;
377 unsigned int nl=
npt*2;
385 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it)
418 b[k+1][8] = P.
get_y();
427 std::cout <<
"a " << a << std::endl ;
428 std::cout <<
"b " << b << std::endl ;
443 std::cout <<
"ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
444 std::cout <<
"norme X1 " << X1.
sumSquare() << std::endl ;;
454 for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
455 for (i=0;i<3;i++) {X2[i] -= (s*X1[i]);}
458 for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
464 "division by zero ")) ;
469 for (i=0;i<3;i++) {X2[i] *= s;}
471 X2[3] = (X1[1]*X2[2])-(X1[2]*X2[1]);
472 X2[4] = (X1[2]*X2[0])-(X1[0]*X2[2]);
473 X2[5] = (X1[0]*X2[1])-(X1[1]*X2[0]);
475 calculTranslation (a, b, nl, 3, 6, X1, X2) ;
478 for (i=0 ; i<3 ; i++)
494 std::cout <<
"end vpCalculPose::PoseLagrange(...) " << std::endl ;
Definition of the vpMatrix class.
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
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.
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)
Class that defines what is a point.
void svd(vpColVector &w, vpMatrix &v)
vpRowVector t() const
transpose of Vector
double get_oZ() const
Get the point Z coordinate in the object frame.
void poseLagrangePlan(vpHomogeneousMatrix &cMo)
compute the pose using Lagrange approach (planar object)
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.
Class that provides a data structure for the column vectors as well as a set of operations on these v...
vpMatrix inverseByLU() const
unsigned int getCols() const
Return the number of columns of the matrix.
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.