Visual Servoing Platform  version 3.5.1 under development (2022-07-07)
vpPose.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 
46 #include <visp3/core/vpCameraParameters.h>
47 #include <visp3/core/vpDebug.h>
48 #include <visp3/core/vpDisplay.h>
49 #include <visp3/core/vpException.h>
50 #include <visp3/core/vpMath.h>
51 #include <visp3/core/vpMeterPixelConversion.h>
52 #include <visp3/vision/vpPose.h>
53 #include <visp3/vision/vpPoseException.h>
54 
55 #include <cmath> // std::fabs
56 #include <limits> // numeric_limits
57 
58 #define DEBUG_LEVEL1 0
59 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
64 {
65 #if (DEBUG_LEVEL1)
66  std::cout << "begin vpPose::Init() " << std::endl;
67 #endif
68 
69  npt = 0;
70  listP.clear();
71  residual = 0;
72  lambda = 0.9;
73  vvsIterMax = 200;
74  c3d.clear();
75  computeCovariance = false;
76  covarianceMatrix.clear();
77  ransacNbInlierConsensus = 4;
78  ransacMaxTrials = 1000;
79  ransacInliers.clear();
80  ransacInlierIndex.clear();
81  ransacThreshold = 0.0001;
82  distanceToPlaneForCoplanarityTest = 0.001;
83  ransacFlag = NO_FILTER;
84  listOfPoints.clear();
85  useParallelRansac = false;
86  nbParallelRansacThreads = 0;
87  vvsEpsilon = 1e-8;
88 
89 #if (DEBUG_LEVEL1)
90  std::cout << "end vpPose::Init() " << std::endl;
91 #endif
92 }
93 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
94 
97  : npt(0), listP(), residual(0), lambda(0.9), vvsIterMax(200), c3d(), computeCovariance(false), covarianceMatrix(),
98  ransacNbInlierConsensus(4), ransacMaxTrials(1000), ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001),
99  distanceToPlaneForCoplanarityTest(0.001), ransacFlag(vpPose::NO_FILTER), listOfPoints(), useParallelRansac(false),
100  nbParallelRansacThreads(0), // 0 means that we use C++11 (if available) to get the number of threads
101  vvsEpsilon(1e-8)
102 {
103 }
104 
105 vpPose::vpPose(const std::vector<vpPoint> &lP)
106  : npt(static_cast<unsigned int>(lP.size())), listP(lP.begin(), lP.end()), residual(0), lambda(0.9), vvsIterMax(200),
107  c3d(), computeCovariance(false), covarianceMatrix(), ransacNbInlierConsensus(4), ransacMaxTrials(1000),
108  ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001), distanceToPlaneForCoplanarityTest(0.001),
109  ransacFlag(vpPose::NO_FILTER), listOfPoints(lP), useParallelRansac(false),
110  nbParallelRansacThreads(0), // 0 means that we use C++11 (if available) to get the number of threads
111  vvsEpsilon(1e-8)
112 {
113 }
114 
119 {
120 #if (DEBUG_LEVEL1)
121  std::cout << "begin vpPose::~vpPose() " << std::endl;
122 #endif
123 
124  listP.clear();
125 
126 #if (DEBUG_LEVEL1)
127  std::cout << "end vpPose::~vpPose() " << std::endl;
128 #endif
129 }
134 {
135  listP.clear();
136  listOfPoints.clear();
137  npt = 0;
138 }
139 
148 void vpPose::addPoint(const vpPoint &newP)
149 {
150  listP.push_back(newP);
151  listOfPoints.push_back(newP);
152  npt++;
153 }
154 
163 void vpPose::addPoints(const std::vector<vpPoint> &lP)
164 {
165  listP.insert(listP.end(), lP.begin(), lP.end());
166  listOfPoints.insert(listOfPoints.end(), lP.begin(), lP.end());
167  npt = (unsigned int)listP.size();
168 }
169 
170 void vpPose::setDistanceToPlaneForCoplanarityTest(double d) { distanceToPlaneForCoplanarityTest = d; }
171 
183 bool vpPose::coplanar(int &coplanar_plane_type)
184 {
185  coplanar_plane_type = 0;
186  if (npt < 2) {
187  vpERROR_TRACE("Not enough point (%d) to compute the pose ", npt);
188  throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough points "));
189  }
190 
191  if (npt == 3)
192  return true;
193 
194  double x1 = 0, x2 = 0, x3 = 0, y1 = 0, y2 = 0, y3 = 0, z1 = 0, z2 = 0, z3 = 0;
195 
196  std::list<vpPoint>::const_iterator it = listP.begin();
197 
198  vpPoint P1, P2, P3;
199 
200  // Get three 3D points that are not collinear and that is not at origin
201  bool degenerate = true;
202  bool not_on_origin = true;
203  std::list<vpPoint>::const_iterator it_tmp;
204 
205  std::list<vpPoint>::const_iterator it_i, it_j, it_k;
206  for (it_i = listP.begin(); it_i != listP.end(); ++it_i) {
207  if (degenerate == false) {
208  // std::cout << "Found a non degenerate configuration" << std::endl;
209  break;
210  }
211  P1 = *it_i;
212  // Test if point is on origin
213  if ((std::fabs(P1.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
214  (std::fabs(P1.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
215  (std::fabs(P1.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
216  not_on_origin = false;
217  } else {
218  not_on_origin = true;
219  }
220  if (not_on_origin) {
221  it_tmp = it_i;
222  ++it_tmp; // j = i+1
223  for (it_j = it_tmp; it_j != listP.end(); ++it_j) {
224  if (degenerate == false) {
225  // std::cout << "Found a non degenerate configuration" << std::endl;
226  break;
227  }
228  P2 = *it_j;
229  if ((std::fabs(P2.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
230  (std::fabs(P2.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
231  (std::fabs(P2.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
232  not_on_origin = false;
233  } else {
234  not_on_origin = true;
235  }
236  if (not_on_origin) {
237  it_tmp = it_j;
238  ++it_tmp; // k = j+1
239  for (it_k = it_tmp; it_k != listP.end(); ++it_k) {
240  P3 = *it_k;
241  if ((std::fabs(P3.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
242  (std::fabs(P3.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
243  (std::fabs(P3.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
244  not_on_origin = false;
245  } else {
246  not_on_origin = true;
247  }
248  if (not_on_origin) {
249  x1 = P1.get_oX();
250  x2 = P2.get_oX();
251  x3 = P3.get_oX();
252 
253  y1 = P1.get_oY();
254  y2 = P2.get_oY();
255  y3 = P3.get_oY();
256 
257  z1 = P1.get_oZ();
258  z2 = P2.get_oZ();
259  z3 = P3.get_oZ();
260 
261  vpColVector a_b(3), b_c(3), cross_prod;
262  a_b[0] = x1 - x2;
263  a_b[1] = y1 - y2;
264  a_b[2] = z1 - z2;
265  b_c[0] = x2 - x3;
266  b_c[1] = y2 - y3;
267  b_c[2] = z2 - z3;
268 
269  cross_prod = vpColVector::crossProd(a_b, b_c);
270  if (cross_prod.sumSquare() <= std::numeric_limits<double>::epsilon())
271  degenerate = true; // points are collinear
272  else
273  degenerate = false;
274  }
275  if (degenerate == false)
276  break;
277  }
278  }
279  }
280  }
281  }
282 
283  if (degenerate) {
284  coplanar_plane_type = 4; // points are collinear
285  return true;
286  }
287 
288  double a = y1 * z2 - y1 * z3 - y2 * z1 + y2 * z3 + y3 * z1 - y3 * z2;
289  double b = -x1 * z2 + x1 * z3 + x2 * z1 - x2 * z3 - x3 * z1 + x3 * z2;
290  double c = x1 * y2 - x1 * y3 - x2 * y1 + x2 * y3 + x3 * y1 - x3 * y2;
291  double d = -x1 * y2 * z3 + x1 * y3 * z2 + x2 * y1 * z3 - x2 * y3 * z1 - x3 * y1 * z2 + x3 * y2 * z1;
292 
293  // std::cout << "a=" << a << " b=" << b << " c=" << c << " d=" << d <<
294  // std::endl;
295  if (std::fabs(b) <= std::numeric_limits<double>::epsilon() &&
296  std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
297  coplanar_plane_type = 1; // ax=d
298  } else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
299  std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
300  coplanar_plane_type = 2; // by=d
301  } else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
302  std::fabs(b) <= std::numeric_limits<double>::epsilon()) {
303  coplanar_plane_type = 3; // cz=d
304  }
305 
306  double D = sqrt(vpMath::sqr(a) + vpMath::sqr(b) + vpMath::sqr(c));
307 
308  for (it = listP.begin(); it != listP.end(); ++it) {
309  P1 = *it;
310  double dist = (a * P1.get_oX() + b * P1.get_oY() + c * P1.get_oZ() + d) / D;
311  // std::cout << "dist= " << dist << std::endl;
312 
313  if (fabs(dist) > distanceToPlaneForCoplanarityTest) {
314  vpDEBUG_TRACE(10, " points are not coplanar ");
315  // TRACE(" points are not coplanar ") ;
316  return false;
317  }
318  }
319 
320  vpDEBUG_TRACE(10, " points are coplanar ");
321  // vpTRACE(" points are coplanar ") ;
322 
323  return true;
324 }
325 
336 {
337  double squared_error = 0;
338  vpPoint P;
339  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
340  P = *it;
341  double x = P.get_x();
342  double y = P.get_y();
343 
344  P.track(cMo);
345 
346  squared_error += vpMath::sqr(x - P.get_x()) + vpMath::sqr(y - P.get_y());
347  }
348  return (squared_error);
349 }
350 
374 {
375  if (npt < 4) {
376  throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough point (%d) to compute the pose ", npt));
377  }
378 
379  switch (method) {
380  case DEMENTHON:
382  case DEMENTHON_LOWE: {
383  if (npt < 4) {
385  "Dementhon method cannot be used in that case "
386  "(at least 4 points are required)"
387  "Not enough point (%d) to compute the pose ",
388  npt));
389  }
390 
391  // test si les point 3D sont coplanaires
392  int coplanar_plane_type = 0;
393  bool plan = coplanar(coplanar_plane_type);
394  if (plan == true) {
395  poseDementhonPlan(cMo);
396  } else {
398  }
399  } break;
400  case LAGRANGE:
401  case LAGRANGE_VIRTUAL_VS:
402  case LAGRANGE_LOWE: {
403  // test si les point 3D sont coplanaires
404  int coplanar_plane_type;
405  bool plan = coplanar(coplanar_plane_type);
406 
407  if (plan == true) {
408 
409  if (coplanar_plane_type == 4) {
410  throw(vpPoseException(vpPoseException::notEnoughPointError, "Lagrange method cannot be used in that case "
411  "(points are collinear)"));
412  }
413  if (npt < 4) {
415  "Lagrange method cannot be used in that case "
416  "(at least 4 points are required). "
417  "Not enough point (%d) to compute the pose ",
418  npt));
419  }
420  poseLagrangePlan(cMo);
421  } else {
422  if (npt < 6) {
424  "Lagrange method cannot be used in that case "
425  "(at least 6 points are required when 3D points are non coplanar). "
426  "Not enough point (%d) to compute the pose ",
427  npt));
428  }
429  poseLagrangeNonPlan(cMo);
430  }
431  } break;
432  case RANSAC:
433  if (npt < 4) {
435  "Ransac method cannot be used in that case "
436  "(at least 4 points are required). "
437  "Not enough point (%d) to compute the pose ",
438  npt));
439  }
440  return poseRansac(cMo, func);
441  break;
442  case LOWE:
443  case VIRTUAL_VS:
444  break;
445  }
446 
447  switch (method) {
448  case LAGRANGE:
449  case DEMENTHON:
450  case RANSAC:
451  break;
452  case VIRTUAL_VS:
453  case LAGRANGE_VIRTUAL_VS:
454  case DEMENTHON_VIRTUAL_VS: {
455  poseVirtualVS(cMo);
456  } break;
457  case LOWE:
458  case LAGRANGE_LOWE:
459  case DEMENTHON_LOWE: {
460  poseLowe(cMo);
461  } break;
462  }
463 
464  // If here, there was no exception thrown so return true
465  return true;
466 }
467 
469 {
470  vpPoint P;
471  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
472  P = *it;
473 
474  std::cout << "3D oP " << P.oP.t();
475  std::cout << "3D cP " << P.cP.t();
476  std::cout << "2D " << P.p.t();
477  }
478 }
479 
490  vpColor col)
491 {
492  vpDisplay::displayFrame(I, cMo, cam, size, col);
493 }
494 
505 {
506  vpDisplay::displayFrame(I, cMo, cam, size, col);
507 }
508 
514 {
515  vpPoint P;
516  vpImagePoint ip;
517  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
518  P = *it;
519  vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip);
520  vpDisplay::displayCross(I, ip, 5, col);
521  // std::cout << "3D oP " << P.oP.t() ;
522  // std::cout << "3D cP " << P.cP.t() ;
523  // std::cout << "2D " << P.p.t() ;
524  }
525 }
526 
532 {
533  vpPoint P;
534  vpImagePoint ip;
535  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
536  P = *it;
537  vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip);
538  vpDisplay::displayCross(I, ip, 5, col);
539  // std::cout << "3D oP " << P.oP.t() ;
540  // std::cout << "3D cP " << P.cP.t() ;
541  // std::cout << "2D " << P.p.t() ;
542  }
543 }
544 
564 double vpPose::poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx, vpCameraParameters &cam,
565  vpHomogeneousMatrix &cMo)
566 {
567 
568  std::vector<double> rectx(4);
569  std::vector<double> recty(4);
570  rectx[0] = 0;
571  recty[0] = 0;
572  rectx[1] = 1;
573  recty[1] = 0;
574  rectx[2] = 1;
575  recty[2] = 1;
576  rectx[3] = 0;
577  recty[3] = 1;
578  std::vector<double> irectx(4);
579  std::vector<double> irecty(4);
580  irectx[0] = (p1.get_x());
581  irecty[0] = (p1.get_y());
582  irectx[1] = (p2.get_x());
583  irecty[1] = (p2.get_y());
584  irectx[2] = (p3.get_x());
585  irecty[2] = (p3.get_y());
586  irectx[3] = (p4.get_x());
587  irecty[3] = (p4.get_y());
588 
589  // calcul de l'homographie
590  vpMatrix H(3, 3);
591  vpHomography hom;
592 
593  // vpHomography::HartleyDLT(rectx,recty,irectx,irecty,hom);
594  vpHomography::HLM(rectx, recty, irectx, irecty, 1, hom);
595  for (unsigned int i = 0; i < 3; i++)
596  for (unsigned int j = 0; j < 3; j++)
597  H[i][j] = hom[i][j];
598  // calcul de s = ||Kh1||/ ||Kh2|| =ratio (length on x axis/ length on y
599  // axis)
600  vpColVector kh1(3);
601  vpColVector kh2(3);
602  vpMatrix K(3, 3);
603  K = cam.get_K();
604  K.eye();
605  vpMatrix Kinv = K.pseudoInverse();
606 
607  vpMatrix KinvH = Kinv * H;
608  kh1 = KinvH.getCol(0);
609  kh2 = KinvH.getCol(1);
610 
611  double s = sqrt(kh1.sumSquare()) / sqrt(kh2.sumSquare());
612 
613  vpMatrix D(3, 3);
614  D.eye();
615  D[1][1] = 1 / s;
616  vpMatrix cHo = H * D;
617 
618  // Calcul de la rotation et de la translation
619  // PoseFromRectangle(p1,p2,p3,p4,1/s,lx,cam,cMo );
620  p1.setWorldCoordinates(0, 0, 0);
621  p2.setWorldCoordinates(lx, 0, 0);
622  p3.setWorldCoordinates(lx, lx / s, 0);
623  p4.setWorldCoordinates(0, lx / s, 0);
624 
625  vpPose P;
626  P.addPoint(p1);
627  P.addPoint(p2);
628  P.addPoint(p3);
629  P.addPoint(p4);
630 
632  return lx / s;
633 }
Generic class defining intrinsic camera parameters.
vpMatrix get_K() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
double sumSquare() const
vpRowVector t() const
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
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))
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:175
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:89
static double sqr(double x)
Definition: vpMath.h:123
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void eye()
Definition: vpMatrix.cpp:450
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2235
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5173
void clear()
Definition: vpMatrix.h:219
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:82
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:461
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:472
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:465
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:470
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:463
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
Error that can be emited by the vpPose class and its derivates.
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:90
vp_deprecated void init()
Definition: vpPose.cpp:63
vpPose()
Definition: vpPose.cpp:96
void poseVirtualVS(vpHomogeneousMatrix &cMo)
Compute the pose using virtual visual servoing approach.
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:148
void printPoint()
Definition: vpPose.cpp:468
vpPoseMethodType
Methods that could be used to estimate the pose from points.
Definition: vpPose.h:93
@ DEMENTHON
Definition: vpPose.h:95
@ LAGRANGE_LOWE
Definition: vpPose.h:100
@ RANSAC
Definition: vpPose.h:99
@ DEMENTHON_LOWE
Definition: vpPose.h:102
@ LAGRANGE_VIRTUAL_VS
Definition: vpPose.h:108
@ VIRTUAL_VS
Definition: vpPose.h:104
@ LAGRANGE
Definition: vpPose.h:94
@ DEMENTHON_VIRTUAL_VS
Definition: vpPose.h:106
@ LOWE
Definition: vpPose.h:96
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:118
void setDistanceToPlaneForCoplanarityTest(double d)
Definition: vpPose.cpp:170
void addPoints(const std::vector< vpPoint > &lP)
Definition: vpPose.cpp:163
void poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:119
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:335
void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
void clearPoint()
Definition: vpPose.cpp:133
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:373
void poseLowe(vpHomogeneousMatrix &cMo)
Compute the pose using the Lowe non linear approach it consider the minimization of a residual using ...
Definition: vpPoseLowe.cpp:262
@ NO_FILTER
Definition: vpPose.h:113
void poseLagrangePlan(vpHomogeneousMatrix &cMo)
Compute the pose of a planar object using Lagrange approach.
static void display(vpImage< unsigned char > &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, vpColor col=vpColor::none)
Definition: vpPose.cpp:489
static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx, vpCameraParameters &cam, vpHomogeneousMatrix &cMo)
Carries out the camera pose the image of a rectangle and the intrinsec parameters,...
Definition: vpPose.cpp:564
double lambda
parameters use for the virtual visual servoing approach
Definition: vpPose.h:124
bool poseRansac(vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
void displayModel(vpImage< unsigned char > &I, vpCameraParameters &cam, vpColor col=vpColor::none)
Definition: vpPose.cpp:513
bool coplanar(int &coplanar_plane_type)
Definition: vpPose.cpp:183
double residual
Residual in meter.
Definition: vpPose.h:121
virtual ~vpPose()
Definition: vpPose.cpp:118
void poseDementhonPlan(vpHomogeneousMatrix &cMo)
Compute the pose using Dementhon approach for planar objects this is a direct implementation of the a...
vpColVector cP
Definition: vpTracker.h:77
vpColVector p
Definition: vpTracker.h:73
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
#define vpERROR_TRACE
Definition: vpDebug.h:393