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