34 #include <visp3/vision/vpPose.h>
36 #define DEBUG_LEVEL1 0
37 #define DEBUG_LEVEL2 0
46 static void calculTranslation(
vpMatrix &a,
vpMatrix &b,
unsigned int nl,
unsigned int nc1,
unsigned int nc3,
54 for (i = 0; i < 3; ++i) {
55 for (j = 0; j < nl; ++j) {
56 ct[i][j] = b[j][i + nc3];
76 std::cout <<
"ctc " << std::endl << ctc;
77 std::cout <<
"cta " << std::endl << cta;
78 std::cout <<
"ctb " << std::endl << ctb;
84 for (i = 0; i < nc1; ++i) {
85 for (j = 0; j < nc3; ++j) {
86 CTB[i][j] = ctb[i][j];
90 for (j = 0; j < nc3; ++j) {
95 sv = (cta * x1) + (CTB * X2);
98 std::cout <<
"sv " << sv.
t();
105 std::cout <<
"x3 " << X3.
t();
108 for (i = 0; i < nc1; ++i) {
134 std::cout <<
"begin (CLagrange.cc)Lagrange(...) " << std::endl;
138 unsigned int i, imin;
184 std::cout <<
" BTB1 * BTB : " << std::endl << btb1 * btb << std::endl;
185 std::cout <<
" BTB * BTB1 : " << std::endl << btb * btb1 << std::endl;
193 e = -(a.
t() * b) * r;
199 std::cout <<
" E :" << std::endl << e << std::endl;
217 unsigned int v_x1_rows = x1.
getRows();
218 for (i = 0; i < v_x1_rows; ++i) {
219 if (x1[i] < x1[imin]) {
226 printf(
"SV(E) : %.15lf %.15lf %.15lf\n", x1[0], x1[1], x1[2]);
227 std::cout <<
" i_min " << imin << std::endl;
230 unsigned int x1_rows = x1.
getRows();
231 for (i = 0; i < x1_rows; ++i) {
232 x1[i] = ata[i][imin];
239 std::cout <<
" X1 : " << x1.
t() << std::endl;
240 std::cout <<
" V : " << std::endl << ata << std::endl;
249 std::cout <<
"end (CLagrange.cc)Lagrange(...) " << std::endl;
258 std::cout <<
"begin vpPose::PoseLagrangePlan(...) " << std::endl;
264 if ((p_isPlan !=
nullptr) && (p_a !=
nullptr) && (p_b !=
nullptr) && (p_c !=
nullptr) && (p_d !=
nullptr)) {
274 throw vpException(
vpException::fatalError,
"Called vpPose::poseLagrangePlan but the call to vpPose::coplanar done outside the method indicated that the points are not coplanar");
280 bool areCoplanar =
coplanar(coplanarType, &a, &b, &c, &d);
293 double n = 1.0 / sqrt((a * a) + (b * b) + (c * c));
306 double n1 = sqrt(1.0 - (a * a));
307 double n2 = sqrt(1.0 - (b * b));
310 r1[1] = (-a * b) / n1;
311 r1[2] = (-a * c) / n1;
314 r1[0] = (-a * b) / n2;
316 r1[2] = (-b * c) / n2;
325 for (
unsigned int i = 0; i < 3; ++i) {
337 unsigned int nl =
npt * 2;
343 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
357 A[k][2] = Xf[0] * P.
get_x();
360 A[k + 1][1] = -Xf[0];
361 A[k + 1][2] = Xf[0] * P.
get_y();
365 B[k][2] = Xf[1] * P.
get_x();
371 B[k + 1][1] = -Xf[1];
372 B[k + 1][2] = Xf[1] * P.
get_y();
375 B[k + 1][5] = P.
get_y();
384 std::cout <<
"A " << std::endl << A << std::endl;
385 std::cout <<
"B " << std::endl << B << std::endl;
389 lagrange(A, B, X1, X2);
393 std::cout <<
"A X1+B X2 (should be 0): " << (A * X1 + B * X2).t() << std::endl;
394 std::cout <<
" X1 norm: " << X1.
sumSquare() << std::endl;
399 for (
unsigned int i = 0; i < 3; ++i) {
402 for (
unsigned int i = 0; i < 6; ++i) {
407 for (
unsigned int i = 0; i < 3; ++i) {
408 s += (X1[i] * X2[i]);
410 for (
unsigned int i = 0; i < 3; ++i) {
411 X2[i] -= (s * X1[i]);
415 s = (X2[0] * X2[0]) + (X2[1] * X2[1]) + (X2[2] * X2[2]);
428 "(planar plane case)"));
432 for (
unsigned int i = 0; i < 3; ++i) {
436 calculTranslation(A, B, nl, 3, 3, X1, X2);
445 cMf[0][2] = (X1[1] * X2[2]) - (X1[2] * X2[1]);
446 cMf[1][2] = (X1[2] * X2[0]) - (X1[0] * X2[2]);
447 cMf[2][2] = (X1[0] * X2[1]) - (X1[1] * X2[0]);
449 for (
unsigned int i = 0; i < 3; ++i) {
452 cMf[i][3] = X2[i + 3];
459 std::cout <<
"end vpCalculPose::PoseLagrangePlan(...) " << std::endl;
468 std::cout <<
"begin CPose::PoseLagrangeNonPlan(...) " << std::endl;
475 unsigned int nl =
npt * 2;
479 "Lagrange, non planar case, insufficient number of points %d < 6\n",
npt));
488 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
495 a[k + 1][1] = -P.
get_oX();
511 b[k + 1][1] = -P.
get_oY();
515 b[k + 1][4] = -P.
get_oZ();
520 b[k + 1][8] = P.
get_y();
529 std::cout <<
"a " << a << std::endl;
530 std::cout <<
"b " << b << std::endl;
534 lagrange(a, b, X1, X2);
542 std::cout <<
"ax1+bx2 (devrait etre 0) " << (a * X1 + b * X2).t() << std::endl;
543 std::cout <<
"norme X1 " << X1.
sumSquare() << std::endl;
552 for (i = 0; i < 3; ++i) {
553 s += (X1[i] * X2[i]);
555 for (i = 0; i < 3; ++i) {
556 X2[i] -= (s * X1[i]);
559 s = (X2[0] * X2[0]) + (X2[1] * X2[1]) + (X2[2] * X2[2]);
573 "planar plane case)"));
577 for (i = 0; i < 3; ++i) {
581 X2[3] = (X1[1] * X2[2]) - (X1[2] * X2[1]);
582 X2[4] = (X1[2] * X2[0]) - (X1[0] * X2[2]);
583 X2[5] = (X1[0] * X2[1]) - (X1[1] * X2[0]);
585 calculTranslation(a, b, nl, 3, 6, X1, X2);
587 for (i = 0; i < 3; ++i) {
590 cMo[i][2] = X2[i + 3];
591 cMo[i][3] = X2[i + 6];
600 std::cout <<
"end vpCalculPose::PoseLagrangeNonPlan(...) " << std::endl;
unsigned int getCols() const
unsigned int getRows() const
Implementation of column vector and the associated operations.
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
error that can be emitted by ViSP classes.
@ dimensionError
Bad dimension.
@ divideByZeroError
Division by zero.
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
vpMatrix inverseByLU() const
void svd(vpColVector &w, vpMatrix &V)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
double get_oX() const
Get the point oX coordinate in the object frame.
double get_y() const
Get the point y coordinate in the image plane.
double get_oZ() const
Get the point oZ coordinate in the object frame.
double get_x() const
Get the point x coordinate in the image plane.
double get_oY() const
Get the point oY coordinate in the object frame.
unsigned int npt
Number of point used in pose computation.
void poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan=nullptr, double *p_a=nullptr, double *p_b=nullptr, double *p_c=nullptr, double *p_d=nullptr)
std::list< vpPoint > listP
Array of point (use here class vpPoint)
void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
bool coplanar(int &coplanar_plane_type, double *p_a=nullptr, double *p_b=nullptr, double *p_c=nullptr, double *p_d=nullptr)