Visual Servoing Platform  version 3.5.1 under development (2023-05-30)
vpPoseDementhon.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Pose computation.
33  *
34  * Authors:
35  * Eric Marchand
36  * Francois Chaumette
37  *
38  *****************************************************************************/
39 
40 #include <visp3/core/vpMath.h>
41 #include <visp3/vision/vpPose.h>
42 
43 #define DEBUG_LEVEL1 0
44 #define DEBUG_LEVEL2 0
45 #define DEBUG_LEVEL3 0
46 
47 #define SEUIL_RESIDUAL 0.0001 /* avant 0.01 dans while du calculArbreDementhon */
48 #define EPS_DEM 0.001
49 
50 #define ITER_MAX 30 /* max number of iterations for Demonthon's loop */
51 
52 static void calculSolutionDementhon(vpColVector &I4, vpColVector &J4, vpHomogeneousMatrix &cMo)
53 {
54 
55 #if (DEBUG_LEVEL1)
56  std::cout << "begin (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
57 #endif
58 
59  // norm of the 3 first components of I4 and J4
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]);
62 
63  if ((normI3 < 1e-10) || (normJ3 < 1e-10)) {
64  // vpERROR_TRACE(" normI+normJ = 0, division par zero " ) ;
66  "Division by zero in Dementhon pose computation: normI or normJ = 0"));
67  }
68 
69  double Z0 = 2.0 / (normI3 + normJ3);
70 
71  vpColVector I3(3), J3(3), K3(3);
72  for (unsigned int i = 0; i < 3; i++) {
73  I3[i] = I4[i] / normI3;
74  J3[i] = J4[i] / normJ3;
75  }
76 
77  K3 = vpColVector::cross(I3, J3); // k = I x J
78  K3.normalize();
79 
80  J3 = vpColVector::cross(K3, I3);
81 
82  // calcul de la matrice de passage
83  cMo[0][0] = I3[0];
84  cMo[0][1] = I3[1];
85  cMo[0][2] = I3[2];
86  cMo[0][3] = I4[3] * Z0;
87 
88  cMo[1][0] = J3[0];
89  cMo[1][1] = J3[1];
90  cMo[1][2] = J3[2];
91  cMo[1][3] = J4[3] * Z0;
92 
93  cMo[2][0] = K3[0];
94  cMo[2][1] = K3[1];
95  cMo[2][2] = K3[2];
96  cMo[2][3] = Z0;
97 
98 #if (DEBUG_LEVEL1)
99  std::cout << "end (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
100 #endif
101 }
102 
109 {
110  vpPoint P;
111  double cdg[3];
112  /* compute the cog of the 3D points */
113  cdg[0] = cdg[1] = cdg[2] = 0.0;
114  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
115  P = (*it);
116  cdg[0] += P.get_oX();
117  cdg[1] += P.get_oY();
118  cdg[2] += P.get_oZ();
119  }
120  for (unsigned int i = 0; i < 3; i++)
121  cdg[i] /= npt;
122  // printf("cdg : %lf %lf %lf\n", cdg[0], cdg[1],cdg[2]);
123 
124  c3d.clear();
125  /* translate the 3D points wrt cog */
126  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
127  P = (*it);
128  P.set_oX(P.get_oX() - cdg[0]);
129  P.set_oY(P.get_oY() - cdg[1]);
130  P.set_oZ(P.get_oZ() - cdg[2]);
131  c3d.push_back(P);
132  }
133 
134  vpMatrix A(npt, 4), Ap;
135 
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();
140  A[i][3] = 1.0;
141  }
143 
144 #if (DEBUG_LEVEL2)
145  {
146  std::cout << "A" << std::endl << A << std::endl;
147  std::cout << "A^+" << std::endl << Ap << std::endl;
148  }
149 #endif
150 
151  vpColVector xprim(npt);
152  vpColVector yprim(npt);
153 
154  for (unsigned int i = 0; i < npt; i++) {
155  xprim[i] = c3d[i].get_x();
156  yprim[i] = c3d[i].get_y();
157  }
158  vpColVector I4(4), J4(4);
159 
160  I4 = Ap * xprim;
161  J4 = Ap * yprim;
162 
163  calculSolutionDementhon(I4, J4, cMo);
164 
165  int erreur = 0;
166  for (unsigned int i = 0; i < npt; i++) {
167  double z;
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];
169  if (z <= 0.0)
170  erreur = -1;
171  }
172  if (erreur == -1) {
173  throw(vpException(vpException::fatalError, "End of Dementhon since z < 0 for both solutions at the beginning"));
174  }
175  int cpt = 0;
176  double res = sqrt(computeResidualDementhon(cMo) / npt);
177  double res_old = 2.0 * res;
178 
179  // In his paper, Dementhon suggests to use the difference
180  // between 2 successive norm of eps. We prefer to use the mean of the
181  // residuals in the image
182  while ((cpt < ITER_MAX) && (res > SEUIL_RESIDUAL) && (res < res_old)) {
183 
184  vpHomogeneousMatrix cMo_old;
185  res_old = res;
186  cMo_old = cMo;
187 
188  for (unsigned int i = 0; i < npt; i++) {
189  double eps =
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];
191 
192  xprim[i] = (1.0 + eps) * c3d[i].get_x();
193  yprim[i] = (1.0 + eps) * c3d[i].get_y();
194  }
195  I4 = Ap * xprim;
196  J4 = Ap * yprim;
197 
198  calculSolutionDementhon(I4, J4, cMo);
199  res = sqrt(computeResidualDementhon(cMo) / npt);
200  for (unsigned int i = 0; i < npt; i++) {
201  double z;
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];
203  if (z <= 0.0)
204  erreur = -1;
205  }
206  if (erreur == -1) {
207  cMo = cMo_old;
208  res = res_old; // to leave the while loop
209 #if (DEBUG_LEVEL3)
210  std::cout << "Pb z < 0 with cMo in Dementhon's loop" << std::endl;
211 #endif
212  }
213 #if (DEBUG_LEVEL3)
214  vpThetaUVector erc;
215  cMo.extract(erc);
216  std::cout << "it = " << cpt << " residu = " << res << " Theta U rotation: " << vpMath::deg(erc[0]) << " "
217  << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
218 #endif
219  if (res > res_old) {
220 #if (DEBUG_LEVEL3)
221  std::cout << "Divergence : res = " << res << " res_old = " << res_old << std::endl;
222 #endif
223  cMo = cMo_old;
224  }
225  cpt++;
226  }
227  // go back to the initial frame
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]);
231 }
232 
233 static void calculRTheta(double s, double c, double &r, double &theta)
234 {
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;
238  } else {
239  if (fabs(c) > fabs(s)) {
240  r = fabs(c);
241  if (c >= 0.0)
242  theta = M_PI / 2;
243  else
244  theta = -M_PI / 2;
245  } else {
246  r = fabs(s);
247  if (s >= 0.0)
248  theta = M_PI / 4.0;
249  else
250  theta = -M_PI / 4.0;
251  }
252  }
253 }
254 
255 static void calculTwoSolutionsDementhonPlan(vpColVector &I04, vpColVector &J04, vpColVector &U,
257 {
258  vpColVector I0(3), J0(3);
259  for (unsigned int i = 0; i < 3; i++) {
260  I0[i] = I04[i];
261  J0[i] = J04[i];
262  }
263  double s = -2.0 * vpColVector::dotProd(I0, J0);
264  double c = J0.sumSquare() - I0.sumSquare();
265 
266  double r, theta;
267  calculRTheta(s, c, r, theta);
268  double co = cos(theta);
269  double si = sin(theta);
270 
271  // calcul de la premiere solution
272  vpColVector I(4), J(4);
273  I = I04 + U * r * co;
274  J = J04 + U * r * si;
275 
276 #if (DEBUG_LEVEL2)
277  {
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;
282  }
283 #endif
284  calculSolutionDementhon(I, J, cMo1);
285 
286  // calcul de la deuxieme solution
287  I = I04 - U * r * co;
288  J = J04 - U * r * si;
289 #if (DEBUG_LEVEL2)
290  {
291  std::cout << "I2 " << I.t() << std::endl;
292  std::cout << "J2 " << J.t() << std::endl;
293  }
294 #endif
295  calculSolutionDementhon(I, J, cMo2);
296 }
297 
302 {
303 #if (DEBUG_LEVEL1)
304  std::cout << "begin vpPose::CalculArbreDementhon() " << std::endl;
305 #endif
306 
307  int erreur = 0;
308 
309  // test if all points are in front of the camera
310  for (unsigned int i = 0; i < npt; i++) {
311  double z;
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];
313  if (z <= 0.0) {
314  erreur = -1;
315  return erreur;
316  }
317  }
318 
319  unsigned int cpt = 0;
320  double res_min = sqrt(computeResidualDementhon(cMo) / npt);
321  double res_old = 2.0 * res_min;
322 
323  /* FC modif SEUIL_RESIDUAL 0.01 a 0.001 */
324  while ((cpt < ITER_MAX) && (res_min > SEUIL_RESIDUAL) && (res_min < res_old)) {
325  vpHomogeneousMatrix cMo1, cMo2, cMo_old;
326 
327  res_old = res_min;
328  cMo_old = cMo;
329 
330  vpColVector xprim(npt), yprim(npt);
331  for (unsigned int i = 0; i < npt; i++) {
332  double eps =
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];
334 
335  xprim[i] = (1.0 + eps) * c3d[i].get_x();
336  yprim[i] = (1.0 + eps) * c3d[i].get_y();
337  }
338 
339  vpColVector I04(4), J04(4);
340  I04 = Ap * xprim;
341  J04 = Ap * yprim;
342 
343  calculTwoSolutionsDementhonPlan(I04, J04, U, cMo1, cMo2);
344 
345  // test if all points are in front of the camera for cMo1 and cMo2
346  int erreur1 = 0;
347  int erreur2 = 0;
348  for (unsigned int i = 0; i < npt; i++) {
349  double z;
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];
351  if (z <= 0.0)
352  erreur1 = -1;
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];
354  if (z <= 0.0)
355  erreur2 = -1;
356  }
357 
358  if ((erreur1 == -1) && (erreur2 == -1)) {
359  cMo = cMo_old;
360 #if (DEBUG_LEVEL3)
361  std::cout << " End of loop since z < 0 for both solutions" << std::endl;
362 #endif
363  break; // outside of while due to z < 0
364  }
365  if ((erreur1 == 0) && (erreur2 == -1)) {
366  cMo = cMo1;
367  res_min = sqrt(computeResidualDementhon(cMo) / npt);
368  }
369  if ((erreur1 == -1) && (erreur2 == 0)) {
370  cMo = cMo2;
371  res_min = sqrt(computeResidualDementhon(cMo) / npt);
372  }
373  if ((erreur1 == 0) && (erreur2 == 0)) {
374  double res1 = sqrt(computeResidualDementhon(cMo1) / npt);
375  double res2 = sqrt(computeResidualDementhon(cMo2) / npt);
376  if (res1 <= res2) {
377  res_min = res1;
378  cMo = cMo1;
379  } else {
380  res_min = res2;
381  cMo = cMo2;
382  }
383  }
384 
385 #if (DEBUG_LEVEL3)
386  if (erreur1 == 0) {
387  double s = sqrt(computeResidualDementhon(cMo1) / npt);
388  vpThetaUVector erc;
389  cMo1.extract(erc);
390  std::cout << "it = " << cpt << " cMo1 : residu: " << s << " Theta U rotation: " << vpMath::deg(erc[0]) << " "
391  << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
392  } else
393  std::cout << "Pb z < 0 with cMo1" << std::endl;
394 
395  if (erreur2 == 0) {
396  double s = sqrt(computeResidualDementhon(cMo2) / npt);
397  vpThetaUVector erc;
398  cMo2.extract(erc);
399  std::cout << "it = " << cpt << " cMo2 : residu: " << s << " Theta U rotation: " << vpMath::deg(erc[0]) << " "
400  << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
401  } else
402  std::cout << "Pb z < 0 with cMo2" << std::endl;
403 #endif
404 
405  if (res_min > res_old) {
406 #if (DEBUG_LEVEL3)
407  std::cout << "Divergence : res_min = " << res_min << " res_old = " << res_old << std::endl;
408 #endif
409  cMo = cMo_old;
410  }
411  cpt++;
412  } /* end of while */
413 
414 #if (DEBUG_LEVEL1)
415  std::cout << "end vpPose::CalculArbreDementhon() return " << erreur << std::endl;
416 #endif
417 
418  return erreur;
419 }
420 
430 {
431 #if (DEBUG_LEVEL1)
432  std::cout << "begin CCalculPose::PoseDementhonPlan()" << std::endl;
433 #endif
434  const double svdFactorUsedWhenFailure = 10.; // Factor by which is multipled dementhonSvThresh each time the svdDecomposition fails
435  const double svdThresholdLimit = 1e-2; // The svd decomposition will be tested with a threshold up to this value. If with this threshold, the rank of A is still !=3, an exception is thrown
436  const double lnOfSvdFactorUsed = std::log(svdFactorUsedWhenFailure);
437  const double logNOfSvdThresholdLimit = std::log(svdThresholdLimit)/lnOfSvdFactorUsed;
438  vpPoint P;
439  double cdg[3];
440  /* compute the cog of the 3D points */
441  cdg[0] = cdg[1] = cdg[2] = 0.0;
442  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
443  P = (*it);
444  cdg[0] += P.get_oX();
445  cdg[1] += P.get_oY();
446  cdg[2] += P.get_oZ();
447  }
448  for (unsigned int i = 0; i < 3; i++)
449  cdg[i] /= npt;
450  // printf("cdg : %lf %lf %lf\n", cdg[0], cdg[1],cdg[2]);
451 
452  c3d.clear();
453  /* translate the 3D points wrt cog */
454  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
455  P = (*it);
456  P.set_oX(P.get_oX() - cdg[0]);
457  P.set_oY(P.get_oY() - cdg[1]);
458  P.set_oZ(P.get_oZ() - cdg[2]);
459  c3d.push_back(P);
460  }
461 
462  vpMatrix A(npt, 4);
463 
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();
468  A[i][3] = 1.0;
469  }
470  vpColVector sv;
471  vpMatrix Ap, imA, imAt, kAt;
472  bool isRankEqualTo3 = false; // Indicates if the rank of A is the expected one
473  double logNofSvdThresh = std::log(dementhonSvThresh)/lnOfSvdFactorUsed; // Get the log_n(dementhonSvThresh), where n is the factor by which we will multiply it if the svd decomposition fails.
474  int nbMaxIter = static_cast<int>(std::max(std::ceil(logNOfSvdThresholdLimit - logNofSvdThresh), 1.)); // Ensure that if the user chose a threshold > svdThresholdLimit, at least 1 iteration of svd decomposition is performed
475  double svdThreshold = dementhonSvThresh;
476  int irank = 0;
477  for(int i = 0; i < nbMaxIter && !isRankEqualTo3; i++)
478  {
479  irank = A.pseudoInverse(Ap, sv, svdThreshold, imA, imAt, kAt);
480  if(irank == 3)
481  {
482  isRankEqualTo3 = true;
483  }
484  else
485  {
486  isRankEqualTo3 = false;
487  svdThreshold *= svdFactorUsedWhenFailure;
488  }
489  }
490 
491  if (!isRankEqualTo3) {
492  std::stringstream errorMsg;
493  errorMsg << "In Dementhon planar, after ";
494  errorMsg << nbMaxIter;
495  errorMsg << " trials multiplying the svd threshold by ";
496  errorMsg << svdFactorUsedWhenFailure;
497  errorMsg << ", rank (";
498  errorMsg << irank;
499  errorMsg << ") is still not 3";
500  throw(vpException(vpException::fatalError, errorMsg.str()));
501  }
502  // calcul de U
503  vpColVector U(4);
504  for (unsigned int i = 0; i < 4; i++) {
505  U[i] = kAt[0][i];
506  }
507 #if (DEBUG_LEVEL2)
508  {
509  std::cout << "A" << std::endl << A << std::endl;
510  std::cout << "A^+" << std::endl << Ap << std::endl;
511  std::cout << "U^T = " << U.t() << std::endl;
512  }
513 #endif
514 
515  vpColVector xi(npt);
516  vpColVector yi(npt);
517 
518  for (unsigned int i = 0; i < npt; i++) {
519  xi[i] = c3d[i].get_x();
520  yi[i] = c3d[i].get_y();
521  }
522 
523  vpColVector I04(4), J04(4);
524  I04 = Ap * xi;
525  J04 = Ap * yi;
526 
527  vpHomogeneousMatrix cMo1, cMo2;
528  calculTwoSolutionsDementhonPlan(I04, J04, U, cMo1, cMo2);
529 
530 #if DEBUG_LEVEL3
531  double res = sqrt(computeResidualDementhon(cMo1) / npt);
532  vpThetaUVector erc;
533  cMo1.extract(erc);
534  std::cout << "cMo Start Tree 1 : res " << res << " Theta U rotation: " << vpMath::deg(erc[0]) << " "
535  << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
536  res = sqrt(computeResidualDementhon(cMo2) / npt);
537  cMo2.extract(erc);
538  std::cout << "cMo Start Tree 2 : res " << res << " Theta U rotation: " << vpMath::deg(erc[0]) << " "
539  << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
540 #endif
541 
542  int erreur1 = calculArbreDementhon(Ap, U, cMo1);
543  int erreur2 = calculArbreDementhon(Ap, U, cMo2);
544 
545  if ((erreur1 == -1) && (erreur2 == -1)) {
546  throw(
547  vpException(vpException::fatalError, "Error in Dementhon planar: z < 0 with Start Tree 1 and Start Tree 2..."));
548  }
549  if ((erreur1 == 0) && (erreur2 == -1))
550  cMo = cMo1;
551  if ((erreur1 == -1) && (erreur2 == 0))
552  cMo = cMo2;
553  if ((erreur1 == 0) && (erreur2 == 0)) {
554  double s1 = computeResidualDementhon(cMo1);
555  double s2 = computeResidualDementhon(cMo2);
556 
557  if (s1 <= s2)
558  cMo = cMo1;
559  else
560  cMo = cMo2;
561 
562 #if DEBUG_LEVEL3
563  if (erreur1 == -1)
564  std::cout << "Pb z < 0 with Start Tree 1" << std::endl;
565  if (erreur2 == -1)
566  std::cout << "Pb z < 0 with Start Tree 2" << std::endl;
567  if (s1 <= s2)
568  std::cout << " Tree 1 chosen " << std::endl;
569  else
570  std::cout << " Tree 2 chosen " << std::endl;
571 #endif
572  }
573 
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]);
577 
578 #if (DEBUG_LEVEL1)
579  std::cout << "end CCalculPose::PoseDementhonPlan()" << std::endl;
580 #endif
581 }
582 
592 {
593  double squared_error = 0;
594 
595  // Be careful: since c3d has been translated so that point0 is at the cdg,
596  // cMo here corresponds to object frame at that point, i.e, only the one used
597  // internally to Dementhon's method
598 
599  for (unsigned int i = 0; i < npt; i++) {
600 
601  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];
602  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];
603  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];
604 
605  double x = X / Z;
606  double y = Y / Z;
607 
608  squared_error += vpMath::sqr(x - c3d[i].get_x()) + vpMath::sqr(y - c3d[i].get_y());
609  }
610  return squared_error;
611 }
612 
613 #undef EPS_DEM
614 #undef SEUIL_RESIDUAL
615 #undef ITER_MAX
616 #undef DEBUG_LEVEL1
617 #undef DEBUG_LEVEL2
618 #undef DEBUG_LEVEL3
Implementation of column vector and the associated operations.
Definition: vpColVector.h:172
static double dotProd(const vpColVector &a, const vpColVector &b)
vpRowVector t() const
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:397
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ fatalError
Fatal error.
Definition: vpException.h:96
@ divideByZeroError
Division by zero.
Definition: vpException.h:94
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
static double sqr(double x)
Definition: vpMath.h:122
static double deg(double rad)
Definition: vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2232
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:461
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:465
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition: vpPoint.cpp:504
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:506
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition: vpPoint.cpp:502
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:463
double computeResidualDementhon(const vpHomogeneousMatrix &cMo)
Compute and return the residual corresponding to the sum of squared residuals in meter^2 for the pose...
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:121
double dementhonSvThresh
SVD threshold use for the pseudo-inverse computation in poseDementhonPlan.
Definition: vpPose.h:207
void poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:122
int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo)
void poseDementhonPlan(vpHomogeneousMatrix &cMo)
Compute the pose using Dementhon approach for planar objects this is a direct implementation of the a...
Implementation of a rotation vector as axis-angle minimal representation.