Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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++) cdg[i] /= npt;
121  // printf("cdg : %lf %lf %lf\n", cdg[0], cdg[1],cdg[2]);
122 
123  c3d.clear();
124  /* translate the 3D points wrt cog */
125  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
126  P = (*it);
127  P.set_oX(P.get_oX() - cdg[0]);
128  P.set_oY(P.get_oY() - cdg[1]);
129  P.set_oZ(P.get_oZ() - cdg[2]);
130  c3d.push_back(P);
131  }
132 
133  vpMatrix A(npt, 4), Ap;
134 
135  for (unsigned int i = 0; i < npt; i++) {
136  A[i][0] = c3d[i].get_oX();
137  A[i][1] = c3d[i].get_oY();
138  A[i][2] = c3d[i].get_oZ();
139  A[i][3] = 1.0;
140  }
141  Ap = A.pseudoInverse();
142 
143 #if (DEBUG_LEVEL2)
144  {
145  std::cout << "A" << std::endl << A << std::endl;
146  std::cout << "A^+" << std::endl << Ap << std::endl;
147  }
148 #endif
149 
150  vpColVector xprim(npt);
151  vpColVector yprim(npt);
152 
153  for (unsigned int i = 0; i < npt; i++) {
154  xprim[i] = c3d[i].get_x();
155  yprim[i] = c3d[i].get_y();
156  }
157  vpColVector I4(4), J4(4);
158 
159  I4 = Ap * xprim;
160  J4 = Ap * yprim;
161 
162  calculSolutionDementhon(I4, J4, cMo);
163 
164  int erreur = 0;
165  for (unsigned int i = 0; i < npt; i++) {
166  double z;
167  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];
168  if (z <= 0.0) erreur = -1;
169  }
170  if (erreur == -1) {
171  throw(vpException(vpException::fatalError, "End of Dementhon since z < 0 for both solutions at the beginning"));
172  }
173  int cpt = 0;
174  double res = sqrt(computeResidualDementhon(cMo) / npt);
175  double res_old = 2.0*res;
176 
177  // In his paper, Dementhon suggests to use the difference
178  // between 2 successive norm of eps. We prefer to use the mean of the
179  // residuals in the image
180  while ((cpt < ITER_MAX) && (res > SEUIL_RESIDUAL) && (res < res_old)) {
181 
182  vpHomogeneousMatrix cMo_old;
183  res_old = res;
184  cMo_old = cMo;
185 
186  for (unsigned int i = 0; i < npt; i++) {
187  double eps = (cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ()) / cMo[2][3];
188 
189  xprim[i] = (1.0 + eps) * c3d[i].get_x();
190  yprim[i] = (1.0 + eps) * c3d[i].get_y();
191  }
192  I4 = Ap * xprim;
193  J4 = Ap * yprim;
194 
195  calculSolutionDementhon(I4, J4, cMo);
196  res = sqrt(computeResidualDementhon(cMo) / npt);
197  for (unsigned int i = 0; i < npt; i++) {
198  double z;
199  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];
200  if (z <= 0.0) erreur = -1;
201  }
202  if (erreur == -1) {
203  cMo = cMo_old;
204  res = res_old; // to leave the while loop
205 #if (DEBUG_LEVEL3)
206  std::cout << "Pb z < 0 with cMo in Dementhon's loop" << std::endl;
207 #endif
208  }
209 #if (DEBUG_LEVEL3)
210  vpThetaUVector erc;
211  cMo.extract(erc);
212  std::cout << "it = " << cpt << " residu = " << res << " Theta U rotation: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
213 #endif
214  if (res > res_old) {
215 #if (DEBUG_LEVEL3)
216  std::cout << "Divergence : res = " << res << " res_old = " << res_old << std::endl;
217 #endif
218  cMo = cMo_old;
219  }
220  cpt++;
221  }
222  // go back to the initial frame
223  cMo[0][3] -= (cdg[0] * cMo[0][0] + cdg[1] * cMo[0][1] + cdg[2] * cMo[0][2]);
224  cMo[1][3] -= (cdg[0] * cMo[1][0] + cdg[1] * cMo[1][1] + cdg[2] * cMo[1][2]);
225  cMo[2][3] -= (cdg[0] * cMo[2][0] + cdg[1] * cMo[2][1] + cdg[2] * cMo[2][2]);
226 }
227 
228 
229 static void calculRTheta(double s, double c, double &r, double &theta)
230 {
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;
234  } else {
235  if (fabs(c) > fabs(s)) {
236  r = fabs(c);
237  if (c >= 0.0)
238  theta = M_PI / 2;
239  else
240  theta = -M_PI / 2;
241  } else {
242  r = fabs(s);
243  if (s >= 0.0)
244  theta = M_PI / 4.0;
245  else
246  theta = -M_PI / 4.0;
247  }
248  }
249 }
250 
251 
252 static void calculTwoSolutionsDementhonPlan(vpColVector &I04, vpColVector &J04, vpColVector &U, vpHomogeneousMatrix &cMo1, vpHomogeneousMatrix &cMo2)
253 {
254  vpColVector I0(3), J0(3);
255  for (unsigned int i=0; i<3; i++) {
256  I0[i] = I04[i];
257  J0[i] = J04[i];
258  }
259  double s = -2.0 * vpColVector::dotProd(I0, J0);
260  double c = J0.sumSquare() - I0.sumSquare();
261 
262  double r, theta;
263  calculRTheta(s, c, r, theta);
264  double co = cos(theta);
265  double si = sin(theta);
266 
267  // calcul de la premiere solution
268  vpColVector I(4),J(4);
269  I = I04 + U * r * co;
270  J = J04 + U * r * si;
271 
272 #if (DEBUG_LEVEL2)
273  {
274  std::cout << "I0 " << I04.t() << std::endl;
275  std::cout << "J0 " << J04.t() << std::endl;
276  std::cout << "I1 " << I.t() << std::endl;
277  std::cout << "J1 " << J.t() << std::endl;
278  }
279 #endif
280  calculSolutionDementhon(I, J, cMo1);
281 
282  // calcul de la deuxieme solution
283  I = I04 - U * r * co;
284  J = J04 - U * r * si;
285 #if (DEBUG_LEVEL2)
286  {
287  std::cout << "I2 " << I.t() << std::endl;
288  std::cout << "J2 " << J.t() << std::endl;
289  }
290 #endif
291  calculSolutionDementhon(I, J, cMo2);
292 }
293 
294 
299 {
300 #if (DEBUG_LEVEL1)
301  std::cout << "begin vpPose::CalculArbreDementhon() " << std::endl;
302 #endif
303 
304  int erreur = 0;
305 
306  // test if all points are in front of the camera
307  for (unsigned int i = 0; i < npt; i++) {
308  double z;
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];
310  if (z <= 0.0) {
311  erreur = -1;
312  return erreur;
313  }
314  }
315 
316  unsigned int cpt = 0;
317  double res_min = sqrt(computeResidualDementhon(cMo)/npt);
318  double res_old = 2.0*res_min;
319 
320  /* FC modif SEUIL_RESIDUAL 0.01 a 0.001 */
321  while ((cpt < ITER_MAX) && (res_min > SEUIL_RESIDUAL) && (res_min < res_old)) {
322  vpHomogeneousMatrix cMo1, cMo2, cMo_old;
323 
324  res_old = res_min;
325  cMo_old = cMo;
326 
327  vpColVector xprim(npt),yprim(npt);
328  for (unsigned int i = 0; i < npt; i++) {
329  double eps = (cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ()) / cMo[2][3];
330 
331  xprim[i] = (1.0 + eps)*c3d[i].get_x();
332  yprim[i] = (1.0 + eps)*c3d[i].get_y();
333  }
334 
335  vpColVector I04(4), J04(4);
336  I04 = Ap * xprim;
337  J04 = Ap * yprim;
338 
339  calculTwoSolutionsDementhonPlan(I04,J04,U,cMo1,cMo2);
340 
341  // test if all points are in front of the camera for cMo1 and cMo2
342  int erreur1 = 0;
343  int erreur2 = 0;
344  for (unsigned int i = 0; i < npt; i++) {
345  double z;
346  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];
347  if (z <= 0.0) erreur1 = -1;
348  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];
349  if (z <= 0.0) erreur2 = -1;
350  }
351 
352  if ((erreur1 == -1) && (erreur2 == -1)) {
353  cMo = cMo_old;
354 #if (DEBUG_LEVEL3)
355  std::cout << " End of loop since z < 0 for both solutions" << std::endl;
356 #endif
357  break; // outside of while due to z < 0
358  }
359  if ((erreur1 == 0) && (erreur2 == -1)) {
360  cMo = cMo1;
361  res_min = sqrt(computeResidualDementhon(cMo)/npt);
362  }
363  if ((erreur1 == -1) && (erreur2 == 0)) {
364  cMo = cMo2;
365  res_min = sqrt(computeResidualDementhon(cMo)/npt);
366  }
367  if ((erreur1 == 0) && (erreur2 == 0)) {
368  double res1 = sqrt(computeResidualDementhon(cMo1)/npt);
369  double res2 = sqrt(computeResidualDementhon(cMo2)/npt);
370  if (res1 <= res2) {
371  res_min = res1;
372  cMo = cMo1;
373  } else {
374  res_min = res2;
375  cMo = cMo2;
376  }
377  }
378 
379 #if (DEBUG_LEVEL3)
380  if (erreur1 == 0) {
381  double s = sqrt(computeResidualDementhon(cMo1)/npt);
382  vpThetaUVector erc;
383  cMo1.extract(erc);
384  std::cout << "it = " << cpt << " cMo1 : residu: " << s << " Theta U rotation: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
385  }
386  else std::cout << "Pb z < 0 with cMo1" << std::endl;
387 
388  if (erreur2 == 0) {
389  double s = sqrt(computeResidualDementhon(cMo2)/npt);
390  vpThetaUVector erc;
391  cMo2.extract(erc);
392  std::cout << "it = " << cpt << " cMo2 : residu: " << s << " Theta U rotation: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
393  }
394  else std::cout << "Pb z < 0 with cMo2" << std::endl;
395 #endif
396 
397  if (res_min > res_old) {
398 #if (DEBUG_LEVEL3)
399  std::cout << "Divergence : res_min = " << res_min << " res_old = " << res_old << std::endl;
400 #endif
401  cMo = cMo_old;
402  }
403  cpt++;
404  } /* end of while */
405 
406 #if (DEBUG_LEVEL1)
407  std::cout << "end vpPose::CalculArbreDementhon() return " << erreur << std::endl;
408 #endif
409 
410  return erreur;
411 }
412 
422 {
423 #if (DEBUG_LEVEL1)
424  std::cout << "begin CCalculPose::PoseDementhonPlan()" << std::endl;
425 #endif
426 
427  vpPoint P;
428  double cdg[3];
429  /* compute the cog of the 3D points */
430  cdg[0] = cdg[1] = cdg[2] = 0.0;
431  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
432  P = (*it);
433  cdg[0] += P.get_oX();
434  cdg[1] += P.get_oY();
435  cdg[2] += P.get_oZ();
436  }
437  for (unsigned int i=0; i<3;i++) cdg[i] /= npt;
438  // printf("cdg : %lf %lf %lf\n", cdg[0], cdg[1],cdg[2]);
439 
440  c3d.clear();
441  /* translate the 3D points wrt cog */
442  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
443  P = (*it);
444  P.set_oX(P.get_oX() - cdg[0]);
445  P.set_oY(P.get_oY() - cdg[1]);
446  P.set_oZ(P.get_oZ() - cdg[2]);
447  c3d.push_back(P);
448  }
449 
450  vpMatrix A(npt,4);
451 
452  for (unsigned int i = 0; i < npt; i++) {
453  A[i][0] = c3d[i].get_oX();
454  A[i][1] = c3d[i].get_oY();
455  A[i][2] = c3d[i].get_oZ();
456  A[i][3] = 1.0;
457  }
458  vpColVector sv;
459  vpMatrix Ap,imA, imAt, kAt;
460  int irank = A.pseudoInverse(Ap, sv, 1.e-6, imA, imAt, kAt);
461  if (irank != 3) {
462  throw(vpException(vpException::fatalError, "In Dementhon planar, rank is not 3"));
463  }
464  // calcul de U
465  vpColVector U(4);
466  for (unsigned int i = 0; i<4; i++) {
467  U[i] = kAt[0][i];
468  }
469 #if (DEBUG_LEVEL2)
470  {
471  std::cout << "A" << std::endl << A << std::endl;
472  std::cout << "A^+" << std::endl << Ap << std::endl;
473  std::cout << "U^T = " << U.t() << std::endl;
474  }
475 #endif
476 
477  vpColVector xi(npt);
478  vpColVector yi(npt);
479 
480  for (unsigned int i = 0; i < npt; i++) {
481  xi[i] = c3d[i].get_x();
482  yi[i] = c3d[i].get_y();
483  }
484 
485  vpColVector I04(4), J04(4);
486  I04 = Ap * xi;
487  J04 = Ap * yi;
488 
489  vpHomogeneousMatrix cMo1,cMo2;
490  calculTwoSolutionsDementhonPlan(I04,J04,U,cMo1,cMo2);
491 
492 #if DEBUG_LEVEL3
493  double res = sqrt(computeResidualDementhon(cMo1)/npt);
494  vpThetaUVector erc;
495  cMo1.extract(erc);
496  std::cout << "cMo Start Tree 1 : res " << res << " Theta U rotation: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
497  res = sqrt(computeResidualDementhon(cMo2)/npt);
498  cMo2.extract(erc);
499  std::cout << "cMo Start Tree 2 : res " << res << " Theta U rotation: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
500 #endif
501 
502  int erreur1 = calculArbreDementhon(Ap, U, cMo1);
503  int erreur2 = calculArbreDementhon(Ap, U, cMo2);
504 
505  if ((erreur1 == -1) && (erreur2 == -1)) {
506  throw(vpException(vpException::fatalError, "Error in Dementhon planar: z < 0 with Start Tree 1 and Start Tree 2..."));
507  }
508  if ((erreur1 == 0) && (erreur2 == -1))
509  cMo = cMo1;
510  if ((erreur1 == -1) && (erreur2 == 0))
511  cMo = cMo2;
512  if ((erreur1 == 0) && (erreur2 == 0)) {
513  double s1 = computeResidualDementhon(cMo1);
514  double s2 = computeResidualDementhon(cMo2);
515 
516  if (s1 <= s2)
517  cMo = cMo1;
518  else
519  cMo = cMo2;
520 
521 #if DEBUG_LEVEL3
522  if (erreur1 == -1) std::cout << "Pb z < 0 with Start Tree 1" << std::endl;
523  if (erreur2 == -1) std::cout << "Pb z < 0 with Start Tree 2" << std::endl;
524  if (s1 <= s2) std::cout << " Tree 1 chosen " << std::endl;
525  else std::cout << " Tree 2 chosen " << std::endl;
526 #endif
527  }
528 
529  cMo[0][3] -= (cdg[0] * cMo[0][0] + cdg[1] * cMo[0][1] + cdg[2] * cMo[0][2]);
530  cMo[1][3] -= (cdg[0] * cMo[1][0] + cdg[1] * cMo[1][1] + cdg[2] * cMo[1][2]);
531  cMo[2][3] -= (cdg[0] * cMo[2][0] + cdg[1] * cMo[2][1] + cdg[2] * cMo[2][2]);
532 
533 #if (DEBUG_LEVEL1)
534  std::cout << "end CCalculPose::PoseDementhonPlan()" << std::endl;
535 #endif
536 }
537 
547 {
548  double squared_error = 0;
549 
550  // Be careful: since c3d has been translated so that point0 is at the cdg,
551  // cMo here corresponds to object frame at that point, i.e, only the one used
552  // internally to Dementhon's method
553 
554  for (unsigned int i = 0; i < npt; i++) {
555 
556  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];
557  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];
558  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];
559 
560  double x = X / Z;
561  double y = Y / Z;
562 
563  squared_error += vpMath::sqr(x - c3d[i].get_x()) + vpMath::sqr(y - c3d[i].get_y());
564  }
565  return squared_error;
566 }
567 
568 #undef EPS_DEM
569 #undef SEUIL_RESIDUAL
570 #undef ITER_MAX
571 #undef DEBUG_LEVEL1
572 #undef DEBUG_LEVEL2
573 #undef DEBUG_LEVEL3
int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
void set_oZ(const double oZ)
Set the point Z coordinate in the object frame.
Definition: vpPoint.cpp:465
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:285
Implementation of an homogeneous matrix and operations on such kind of matrices.
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:422
error that can be emited by ViSP classes.
Definition: vpException.h:71
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:108
Class that defines what is a point.
Definition: vpPoint.h:58
static double sqr(double x)
Definition: vpMath.h:108
vpRowVector t() const
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:424
void set_oX(const double oX)
Set the point X coordinate in the object frame.
Definition: vpPoint.cpp:461
void extract(vpRotationMatrix &R) const
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:107
double sumSquare() const
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:420
static double deg(double rad)
Definition: vpMath.h:95
void poseDementhonPlan(vpHomogeneousMatrix &cMo)
Compute the pose using Dementhon approach for planar objects this is a direct implementation of the a...
void poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
static double dotProd(const vpColVector &a, const vpColVector &b)
void set_oY(const double oY)
Set the point Y coordinate in the object frame.
Definition: vpPoint.cpp:463
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:1932
double computeResidualDementhon(const vpHomogeneousMatrix &cMo)
Compute and return the residual corresponding to the sum of squared residuals in meter^2 for the pose...
Implementation of a rotation vector as axis-angle minimal representation.