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;
102 cdg[0] = cdg[1] = cdg[2] = 0.0;
103 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
109 for (
unsigned int i = 0; i < 3; i++)
115 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
125 for (
unsigned int i = 0; i <
npt; i++) {
126 A[i][0] = c3d[i].get_oX();
127 A[i][1] = c3d[i].get_oY();
128 A[i][2] = c3d[i].get_oZ();
135 std::cout <<
"A" << std::endl << A << std::endl;
136 std::cout <<
"A^+" << std::endl << Ap << std::endl;
143 for (
unsigned int i = 0; i <
npt; i++) {
144 xprim[i] = c3d[i].get_x();
145 yprim[i] = c3d[i].get_y();
152 calculSolutionDementhon(I4, J4, cMo);
155 for (
unsigned int i = 0; i <
npt; i++) {
157 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];
166 double res_old = 2.0 * res;
171 while ((cpt < ITER_MAX) && (res > SEUIL_RESIDUAL) && (res < res_old)) {
177 for (
unsigned int i = 0; i <
npt; i++) {
179 (cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ()) / cMo[2][3];
181 xprim[i] = (1.0 + eps) * c3d[i].get_x();
182 yprim[i] = (1.0 + eps) * c3d[i].get_y();
187 calculSolutionDementhon(I4, J4, cMo);
189 for (
unsigned int i = 0; i <
npt; i++) {
191 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];
199 std::cout <<
"Pb z < 0 with cMo in Dementhon's loop" << std::endl;
205 std::cout <<
"it = " << cpt <<
" residu = " << res <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" "
210 std::cout <<
"Divergence : res = " << res <<
" res_old = " << res_old << std::endl;
217 cMo[0][3] -= (cdg[0] * cMo[0][0] + cdg[1] * cMo[0][1] + cdg[2] * cMo[0][2]);
218 cMo[1][3] -= (cdg[0] * cMo[1][0] + cdg[1] * cMo[1][1] + cdg[2] * cMo[1][2]);
219 cMo[2][3] -= (cdg[0] * cMo[2][0] + cdg[1] * cMo[2][1] + cdg[2] * cMo[2][2]);
222 static void calculRTheta(
double s,
double c,
double &r,
double &theta)
224 if ((fabs(c) > EPS_DEM) || (fabs(s) > EPS_DEM)) {
225 r = sqrt(sqrt(s * s + c * c));
226 theta = atan2(s, c) / 2.0;
229 if (fabs(c) > fabs(s)) {
250 for (
unsigned int i = 0; i < 3; i++) {
255 double c = J0.sumSquare() - I0.sumSquare();
258 calculRTheta(s, c, r, theta);
259 double co = cos(theta);
260 double si = sin(theta);
264 I = I04 + U * r * co;
265 J = J04 + U * r * si;
269 std::cout <<
"I0 " << I04.
t() << std::endl;
270 std::cout <<
"J0 " << J04.
t() << std::endl;
271 std::cout <<
"I1 " << I.t() << std::endl;
272 std::cout <<
"J1 " << J.t() << std::endl;
275 calculSolutionDementhon(I, J, cMo1);
278 I = I04 - U * r * co;
279 J = J04 - U * r * si;
282 std::cout <<
"I2 " << I.t() << std::endl;
283 std::cout <<
"J2 " << J.t() << std::endl;
286 calculSolutionDementhon(I, J, cMo2);
292 std::cout <<
"begin vpPose::CalculArbreDementhon() " << std::endl;
298 for (
unsigned int i = 0; i <
npt; i++) {
300 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];
307 unsigned int cpt = 0;
309 double res_old = 2.0 * res_min;
312 while ((cpt < ITER_MAX) && (res_min > SEUIL_RESIDUAL) && (res_min < res_old)) {
319 for (
unsigned int i = 0; i <
npt; i++) {
321 (cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ()) / cMo[2][3];
323 xprim[i] = (1.0 + eps) * c3d[i].get_x();
324 yprim[i] = (1.0 + eps) * c3d[i].get_y();
331 calculTwoSolutionsDementhonPlan(I04, J04, U, cMo1, cMo2);
336 for (
unsigned int i = 0; i <
npt; i++) {
338 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];
341 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];
346 if ((erreur1 == -1) && (erreur2 == -1)) {
349 std::cout <<
" End of loop since z < 0 for both solutions" << std::endl;
353 if ((erreur1 == 0) && (erreur2 == -1)) {
357 if ((erreur1 == -1) && (erreur2 == 0)) {
361 if ((erreur1 == 0) && (erreur2 == 0)) {
379 std::cout <<
"it = " << cpt <<
" cMo1 : residu: " << s <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" "
383 std::cout <<
"Pb z < 0 with cMo1" << std::endl;
389 std::cout <<
"it = " << cpt <<
" cMo2 : residu: " << s <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" "
393 std::cout <<
"Pb z < 0 with cMo2" << std::endl;
396 if (res_min > res_old) {
398 std::cout <<
"Divergence : res_min = " << res_min <<
" res_old = " << res_old << std::endl;
406 std::cout <<
"end vpPose::CalculArbreDementhon() return " << erreur << std::endl;
415 std::cout <<
"begin CCalculPose::PoseDementhonPlan()" << std::endl;
417 const double svdFactorUsedWhenFailure = 10.;
418 const double svdThresholdLimit = 1e-2;
419 const double lnOfSvdFactorUsed = std::log(svdFactorUsedWhenFailure);
420 const double logNOfSvdThresholdLimit = std::log(svdThresholdLimit)/lnOfSvdFactorUsed;
424 cdg[0] = cdg[1] = cdg[2] = 0.0;
425 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
431 for (
unsigned int i = 0; i < 3; i++)
437 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
447 for (
unsigned int i = 0; i <
npt; i++) {
448 A[i][0] = c3d[i].get_oX();
449 A[i][1] = c3d[i].get_oY();
450 A[i][2] = c3d[i].get_oZ();
455 bool isRankEqualTo3 =
false;
457 int nbMaxIter =
static_cast<int>(std::max(std::ceil(logNOfSvdThresholdLimit - logNofSvdThresh), 1.));
460 for (
int i = 0; i < nbMaxIter && !isRankEqualTo3; i++) {
461 irank = A.
pseudoInverse(Ap, sv, svdThreshold, imA, imAt, kAt);
463 isRankEqualTo3 =
true;
466 isRankEqualTo3 =
false;
467 svdThreshold *= svdFactorUsedWhenFailure;
471 if (!isRankEqualTo3) {
472 std::stringstream errorMsg;
473 errorMsg <<
"In Dementhon planar, after ";
474 errorMsg << nbMaxIter;
475 errorMsg <<
" trials multiplying the svd threshold by ";
476 errorMsg << svdFactorUsedWhenFailure;
477 errorMsg <<
", rank (";
479 errorMsg <<
") is still not 3";
484 for (
unsigned int i = 0; i < 4; i++) {
489 std::cout <<
"A" << std::endl << A << std::endl;
490 std::cout <<
"A^+" << std::endl << Ap << std::endl;
491 std::cout <<
"U^T = " << U.
t() << std::endl;
498 for (
unsigned int i = 0; i <
npt; i++) {
499 xi[i] = c3d[i].get_x();
500 yi[i] = c3d[i].get_y();
508 calculTwoSolutionsDementhonPlan(I04, J04, U, cMo1, cMo2);
514 std::cout <<
"cMo Start Tree 1 : res " << res <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" "
518 std::cout <<
"cMo Start Tree 2 : res " << res <<
" Theta U rotation: " <<
vpMath::deg(erc[0]) <<
" "
525 if ((erreur1 == -1) && (erreur2 == -1)) {
529 if ((erreur1 == 0) && (erreur2 == -1))
531 if ((erreur1 == -1) && (erreur2 == 0))
533 if ((erreur1 == 0) && (erreur2 == 0)) {
544 std::cout <<
"Pb z < 0 with Start Tree 1" << std::endl;
546 std::cout <<
"Pb z < 0 with Start Tree 2" << std::endl;
548 std::cout <<
" Tree 1 chosen " << std::endl;
550 std::cout <<
" Tree 2 chosen " << std::endl;
554 cMo[0][3] -= (cdg[0] * cMo[0][0] + cdg[1] * cMo[0][1] + cdg[2] * cMo[0][2]);
555 cMo[1][3] -= (cdg[0] * cMo[1][0] + cdg[1] * cMo[1][1] + cdg[2] * cMo[1][2]);
556 cMo[2][3] -= (cdg[0] * cMo[2][0] + cdg[1] * cMo[2][1] + cdg[2] * cMo[2][2]);
559 std::cout <<
"end CCalculPose::PoseDementhonPlan()" << std::endl;
565 double squared_error = 0;
571 for (
unsigned int i = 0; i <
npt; i++) {
573 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];
574 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];
575 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];
582 return squared_error;
586 #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.