Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
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)
60 
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(),
100  useParallelRansac(false),
101  nbParallelRansacThreads(0), // 0 means that we use C++11 (if available) to get the number of threads
102  vvsEpsilon(1e-8)
103 {
104 }
105 
106 vpPose::vpPose(const std::vector<vpPoint>& lP)
107  : npt(static_cast<unsigned int>(lP.size())), listP(lP.begin(), lP.end()), residual(0), lambda(0.9), vvsIterMax(200), c3d(),
108  computeCovariance(false), covarianceMatrix(), ransacNbInlierConsensus(4), ransacMaxTrials(1000), ransacInliers(),
109  ransacInlierIndex(), ransacThreshold(0.0001), distanceToPlaneForCoplanarityTest(0.001), ransacFlag(vpPose::NO_FILTER),
110  listOfPoints(lP), useParallelRansac(false),
111  nbParallelRansacThreads(0), // 0 means that we use C++11 (if available) to get the number of threads
112  vvsEpsilon(1e-8)
113 {
114 }
115 
120 {
121 #if (DEBUG_LEVEL1)
122  std::cout << "begin vpPose::~vpPose() " << std::endl;
123 #endif
124 
125  listP.clear();
126 
127 #if (DEBUG_LEVEL1)
128  std::cout << "end vpPose::~vpPose() " << std::endl;
129 #endif
130 }
135 {
136  listP.clear();
137  listOfPoints.clear();
138  npt = 0;
139 }
140 
149 void vpPose::addPoint(const vpPoint &newP)
150 {
151  listP.push_back(newP);
152  listOfPoints.push_back(newP);
153  npt++;
154 }
155 
164 void vpPose::addPoints(const std::vector<vpPoint> &lP)
165 {
166  listP.insert(listP.end(), lP.begin(), lP.end());
167  listOfPoints.insert(listOfPoints.end(), lP.begin(), lP.end());
168  npt = (unsigned int)listP.size();
169 }
170 
171 void vpPose::setDistanceToPlaneForCoplanarityTest(double d) { distanceToPlaneForCoplanarityTest = d; }
172 
184 bool vpPose::coplanar(int &coplanar_plane_type)
185 {
186  coplanar_plane_type = 0;
187  if (npt < 2) {
188  vpERROR_TRACE("Not enough point (%d) to compute the pose ", npt);
189  throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough points "));
190  }
191 
192  if (npt == 3)
193  return true;
194 
195  double x1 = 0, x2 = 0, x3 = 0, y1 = 0, y2 = 0, y3 = 0, z1 = 0, z2 = 0, z3 = 0;
196 
197  std::list<vpPoint>::const_iterator it = listP.begin();
198 
199  vpPoint P1, P2, P3;
200 
201  // Get three 3D points that are not collinear and that is not at origin
202  bool degenerate = true;
203  bool not_on_origin = true;
204  std::list<vpPoint>::const_iterator it_tmp;
205 
206  std::list<vpPoint>::const_iterator it_i, it_j, it_k;
207  for (it_i = listP.begin(); it_i != listP.end(); ++it_i) {
208  if (degenerate == false) {
209  // std::cout << "Found a non degenerate configuration" << std::endl;
210  break;
211  }
212  P1 = *it_i;
213  // Test if point is on origin
214  if ((std::fabs(P1.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
215  (std::fabs(P1.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
216  (std::fabs(P1.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
217  not_on_origin = false;
218  } else {
219  not_on_origin = true;
220  }
221  if (not_on_origin) {
222  it_tmp = it_i;
223  ++it_tmp; // j = i+1
224  for (it_j = it_tmp; it_j != listP.end(); ++it_j) {
225  if (degenerate == false) {
226  // std::cout << "Found a non degenerate configuration" << std::endl;
227  break;
228  }
229  P2 = *it_j;
230  if ((std::fabs(P2.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
231  (std::fabs(P2.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
232  (std::fabs(P2.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
233  not_on_origin = false;
234  } else {
235  not_on_origin = true;
236  }
237  if (not_on_origin) {
238  it_tmp = it_j;
239  ++it_tmp; // k = j+1
240  for (it_k = it_tmp; it_k != listP.end(); ++it_k) {
241  P3 = *it_k;
242  if ((std::fabs(P3.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
243  (std::fabs(P3.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
244  (std::fabs(P3.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
245  not_on_origin = false;
246  } else {
247  not_on_origin = true;
248  }
249  if (not_on_origin) {
250  x1 = P1.get_oX();
251  x2 = P2.get_oX();
252  x3 = P3.get_oX();
253 
254  y1 = P1.get_oY();
255  y2 = P2.get_oY();
256  y3 = P3.get_oY();
257 
258  z1 = P1.get_oZ();
259  z2 = P2.get_oZ();
260  z3 = P3.get_oZ();
261 
262  vpColVector a_b(3), b_c(3), cross_prod;
263  a_b[0] = x1 - x2;
264  a_b[1] = y1 - y2;
265  a_b[2] = z1 - z2;
266  b_c[0] = x2 - x3;
267  b_c[1] = y2 - y3;
268  b_c[2] = z2 - z3;
269 
270  cross_prod = vpColVector::crossProd(a_b, b_c);
271  if (cross_prod.sumSquare() <= std::numeric_limits<double>::epsilon())
272  degenerate = true; // points are collinear
273  else
274  degenerate = false;
275  }
276  if (degenerate == false)
277  break;
278  }
279  }
280  }
281  }
282  }
283 
284  if (degenerate) {
285  coplanar_plane_type = 4; // points are collinear
286  return true;
287  }
288 
289  double a = y1 * z2 - y1 * z3 - y2 * z1 + y2 * z3 + y3 * z1 - y3 * z2;
290  double b = -x1 * z2 + x1 * z3 + x2 * z1 - x2 * z3 - x3 * z1 + x3 * z2;
291  double c = x1 * y2 - x1 * y3 - x2 * y1 + x2 * y3 + x3 * y1 - x3 * y2;
292  double d = -x1 * y2 * z3 + x1 * y3 * z2 + x2 * y1 * z3 - x2 * y3 * z1 - x3 * y1 * z2 + x3 * y2 * z1;
293 
294  // std::cout << "a=" << a << " b=" << b << " c=" << c << " d=" << d <<
295  // std::endl;
296  if (std::fabs(b) <= std::numeric_limits<double>::epsilon() &&
297  std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
298  coplanar_plane_type = 1; // ax=d
299  } else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
300  std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
301  coplanar_plane_type = 2; // by=d
302  } else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
303  std::fabs(b) <= std::numeric_limits<double>::epsilon()) {
304  coplanar_plane_type = 3; // cz=d
305  }
306 
307  double D = sqrt(vpMath::sqr(a) + vpMath::sqr(b) + vpMath::sqr(c));
308 
309  for (it = listP.begin(); it != listP.end(); ++it) {
310  P1 = *it;
311  double dist = (a * P1.get_oX() + b * P1.get_oY() + c * P1.get_oZ() + d) / D;
312  // std::cout << "dist= " << dist << std::endl;
313 
314  if (fabs(dist) > distanceToPlaneForCoplanarityTest) {
315  vpDEBUG_TRACE(10, " points are not coplanar ");
316  // TRACE(" points are not coplanar ") ;
317  return false;
318  }
319  }
320 
321  vpDEBUG_TRACE(10, " points are coplanar ");
322  // vpTRACE(" points are coplanar ") ;
323 
324  return true;
325 }
326 
337 {
338  double squared_error = 0;
339  vpPoint P;
340  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
341  P = *it;
342  double x = P.get_x();
343  double y = P.get_y();
344 
345  P.track(cMo);
346 
347  squared_error += vpMath::sqr(x - P.get_x()) + vpMath::sqr(y - P.get_y());
348  }
349  return (squared_error);
350 }
351 
375 {
376  if (npt < 4) {
378  "Not enough point (%d) to compute the pose ", npt));
379  }
380 
381  switch (method) {
382  case DEMENTHON:
384  case DEMENTHON_LOWE: {
385  if (npt < 4) {
387  "Dementhon method cannot be used in that case "
388  "(at least 4 points are required)"
389  "Not enough point (%d) to compute the pose ", npt));
390  }
391 
392  // test si les point 3D sont coplanaires
393  int coplanar_plane_type = 0;
394  bool plan = coplanar(coplanar_plane_type);
395  if (plan == true) {
396  poseDementhonPlan(cMo);
397  } else {
399  }
400  } break;
401  case LAGRANGE:
402  case LAGRANGE_VIRTUAL_VS:
403  case LAGRANGE_LOWE: {
404  // test si les point 3D sont coplanaires
405  int coplanar_plane_type;
406  bool plan = coplanar(coplanar_plane_type);
407 
408  if (plan == true)
409  {
410 
411  if (coplanar_plane_type == 4) {
413  "Lagrange method cannot be used in that case "
414  "(points are collinear)"));
415  }
416  if (npt < 4) {
418  "Lagrange method cannot be used in that case "
419  "(at least 4 points are required). "
420  "Not enough point (%d) to compute the pose ", npt));
421  }
422  poseLagrangePlan(cMo);
423  } else {
424  if (npt < 6) {
426  "Lagrange method cannot be used in that case "
427  "(at least 6 points are required when 3D points are non coplanar). "
428  "Not enough point (%d) to compute the pose ", npt));
429  }
430  poseLagrangeNonPlan(cMo);
431  }
432  } break;
433  case RANSAC:
434  if (npt < 4) {
436  "Ransac method cannot be used in that case "
437  "(at least 4 points are required). "
438  "Not enough point (%d) to compute the pose ", 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 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2206
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:374
virtual ~vpPose()
Definition: vpPose.cpp:119
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:424
double residual
Residual in meter.
Definition: vpPose.h:112
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
bool coplanar(int &coplanar_plane_type)
Definition: vpPose.cpp:184
Implementation of an homogeneous matrix and operations on such kind of matrices.
void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
#define vpERROR_TRACE
Definition: vpDebug.h:393
vp_deprecated void init()
Definition: vpPose.cpp:63
Class to define colors available for display functionnalities.
Definition: vpColor.h:119
void displayModel(vpImage< unsigned char > &I, vpCameraParameters &cam, vpColor col=vpColor::none)
Definition: vpPose.cpp:513
void addPoints(const std::vector< vpPoint > &lP)
Definition: vpPose.cpp:164
void track(const vpHomogeneousMatrix &cMo)
vpRowVector t() const
vpMatrix get_K() const
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:110
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:422
void poseVirtualVS(vpHomogeneousMatrix &cMo)
Compute the pose using virtual visual servoing approach.
Class that defines what is a point.
Definition: vpPoint.h:58
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)
double lambda
parameters use for the virtual visual servoing approach
Definition: vpPose.h:115
vpColVector cP
Definition: vpTracker.h:75
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
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, the length on x axis is known but the proprtion of the rectangle are unknown.
Definition: vpPose.cpp:564
static double sqr(double x)
Definition: vpMath.h:114
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:80
Generic class defining intrinsic camera parameters.
void printPoint()
Definition: vpPose.cpp:468
void poseLagrangePlan(vpHomogeneousMatrix &cMo)
Compute the pose of a planar object using Lagrange approach.
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
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:426
void clear()
Definition: vpMatrix.h:230
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:4096
vpPose()
Definition: vpPose.cpp:96
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:109
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
void poseDementhonPlan(vpHomogeneousMatrix &cMo)
Compute the pose using Dementhon approach for planar objects this is a direct implementation of the a...
Error that can be emited by the vpPose class and its derivates.
void poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
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))
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:431
double sumSquare() const
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:433
vpPoseMethodType
Methods that could be used to estimate the pose from points.
Definition: vpPose.h:84
bool poseRansac(vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
void setDistanceToPlaneForCoplanarityTest(double d)
Definition: vpPose.cpp:171
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
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:336
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:149
void eye()
Definition: vpMatrix.cpp:492
vpColVector p
Definition: vpTracker.h:71
void clearPoint()
Definition: vpPose.cpp:134