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 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
111 for (
unsigned int i = 0; i < 3; ++i) {
118 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
128 for (
unsigned int i = 0; i <
npt; ++i) {
129 A[i][0] = c3d[i].get_oX();
130 A[i][1] = c3d[i].get_oY();
131 A[i][2] = c3d[i].get_oZ();
138 std::cout <<
"A" << std::endl << A << std::endl;
139 std::cout <<
"A^+" << std::endl << Ap << std::endl;
146 for (
unsigned int i = 0; i <
npt; ++i) {
147 xprim[i] = c3d[i].get_x();
148 yprim[i] = c3d[i].get_y();
155 calculSolutionDementhon(I4, J4, cMo);
158 for (
unsigned int i = 0; i <
npt; ++i) {
160 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];
170 double res_old = 2.0 * res;
175 while ((cpt < ITER_MAX) && (res > SEUIL_RESIDUAL) && (res < res_old)) {
181 for (
unsigned int i = 0; i <
npt; ++i) {
183 ((cMo[2][0] * c3d[i].get_oX()) + (cMo[2][1] * c3d[i].get_oY()) + (cMo[2][2] * c3d[i].get_oZ())) / cMo[2][3];
185 xprim[i] = (1.0 + eps) * c3d[i].get_x();
186 yprim[i] = (1.0 + eps) * c3d[i].get_y();
191 calculSolutionDementhon(I4, J4, cMo);
193 for (
unsigned int i = 0; i <
npt; ++i) {
195 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];
204 std::cout <<
"Pb z < 0 with cMo in Dementhon's loop" << std::endl;
210 std::cout <<
"it = " << cpt <<
" residu = " << res <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" "
215 std::cout <<
"Divergence : res = " << res <<
" res_old = " << res_old << std::endl;
222 cMo[0][3] -= ((cdg[0] * cMo[0][0]) + (cdg[1] * cMo[0][1]) + (cdg[2] * cMo[0][2]));
223 cMo[1][3] -= ((cdg[0] * cMo[1][0]) + (cdg[1] * cMo[1][1]) + (cdg[2] * cMo[1][2]));
224 cMo[2][3] -= ((cdg[0] * cMo[2][0]) + (cdg[1] * cMo[2][1]) + (cdg[2] * cMo[2][2]));
227 static void calculRTheta(
double s,
double c,
double &r,
double &theta)
229 if ((fabs(c) > EPS_DEM) || (fabs(s) > EPS_DEM)) {
230 r = sqrt(sqrt((s * s) + (c * c)));
231 theta = atan2(s, c) / 2.0;
234 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);
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) {
330 ((cMo[2][0] * c3d[i].get_oX()) + (cMo[2][1] * c3d[i].get_oY()) + (cMo[2][2] * c3d[i].get_oZ())) / cMo[2][3];
332 xprim[i] = (1.0 + eps) * c3d[i].get_x();
333 yprim[i] = (1.0 + eps) * c3d[i].get_y();
340 calculTwoSolutionsDementhonPlan(I04, J04, U, cMo1, cMo2);
345 for (
unsigned int i = 0; i <
npt; ++i) {
347 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];
351 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];
357 if ((erreur1 == -1) && (erreur2 == -1)) {
360 std::cout <<
" End of loop since z < 0 for both solutions" << std::endl;
364 if ((erreur1 == 0) && (erreur2 == -1)) {
368 if ((erreur1 == -1) && (erreur2 == 0)) {
372 if ((erreur1 == 0) && (erreur2 == 0)) {
390 std::cout <<
"it = " << cpt <<
" cMo1 : residu: " << s <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" "
394 std::cout <<
"Pb z < 0 with cMo1" << std::endl;
400 std::cout <<
"it = " << cpt <<
" cMo2 : residu: " << s <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" "
404 std::cout <<
"Pb z < 0 with cMo2" << std::endl;
407 if (res_min > res_old) {
409 std::cout <<
"Divergence : res_min = " << res_min <<
" res_old = " << res_old << std::endl;
417 std::cout <<
"end vpPose::CalculArbreDementhon() return " << erreur << std::endl;
426 std::cout <<
"begin CCalculPose::PoseDementhonPlan()" << std::endl;
428 const double svdFactorUsedWhenFailure = 10.;
429 const double svdThresholdLimit = 1e-2;
430 const double lnOfSvdFactorUsed = std::log(svdFactorUsedWhenFailure);
431 const double logNOfSvdThresholdLimit = std::log(svdThresholdLimit)/lnOfSvdFactorUsed;
438 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
444 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();
469 bool isRankEqualTo3 =
false;
473 int nbMaxIter =
static_cast<int>(std::max<double>(std::ceil(logNOfSvdThresholdLimit - logNofSvdThresh), 1.));
476 for (
int i = 0; (i < nbMaxIter) && (!isRankEqualTo3); ++i) {
477 irank = A.
pseudoInverse(Ap, sv, svdThreshold, imA, imAt, kAt);
479 isRankEqualTo3 =
true;
482 isRankEqualTo3 =
false;
483 svdThreshold *= svdFactorUsedWhenFailure;
487 if (!isRankEqualTo3) {
488 std::stringstream errorMsg;
489 errorMsg <<
"In Dementhon planar, after ";
490 errorMsg << nbMaxIter;
491 errorMsg <<
" trials multiplying the svd threshold by ";
492 errorMsg << svdFactorUsedWhenFailure;
493 errorMsg <<
", rank (";
495 errorMsg <<
") is still not 3";
500 for (
unsigned int i = 0; i < 4; ++i) {
505 std::cout <<
"A" << std::endl << A << std::endl;
506 std::cout <<
"A^+" << std::endl << Ap << std::endl;
507 std::cout <<
"U^T = " << U.
t() << std::endl;
514 for (
unsigned int i = 0; i <
npt; ++i) {
515 xi[i] = c3d[i].get_x();
516 yi[i] = c3d[i].get_y();
524 calculTwoSolutionsDementhonPlan(I04, J04, U, cMo1, cMo2);
530 std::cout <<
"cMo Start Tree 1 : res " << res <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" "
534 std::cout <<
"cMo Start Tree 2 : res " << res <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" "
541 if ((erreur1 == -1) && (erreur2 == -1)) {
545 if ((erreur1 == 0) && (erreur2 == -1)) {
548 if ((erreur1 == -1) && (erreur2 == 0)) {
551 if ((erreur1 == 0) && (erreur2 == 0)) {
564 std::cout <<
"Pb z < 0 with Start Tree 1" << std::endl;
566 std::cout <<
"Pb z < 0 with Start Tree 2" << std::endl;
568 std::cout <<
" Tree 1 chosen " << std::endl;
570 std::cout <<
" Tree 2 chosen " << std::endl;
574 cMo[0][3] -= ((cdg[0] * cMo[0][0]) + (cdg[1] * cMo[0][1]) + (cdg[2] * cMo[0][2]));
575 cMo[1][3] -= ((cdg[0] * cMo[1][0]) + (cdg[1] * cMo[1][1]) + (cdg[2] * cMo[1][2]));
576 cMo[2][3] -= ((cdg[0] * cMo[2][0]) + (cdg[1] * cMo[2][1]) + (cdg[2] * cMo[2][2]));
579 std::cout <<
"end CCalculPose::PoseDementhonPlan()" << std::endl;
585 double squared_error = 0;
591 for (
unsigned int i = 0; i <
npt; ++i) {
593 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];
594 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];
595 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];
602 return squared_error;
606 #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.