34 #include <visp3/core/vpMath.h>
35 #include <visp3/vision/vpPose.h>
37 #define DEBUG_LEVEL1 0
38 #define DEBUG_LEVEL2 0
39 #define DEBUG_LEVEL3 0
41 #define SEUIL_RESIDUAL 0.0001
50 std::cout <<
"begin (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
54 double normI3 = sqrt((I4[0] * I4[0]) + (I4[1] * I4[1]) + (I4[2] * I4[2]));
55 double normJ3 = sqrt((J4[0] * J4[0]) + (J4[1] * J4[1]) + (J4[2] * J4[2]));
57 if ((normI3 < 1e-10) || (normJ3 < 1e-10)) {
60 "Division by zero in Dementhon pose computation: normI or normJ = 0"));
63 double Z0 = 2.0 / (normI3 + normJ3);
66 for (
unsigned int i = 0; i < 3; ++i) {
67 I3[i] = I4[i] / normI3;
68 J3[i] = J4[i] / normJ3;
80 cMo[0][3] = I4[3] * Z0;
85 cMo[1][3] = J4[3] * Z0;
93 std::cout <<
"end (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
105 std::list<vpPoint>::const_iterator listp_end =
listP.end();
106 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it != listp_end; ++it) {
112 for (
unsigned int i = 0; i < 3; ++i) {
119 std::list<vpPoint>::const_iterator listp_end_s =
listP.end();
120 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it != listp_end_s; ++it) {
130 for (
unsigned int i = 0; i <
npt; ++i) {
131 A[i][0] = c3d[i].get_oX();
132 A[i][1] = c3d[i].get_oY();
133 A[i][2] = c3d[i].get_oZ();
140 std::cout <<
"A" << std::endl << A << std::endl;
141 std::cout <<
"A^+" << std::endl << Ap << std::endl;
148 for (
unsigned int i = 0; i <
npt; ++i) {
149 xprim[i] = c3d[i].get_x();
150 yprim[i] = c3d[i].get_y();
157 calculSolutionDementhon(I4, J4, cMo);
160 for (
unsigned int i = 0; i <
npt; ++i) {
162 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];
172 double res_old = 2.0 * res;
177 while ((cpt < ITER_MAX) && (res > SEUIL_RESIDUAL) && (res < res_old)) {
183 for (
unsigned int i = 0; i <
npt; ++i) {
185 ((cMo[2][0] * c3d[i].get_oX()) + (cMo[2][1] * c3d[i].get_oY()) + (cMo[2][2] * c3d[i].get_oZ())) / cMo[2][3];
187 xprim[i] = (1.0 + eps) * c3d[i].get_x();
188 yprim[i] = (1.0 + eps) * c3d[i].get_y();
193 calculSolutionDementhon(I4, J4, cMo);
195 for (
unsigned int i = 0; i <
npt; ++i) {
197 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];
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]) <<
" "
217 std::cout <<
"Divergence : res = " << res <<
" res_old = " << res_old << std::endl;
224 cMo[0][3] -= ((cdg[0] * cMo[0][0]) + (cdg[1] * cMo[0][1]) + (cdg[2] * cMo[0][2]));
225 cMo[1][3] -= ((cdg[0] * cMo[1][0]) + (cdg[1] * cMo[1][1]) + (cdg[2] * cMo[1][2]));
226 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;
236 if (fabs(c) > fabs(s)) {
261 for (
unsigned int i = 0; i < 3; ++i) {
266 double c = J0.sumSquare() - I0.sumSquare();
269 calculRTheta(s, c, r, theta);
270 double co = cos(theta);
271 double si = sin(theta);
275 I = I04 + (U * r * co);
276 J = J04 + (U * r * si);
280 std::cout <<
"I0 " << I04.
t() << std::endl;
281 std::cout <<
"J0 " << J04.
t() << std::endl;
282 std::cout <<
"I1 " << I.t() << std::endl;
283 std::cout <<
"J1 " << J.t() << std::endl;
286 calculSolutionDementhon(I, J, cMo1);
289 I = I04 - (U * r * co);
290 J = J04 - (U * r * si);
293 std::cout <<
"I2 " << I.t() << std::endl;
294 std::cout <<
"J2 " << J.t() << std::endl;
297 calculSolutionDementhon(I, J, cMo2);
303 std::cout <<
"begin vpPose::CalculArbreDementhon() " << std::endl;
309 for (
unsigned int i = 0; i <
npt; ++i) {
311 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];
318 unsigned int cpt = 0;
320 double res_old = 2.0 * res_min;
323 while ((cpt < ITER_MAX) && (res_min > SEUIL_RESIDUAL) && (res_min < res_old)) {
330 for (
unsigned int i = 0; i <
npt; ++i) {
332 ((cMo[2][0] * c3d[i].get_oX()) + (cMo[2][1] * c3d[i].get_oY()) + (cMo[2][2] * c3d[i].get_oZ())) / cMo[2][3];
334 xprim[i] = (1.0 + eps) * c3d[i].get_x();
335 yprim[i] = (1.0 + eps) * c3d[i].get_y();
342 calculTwoSolutionsDementhonPlan(I04, J04, U, cMo1, cMo2);
347 for (
unsigned int i = 0; i <
npt; ++i) {
349 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];
359 if ((erreur1 == -1) && (erreur2 == -1)) {
362 std::cout <<
" End of loop since z < 0 for both solutions" << std::endl;
366 if ((erreur1 == 0) && (erreur2 == -1)) {
370 if ((erreur1 == -1) && (erreur2 == 0)) {
374 if ((erreur1 == 0) && (erreur2 == 0)) {
392 std::cout <<
"it = " << cpt <<
" cMo1 : residu: " << s <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" "
396 std::cout <<
"Pb z < 0 with cMo1" << std::endl;
402 std::cout <<
"it = " << cpt <<
" cMo2 : residu: " << s <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" "
406 std::cout <<
"Pb z < 0 with cMo2" << std::endl;
409 if (res_min > res_old) {
411 std::cout <<
"Divergence : res_min = " << res_min <<
" res_old = " << res_old << std::endl;
419 std::cout <<
"end vpPose::CalculArbreDementhon() return " << erreur << std::endl;
428 std::cout <<
"begin CCalculPose::PoseDementhonPlan()" << std::endl;
430 const double svdFactorUsedWhenFailure = 10.;
431 const double svdThresholdLimit = 1e-2;
432 const double lnOfSvdFactorUsed = std::log(svdFactorUsedWhenFailure);
433 const double logNOfSvdThresholdLimit = std::log(svdThresholdLimit)/lnOfSvdFactorUsed;
440 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
446 for (
unsigned int i = 0; i < 3; ++i) {
453 std::list<vpPoint>::const_iterator listp_end =
listP.end();
454 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it != listp_end; ++it) {
464 for (
unsigned int i = 0; i <
npt; ++i) {
465 A[i][0] = c3d[i].get_oX();
466 A[i][1] = c3d[i].get_oY();
467 A[i][2] = c3d[i].get_oZ();
472 bool isRankEqualTo3 =
false;
476 int nbMaxIter =
static_cast<int>(std::max<double>(std::ceil(logNOfSvdThresholdLimit - logNofSvdThresh), 1.));
479 for (
int i = 0; (i < nbMaxIter) && (!isRankEqualTo3); ++i) {
480 irank = A.
pseudoInverse(Ap, sv, svdThreshold, imA, imAt, kAt);
482 isRankEqualTo3 =
true;
485 isRankEqualTo3 =
false;
486 svdThreshold *= svdFactorUsedWhenFailure;
490 if (!isRankEqualTo3) {
491 std::stringstream errorMsg;
492 errorMsg <<
"In Dementhon planar, after ";
493 errorMsg << nbMaxIter;
494 errorMsg <<
" trials multiplying the svd threshold by ";
495 errorMsg << svdFactorUsedWhenFailure;
496 errorMsg <<
", rank (";
498 errorMsg <<
") is still not 3";
503 for (
unsigned int i = 0; i < 4; ++i) {
508 std::cout <<
"A" << std::endl << A << std::endl;
509 std::cout <<
"A^+" << std::endl << Ap << std::endl;
510 std::cout <<
"U^T = " << U.
t() << std::endl;
517 for (
unsigned int i = 0; i <
npt; ++i) {
518 xi[i] = c3d[i].get_x();
519 yi[i] = c3d[i].get_y();
527 calculTwoSolutionsDementhonPlan(I04, J04, U, cMo1, cMo2);
533 std::cout <<
"cMo Start Tree 1 : res " << res <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" "
537 std::cout <<
"cMo Start Tree 2 : res " << res <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" "
544 if ((erreur1 == -1) && (erreur2 == -1)) {
548 if ((erreur1 == 0) && (erreur2 == -1)) {
551 if ((erreur1 == -1) && (erreur2 == 0)) {
554 if ((erreur1 == 0) && (erreur2 == 0)) {
567 std::cout <<
"Pb z < 0 with Start Tree 1" << std::endl;
569 std::cout <<
"Pb z < 0 with Start Tree 2" << std::endl;
571 std::cout <<
" Tree 1 chosen " << std::endl;
573 std::cout <<
" Tree 2 chosen " << std::endl;
577 cMo[0][3] -= ((cdg[0] * cMo[0][0]) + (cdg[1] * cMo[0][1]) + (cdg[2] * cMo[0][2]));
578 cMo[1][3] -= ((cdg[0] * cMo[1][0]) + (cdg[1] * cMo[1][1]) + (cdg[2] * cMo[1][2]));
579 cMo[2][3] -= ((cdg[0] * cMo[2][0]) + (cdg[1] * cMo[2][1]) + (cdg[2] * cMo[2][2]));
582 std::cout <<
"end CCalculPose::PoseDementhonPlan()" << std::endl;
588 double squared_error = 0;
594 for (
unsigned int i = 0; i <
npt; ++i) {
596 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];
597 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];
598 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];
605 return squared_error;
609 #undef SEUIL_RESIDUAL
Implementation of column vector and the associated operations.
static double dotProd(const vpColVector &a, const vpColVector &b)
static vpColVector cross(const vpColVector &a, const vpColVector &b)
error that can be emitted by ViSP classes.
@ divideByZeroError
Division by zero.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
static double sqr(double x)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
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_oZ() const
Get the point oZ coordinate in the object frame.
void set_oY(double oY)
Set the point oY coordinate in the object frame.
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
void set_oX(double oX)
Set the point oX coordinate in the object frame.
double get_oY() const
Get the point oY coordinate in the object frame.
double computeResidualDementhon(const vpHomogeneousMatrix &cMo)
unsigned int npt
Number of point used in pose computation.
void poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
double m_dementhonSvThresh
SVD threshold use for the pseudo-inverse computation in poseDementhonPlan.
std::list< vpPoint > listP
Array of point (use here class vpPoint)
int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo)
void poseDementhonPlan(vpHomogeneousMatrix &cMo)
Implementation of a rotation vector as axis-angle minimal representation.