Visual Servoing Platform  version 3.6.1 under development (2024-10-10)
vpPose.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 
39 #include <visp3/core/vpCameraParameters.h>
40 #include <visp3/core/vpDisplay.h>
41 #include <visp3/core/vpException.h>
42 #include <visp3/core/vpMath.h>
43 #include <visp3/core/vpMeterPixelConversion.h>
44 #include <visp3/core/vpUniRand.h>
45 #include <visp3/vision/vpPose.h>
46 #include <visp3/vision/vpPoseException.h>
47 #ifdef VISP_HAVE_HOMOGRAPHY
48 #include <visp3/vision/vpHomography.h>
49 #endif
50 
51 #include <cmath> // std::fabs
52 #include <limits> // numeric_limits
53 
54 BEGIN_VISP_NAMESPACE
55 
56 #ifndef DOXYGEN_SHOULD_SKIP_THIS
57 namespace
58 {
59 const int def_vvsIterMax = 200;
60 const unsigned int def_ransacNbInlier = 4;
61 const int def_ransacMaxTrials = 1000;
62 }
63 #endif // DOXYGEN_SHOULD_SKIP_THIS
64 
66  : npt(0), listP(), residual(0), m_lambda(0.9), m_dementhonSvThresh(1e-6), vvsIterMax(def_vvsIterMax), c3d(),
67  computeCovariance(false), covarianceMatrix(),
68  ransacNbInlierConsensus(def_ransacNbInlier), ransacMaxTrials(def_ransacMaxTrials), ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001),
69  distToPlaneForCoplanarityTest(0.001), ransacFlag(vpPose::NO_FILTER), listOfPoints(), useParallelRansac(false),
70  nbParallelRansacThreads(0), // 0 means that we use C++11 (if available) to get the number of threads
71  vvsEpsilon(1e-8)
72 { }
73 
74 vpPose::vpPose(const std::vector<vpPoint> &lP)
75  : npt(static_cast<unsigned int>(lP.size())), listP(lP.begin(), lP.end()), residual(0), m_lambda(0.9),
76  m_dementhonSvThresh(1e-6), vvsIterMax(def_vvsIterMax),
77  c3d(), computeCovariance(false), covarianceMatrix(), ransacNbInlierConsensus(def_ransacNbInlier), ransacMaxTrials(def_ransacMaxTrials),
78  ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001), distToPlaneForCoplanarityTest(0.001),
79  ransacFlag(vpPose::NO_FILTER), listOfPoints(lP), useParallelRansac(false),
80  nbParallelRansacThreads(0), // 0 means that we use C++11 (if available) to get the number of threads
81  vvsEpsilon(1e-8)
82 { }
83 
85 {
86  listP.clear();
87 }
88 
90 {
91  listP.clear();
92  listOfPoints.clear();
93  npt = 0;
94 }
95 
96 void vpPose::addPoint(const vpPoint &newP)
97 {
98  listP.push_back(newP);
99  listOfPoints.push_back(newP);
100  ++npt;
101 }
102 
103 void vpPose::addPoints(const std::vector<vpPoint> &lP)
104 {
105  listP.insert(listP.end(), lP.begin(), lP.end());
106  listOfPoints.insert(listOfPoints.end(), lP.begin(), lP.end());
107  npt = static_cast<unsigned int>(listP.size());
108 }
109 
110 void vpPose::setDistToPlaneForCoplanTest(double d) { distToPlaneForCoplanarityTest = d; }
111 
112 void vpPose::setDementhonSvThreshold(const double &svThresh)
113 {
114  if (svThresh < 0) {
115  throw vpException(vpException::badValue, "The svd threshold must be positive");
116  }
117  m_dementhonSvThresh = svThresh;
118 }
119 
120 bool vpPose::coplanar(int &coplanar_plane_type, double *p_a, double *p_b, double *p_c, double *p_d)
121 {
122  coplanar_plane_type = 0;
123  const unsigned int nbMinPt = 2;
124  if (npt < nbMinPt) {
125  throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough point (%d) to compute the pose ", npt));
126  }
127 
128  const unsigned int nbPtPlan = 3;
129  if (npt == nbPtPlan) {
130  return true;
131  }
132 
133  // Shuffling the points to limit the risk of using points close to each other
134  std::vector<vpPoint> shuffled_listP = vpUniRand::shuffleVector<vpPoint>(listOfPoints);
135 
136  double x1 = 0, x2 = 0, x3 = 0, y1 = 0, y2 = 0, y3 = 0, z1 = 0, z2 = 0, z3 = 0;
137 
138  std::vector<vpPoint>::const_iterator it = shuffled_listP.begin();
139 
140  vpPoint P1, P2, P3;
141 
142  // Get three 3D points that are not collinear and that is not at origin
143  bool degenerate = true;
144  bool not_on_origin = true;
145  std::vector<vpPoint>::const_iterator it_tmp;
146 
147  std::vector<vpPoint>::const_iterator it_i, it_j, it_k;
148  std::vector<vpPoint>::const_iterator shuffled_listp_end = shuffled_listP.end();
149  for (it_i = shuffled_listP.begin(); it_i != shuffled_listp_end; ++it_i) {
150  if (degenerate == false) {
151  // --comment: print "Found a non degenerate configuration"
152  break;
153  }
154  P1 = *it_i;
155  // Test if point is on origin
156  if ((std::fabs(P1.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
157  (std::fabs(P1.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
158  (std::fabs(P1.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
159  not_on_origin = false;
160  }
161  else {
162  not_on_origin = true;
163  }
164  if (not_on_origin) {
165  it_tmp = it_i;
166  ++it_tmp; // j = i+1
167  std::vector<vpPoint>::const_iterator shuffled_listp_end_1 = shuffled_listP.end();
168  for (it_j = it_tmp; it_j != shuffled_listp_end_1; ++it_j) {
169  if (degenerate == false) {
170  // --comment: cout "Found a non degenerate configuration"
171  break;
172  }
173  P2 = *it_j;
174  if ((std::fabs(P2.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
175  (std::fabs(P2.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
176  (std::fabs(P2.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
177  not_on_origin = false;
178  }
179  else {
180  not_on_origin = true;
181  }
182  if (not_on_origin) {
183  it_tmp = it_j;
184  ++it_tmp; // k = j+1
185  std::vector<vpPoint>::const_iterator shuffled_listp_end_2 = shuffled_listP.end();
186  for (it_k = it_tmp; it_k != shuffled_listp_end_2; ++it_k) {
187  P3 = *it_k;
188  if ((std::fabs(P3.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
189  (std::fabs(P3.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
190  (std::fabs(P3.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
191  not_on_origin = false;
192  }
193  else {
194  not_on_origin = true;
195  }
196  if (not_on_origin) {
197  x1 = P1.get_oX();
198  x2 = P2.get_oX();
199  x3 = P3.get_oX();
200 
201  y1 = P1.get_oY();
202  y2 = P2.get_oY();
203  y3 = P3.get_oY();
204 
205  z1 = P1.get_oZ();
206  z2 = P2.get_oZ();
207  z3 = P3.get_oZ();
208 
209  const int idX = 0, idY = 1, idZ = 2;
210  vpColVector a_b(3), b_c(3), cross_prod;
211  a_b[idX] = x1 - x2;
212  a_b[idY] = y1 - y2;
213  a_b[idZ] = z1 - z2;
214  b_c[idX] = x2 - x3;
215  b_c[idY] = y2 - y3;
216  b_c[idZ] = z2 - z3;
217 
218  cross_prod = vpColVector::crossProd(a_b, b_c);
219  if (cross_prod.sumSquare() <= std::numeric_limits<double>::epsilon()) {
220  degenerate = true; // points are collinear
221  }
222  else {
223  degenerate = false;
224  }
225  }
226  if (degenerate == false) {
227  break;
228  }
229  }
230  }
231  }
232  }
233  }
234 
235  if (degenerate) {
236  const int typeCollinear = 4;
237  coplanar_plane_type = typeCollinear; // points are collinear
238  return true;
239  }
240 
241  double a = (((y1 * z2) - (y1 * z3) - (y2 * z1)) + (y2 * z3) + (y3 * z1)) - (y3 * z2);
242  double b = ((((-x1 * z2) + (x1 * z3) + (x2 * z1)) - (x2 * z3)) - (x3 * z1)) + (x3 * z2);
243  double c = (((x1 * y2) - (x1 * y3) - (x2 * y1)) + (x2 * y3) + (x3 * y1)) - (x3 * y2);
244  double d = (((-x1 * y2 * z3) + (x1 * y3 * z2) + (x2 * y1 * z3)) - (x2 * y3 * z1) - (x3 * y1 * z2)) + (x3 * y2 * z1);
245 
246  if ((std::fabs(b) <= std::numeric_limits<double>::epsilon()) &&
247  (std::fabs(c) <= std::numeric_limits<double>::epsilon())) {
248  const int typeAxD = 1;
249  coplanar_plane_type = typeAxD; // ax=d
250  }
251  else if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
252  (std::fabs(c) <= std::numeric_limits<double>::epsilon())) {
253  const int typeByD = 2;
254  coplanar_plane_type = typeByD; // by=d
255  }
256  else if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
257  (std::fabs(b) <= std::numeric_limits<double>::epsilon())) {
258  const int typeCzD = 3;
259  coplanar_plane_type = typeCzD; // cz=d
260  }
261 
262  double D = sqrt(vpMath::sqr(a) + vpMath::sqr(b) + vpMath::sqr(c));
263 
264  std::vector<vpPoint>::const_iterator shuffled_listp_end_decl2 = shuffled_listP.end();
265  for (it = shuffled_listP.begin(); it != shuffled_listp_end_decl2; ++it) {
266  P1 = *it;
267  double dist = ((a * P1.get_oX()) + (b * P1.get_oY()) + (c * P1.get_oZ()) + d) / D;
268 
269  if (fabs(dist) > distToPlaneForCoplanarityTest) {
270  // points are not coplanar
271  return false;
272  }
273  }
274 
275  // points are coplanar
276 
277  // If the points are coplanar and the input/output parameters are different from nullptr,
278  // getting the values of the plan coefficient and storing in the input/output parameters
279  if (p_a != nullptr) {
280  *p_a = a;
281  }
282 
283  if (p_b != nullptr) {
284  *p_b = b;
285  }
286 
287  if (p_c != nullptr) {
288  *p_c = c;
289  }
290 
291  if (p_d != nullptr) {
292  *p_d = d;
293  }
294 
295  return true;
296 }
297 
299 {
300  double squared_error = 0;
301  vpPoint P;
302  std::list<vpPoint>::const_iterator listp_end = listP.end();
303  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listp_end; ++it) {
304  P = *it;
305  double x = P.get_x();
306  double y = P.get_y();
307 
308  P.track(cMo);
309 
310  squared_error += vpMath::sqr(x - P.get_x()) + vpMath::sqr(y - P.get_y());
311  }
312  return squared_error;
313 }
314 
316 {
317  vpColVector residuals;
318  return computeResidual(cMo, cam, residuals);
319 }
320 
321 double vpPose::computeResidual(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, vpColVector &residuals) const
322 {
323  double squared_error = 0;
324  residuals.resize(static_cast<unsigned int>(listP.size()));
325  vpPoint P;
326  unsigned int i = 0;
327  std::list<vpPoint>::const_iterator listp_end = listP.end();
328  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listp_end; ++it) {
329  P = *it;
330  double x = P.get_x();
331  double y = P.get_y();
332 
333  double u_initial = 0., v_initial = 0.;
334  vpMeterPixelConversion::convertPoint(cam, x, y, u_initial, v_initial);
335 
336  P.track(cMo);
337 
338  double u_moved = 0., v_moved = 0.;
339  vpMeterPixelConversion::convertPoint(cam, P.get_x(), P.get_y(), u_moved, v_moved);
340 
341  double squaredResidual = vpMath::sqr(u_moved - u_initial) + vpMath::sqr(v_moved - v_initial);
342  residuals[i] = squaredResidual;
343  ++i;
344  squared_error += squaredResidual;
345  }
346  return squared_error;
347 }
348 
349 void vpPose::callLagrangePose(vpHomogeneousMatrix &cMo)
350 {
351  const unsigned int minNbPtLagrangePlan = 4;
352  const unsigned int minNbPtLagrangeNoPlan = 6;
353  // test if the 3D points are coplanar
354  double a, b, c, d; // To get the plan coefficients if the points are coplanar
355  int coplanar_plane_type = 0;
356  bool plan = coplanar(coplanar_plane_type, &a, &b, &c, &d);
357 
358  if (plan == true) {
359  const int typeCollinear = 4;
360  if (coplanar_plane_type == typeCollinear) {
361  throw(vpPoseException(vpPoseException::notEnoughPointError, "Lagrange method cannot be used in that case "
362  "(points are collinear)"));
363  }
364  if (npt < minNbPtLagrangePlan) {
366  "Lagrange method cannot be used in that case "
367  "(at least 4 points are required). "
368  "Not enough point (%d) to compute the pose ",
369  npt));
370  }
371  poseLagrangePlan(cMo, &plan, &a, &b, &c, &d);
372  }
373  else {
374  if (npt < minNbPtLagrangeNoPlan) {
376  "Lagrange method cannot be used in that case "
377  "(at least 6 points are required when 3D points are non coplanar). "
378  "Not enough point (%d) to compute the pose ",
379  npt));
380  }
381  poseLagrangeNonPlan(cMo);
382  }
383 }
384 
385 bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, FuncCheckValidityPose func)
386 {
387  const unsigned int minNbPtDementhon = 4;
388  const unsigned int minNbPtRansac = 4;
389  std::stringstream errMsgDementhon;
390  errMsgDementhon << "Dementhon method cannot be used in that case "
391  << "(at least " << minNbPtDementhon << " points are required)"
392  << "Not enough point (" << npt << ") to compute the pose ";
393 
394  switch (method) {
395  case DEMENTHON:
397  case DEMENTHON_LOWE: {
398  if (npt < minNbPtDementhon) {
399  throw(vpPoseException(vpPoseException::notEnoughPointError, errMsgDementhon.str()));
400  }
401  // test if the 3D points are coplanar
402  int coplanar_plane_type = 0;
403  bool plan = coplanar(coplanar_plane_type);
404  plan ? poseDementhonPlan(cMo) : poseDementhonNonPlan(cMo);
405  break;
406  }
407  case LAGRANGE:
408  case LAGRANGE_VIRTUAL_VS:
409  case LAGRANGE_LOWE: {
410  callLagrangePose(cMo);
411  break;
412  }
413  case RANSAC: {
414  if (npt < minNbPtRansac) {
416  "Ransac method cannot be used in that case "
417  "(at least 4 points are required). "
418  "Not enough point (%d) to compute the pose ",
419  npt));
420  }
421  return poseRansac(cMo, func);
422  }
423  case LOWE:
424  case VIRTUAL_VS:
425  break;
428  }
429  default:
430  {
431  std::cout << "method not identified" << std::endl;
432  }
433  }
434 
435  switch (method) {
436  case LAGRANGE:
437  case DEMENTHON:
439  case RANSAC:
440  break;
441  case VIRTUAL_VS:
442  case LAGRANGE_VIRTUAL_VS:
443  case DEMENTHON_VIRTUAL_VS: {
444  poseVirtualVS(cMo);
445  break;
446  }
447  case LOWE:
448  case LAGRANGE_LOWE:
449  case DEMENTHON_LOWE:
450  {
451  poseLowe(cMo);
452  }
453  break;
454  default:
455  {
456  std::cout << "method not identified" << std::endl;
457  }
458  }
459 
460  return true;
461 }
462 
464 {
465  vpHomogeneousMatrix cMo_dementhon, cMo_lagrange;
466  double r_dementhon = std::numeric_limits<double>::max(), r_lagrange = std::numeric_limits<double>::max();
467  // test if the 3D points are coplanar
468  double a, b, c, d; // To get the plan coefficients if the points are coplanar
469  int coplanar_plane_type = 0;
470  bool plan = coplanar(coplanar_plane_type, &a, &b, &c, &d);
471  bool hasDementhonSucceeded(false), hasLagrangeSucceeded(false);
472  try {
473  if (plan) {
474  poseDementhonPlan(cMo_dementhon);
475  }
476  else {
477  poseDementhonNonPlan(cMo_dementhon);
478  }
479 
480  r_dementhon = computeResidual(cMo_dementhon);
481  hasDementhonSucceeded = true; // We reached this point => no exception was thrown = method succeeded
482  }
483  catch (...) {
484  // An exception was thrown using the original assumption, trying we the other one
485  try {
486  if (plan) {
487  // Already tested poseDementhonPlan, now trying poseDementhonNonPlan
488  poseDementhonNonPlan(cMo_dementhon);
489  }
490  else {
491  // Already tested poseDementhonNonPlan, now trying poseDementhonPlan
492  poseDementhonPlan(cMo_dementhon);
493  }
494 
495  r_dementhon = computeResidual(cMo_dementhon);
496  hasDementhonSucceeded = true; // We reached this point => no exception was thrown = method succeeded
497  }
498  catch (...) {
499  // The Dementhon method failed both with the planar and non-planar assumptions.
500  hasDementhonSucceeded = false;
501  }
502  }
503 
504  try {
505  if (plan) {
506  // If plan is true, then a, b, c, d will have been set when we called coplanar.
507  poseLagrangePlan(cMo_lagrange, &plan, &a, &b, &c, &d);
508  }
509  else {
510  poseLagrangeNonPlan(cMo_lagrange);
511  }
512 
513  r_lagrange = computeResidual(cMo_lagrange);
514  hasLagrangeSucceeded = true; // We reached this point => no exception was thrown = method succeeded
515  }
516  catch (...) {
517  // An exception was thrown using the original assumption, trying we the other one
518  try {
519  if (plan) {
520  // Already tested poseLagrangePlan, now trying poseLagrangeNonPlan
521  poseLagrangeNonPlan(cMo_lagrange);
522  }
523  else {
524  // Already tested poseLagrangeNonPlan, now trying poseLagrangePlan
525  // Because plan is false, then a, b, c, d will not have
526  // been initialized when calling coplanar
527  // We are expecting that the call to poseLagrangePlan will throw an exception
528  // because coplanar return false.
529  poseLagrangePlan(cMo_lagrange, &plan, &a, &b, &c, &d);
530  }
531 
532  r_lagrange = computeResidual(cMo_lagrange);
533  hasLagrangeSucceeded = true; // We reached this point => no exception was thrown = method succeeded
534  }
535  catch (...) {
536  // The Lagrange method both failed with the planar and non-planar assumptions.
537  hasLagrangeSucceeded = false;
538  }
539  }
540 
541  if (hasDementhonSucceeded || hasLagrangeSucceeded) {
542  // At least one of the linear methods managed to compute an initial pose.
543  // We initialize cMo with the method that had the lowest residual
544  cMo = (r_dementhon < r_lagrange) ? cMo_dementhon : cMo_lagrange;
545  // We now use the non-linear Virtual Visual Servoing method to improve the estimated cMo
546  return computePose(vpPose::VIRTUAL_VS, cMo);
547  }
548  else {
549  // None of the linear methods manage to compute an initial pose
550  return false;
551  }
552 }
553 
555 {
556  vpPoint P;
557  std::list<vpPoint>::const_iterator listp_end = listP.end();
558  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listp_end; ++it) {
559  P = *it;
560 
561  std::cout << "3D oP " << P.oP.t();
562  std::cout << "3D cP " << P.cP.t();
563  std::cout << "2D " << P.p.t();
564  }
565 }
566 
568  vpColor col)
569 {
570  vpDisplay::displayFrame(I, cMo, cam, size, col);
571 }
572 
574 {
575  vpDisplay::displayFrame(I, cMo, cam, size, col);
576 }
577 
579 {
580  vpPoint P;
581  vpImagePoint ip;
582  std::list<vpPoint>::const_iterator listp_end = listP.end();
583  const unsigned int sizeCross = 5;
584  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listp_end; ++it) {
585  P = *it;
586  vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip);
587  vpDisplay::displayCross(I, ip, sizeCross, col);
588  }
589 }
590 
592 {
593  vpPoint P;
594  vpImagePoint ip;
595  std::list<vpPoint>::const_iterator listp_end = listP.end();
596  const unsigned int sizeCross = 5;
597 
598  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listp_end; ++it) {
599  P = *it;
600  vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip);
601  vpDisplay::displayCross(I, ip, sizeCross, col);
602  }
603 }
604 
605 #ifdef VISP_HAVE_HOMOGRAPHY
606 double vpPose::poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx, vpCameraParameters &cam,
607  vpHomogeneousMatrix &cMo)
608 {
609  const unsigned int id0 = 0, id1 = 1, id2 = 2, id3 = 3;
610  std::vector<double> rectx(4);
611  std::vector<double> recty(4);
612  rectx[id0] = 0;
613  recty[id0] = 0;
614  rectx[id1] = 1;
615  recty[id1] = 0;
616  rectx[id2] = 1;
617  recty[id2] = 1;
618  rectx[id3] = 0;
619  recty[id3] = 1;
620  std::vector<double> irectx(4);
621  std::vector<double> irecty(4);
622  irectx[id0] = (p1.get_x());
623  irecty[id0] = (p1.get_y());
624  irectx[id1] = (p2.get_x());
625  irecty[id1] = (p2.get_y());
626  irectx[id2] = (p3.get_x());
627  irecty[id2] = (p3.get_y());
628  irectx[id3] = (p4.get_x());
629  irecty[id3] = (p4.get_y());
630 
631  // calcul de l'homographie
632  vpMatrix H(3, 3);
633  vpHomography hom;
634 
635  vpHomography::HLM(rectx, recty, irectx, irecty, 1, hom);
636  const unsigned int val_3 = 3;
637  for (unsigned int i = 0; i < val_3; ++i) {
638  for (unsigned int j = 0; j < val_3; ++j) {
639  H[i][j] = hom[i][j];
640  }
641  }
642  // calcul de s = ||Kh1||/ ||Kh2|| =ratio (length on x axis/ length on y
643  // axis)
644  vpColVector kh1(3);
645  vpColVector kh2(3);
646  vpMatrix K(3, 3);
647  K = cam.get_K();
648  K.eye();
649  vpMatrix Kinv = K.pseudoInverse();
650 
651  vpMatrix KinvH = Kinv * H;
652  kh1 = KinvH.getCol(0);
653  kh2 = KinvH.getCol(1);
654 
655  double s = sqrt(kh1.sumSquare()) / sqrt(kh2.sumSquare());
656 
657  vpMatrix D(3, 3);
658  D.eye();
659  D[1][1] = 1 / s;
660  vpMatrix cHo = H * D;
661 
662  // Calcul de la rotation et de la translation
663  // PoseFromRectangle(p1,p2,p3,p4,1/s,lx,cam,cMo );
664  p1.setWorldCoordinates(0, 0, 0);
665  p2.setWorldCoordinates(lx, 0, 0);
666  p3.setWorldCoordinates(lx, lx / s, 0);
667  p4.setWorldCoordinates(0, lx / s, 0);
668 
669  vpPose P;
670  P.addPoint(p1);
671  P.addPoint(p2);
672  P.addPoint(p3);
673  P.addPoint(p4);
674 
676  return lx / s;
677 }
678 #endif
679 
680 END_VISP_NAMESPACE
Generic class defining intrinsic camera parameters.
vpMatrix get_K() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
double sumSquare() const
vpRowVector t() const
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:73
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
static void HLM(const std::vector< double > &xb, const std::vector< double > &yb, const std::vector< double > &xa, const std::vector< double > &ya, bool isplanar, vpHomography &aHb)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
static double sqr(double x)
Definition: vpMath.h:203
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
vpMatrix pseudoInverse(double svThreshold=1e-6) const
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:548
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
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_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:422
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:415
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:420
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:413
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:111
Error that can be emitted by the vpPose class and its derivatives.
@ notEnoughPointError
Not enough points to compute the pose.
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:77
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:96
void printPoint()
Definition: vpPose.cpp:554
vpPoseMethodType
Methods that could be used to estimate the pose from points.
Definition: vpPose.h:81
@ DEMENTHON
Definition: vpPose.h:83
@ LAGRANGE_LOWE
Definition: vpPose.h:88
@ RANSAC
Definition: vpPose.h:87
@ DEMENTHON_LOWE
Definition: vpPose.h:90
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition: vpPose.h:98
@ LAGRANGE_VIRTUAL_VS
Definition: vpPose.h:96
@ VIRTUAL_VS
Definition: vpPose.h:92
@ LAGRANGE
Definition: vpPose.h:82
@ DEMENTHON_VIRTUAL_VS
Definition: vpPose.h:94
@ LOWE
Definition: vpPose.h:84
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:113
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, FuncCheckValidityPose func=nullptr)
Definition: vpPose.cpp:385
void setDistToPlaneForCoplanTest(double d)
Definition: vpPose.cpp:110
void addPoints(const std::vector< vpPoint > &lP)
Definition: vpPose.cpp:103
void poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan=nullptr, double *p_a=nullptr, double *p_b=nullptr, double *p_c=nullptr, double *p_d=nullptr)
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
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.
Definition: vpPose.cpp:298
void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
void clearPoint()
Definition: vpPose.cpp:89
bool coplanar(int &coplanar_plane_type, double *p_a=nullptr, double *p_b=nullptr, double *p_c=nullptr, double *p_d=nullptr)
Definition: vpPose.cpp:120
vpPose()
Definition: vpPose.cpp:65
void poseLowe(vpHomogeneousMatrix &cMo)
Compute the pose using the Lowe non linear approach it consider the minimization of a residual using ...
Definition: vpPoseLowe.cpp:270
bool computePoseDementhonLagrangeVVS(vpHomogeneousMatrix &cMo)
Method that first computes the pose cMo using the linear approaches of Dementhon and Lagrange and the...
Definition: vpPose.cpp:463
static void display(vpImage< unsigned char > &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, vpColor col=vpColor::none)
Definition: vpPose.cpp:567
static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx, vpCameraParameters &cam, vpHomogeneousMatrix &cMo)
Definition: vpPose.cpp:606
void displayModel(vpImage< unsigned char > &I, vpCameraParameters &cam, vpColor col=vpColor::none)
Definition: vpPose.cpp:578
virtual ~vpPose()
Definition: vpPose.cpp:84
bool poseRansac(vpHomogeneousMatrix &cMo, FuncCheckValidityPose func=nullptr)
void poseVirtualVS(vpHomogeneousMatrix &cMo)
void poseDementhonPlan(vpHomogeneousMatrix &cMo)
void setDementhonSvThreshold(const double &svThresh)
Definition: vpPose.cpp:112
vpColVector cP
Definition: vpTracker.h:73
vpColVector p
Definition: vpTracker.h:69