Visual Servoing Platform  version 3.6.1 under development (2024-06-22)
vpPoseDementhon.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 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 SEUIL_RESIDUAL 0.0001 /* avant 0.01 dans while du calculArbreDementhon */
38 #define EPS_DEM 0.001
39 #define ITER_MAX 30 /* max number of iterations for Demonthon's loop */
40 
42 
43 static void calculSolutionDementhon(vpColVector &I4, vpColVector &J4, vpHomogeneousMatrix &cMo)
44 {
45  // norm of the 3 first components of I4 and J4
46  const int id0 = 0, id1 = 1, id2 = 2;
47  double normI3 = sqrt((I4[id0] * I4[id0]) + (I4[id1] * I4[id1]) + (I4[id2] * I4[id2]));
48  double normJ3 = sqrt((J4[id0] * J4[id0]) + (J4[id1] * J4[id1]) + (J4[id2] * J4[id2]));
49 
50  if ((normI3 < 1e-10) || (normJ3 < 1e-10)) {
52  "Division by zero in Dementhon pose computation: normI or normJ = 0"));
53  }
54 
55  double Z0 = 2.0 / (normI3 + normJ3);
56 
57  const unsigned int sizeVectors = 3;
58  vpColVector I3(sizeVectors), J3(sizeVectors), K3(sizeVectors);
59  for (unsigned int i = 0; i < sizeVectors; ++i) {
60  I3[i] = I4[i] / normI3;
61  J3[i] = J4[i] / normJ3;
62  }
63 
64  K3 = vpColVector::cross(I3, J3); // k = I x J
65  K3.normalize();
66 
67  J3 = vpColVector::cross(K3, I3);
68 
69  // calcul de la matrice de passage
70  const unsigned int idX = 0, idY = 1, idZ = 2, idTranslation = 3;
71  cMo[idX][idX] = I3[idX];
72  cMo[idX][idY] = I3[idY];
73  cMo[idX][idZ] = I3[idZ];
74  cMo[idX][idTranslation] = I4[idTranslation] * Z0;
75 
76  cMo[idY][idX] = J3[idX];
77  cMo[idY][idY] = J3[idY];
78  cMo[idY][idZ] = J3[idZ];
79  cMo[idY][idTranslation] = J4[idTranslation] * Z0;
80 
81  cMo[idZ][idX] = K3[idX];
82  cMo[idZ][idY] = K3[idY];
83  cMo[idZ][idZ] = K3[idZ];
84  cMo[idZ][idTranslation] = Z0;
85 }
86 
88 {
89  vpPoint P;
90  const unsigned int idX = 0, idY = 1, idZ = 2, size = 3;
91  double cdg[size];
92  /* compute the cog of the 3D points */
93  cdg[idX] = 0.0;
94  cdg[idY] = 0.0;
95  cdg[idZ] = 0.0;
96  std::list<vpPoint>::const_iterator listp_end = listP.end();
97  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listp_end; ++it) {
98  P = (*it);
99  cdg[idX] += P.get_oX();
100  cdg[idY] += P.get_oY();
101  cdg[idZ] += P.get_oZ();
102  }
103 
104  for (unsigned int i = 0; i < size; ++i) {
105  cdg[i] /= npt;
106  }
107  // --comment: print cdg cdg of 0 cdg of 1 cdg of 2
108 
109  c3d.clear();
110  /* translate the 3D points wrt cog */
111  std::list<vpPoint>::const_iterator listp_end_s = listP.end();
112  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listp_end_s; ++it) {
113  P = (*it);
114  P.set_oX(P.get_oX() - cdg[idX]);
115  P.set_oY(P.get_oY() - cdg[idY]);
116  P.set_oZ(P.get_oZ() - cdg[idZ]);
117  c3d.push_back(P);
118  }
119 
120  const unsigned int idHomogeneousCoord = 3;
121  const unsigned int homogeneousCoordSize = 4;
122  vpMatrix A(npt, homogeneousCoordSize), Ap;
123 
124  for (unsigned int i = 0; i < npt; ++i) {
125  A[i][idX] = c3d[i].get_oX();
126  A[i][idY] = c3d[i].get_oY();
127  A[i][idZ] = c3d[i].get_oZ();
128  A[i][idHomogeneousCoord] = 1.0;
129  }
131 
132  vpColVector xprim(npt);
133  vpColVector yprim(npt);
134 
135  for (unsigned int i = 0; i < npt; ++i) {
136  xprim[i] = c3d[i].get_x();
137  yprim[i] = c3d[i].get_y();
138  }
139  vpColVector I4(homogeneousCoordSize), J4(homogeneousCoordSize);
140 
141  I4 = Ap * xprim;
142  J4 = Ap * yprim;
143 
144  calculSolutionDementhon(I4, J4, cMo);
145 
146  const unsigned int idTranslation = 3;
147  int erreur = 0;
148  for (unsigned int i = 0; i < npt; ++i) {
149  double z;
150  z = (cMo[idZ][idX] * c3d[i].get_oX()) + (cMo[idZ][idY] * c3d[i].get_oY()) + (cMo[idZ][idZ] * c3d[i].get_oZ()) + cMo[idZ][idTranslation];
151  if (z <= 0.0) {
152  erreur = -1;
153  }
154  }
155  if (erreur == -1) {
156  throw(vpException(vpException::fatalError, "End of Dementhon since z < 0 for both solutions at the beginning"));
157  }
158  int cpt = 0;
159  double res = sqrt(computeResidualDementhon(cMo) / npt);
160  double res_old = 2.0 * res;
161 
162  // In his paper, Dementhon suggests to use the difference
163  // between 2 successive norm of eps. We prefer to use the mean of the
164  // residuals in the image
165  while ((cpt < ITER_MAX) && (res > SEUIL_RESIDUAL) && (res < res_old)) {
166 
167  vpHomogeneousMatrix cMo_old;
168  res_old = res;
169  cMo_old = cMo;
170 
171  for (unsigned int i = 0; i < npt; ++i) {
172  double eps =
173  ((cMo[idZ][idX] * c3d[i].get_oX()) + (cMo[idZ][idY] * c3d[i].get_oY()) + (cMo[idZ][idZ] * c3d[i].get_oZ())) / cMo[idZ][idTranslation];
174 
175  xprim[i] = (1.0 + eps) * c3d[i].get_x();
176  yprim[i] = (1.0 + eps) * c3d[i].get_y();
177  }
178  I4 = Ap * xprim;
179  J4 = Ap * yprim;
180 
181  calculSolutionDementhon(I4, J4, cMo);
182  res = sqrt(computeResidualDementhon(cMo) / npt);
183  for (unsigned int i = 0; i < npt; ++i) {
184  double z;
185  z = (cMo[idZ][idX] * c3d[i].get_oX()) + (cMo[idZ][idY] * c3d[i].get_oY()) + (cMo[idZ][idZ] * c3d[i].get_oZ()) + cMo[idZ][idTranslation];
186  if (z <= 0.0) {
187  erreur = -1;
188  }
189  }
190  if (erreur == -1) {
191  cMo = cMo_old;
192  res = res_old; // to leave the while loop
193  }
194 
195  if (res > res_old) {
196  cMo = cMo_old;
197  }
198  ++cpt;
199  }
200  // go back to the initial frame
201  cMo[idX][3] -= ((cdg[idX] * cMo[idX][idX]) + (cdg[idY] * cMo[idX][idY]) + (cdg[idZ] * cMo[idX][idZ]));
202  cMo[idY][3] -= ((cdg[idX] * cMo[idY][idX]) + (cdg[idY] * cMo[idY][idY]) + (cdg[idZ] * cMo[idY][idZ]));
203  cMo[idZ][3] -= ((cdg[idX] * cMo[idZ][idX]) + (cdg[idY] * cMo[idZ][idY]) + (cdg[idZ] * cMo[idZ][idZ]));
204 }
205 
206 static void calculRTheta(double s, double c, double &r, double &theta)
207 {
208  if ((fabs(c) > EPS_DEM) || (fabs(s) > EPS_DEM)) {
209  r = sqrt(sqrt((s * s) + (c * c)));
210  theta = atan2(s, c) / 2.0;
211  }
212  else {
213  if (fabs(c) > fabs(s)) {
214  r = fabs(c);
215  if (c >= 0.0) {
216  theta = M_PI / 2.;
217  }
218  else {
219  theta = -M_PI / 2.;
220  }
221  }
222  else {
223  r = fabs(s);
224  if (s >= 0.0) {
225  theta = M_PI / 4.0;
226  }
227  else {
228  theta = -M_PI / 4.0;
229  }
230  }
231  }
232 }
233 
234 static void calculTwoSolutionsDementhonPlan(vpColVector &I04, vpColVector &J04, vpColVector &U,
236 {
237  const unsigned int size = 3;
238  vpColVector I0(size), J0(size);
239  for (unsigned int i = 0; i < size; ++i) {
240  I0[i] = I04[i];
241  J0[i] = J04[i];
242  }
243  double s = -2.0 * vpColVector::dotProd(I0, J0);
244  double c = J0.sumSquare() - I0.sumSquare();
245 
246  double r, theta;
247  calculRTheta(s, c, r, theta);
248  double co = cos(theta);
249  double si = sin(theta);
250 
251  // calcul de la premiere solution
252  const unsigned int sizeHomogeneous = 4;
253  vpColVector I(sizeHomogeneous), J(sizeHomogeneous);
254  I = I04 + (U * r * co);
255  J = J04 + (U * r * si);
256 
257  calculSolutionDementhon(I, J, cMo1);
258 
259  // calcul de la deuxieme solution
260  I = I04 - (U * r * co);
261  J = J04 - (U * r * si);
262 
263  calculSolutionDementhon(I, J, cMo2);
264 }
265 
267 {
268  int erreur = 0;
269 
270  // test if all points are in front of the camera
271  const unsigned int idX = 0, idY = 1, idZ = 2, idTranslation = 3;
272  for (unsigned int i = 0; i < npt; ++i) {
273  double z;
274  z = (cMo[idZ][idX] * c3d[i].get_oX()) + (cMo[idZ][idY] * c3d[i].get_oY()) + (cMo[idZ][idZ] * c3d[i].get_oZ()) + cMo[idZ][idTranslation];
275  if (z <= 0.0) {
276  erreur = -1;
277  return erreur;
278  }
279  }
280 
281  unsigned int cpt = 0;
282  double res_min = sqrt(computeResidualDementhon(cMo) / npt);
283  double res_old = 2.0 * res_min;
284 
285  /* FC modif SEUIL_RESIDUAL 0.01 a 0.001 */
286  while ((cpt < ITER_MAX) && (res_min > SEUIL_RESIDUAL) && (res_min < res_old)) {
287  vpHomogeneousMatrix cMo1, cMo2, cMo_old;
288 
289  res_old = res_min;
290  cMo_old = cMo;
291 
292  vpColVector xprim(npt), yprim(npt);
293  for (unsigned int i = 0; i < npt; ++i) {
294  double eps =
295  ((cMo[idZ][idX] * c3d[i].get_oX()) + (cMo[idZ][idY] * c3d[i].get_oY()) + (cMo[idZ][idZ] * c3d[i].get_oZ())) / cMo[idZ][idTranslation];
296 
297  xprim[i] = (1.0 + eps) * c3d[i].get_x();
298  yprim[i] = (1.0 + eps) * c3d[i].get_y();
299  }
300 
301  const unsigned int sizeHomogeneous = 4;
302  vpColVector I04(sizeHomogeneous), J04(sizeHomogeneous);
303  I04 = Ap * xprim;
304  J04 = Ap * yprim;
305 
306  calculTwoSolutionsDementhonPlan(I04, J04, U, cMo1, cMo2);
307 
308  // test if all points are in front of the camera for cMo1 and cMo2
309  int erreur1 = 0;
310  int erreur2 = 0;
311  for (unsigned int i = 0; i < npt; ++i) {
312  double z;
313  z = (cMo1[idZ][idX] * c3d[i].get_oX()) + (cMo1[idZ][idY] * c3d[i].get_oY()) + (cMo1[idZ][idZ] * c3d[i].get_oZ()) + cMo1[idZ][idTranslation];
314  if (z <= 0.0) {
315  erreur1 = -1;
316  }
317  z = (cMo2[idZ][idX] * c3d[i].get_oX()) + (cMo2[idZ][idY] * c3d[i].get_oY()) + (cMo2[idZ][idZ] * c3d[i].get_oZ()) + cMo2[idZ][idTranslation];
318  if (z <= 0.0) {
319  erreur2 = -1;
320  }
321  }
322 
323  if ((erreur1 == -1) && (erreur2 == -1)) {
324  cMo = cMo_old;
325  break; // outside of while due to z < 0
326  }
327  if ((erreur1 == 0) && (erreur2 == -1)) {
328  cMo = cMo1;
329  res_min = sqrt(computeResidualDementhon(cMo) / npt);
330  }
331  if ((erreur1 == -1) && (erreur2 == 0)) {
332  cMo = cMo2;
333  res_min = sqrt(computeResidualDementhon(cMo) / npt);
334  }
335  if ((erreur1 == 0) && (erreur2 == 0)) {
336  double res1 = sqrt(computeResidualDementhon(cMo1) / npt);
337  double res2 = sqrt(computeResidualDementhon(cMo2) / npt);
338  if (res1 <= res2) {
339  res_min = res1;
340  cMo = cMo1;
341  }
342  else {
343  res_min = res2;
344  cMo = cMo2;
345  }
346  }
347 
348  if (res_min > res_old) {
349  cMo = cMo_old;
350  }
351  ++cpt;
352  } /* end of while */
353 
354  return erreur;
355 }
356 
358 {
359  const double svdFactorUsedWhenFailure = 10.; // Factor by which is multipled m_dementhonSvThresh each time the svdDecomposition fails
360  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
361  const double lnOfSvdFactorUsed = std::log(svdFactorUsedWhenFailure);
362  const double logNOfSvdThresholdLimit = std::log(svdThresholdLimit)/lnOfSvdFactorUsed;
363  vpPoint P;
364  const unsigned int idX = 0, idY = 1, idZ = 2, size3Dpt = 3;
365  double cdg[size3Dpt];
366  /* compute the cog of the 3D points */
367  cdg[idX] = 0.0;
368  cdg[idY] = 0.0;
369  cdg[idZ] = 0.0;
370  std::list<vpPoint>::const_iterator listp_end = listP.end();
371  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listp_end; ++it) {
372  P = (*it);
373  cdg[idX] += P.get_oX();
374  cdg[idY] += P.get_oY();
375  cdg[idZ] += P.get_oZ();
376  }
377 
378  for (unsigned int i = 0; i < size3Dpt; ++i) {
379  cdg[i] /= npt;
380  }
381  // --comment: print cdg cdg of 0 of 1 and of 2
382 
383  c3d.clear();
384  /* translate the 3D points wrt cog */
385  std::list<vpPoint>::const_iterator listp_end_decl2 = listP.end();
386  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listp_end_decl2; ++it) {
387  P = (*it);
388  P.set_oX(P.get_oX() - cdg[0]);
389  P.set_oY(P.get_oY() - cdg[1]);
390  P.set_oZ(P.get_oZ() - cdg[2]);
391  c3d.push_back(P);
392  }
393 
394  const unsigned int sizeHomogeneous = 4, idHomogeneous = 3;
395  vpMatrix A(npt, sizeHomogeneous);
396 
397  for (unsigned int i = 0; i < npt; ++i) {
398  A[i][idX] = c3d[i].get_oX();
399  A[i][idY] = c3d[i].get_oY();
400  A[i][idZ] = c3d[i].get_oZ();
401  A[i][idHomogeneous] = 1.0;
402  }
403  vpColVector sv;
404  vpMatrix Ap, imA, imAt, kAt;
405  bool isRankEqualTo3 = false; // Indicates if the rank of A is the expected one
406  // Get the log_n(m_dementhonSvThresh), where n is the factor by which we will multiply it if the svd decomposition fails.
407  double logNofSvdThresh = std::log(m_dementhonSvThresh)/lnOfSvdFactorUsed;
408  // Ensure that if the user chose a threshold > svdThresholdLimit, at least 1 iteration of svd decomposition is performed
409  int nbMaxIter = static_cast<int>(std::max<double>(std::ceil(logNOfSvdThresholdLimit - logNofSvdThresh), 1.));
410  double svdThreshold = m_dementhonSvThresh;
411  int irank = 0;
412  int i = 0;
413  const unsigned int expectedRank = 3;
414  while ((i < nbMaxIter) && (!isRankEqualTo3)) {
415  irank = A.pseudoInverse(Ap, sv, svdThreshold, imA, imAt, kAt);
416  if (irank == expectedRank) {
417  isRankEqualTo3 = true;
418  }
419  else {
420  isRankEqualTo3 = false;
421  svdThreshold *= svdFactorUsedWhenFailure;
422  }
423  ++i;
424  }
425 
426  if (!isRankEqualTo3) {
427  std::stringstream errorMsg;
428  errorMsg << "In Dementhon planar, after ";
429  errorMsg << nbMaxIter;
430  errorMsg << " trials multiplying the svd threshold by ";
431  errorMsg << svdFactorUsedWhenFailure;
432  errorMsg << ", rank (";
433  errorMsg << irank;
434  errorMsg << ") is still not 3";
435  throw(vpException(vpException::fatalError, errorMsg.str()));
436  }
437  // calcul de U
438  vpColVector U(sizeHomogeneous);
439  for (unsigned int i = 0; i < sizeHomogeneous; ++i) {
440  U[i] = kAt[0][i];
441  }
442 
443  vpColVector xi(npt);
444  vpColVector yi(npt);
445 
446  for (unsigned int i = 0; i < npt; ++i) {
447  xi[i] = c3d[i].get_x();
448  yi[i] = c3d[i].get_y();
449  }
450 
451  vpColVector I04(sizeHomogeneous), J04(sizeHomogeneous);
452  I04 = Ap * xi;
453  J04 = Ap * yi;
454 
455  vpHomogeneousMatrix cMo1, cMo2;
456  calculTwoSolutionsDementhonPlan(I04, J04, U, cMo1, cMo2);
457 
458  int erreur1 = calculArbreDementhon(Ap, U, cMo1);
459  int erreur2 = calculArbreDementhon(Ap, U, cMo2);
460 
461  if ((erreur1 == -1) && (erreur2 == -1)) {
462  throw(
463  vpException(vpException::fatalError, "Error in Dementhon planar: z < 0 with Start Tree 1 and Start Tree 2..."));
464  }
465  if ((erreur1 == 0) && (erreur2 == -1)) {
466  cMo = cMo1;
467  }
468  if ((erreur1 == -1) && (erreur2 == 0)) {
469  cMo = cMo2;
470  }
471  if ((erreur1 == 0) && (erreur2 == 0)) {
472  double s1 = computeResidualDementhon(cMo1);
473  double s2 = computeResidualDementhon(cMo2);
474 
475  if (s1 <= s2) {
476  cMo = cMo1;
477  }
478  else {
479  cMo = cMo2;
480  }
481  }
482  const unsigned int idTranslation = 3;
483  cMo[idX][idTranslation] -= ((cdg[idX] * cMo[idX][idX]) + (cdg[idY] * cMo[idX][idY]) + (cdg[idZ] * cMo[idX][idZ]));
484  cMo[idY][idTranslation] -= ((cdg[idX] * cMo[idY][idX]) + (cdg[idY] * cMo[idY][idY]) + (cdg[idZ] * cMo[idY][idZ]));
485  cMo[idZ][idTranslation] -= ((cdg[idX] * cMo[idZ][idX]) + (cdg[idY] * cMo[idZ][idY]) + (cdg[idZ] * cMo[idZ][idZ]));
486 }
487 
489 {
490  double squared_error = 0;
491 
492  // Be careful: since c3d has been translated so that point0 is at the cdg,
493  // cMo here corresponds to object frame at that point, i.e, only the one used
494  // internally to Dementhon's method
495  const unsigned int idX = 0, idY = 1, idZ = 2, idTranslation = 3;
496  for (unsigned int i = 0; i < npt; ++i) {
497 
498  double X = (c3d[i].get_oX() * cMo[idX][idX]) + (c3d[i].get_oY() * cMo[idX][idY]) + (c3d[i].get_oZ() * cMo[idX][idZ]) + cMo[idX][idTranslation];
499  double Y = (c3d[i].get_oX() * cMo[idY][idX]) + (c3d[i].get_oY() * cMo[idY][idY]) + (c3d[i].get_oZ() * cMo[idY][idZ]) + cMo[idY][idTranslation];
500  double Z = (c3d[i].get_oX() * cMo[idZ][idX]) + (c3d[i].get_oY() * cMo[idZ][idY]) + (c3d[i].get_oZ() * cMo[idZ][idZ]) + cMo[idZ][idTranslation];
501 
502  double x = X / Z;
503  double y = Y / Z;
504 
505  squared_error += vpMath::sqr(x - c3d[i].get_x()) + vpMath::sqr(y - c3d[i].get_y());
506  }
507  return squared_error;
508 }
509 
511 
512 #undef EPS_DEM
513 #undef SEUIL_RESIDUAL
514 #undef ITER_MAX
Implementation of column vector and the associated operations.
Definition: vpColVector.h:171
static double dotProd(const vpColVector &a, const vpColVector &b)
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:1182
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ fatalError
Fatal error.
Definition: vpException.h:72
@ divideByZeroError
Division by zero.
Definition: vpException.h:70
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double sqr(double x)
Definition: vpMath.h:203
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:151
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2325
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:411
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:415
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition: vpPoint.cpp:457
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:459
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition: vpPoint.cpp:455
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:413
double computeResidualDementhon(const vpHomogeneousMatrix &cMo)
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:113
void poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
double m_dementhonSvThresh
SVD threshold use for the pseudo-inverse computation in poseDementhonPlan.
Definition: vpPose.h:769
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:114
int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo)
void poseDementhonPlan(vpHomogeneousMatrix &cMo)