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++) cdg[i] /=
npt;
125 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
135 for (
unsigned int i = 0; i <
npt; i++) {
136 A[i][0] = c3d[i].get_oX();
137 A[i][1] = c3d[i].get_oY();
138 A[i][2] = c3d[i].get_oZ();
145 std::cout <<
"A" << std::endl << A << std::endl;
146 std::cout <<
"A^+" << std::endl << Ap << std::endl;
153 for (
unsigned int i = 0; i <
npt; i++) {
154 xprim[i] = c3d[i].get_x();
155 yprim[i] = c3d[i].get_y();
162 calculSolutionDementhon(I4, J4, cMo);
165 for (
unsigned int i = 0; i <
npt; i++) {
167 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];
168 if (z <= 0.0) erreur = -1;
175 double res_old = 2.0*res;
180 while ((cpt < ITER_MAX) && (res > SEUIL_RESIDUAL) && (res < res_old)) {
186 for (
unsigned int i = 0; i <
npt; i++) {
187 double eps = (cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ()) / cMo[2][3];
189 xprim[i] = (1.0 + eps) * c3d[i].get_x();
190 yprim[i] = (1.0 + eps) * c3d[i].get_y();
195 calculSolutionDementhon(I4, J4, cMo);
197 for (
unsigned int i = 0; i <
npt; i++) {
199 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];
200 if (z <= 0.0) erreur = -1;
206 std::cout <<
"Pb z < 0 with cMo in Dementhon's loop" << std::endl;
212 std::cout <<
"it = " << cpt <<
" residu = " << res <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" " <<
vpMath::deg(erc[1]) <<
" " <<
vpMath::deg(erc[2]) << std::endl;
216 std::cout <<
"Divergence : res = " << res <<
" res_old = " << res_old << std::endl;
223 cMo[0][3] -= (cdg[0] * cMo[0][0] + cdg[1] * cMo[0][1] + cdg[2] * cMo[0][2]);
224 cMo[1][3] -= (cdg[0] * cMo[1][0] + cdg[1] * cMo[1][1] + cdg[2] * cMo[1][2]);
225 cMo[2][3] -= (cdg[0] * cMo[2][0] + cdg[1] * cMo[2][1] + cdg[2] * cMo[2][2]);
229 static void calculRTheta(
double s,
double c,
double &r,
double &theta)
231 if ((fabs(c) > EPS_DEM) || (fabs(s) > EPS_DEM)) {
232 r = sqrt(sqrt(s * s + c * c));
233 theta = atan2(s, c) / 2.0;
235 if (fabs(c) > fabs(s)) {
255 for (
unsigned int i=0; i<3; i++) {
260 double c = J0.
sumSquare() - I0.sumSquare();
263 calculRTheta(s, c, r, theta);
264 double co = cos(theta);
265 double si = sin(theta);
269 I = I04 + U * r * co;
270 J = J04 + U * r * si;
274 std::cout <<
"I0 " << I04.
t() << std::endl;
275 std::cout <<
"J0 " << J04.
t() << std::endl;
276 std::cout <<
"I1 " << I.t() << std::endl;
277 std::cout <<
"J1 " << J.
t() << std::endl;
280 calculSolutionDementhon(I, J, cMo1);
283 I = I04 - U * r * co;
284 J = J04 - U * r * si;
287 std::cout <<
"I2 " << I.t() << std::endl;
288 std::cout <<
"J2 " << J.
t() << std::endl;
291 calculSolutionDementhon(I, J, cMo2);
301 std::cout <<
"begin vpPose::CalculArbreDementhon() " << std::endl;
307 for (
unsigned int i = 0; i <
npt; i++) {
309 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];
316 unsigned int cpt = 0;
318 double res_old = 2.0*res_min;
321 while ((cpt < ITER_MAX) && (res_min > SEUIL_RESIDUAL) && (res_min < res_old)) {
328 for (
unsigned int i = 0; i <
npt; i++) {
329 double eps = (cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ()) / cMo[2][3];
331 xprim[i] = (1.0 + eps)*c3d[i].get_x();
332 yprim[i] = (1.0 + eps)*c3d[i].get_y();
339 calculTwoSolutionsDementhonPlan(I04,J04,U,cMo1,cMo2);
344 for (
unsigned int i = 0; i <
npt; i++) {
346 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];
347 if (z <= 0.0) erreur1 = -1;
348 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];
349 if (z <= 0.0) erreur2 = -1;
352 if ((erreur1 == -1) && (erreur2 == -1)) {
355 std::cout <<
" End of loop since z < 0 for both solutions" << std::endl;
359 if ((erreur1 == 0) && (erreur2 == -1)) {
363 if ((erreur1 == -1) && (erreur2 == 0)) {
367 if ((erreur1 == 0) && (erreur2 == 0)) {
384 std::cout <<
"it = " << cpt <<
" cMo1 : residu: " << s <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" " <<
vpMath::deg(erc[1]) <<
" " <<
vpMath::deg(erc[2]) << std::endl;
386 else std::cout <<
"Pb z < 0 with cMo1" << std::endl;
392 std::cout <<
"it = " << cpt <<
" cMo2 : residu: " << s <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" " <<
vpMath::deg(erc[1]) <<
" " <<
vpMath::deg(erc[2]) << std::endl;
394 else std::cout <<
"Pb z < 0 with cMo2" << std::endl;
397 if (res_min > res_old) {
399 std::cout <<
"Divergence : res_min = " << res_min <<
" res_old = " << res_old << std::endl;
407 std::cout <<
"end vpPose::CalculArbreDementhon() return " << erreur << std::endl;
424 std::cout <<
"begin CCalculPose::PoseDementhonPlan()" << std::endl;
430 cdg[0] = cdg[1] = cdg[2] = 0.0;
431 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
437 for (
unsigned int i=0; i<3;i++) cdg[i] /=
npt;
442 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
452 for (
unsigned int i = 0; i <
npt; i++) {
453 A[i][0] = c3d[i].get_oX();
454 A[i][1] = c3d[i].get_oY();
455 A[i][2] = c3d[i].get_oZ();
466 for (
unsigned int i = 0; i<4; i++) {
471 std::cout <<
"A" << std::endl << A << std::endl;
472 std::cout <<
"A^+" << std::endl << Ap << std::endl;
473 std::cout <<
"U^T = " << U.
t() << std::endl;
480 for (
unsigned int i = 0; i <
npt; i++) {
481 xi[i] = c3d[i].get_x();
482 yi[i] = c3d[i].get_y();
490 calculTwoSolutionsDementhonPlan(I04,J04,U,cMo1,cMo2);
496 std::cout <<
"cMo Start Tree 1 : res " << res <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" " <<
vpMath::deg(erc[1]) <<
" " <<
vpMath::deg(erc[2]) << std::endl;
499 std::cout <<
"cMo Start Tree 2 : res " << res <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" " <<
vpMath::deg(erc[1]) <<
" " <<
vpMath::deg(erc[2]) << std::endl;
505 if ((erreur1 == -1) && (erreur2 == -1)) {
508 if ((erreur1 == 0) && (erreur2 == -1))
510 if ((erreur1 == -1) && (erreur2 == 0))
512 if ((erreur1 == 0) && (erreur2 == 0)) {
522 if (erreur1 == -1) std::cout <<
"Pb z < 0 with Start Tree 1" << std::endl;
523 if (erreur2 == -1) std::cout <<
"Pb z < 0 with Start Tree 2" << std::endl;
524 if (s1 <= s2) std::cout <<
" Tree 1 chosen " << std::endl;
525 else std::cout <<
" Tree 2 chosen " << std::endl;
529 cMo[0][3] -= (cdg[0] * cMo[0][0] + cdg[1] * cMo[0][1] + cdg[2] * cMo[0][2]);
530 cMo[1][3] -= (cdg[0] * cMo[1][0] + cdg[1] * cMo[1][1] + cdg[2] * cMo[1][2]);
531 cMo[2][3] -= (cdg[0] * cMo[2][0] + cdg[1] * cMo[2][1] + cdg[2] * cMo[2][2]);
534 std::cout <<
"end CCalculPose::PoseDementhonPlan()" << std::endl;
548 double squared_error = 0;
554 for (
unsigned int i = 0; i <
npt; i++) {
556 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];
557 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];
558 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];
565 return squared_error;
569 #undef SEUIL_RESIDUAL int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo)
Implementation of a matrix and operations on matrices.
void set_oZ(const double oZ)
Set the point Z 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.
double get_oY() const
Get the point Y coordinate in the object frame.
error that can be emited by ViSP classes.
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Class that defines what is a point.
static double sqr(double x)
double get_oZ() const
Get the point Z coordinate in the object frame.
void set_oX(const double oX)
Set the point X coordinate in the object frame.
void extract(vpRotationMatrix &R) const
unsigned int npt
Number of point used in pose computation.
double get_oX() const
Get the point X 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.
static double dotProd(const vpColVector &a, const vpColVector &b)
void set_oY(const double oY)
Set the point Y coordinate in the object frame.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
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.