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