40 #include <visp3/core/vpMath.h> 41 #include <visp3/vision/vpPose.h> 43 #define DEBUG_LEVEL1 0 44 #define DEBUG_LEVEL2 0 45 #define DEBUG_LEVEL3 0 47 #define SEUIL_RESIDUAL 0.0001 56 std::cout <<
"begin (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
60 double normI3 = sqrt(I4[0] * I4[0] + I4[1] * I4[1] + I4[2] * I4[2]);
61 double normJ3 = sqrt(J4[0] * J4[0] + J4[1] * J4[1] + J4[2] * J4[2]);
63 if ((normI3 < 1e-10) || (normJ3 < 1e-10)) {
66 "Division by zero in Dementhon pose computation: normI or normJ = 0"));
69 double Z0 = 2.0 / (normI3 + normJ3);
72 for (
unsigned int i = 0; i < 3; i++) {
73 I3[i] = I4[i] / normI3;
74 J3[i] = J4[i] / normJ3;
86 cMo[0][3] = I4[3] * Z0;
91 cMo[1][3] = J4[3] * Z0;
99 std::cout <<
"end (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
113 cdg[0] = cdg[1] = cdg[2] = 0.0;
114 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
120 for (
unsigned int i = 0; i < 3; i++)
126 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
136 for (
unsigned int i = 0; i <
npt; i++) {
137 A[i][0] = c3d[i].get_oX();
138 A[i][1] = c3d[i].get_oY();
139 A[i][2] = c3d[i].get_oZ();
146 std::cout <<
"A" << std::endl << A << std::endl;
147 std::cout <<
"A^+" << std::endl << Ap << std::endl;
154 for (
unsigned int i = 0; i <
npt; i++) {
155 xprim[i] = c3d[i].get_x();
156 yprim[i] = c3d[i].get_y();
163 calculSolutionDementhon(I4, J4, cMo);
166 for (
unsigned int i = 0; i <
npt; i++) {
168 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];
177 double res_old = 2.0 * res;
182 while ((cpt < ITER_MAX) && (res > SEUIL_RESIDUAL) && (res < res_old)) {
188 for (
unsigned int i = 0; i <
npt; i++) {
190 (cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ()) / cMo[2][3];
192 xprim[i] = (1.0 + eps) * c3d[i].get_x();
193 yprim[i] = (1.0 + eps) * c3d[i].get_y();
198 calculSolutionDementhon(I4, J4, cMo);
200 for (
unsigned int i = 0; i <
npt; i++) {
202 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];
210 std::cout <<
"Pb z < 0 with cMo in Dementhon's loop" << std::endl;
216 std::cout <<
"it = " << cpt <<
" residu = " << res <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" " 221 std::cout <<
"Divergence : res = " << res <<
" res_old = " << res_old << std::endl;
228 cMo[0][3] -= (cdg[0] * cMo[0][0] + cdg[1] * cMo[0][1] + cdg[2] * cMo[0][2]);
229 cMo[1][3] -= (cdg[0] * cMo[1][0] + cdg[1] * cMo[1][1] + cdg[2] * cMo[1][2]);
230 cMo[2][3] -= (cdg[0] * cMo[2][0] + cdg[1] * cMo[2][1] + cdg[2] * cMo[2][2]);
233 static void calculRTheta(
double s,
double c,
double &r,
double &theta)
235 if ((fabs(c) > EPS_DEM) || (fabs(s) > EPS_DEM)) {
236 r = sqrt(sqrt(s * s + c * c));
237 theta = atan2(s, c) / 2.0;
239 if (fabs(c) > fabs(s)) {
259 for (
unsigned int i = 0; i < 3; i++) {
264 double c = J0.
sumSquare() - I0.sumSquare();
267 calculRTheta(s, c, r, theta);
268 double co = cos(theta);
269 double si = sin(theta);
273 I = I04 + U * r * co;
274 J = J04 + U * r * si;
278 std::cout <<
"I0 " << I04.
t() << std::endl;
279 std::cout <<
"J0 " << J04.
t() << std::endl;
280 std::cout <<
"I1 " << I.t() << std::endl;
281 std::cout <<
"J1 " << J.
t() << std::endl;
284 calculSolutionDementhon(I, J, cMo1);
287 I = I04 - U * r * co;
288 J = J04 - U * r * si;
291 std::cout <<
"I2 " << I.t() << std::endl;
292 std::cout <<
"J2 " << J.
t() << std::endl;
295 calculSolutionDementhon(I, J, cMo2);
304 std::cout <<
"begin vpPose::CalculArbreDementhon() " << std::endl;
310 for (
unsigned int i = 0; i <
npt; i++) {
312 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];
319 unsigned int cpt = 0;
321 double res_old = 2.0 * res_min;
324 while ((cpt < ITER_MAX) && (res_min > SEUIL_RESIDUAL) && (res_min < res_old)) {
331 for (
unsigned int i = 0; i <
npt; i++) {
333 (cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ()) / cMo[2][3];
335 xprim[i] = (1.0 + eps) * c3d[i].get_x();
336 yprim[i] = (1.0 + eps) * c3d[i].get_y();
343 calculTwoSolutionsDementhonPlan(I04, J04, U, cMo1, cMo2);
348 for (
unsigned int i = 0; i <
npt; i++) {
350 z = cMo1[2][0] * c3d[i].get_oX() + cMo1[2][1] * c3d[i].get_oY() + cMo1[2][2] * c3d[i].get_oZ() + cMo1[2][3];
353 z = cMo2[2][0] * c3d[i].get_oX() + cMo2[2][1] * c3d[i].get_oY() + cMo2[2][2] * c3d[i].get_oZ() + cMo2[2][3];
358 if ((erreur1 == -1) && (erreur2 == -1)) {
361 std::cout <<
" End of loop since z < 0 for both solutions" << std::endl;
365 if ((erreur1 == 0) && (erreur2 == -1)) {
369 if ((erreur1 == -1) && (erreur2 == 0)) {
373 if ((erreur1 == 0) && (erreur2 == 0)) {
390 std::cout <<
"it = " << cpt <<
" cMo1 : residu: " << s <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" " 393 std::cout <<
"Pb z < 0 with cMo1" << std::endl;
399 std::cout <<
"it = " << cpt <<
" cMo2 : residu: " << s <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" " 402 std::cout <<
"Pb z < 0 with cMo2" << std::endl;
405 if (res_min > res_old) {
407 std::cout <<
"Divergence : res_min = " << res_min <<
" res_old = " << res_old << std::endl;
415 std::cout <<
"end vpPose::CalculArbreDementhon() return " << erreur << std::endl;
432 std::cout <<
"begin CCalculPose::PoseDementhonPlan()" << std::endl;
438 cdg[0] = cdg[1] = cdg[2] = 0.0;
439 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
445 for (
unsigned int i = 0; i < 3; i++)
451 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
461 for (
unsigned int i = 0; i <
npt; i++) {
462 A[i][0] = c3d[i].get_oX();
463 A[i][1] = c3d[i].get_oY();
464 A[i][2] = c3d[i].get_oZ();
475 for (
unsigned int i = 0; i < 4; i++) {
480 std::cout <<
"A" << std::endl << A << std::endl;
481 std::cout <<
"A^+" << std::endl << Ap << std::endl;
482 std::cout <<
"U^T = " << U.
t() << std::endl;
489 for (
unsigned int i = 0; i <
npt; i++) {
490 xi[i] = c3d[i].get_x();
491 yi[i] = c3d[i].get_y();
499 calculTwoSolutionsDementhonPlan(I04, J04, U, cMo1, cMo2);
505 std::cout <<
"cMo Start Tree 1 : res " << res <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" " 509 std::cout <<
"cMo Start Tree 2 : res " << res <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" " 516 if ((erreur1 == -1) && (erreur2 == -1)) {
520 if ((erreur1 == 0) && (erreur2 == -1))
522 if ((erreur1 == -1) && (erreur2 == 0))
524 if ((erreur1 == 0) && (erreur2 == 0)) {
535 std::cout <<
"Pb z < 0 with Start Tree 1" << std::endl;
537 std::cout <<
"Pb z < 0 with Start Tree 2" << std::endl;
539 std::cout <<
" Tree 1 chosen " << std::endl;
541 std::cout <<
" Tree 2 chosen " << std::endl;
545 cMo[0][3] -= (cdg[0] * cMo[0][0] + cdg[1] * cMo[0][1] + cdg[2] * cMo[0][2]);
546 cMo[1][3] -= (cdg[0] * cMo[1][0] + cdg[1] * cMo[1][1] + cdg[2] * cMo[1][2]);
547 cMo[2][3] -= (cdg[0] * cMo[2][0] + cdg[1] * cMo[2][1] + cdg[2] * cMo[2][2]);
550 std::cout <<
"end CCalculPose::PoseDementhonPlan()" << std::endl;
564 double squared_error = 0;
570 for (
unsigned int i = 0; i <
npt; i++) {
572 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];
573 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];
574 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];
581 return squared_error;
585 #undef SEUIL_RESIDUAL int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo)
Implementation of a matrix and operations on matrices.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
double get_oY() const
Get the point oY coordinate in the object frame.
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.
void extract(vpRotationMatrix &R) const
std::list< vpPoint > listP
Array of point (use here class vpPoint)
double get_oX() const
Get the point oX coordinate in the object frame.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void set_oY(double oY)
Set the point oY coordinate in the object frame.
static double sqr(double x)
double get_oZ() const
Get the point oZ coordinate in the object frame.
unsigned int npt
Number of point used in pose computation.
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
static double deg(double rad)
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)
Implementation of column vector and the associated operations.
void set_oX(double oX)
Set the point oX coordinate in the object frame.
static double dotProd(const vpColVector &a, const vpColVector &b)
double computeResidualDementhon(const vpHomogeneousMatrix &cMo)
Compute and return the residual corresponding to the sum of squared residuals in meter^2 for the pose...
Implementation of a rotation vector as axis-angle minimal representation.