Visual Servoing Platform  version 3.6.0 under development (2023-09-27)
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 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
58 {
59 #if (DEBUG_LEVEL1)
60  std::cout << "begin vpPose::Init() " << std::endl;
61 #endif
62 
63  npt = 0;
64  listP.clear();
65  residual = 0;
66  lambda = 0.9;
67  vvsIterMax = 200;
68  c3d.clear();
69  computeCovariance = false;
70  covarianceMatrix.clear();
71  ransacNbInlierConsensus = 4;
72  ransacMaxTrials = 1000;
73  ransacInliers.clear();
74  ransacInlierIndex.clear();
75  ransacThreshold = 0.0001;
76  distanceToPlaneForCoplanarityTest = 0.001;
77  ransacFlag = NO_FILTER;
78  listOfPoints.clear();
79  useParallelRansac = false;
80  nbParallelRansacThreads = 0;
81  vvsEpsilon = 1e-8;
82 
83 #if (DEBUG_LEVEL1)
84  std::cout << "end vpPose::Init() " << std::endl;
85 #endif
86 }
87 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
88 
91  : npt(0), listP(), residual(0), lambda(0.9), vvsIterMax(200), c3d(), computeCovariance(false), covarianceMatrix(),
92  ransacNbInlierConsensus(4), ransacMaxTrials(1000), ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001),
93  distanceToPlaneForCoplanarityTest(0.001), ransacFlag(vpPose::NO_FILTER), listOfPoints(), useParallelRansac(false),
94  nbParallelRansacThreads(0), // 0 means that we use C++11 (if available) to get the number of threads
95  vvsEpsilon(1e-8), dementhonSvThresh(1e-6)
96 { }
97 
98 vpPose::vpPose(const std::vector<vpPoint> &lP)
99  : npt(static_cast<unsigned int>(lP.size())), listP(lP.begin(), lP.end()), residual(0), lambda(0.9), vvsIterMax(200),
100  c3d(), computeCovariance(false), covarianceMatrix(), ransacNbInlierConsensus(4), ransacMaxTrials(1000),
101  ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001), distanceToPlaneForCoplanarityTest(0.001),
102  ransacFlag(vpPose::NO_FILTER), listOfPoints(lP), useParallelRansac(false),
103  nbParallelRansacThreads(0), // 0 means that we use C++11 (if available) to get the number of threads
104  vvsEpsilon(1e-8), dementhonSvThresh(1e-6)
105 { }
106 
111 {
112 #if (DEBUG_LEVEL1)
113  std::cout << "begin vpPose::~vpPose() " << std::endl;
114 #endif
115 
116  listP.clear();
117 
118 #if (DEBUG_LEVEL1)
119  std::cout << "end vpPose::~vpPose() " << std::endl;
120 #endif
121 }
126 {
127  listP.clear();
128  listOfPoints.clear();
129  npt = 0;
130 }
131 
140 void vpPose::addPoint(const vpPoint &newP)
141 {
142  listP.push_back(newP);
143  listOfPoints.push_back(newP);
144  npt++;
145 }
146 
155 void vpPose::addPoints(const std::vector<vpPoint> &lP)
156 {
157  listP.insert(listP.end(), lP.begin(), lP.end());
158  listOfPoints.insert(listOfPoints.end(), lP.begin(), lP.end());
159  npt = (unsigned int)listP.size();
160 }
161 
162 void vpPose::setDistanceToPlaneForCoplanarityTest(double d) { distanceToPlaneForCoplanarityTest = d; }
163 
164 void vpPose::setDementhonSvThreshold(const double &svThresh)
165 {
166  if (svThresh < 0) {
167  throw vpException(vpException::badValue, "The svd threshold must be positive");
168  }
169  dementhonSvThresh = svThresh;
170 }
171 
187 bool vpPose::coplanar(int &coplanar_plane_type, double *p_a, double *p_b, double *p_c, double *p_d)
188 {
189  coplanar_plane_type = 0;
190  if (npt < 2) {
191  vpERROR_TRACE("Not enough point (%d) to compute the pose ", npt);
192  throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough points "));
193  }
194 
195  if (npt == 3)
196  return true;
197 
198  // Shuffling the points to limit the risk of using points close to each other
199  std::vector<vpPoint> shuffled_listP = vpUniRand::shuffleVector<vpPoint>(listOfPoints);
200 
201  double x1 = 0, x2 = 0, x3 = 0, y1 = 0, y2 = 0, y3 = 0, z1 = 0, z2 = 0, z3 = 0;
202 
203  std::vector<vpPoint>::const_iterator it = shuffled_listP.begin();
204 
205  vpPoint P1, P2, P3;
206 
207  // Get three 3D points that are not collinear and that is not at origin
208  bool degenerate = true;
209  bool not_on_origin = true;
210  std::vector<vpPoint>::const_iterator it_tmp;
211 
212  std::vector<vpPoint>::const_iterator it_i, it_j, it_k;
213  for (it_i = shuffled_listP.begin(); it_i != shuffled_listP.end(); ++it_i) {
214  if (degenerate == false) {
215  // std::cout << "Found a non degenerate configuration" << std::endl;
216  break;
217  }
218  P1 = *it_i;
219  // Test if point is on origin
220  if ((std::fabs(P1.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
221  (std::fabs(P1.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
222  (std::fabs(P1.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
223  not_on_origin = false;
224  }
225  else {
226  not_on_origin = true;
227  }
228  if (not_on_origin) {
229  it_tmp = it_i;
230  ++it_tmp; // j = i+1
231  for (it_j = it_tmp; it_j != shuffled_listP.end(); ++it_j) {
232  if (degenerate == false) {
233  // std::cout << "Found a non degenerate configuration" << std::endl;
234  break;
235  }
236  P2 = *it_j;
237  if ((std::fabs(P2.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
238  (std::fabs(P2.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
239  (std::fabs(P2.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
240  not_on_origin = false;
241  }
242  else {
243  not_on_origin = true;
244  }
245  if (not_on_origin) {
246  it_tmp = it_j;
247  ++it_tmp; // k = j+1
248  for (it_k = it_tmp; it_k != shuffled_listP.end(); ++it_k) {
249  P3 = *it_k;
250  if ((std::fabs(P3.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
251  (std::fabs(P3.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
252  (std::fabs(P3.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
253  not_on_origin = false;
254  }
255  else {
256  not_on_origin = true;
257  }
258  if (not_on_origin) {
259  x1 = P1.get_oX();
260  x2 = P2.get_oX();
261  x3 = P3.get_oX();
262 
263  y1 = P1.get_oY();
264  y2 = P2.get_oY();
265  y3 = P3.get_oY();
266 
267  z1 = P1.get_oZ();
268  z2 = P2.get_oZ();
269  z3 = P3.get_oZ();
270 
271  vpColVector a_b(3), b_c(3), cross_prod;
272  a_b[0] = x1 - x2;
273  a_b[1] = y1 - y2;
274  a_b[2] = z1 - z2;
275  b_c[0] = x2 - x3;
276  b_c[1] = y2 - y3;
277  b_c[2] = z2 - z3;
278 
279  cross_prod = vpColVector::crossProd(a_b, b_c);
280  if (cross_prod.sumSquare() <= std::numeric_limits<double>::epsilon())
281  degenerate = true; // points are collinear
282  else
283  degenerate = false;
284  }
285  if (degenerate == false)
286  break;
287  }
288  }
289  }
290  }
291  }
292 
293  if (degenerate) {
294  coplanar_plane_type = 4; // points are collinear
295  return true;
296  }
297 
298  double a = y1 * z2 - y1 * z3 - y2 * z1 + y2 * z3 + y3 * z1 - y3 * z2;
299  double b = -x1 * z2 + x1 * z3 + x2 * z1 - x2 * z3 - x3 * z1 + x3 * z2;
300  double c = x1 * y2 - x1 * y3 - x2 * y1 + x2 * y3 + x3 * y1 - x3 * y2;
301  double d = -x1 * y2 * z3 + x1 * y3 * z2 + x2 * y1 * z3 - x2 * y3 * z1 - x3 * y1 * z2 + x3 * y2 * z1;
302 
303  // std::cout << "a=" << a << " b=" << b << " c=" << c << " d=" << d <<
304  // std::endl;
305  if (std::fabs(b) <= std::numeric_limits<double>::epsilon() &&
306  std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
307  coplanar_plane_type = 1; // ax=d
308  }
309  else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
310  std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
311  coplanar_plane_type = 2; // by=d
312  }
313  else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
314  std::fabs(b) <= std::numeric_limits<double>::epsilon()) {
315  coplanar_plane_type = 3; // cz=d
316  }
317 
318  double D = sqrt(vpMath::sqr(a) + vpMath::sqr(b) + vpMath::sqr(c));
319 
320  for (it = shuffled_listP.begin(); it != shuffled_listP.end(); ++it) {
321  P1 = *it;
322  double dist = (a * P1.get_oX() + b * P1.get_oY() + c * P1.get_oZ() + d) / D;
323  // std::cout << "dist= " << dist << std::endl;
324 
325  if (fabs(dist) > distanceToPlaneForCoplanarityTest) {
326  vpDEBUG_TRACE(10, " points are not coplanar ");
327  return false;
328  }
329  }
330 
331  vpDEBUG_TRACE(10, " points are coplanar ");
332  // vpTRACE(" points are coplanar ") ;
333 
334  // If the points are coplanar and the input/output parameters are different from NULL,
335  // getting the values of the plan coefficient and storing in the input/output parameters
336  if (p_a != NULL) {
337  *p_a = a;
338  }
339 
340  if (p_b != NULL) {
341  *p_b = b;
342  }
343 
344  if (p_c != NULL) {
345  *p_c = c;
346  }
347 
348  if (p_d != NULL) {
349  *p_d = d;
350  }
351 
352  return true;
353 }
354 
370 {
371  double squared_error = 0;
372  vpPoint P;
373  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
374  P = *it;
375  double x = P.get_x();
376  double y = P.get_y();
377 
378  P.track(cMo);
379 
380  squared_error += vpMath::sqr(x - P.get_x()) + vpMath::sqr(y - P.get_y());
381  }
382  return (squared_error);
383 }
384 
399 {
400  vpColVector residuals;
401  return computeResidual(cMo, cam, residuals);
402 }
403 
418 double vpPose::computeResidual(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, vpColVector &residuals) const
419 {
420  double squared_error = 0;
421  residuals.resize(static_cast<unsigned int>(listP.size()));
422  vpPoint P;
423  unsigned int i = 0;
424  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
425  P = *it;
426  double x = P.get_x();
427  double y = P.get_y();
428 
429  double u_initial = 0., v_initial = 0.;
430  vpMeterPixelConversion::convertPoint(cam, x, y, u_initial, v_initial);
431 
432  P.track(cMo);
433 
434  double u_moved = 0., v_moved = 0.;
435  vpMeterPixelConversion::convertPoint(cam, P.get_x(), P.get_y(), u_moved, v_moved);
436 
437  double squaredResidual = vpMath::sqr(u_moved - u_initial) + vpMath::sqr(v_moved - v_initial);
438  residuals[i++] = squaredResidual;
439  squared_error += squaredResidual;
440  }
441  return (squared_error);
442 }
443 
470 {
471  if (npt < 4) {
472  throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough point (%d) to compute the pose ", npt));
473  }
474 
475  switch (method) {
476  case DEMENTHON:
478  case DEMENTHON_LOWE: {
479  if (npt < 4) {
481  "Dementhon method cannot be used in that case "
482  "(at least 4 points are required)"
483  "Not enough point (%d) to compute the pose ",
484  npt));
485  }
486 
487  // test if the 3D points are coplanar
488  int coplanar_plane_type = 0;
489  bool plan = coplanar(coplanar_plane_type);
490  if (plan == true) {
491  poseDementhonPlan(cMo);
492  }
493  else {
495  }
496  break;
497  }
498  case LAGRANGE:
499  case LAGRANGE_VIRTUAL_VS:
500  case LAGRANGE_LOWE: {
501  // test if the 3D points are coplanar
502  double a, b, c, d; // To get the plan coefficients if the points are coplanar
503  int coplanar_plane_type = 0;
504  bool plan = coplanar(coplanar_plane_type, &a, &b, &c, &d);
505 
506  if (plan == true) {
507 
508  if (coplanar_plane_type == 4) {
509  throw(vpPoseException(vpPoseException::notEnoughPointError, "Lagrange method cannot be used in that case "
510  "(points are collinear)"));
511  }
512  if (npt < 4) {
514  "Lagrange method cannot be used in that case "
515  "(at least 4 points are required). "
516  "Not enough point (%d) to compute the pose ",
517  npt));
518  }
519  poseLagrangePlan(cMo, &plan, &a, &b, &c, &d);
520  }
521  else {
522  if (npt < 6) {
524  "Lagrange method cannot be used in that case "
525  "(at least 6 points are required when 3D points are non coplanar). "
526  "Not enough point (%d) to compute the pose ",
527  npt));
528  }
529  poseLagrangeNonPlan(cMo);
530  }
531  break;
532  }
533  case RANSAC: {
534  if (npt < 4) {
536  "Ransac method cannot be used in that case "
537  "(at least 4 points are required). "
538  "Not enough point (%d) to compute the pose ",
539  npt));
540  }
541  return poseRansac(cMo, func);
542  }
543  case LOWE:
544  case VIRTUAL_VS:
545  break;
548  }
549  }
550 
551  switch (method) {
552  case LAGRANGE:
553  case DEMENTHON:
555  case RANSAC:
556  break;
557  case VIRTUAL_VS:
558  case LAGRANGE_VIRTUAL_VS:
559  case DEMENTHON_VIRTUAL_VS: {
560  poseVirtualVS(cMo);
561  break;
562  }
563  case LOWE:
564  case LAGRANGE_LOWE:
565  case DEMENTHON_LOWE: {
566  poseLowe(cMo);
567  } break;
568  }
569 
570  return true;
571 }
572 
583 {
584  vpHomogeneousMatrix cMo_dementhon, cMo_lagrange;
585  double r_dementhon = std::numeric_limits<double>::max(), r_lagrange = std::numeric_limits<double>::max();
586  // test if the 3D points are coplanar
587  double a, b, c, d; // To get the plan coefficients if the points are coplanar
588  int coplanar_plane_type = 0;
589  bool plan = coplanar(coplanar_plane_type, &a, &b, &c, &d);
590  bool hasDementhonSucceeded(false), hasLagrangeSucceeded(false);
591  try {
592  if (plan) {
593  poseDementhonPlan(cMo_dementhon);
594  }
595  else {
596  poseDementhonNonPlan(cMo_dementhon);
597  }
598 
599  r_dementhon = computeResidual(cMo_dementhon);
600  hasDementhonSucceeded = true; // We reached this point => no exception was thrown = method succeeded
601  }
602  catch (...) {
603  // An exception was thrown using the original assumption, trying we the other one
604  try {
605  if (plan) {
606  // Already tested poseDementhonPlan, now trying poseDementhonNonPlan
607  poseDementhonNonPlan(cMo_dementhon);
608  }
609  else {
610  // Already tested poseDementhonNonPlan, now trying poseDementhonPlan
611  poseDementhonPlan(cMo_dementhon);
612  }
613 
614  r_dementhon = computeResidual(cMo_dementhon);
615  hasDementhonSucceeded = true; // We reached this point => no exception was thrown = method succeeded
616  }
617  catch (...) {
618  // The Dementhon method failed both with the planar and non-planar assumptions.
619  hasDementhonSucceeded = false;
620  }
621  }
622 
623  try {
624  if (plan) {
625  // If plan is true, then a, b, c, d will have been set when we called coplanar.
626  poseLagrangePlan(cMo_lagrange, &plan, &a, &b, &c, &d);
627  }
628  else {
629  poseLagrangeNonPlan(cMo_lagrange);
630  }
631 
632  r_lagrange = computeResidual(cMo_lagrange);
633  hasLagrangeSucceeded = true; // We reached this point => no exception was thrown = method succeeded
634  }
635  catch (...) {
636  // An exception was thrown using the original assumption, trying we the other one
637  try {
638  if (plan) {
639  // Already tested poseLagrangePlan, now trying poseLagrangeNonPlan
640  poseLagrangeNonPlan(cMo_lagrange);
641  }
642  else {
643  // Already tested poseLagrangeNonPlan, now trying poseLagrangePlan
644  // Because plan is false, then a, b, c, d will not have
645  // been initialized when calling coplanar
646  // We are expecting that the call to poseLagrangePlan will throw an exception
647  // because coplanar return false.
648  poseLagrangePlan(cMo_lagrange, &plan, &a, &b, &c, &d);
649  }
650 
651  r_lagrange = computeResidual(cMo_lagrange);
652  hasLagrangeSucceeded = true; // We reached this point => no exception was thrown = method succeeded
653  }
654  catch (...) {
655  // The Lagrange method both failed with the planar and non-planar assumptions.
656  hasLagrangeSucceeded = false;
657  }
658  }
659 
660  if ((hasDementhonSucceeded || hasLagrangeSucceeded)) {
661  // At least one of the linear methods managed to compute an initial pose.
662  // We initialize cMo with the method that had the lowest residual
663  cMo = (r_dementhon < r_lagrange) ? cMo_dementhon : cMo_lagrange;
664  // We now use the non-linear Virtual Visual Servoing method to improve the estimated cMo
665  return computePose(vpPose::VIRTUAL_VS, cMo);
666  }
667  else {
668  // None of the linear methods manage to compute an initial pose
669  return false;
670  }
671 }
672 
674 {
675  vpPoint P;
676  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
677  P = *it;
678 
679  std::cout << "3D oP " << P.oP.t();
680  std::cout << "3D cP " << P.cP.t();
681  std::cout << "2D " << P.p.t();
682  }
683 }
684 
695  vpColor col)
696 {
697  vpDisplay::displayFrame(I, cMo, cam, size, col);
698 }
699 
710 {
711  vpDisplay::displayFrame(I, cMo, cam, size, col);
712 }
713 
719 {
720  vpPoint P;
721  vpImagePoint ip;
722  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
723  P = *it;
724  vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip);
725  vpDisplay::displayCross(I, ip, 5, col);
726  // std::cout << "3D oP " << P.oP.t() ;
727  // std::cout << "3D cP " << P.cP.t() ;
728  // std::cout << "2D " << P.p.t() ;
729  }
730 }
731 
737 {
738  vpPoint P;
739  vpImagePoint ip;
740  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
741  P = *it;
742  vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip);
743  vpDisplay::displayCross(I, ip, 5, col);
744  // std::cout << "3D oP " << P.oP.t() ;
745  // std::cout << "3D cP " << P.cP.t() ;
746  // std::cout << "2D " << P.p.t() ;
747  }
748 }
749 
769 double vpPose::poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx, vpCameraParameters &cam,
770  vpHomogeneousMatrix &cMo)
771 {
772 
773  std::vector<double> rectx(4);
774  std::vector<double> recty(4);
775  rectx[0] = 0;
776  recty[0] = 0;
777  rectx[1] = 1;
778  recty[1] = 0;
779  rectx[2] = 1;
780  recty[2] = 1;
781  rectx[3] = 0;
782  recty[3] = 1;
783  std::vector<double> irectx(4);
784  std::vector<double> irecty(4);
785  irectx[0] = (p1.get_x());
786  irecty[0] = (p1.get_y());
787  irectx[1] = (p2.get_x());
788  irecty[1] = (p2.get_y());
789  irectx[2] = (p3.get_x());
790  irecty[2] = (p3.get_y());
791  irectx[3] = (p4.get_x());
792  irecty[3] = (p4.get_y());
793 
794  // calcul de l'homographie
795  vpMatrix H(3, 3);
796  vpHomography hom;
797 
798  // vpHomography::HartleyDLT(rectx,recty,irectx,irecty,hom);
799  vpHomography::HLM(rectx, recty, irectx, irecty, 1, hom);
800  for (unsigned int i = 0; i < 3; i++)
801  for (unsigned int j = 0; j < 3; j++)
802  H[i][j] = hom[i][j];
803  // calcul de s = ||Kh1||/ ||Kh2|| =ratio (length on x axis/ length on y
804  // axis)
805  vpColVector kh1(3);
806  vpColVector kh2(3);
807  vpMatrix K(3, 3);
808  K = cam.get_K();
809  K.eye();
810  vpMatrix Kinv = K.pseudoInverse();
811 
812  vpMatrix KinvH = Kinv * H;
813  kh1 = KinvH.getCol(0);
814  kh2 = KinvH.getCol(1);
815 
816  double s = sqrt(kh1.sumSquare()) / sqrt(kh2.sumSquare());
817 
818  vpMatrix D(3, 3);
819  D.eye();
820  D[1][1] = 1 / s;
821  vpMatrix cHo = H * D;
822 
823  // Calcul de la rotation et de la translation
824  // PoseFromRectangle(p1,p2,p3,p4,1/s,lx,cam,cMo );
825  p1.setWorldCoordinates(0, 0, 0);
826  p2.setWorldCoordinates(lx, 0, 0);
827  p3.setWorldCoordinates(lx, lx / s, 0);
828  p4.setWorldCoordinates(0, lx / s, 0);
829 
830  vpPose P;
831  P.addPoint(p1);
832  P.addPoint(p2);
833  P.addPoint(p3);
834  P.addPoint(p4);
835 
837  return lx / s;
838 }
Generic class defining intrinsic camera parameters.
vpMatrix get_K() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:167
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:351
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:124
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:152
void eye()
Definition: vpMatrix.cpp:446
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2238
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5200
void clear()
Definition: vpMatrix.h:218
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:458
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:469
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:462
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:467
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:460
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:81
vp_deprecated void init()
Definition: vpPose.cpp:57
vpPose()
Definition: vpPose.cpp:90
void poseVirtualVS(vpHomogeneousMatrix &cMo)
Compute the pose using virtual visual servoing approach.
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:140
void printPoint()
Definition: vpPose.cpp:673
vpPoseMethodType
Methods that could be used to estimate the pose from points.
Definition: vpPose.h:85
@ DEMENTHON
Definition: vpPose.h:87
@ LAGRANGE_LOWE
Definition: vpPose.h:92
@ RANSAC
Definition: vpPose.h:91
@ DEMENTHON_LOWE
Definition: vpPose.h:94
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition: vpPose.h:102
@ LAGRANGE_VIRTUAL_VS
Definition: vpPose.h:100
@ VIRTUAL_VS
Definition: vpPose.h:96
@ LAGRANGE
Definition: vpPose.h:86
@ DEMENTHON_VIRTUAL_VS
Definition: vpPose.h:98
@ LOWE
Definition: vpPose.h:88
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:114
void setDistanceToPlaneForCoplanarityTest(double d)
Definition: vpPose.cpp:162
void addPoints(const std::vector< vpPoint > &lP)
Definition: vpPose.cpp:155
double dementhonSvThresh
SVD threshold use for the pseudo-inverse computation in poseDementhonPlan.
Definition: vpPose.h:199
void poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan=NULL, double *p_a=NULL, double *p_b=NULL, double *p_c=NULL, double *p_d=NULL)
Compute the pose of a planar object using Lagrange approach.
void poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
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:369
void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
void clearPoint()
Definition: vpPose.cpp:125
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:469
void poseLowe(vpHomogeneousMatrix &cMo)
Compute the pose using the Lowe non linear approach it consider the minimization of a residual using ...
Definition: vpPoseLowe.cpp:256
@ NO_FILTER
Definition: vpPose.h:109
bool computePoseDementhonLagrangeVVS(vpHomogeneousMatrix &cMo)
Method that first computes the pose cMo using the linear approaches of Dementhon and Lagrange and the...
Definition: vpPose.cpp:582
static void display(vpImage< unsigned char > &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, vpColor col=vpColor::none)
Definition: vpPose.cpp:694
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:769
double lambda
parameters use for the virtual visual servoing approach
Definition: vpPose.h:120
bool poseRansac(vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
void displayModel(vpImage< unsigned char > &I, vpCameraParameters &cam, vpColor col=vpColor::none)
Definition: vpPose.cpp:718
bool coplanar(int &coplanar_plane_type, double *p_a=NULL, double *p_b=NULL, double *p_c=NULL, double *p_d=NULL)
Definition: vpPose.cpp:187
double residual
Residual in meter.
Definition: vpPose.h:117
virtual ~vpPose()
Definition: vpPose.cpp:110
void poseDementhonPlan(vpHomogeneousMatrix &cMo)
Compute the pose using Dementhon approach for planar objects this is a direct implementation of the a...
void setDementhonSvThreshold(const double &svThresh)
Definition: vpPose.cpp:164
vpColVector cP
Definition: vpTracker.h:72
vpColVector p
Definition: vpTracker.h:68
#define vpDEBUG_TRACE
Definition: vpDebug.h:482
#define vpERROR_TRACE
Definition: vpDebug.h:388