40 #include <visp3/vision/vpPose.h> 42 #define DEBUG_LEVEL1 0 43 #define DEBUG_LEVEL2 0 52 static void calculTranslation(
vpMatrix &a,
vpMatrix &b,
unsigned int nl,
unsigned int nc1,
unsigned int nc3,
60 for (i = 0; i < 3; i++) {
61 for (j = 0; j < nl; j++)
62 ct[i][j] = b[j][i + nc3];
81 std::cout <<
"ctc " << std::endl << ctc;
82 std::cout <<
"cta " << std::endl << cta;
83 std::cout <<
"ctb " << std::endl << ctb;
89 for (i = 0; i < nc1; i++) {
90 for (j = 0; j < nc3; j++)
91 CTB[i][j] = ctb[i][j];
94 for (j = 0; j < nc3; j++)
98 sv = cta * x1 + CTB * X2;
101 std::cout <<
"sv " << sv.
t();
108 std::cout <<
"x3 " << X3.
t();
111 for (i = 0; i < nc1; i++)
135 std::cout <<
"begin (CLagrange.cc)Lagrange(...) " << std::endl;
139 unsigned int i, imin;
185 std::cout <<
" BTB1 * BTB : " << std::endl << btb1 * btb << std::endl;
186 std::cout <<
" BTB * BTB1 : " << std::endl << btb * btb1 << std::endl;
194 e = -(a.
t() * b) * r;
200 std::cout <<
" E :" << std::endl << e << std::endl;
218 for (i = 0; i < x1.
getRows(); i++)
219 if (x1[i] < x1[imin])
224 printf(
"SV(E) : %.15lf %.15lf %.15lf\n", x1[0], x1[1], x1[2]);
225 std::cout <<
" i_min " << imin << std::endl;
228 for (i = 0; i < x1.
getRows(); i++)
229 x1[i] = ata[i][imin];
235 std::cout <<
" X1 : " << x1.
t() << std::endl;
236 std::cout <<
" V : " << std::endl << ata << std::endl;
244 std::cout <<
"end (CLagrange.cc)Lagrange(...) " << std::endl;
259 std::cout <<
"begin vpPose::PoseLagrangePlan(...) " << std::endl;
264 double x1 = 0, x2 = 0, x3 = 0, y1 = 0, y2 = 0, y3 = 0, z1 = 0, z2 = 0, z3 = 0;
269 bool degenerate =
true;
270 bool not_on_origin =
true;
271 std::list<vpPoint>::const_iterator it_tmp;
273 std::list<vpPoint>::const_iterator it_i, it_j, it_k;
274 for (it_i =
listP.begin(); it_i !=
listP.end(); ++it_i) {
275 if (degenerate ==
false) {
281 if ((std::fabs(P1.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
282 (std::fabs(P1.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
283 (std::fabs(P1.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
284 not_on_origin =
false;
286 not_on_origin =
true;
291 for (it_j = it_tmp; it_j !=
listP.end(); ++it_j) {
292 if (degenerate ==
false) {
297 if ((std::fabs(P2.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
298 (std::fabs(P2.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
299 (std::fabs(P2.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
300 not_on_origin =
false;
302 not_on_origin =
true;
307 for (it_k = it_tmp; it_k !=
listP.end(); ++it_k) {
309 if ((std::fabs(P3.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
310 (std::fabs(P3.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
311 (std::fabs(P3.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
312 not_on_origin =
false;
314 not_on_origin =
true;
338 if (cross_prod.
sumSquare() <= std::numeric_limits<double>::epsilon())
343 if (degenerate ==
false)
350 double a = y1 * z2 - y1 * z3 - y2 * z1 + y2 * z3 + y3 * z1 - y3 * z2;
351 double b = -x1 * z2 + x1 * z3 + x2 * z1 - x2 * z3 - x3 * z1 + x3 * z2;
352 double c = x1 * y2 - x1 * y3 - x2 * y1 + x2 * y3 + x3 * y1 - x3 * y2;
353 double d = -x1 * y2 * z3 + x1 * y3 * z2 + x2 * y1 * z3 - x2 * y3 * z1 - x3 * y1 * z2 + x3 * y2 * z1;
362 double n = 1.0/sqrt(a*a+ b*b + c*c);
375 double n1 = sqrt(1.0-a*a);
376 double n2 = sqrt(1.0-b*b);
394 for (
unsigned int i=0;i<3;i++){
399 fMo[0][3] = fMo[1][3] = 0.0;
405 unsigned int nl =
npt * 2;
411 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
425 A[k][2] = Xf[0] * P.
get_x();
428 A[k + 1][1] = -Xf[0];
429 A[k + 1][2] = Xf[0] * P.
get_y();
433 B[k][2] = Xf[1] * P.
get_x();
439 B[k + 1][1] = -Xf[1];
440 B[k + 1][2] = Xf[1] * P.
get_y();
443 B[k + 1][5] = P.
get_y();
452 std::cout <<
"A " << std::endl << A << std::endl;
453 std::cout <<
"B " << std::endl << B << std::endl;
457 lagrange(A, B, X1, X2);
461 std::cout <<
"A X1+B X2 (should be 0): " << (A * X1 + B * X2).t() << std::endl;
462 std::cout <<
" X1 norm: " << X1.
sumSquare() << std::endl;
467 for (
unsigned int i = 0; i < 3; i++)
469 for (
unsigned int i = 0; i < 6; i++)
473 for (
unsigned int i = 0; i < 3; i++) {
474 s += (X1[i] * X2[i]);
476 for (
unsigned int i = 0; i < 3; i++) {
477 X2[i] -= (s * X1[i]);
482 s = X2[0] * X2[0] + X2[1] * X2[1] + X2[2] * X2[2];
495 "(planar plane case)"));
499 for (
unsigned int i = 0; i < 3; i++) {
503 calculTranslation(A, B, nl, 3, 3, X1, X2);
513 cMf[0][2] = (X1[1] * X2[2]) - (X1[2] * X2[1]);
514 cMf[1][2] = (X1[2] * X2[0]) - (X1[0] * X2[2]);
515 cMf[2][2] = (X1[0] * X2[1]) - (X1[1] * X2[0]);
517 for (
unsigned int i = 0; i < 3; i++) {
520 cMf[i][3] = X2[i + 3];
528 std::cout <<
"end vpCalculPose::PoseLagrangePlan(...) " << std::endl;
537 std::cout <<
"begin CPose::PoseLagrangeNonPlan(...) " << std::endl;
544 unsigned int nl =
npt * 2;
548 "Lagrange, non planar case, insufficient number of points %d < 6\n",
npt));
557 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
564 a[k + 1][1] = -P.
get_oX();
580 b[k + 1][1] = -P.
get_oY();
584 b[k + 1][4] = -P.
get_oZ();
589 b[k + 1][8] = P.
get_y();
598 std::cout <<
"a " << a << std::endl;
599 std::cout <<
"b " << b << std::endl;
603 lagrange(a, b, X1, X2);
613 std::cout <<
"ax1+bx2 (devrait etre 0) " << (a * X1 + b * X2).t() << std::endl;
614 std::cout <<
"norme X1 " << X1.
sumSquare() << std::endl;
624 for (i = 0; i < 3; i++) {
625 s += (X1[i] * X2[i]);
627 for (i = 0; i < 3; i++) {
628 X2[i] -= (s * X1[i]);
633 s = X2[0] * X2[0] + X2[1] * X2[1] + X2[2] * X2[2];
647 "planar plane case)"));
651 for (i = 0; i < 3; i++) {
655 X2[3] = (X1[1] * X2[2]) - (X1[2] * X2[1]);
656 X2[4] = (X1[2] * X2[0]) - (X1[0] * X2[2]);
657 X2[5] = (X1[0] * X2[1]) - (X1[1] * X2[0]);
659 calculTranslation(a, b, nl, 3, 6, X1, X2);
661 for (i = 0; i < 3; i++) {
664 cMo[i][2] = X2[i + 3];
665 cMo[i][3] = X2[i + 6];
673 std::cout <<
"end vpCalculPose::PoseLagrangeNonPlan(...) " << std::endl;
void svd(vpColVector &w, vpMatrix &V)
Implementation of a matrix and operations on matrices.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
double get_oY() const
Get the point Y coordinate in the object frame.
error that can be emited by ViSP classes.
unsigned int getCols() const
double get_y() const
Get the point y coordinate in the image plane.
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Class that defines what is a point.
double get_oZ() const
Get the point Z coordinate in the object frame.
void poseLagrangePlan(vpHomogeneousMatrix &cMo)
Compute the pose of a planar object using Lagrange approach.
double get_x() const
Get the point x coordinate in the image plane.
unsigned int getRows() const
unsigned int npt
Number of point used in pose computation.
double get_oX() const
Get the point X coordinate in the object frame.
Implementation of column vector and the associated operations.
vpMatrix inverseByLU() const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)