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 bool p_isplan_and_p_a_no_null = (p_isPlan !=
nullptr) && (p_a !=
nullptr);
265 bool p_b_p_c_p_d_no_null = (p_b !=
nullptr) && (p_c !=
nullptr) && (p_d !=
nullptr);
266 if (p_isplan_and_p_a_no_null && p_b_p_c_p_d_no_null) {
276 throw vpException(
vpException::fatalError,
"Called vpPose::poseLagrangePlan but the call to vpPose::coplanar done outside the method indicated that the points are not coplanar");
282 bool areCoplanar =
coplanar(coplanarType, &a, &b, &c, &d);
295 double n = 1.0 / sqrt((a * a) + (b * b) + (c * c));
308 double n1 = sqrt(1.0 - (a * a));
309 double n2 = sqrt(1.0 - (b * b));
312 r1[1] = (-a * b) / n1;
313 r1[2] = (-a * c) / n1;
316 r1[0] = (-a * b) / n2;
318 r1[2] = (-b * c) / n2;
327 for (
unsigned int i = 0; i < 3; ++i) {
339 unsigned int nl =
npt * 2;
345 std::list<vpPoint>::const_iterator listp_end =
listP.end();
346 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it != listp_end; ++it) {
360 A[k][2] = Xf[0] * P.
get_x();
363 A[k + 1][1] = -Xf[0];
364 A[k + 1][2] = Xf[0] * P.
get_y();
368 B[k][2] = Xf[1] * P.
get_x();
374 B[k + 1][1] = -Xf[1];
375 B[k + 1][2] = Xf[1] * P.
get_y();
378 B[k + 1][5] = P.
get_y();
387 std::cout <<
"A " << std::endl << A << std::endl;
388 std::cout <<
"B " << std::endl << B << std::endl;
392 lagrange(A, B, X1, X2);
396 std::cout <<
"A X1+B X2 (should be 0): " << (A * X1 + B * X2).t() << std::endl;
397 std::cout <<
" X1 norm: " << X1.
sumSquare() << std::endl;
402 for (
unsigned int i = 0; i < 3; ++i) {
405 for (
unsigned int i = 0; i < 6; ++i) {
410 for (
unsigned int i = 0; i < 3; ++i) {
411 s += (X1[i] * X2[i]);
413 for (
unsigned int i = 0; i < 3; ++i) {
414 X2[i] -= (s * X1[i]);
418 s = (X2[0] * X2[0]) + (X2[1] * X2[1]) + (X2[2] * X2[2]);
431 "(planar plane case)"));
435 for (
unsigned int i = 0; i < 3; ++i) {
439 calculTranslation(A, B, nl, 3, 3, X1, X2);
448 cMf[0][2] = (X1[1] * X2[2]) - (X1[2] * X2[1]);
449 cMf[1][2] = (X1[2] * X2[0]) - (X1[0] * X2[2]);
450 cMf[2][2] = (X1[0] * X2[1]) - (X1[1] * X2[0]);
452 for (
unsigned int i = 0; i < 3; ++i) {
455 cMf[i][3] = X2[i + 3];
462 std::cout <<
"end vpCalculPose::PoseLagrangePlan(...) " << std::endl;
471 std::cout <<
"begin CPose::PoseLagrangeNonPlan(...) " << std::endl;
478 unsigned int nl =
npt * 2;
482 "Lagrange, non planar case, insufficient number of points %d < 6\n",
npt));
491 std::list<vpPoint>::const_iterator listp_end =
listP.end();
492 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it != listp_end; ++it) {
499 a[k + 1][1] = -P.
get_oX();
515 b[k + 1][1] = -P.
get_oY();
519 b[k + 1][4] = -P.
get_oZ();
524 b[k + 1][8] = P.
get_y();
533 std::cout <<
"a " << a << std::endl;
534 std::cout <<
"b " << b << std::endl;
538 lagrange(a, b, X1, X2);
546 std::cout <<
"ax1+bx2 (devrait etre 0) " << (a * X1 + b * X2).t() << std::endl;
547 std::cout <<
"norme X1 " << X1.
sumSquare() << std::endl;
556 for (i = 0; i < 3; ++i) {
557 s += (X1[i] * X2[i]);
559 for (i = 0; i < 3; ++i) {
560 X2[i] -= (s * X1[i]);
563 s = (X2[0] * X2[0]) + (X2[1] * X2[1]) + (X2[2] * X2[2]);
577 "planar plane case)"));
581 for (i = 0; i < 3; ++i) {
585 X2[3] = (X1[1] * X2[2]) - (X1[2] * X2[1]);
586 X2[4] = (X1[2] * X2[0]) - (X1[0] * X2[2]);
587 X2[5] = (X1[0] * X2[1]) - (X1[1] * X2[0]);
589 calculTranslation(a, b, nl, 3, 6, X1, X2);
591 for (i = 0; i < 3; ++i) {
594 cMo[i][2] = X2[i + 3];
595 cMo[i][3] = X2[i + 6];
604 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)