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