Visual Servoing Platform  version 3.2.1 under development (2019-08-08)
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 
63 {
64 #if (DEBUG_LEVEL1)
65  std::cout << "begin vpPose::Init() " << std::endl;
66 #endif
67 
68  npt = 0;
69  listP.clear();
70  residual = 0;
71  lambda = 1.;
72  vvsIterMax = 200;
73  c3d.clear();
74  computeCovariance = false;
75  covarianceMatrix.clear();
76  ransacNbInlierConsensus = 4;
77  ransacMaxTrials = 1000;
78  ransacInliers.clear();
79  ransacInlierIndex.clear();
80  ransacThreshold = 0.0001;
81  distanceToPlaneForCoplanarityTest = 0.001;
82  ransacFlag = NO_FILTER;
83  listOfPoints.clear();
84  useParallelRansac = false;
85  nbParallelRansacThreads = 0;
86  vvsEpsilon = 1e-8;
87 
88 #if (DEBUG_LEVEL1)
89  std::cout << "end vpPose::Init() " << std::endl;
90 #endif
91 }
92 
95  : npt(0), listP(), residual(0), lambda(0.25), vvsIterMax(200), c3d(), computeCovariance(false), covarianceMatrix(),
96  ransacNbInlierConsensus(4), ransacMaxTrials(1000), ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001),
97  distanceToPlaneForCoplanarityTest(0.001), ransacFlag(vpPose::NO_FILTER), listOfPoints(),
98  useParallelRansac(false),
99  nbParallelRansacThreads(0), // 0 means that we use C++11 (if available) to get the number of threads
100  vvsEpsilon(1e-8)
101 {
102 }
103 
108 {
109 #if (DEBUG_LEVEL1)
110  std::cout << "begin vpPose::~vpPose() " << std::endl;
111 #endif
112 
113  listP.clear();
114 
115 #if (DEBUG_LEVEL1)
116  std::cout << "end vpPose::~vpPose() " << std::endl;
117 #endif
118 }
123 {
124  listP.clear();
125  listOfPoints.clear();
126  npt = 0;
127 }
128 
137 void vpPose::addPoint(const vpPoint &newP)
138 {
139  listP.push_back(newP);
140  listOfPoints.push_back(newP);
141  npt++;
142 }
143 
152 void vpPose::addPoints(const std::vector<vpPoint> &lP)
153 {
154  listP.insert(listP.end(), lP.begin(), lP.end());
155  listOfPoints.insert(listOfPoints.end(), lP.begin(), lP.end());
156  npt = (unsigned int)listP.size();
157 }
158 
159 void vpPose::setDistanceToPlaneForCoplanarityTest(double d) { distanceToPlaneForCoplanarityTest = d; }
160 
172 bool vpPose::coplanar(int &coplanar_plane_type)
173 {
174  coplanar_plane_type = 0;
175  if (npt < 2) {
176  vpERROR_TRACE("Not enough point (%d) to compute the pose ", npt);
177  throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough points "));
178  }
179 
180  if (npt == 3)
181  return true;
182 
183  double x1 = 0, x2 = 0, x3 = 0, y1 = 0, y2 = 0, y3 = 0, z1 = 0, z2 = 0, z3 = 0;
184 
185  std::list<vpPoint>::const_iterator it = listP.begin();
186 
187  vpPoint P1, P2, P3;
188 
189  // Get three 3D points that are not collinear and that is not at origin
190  bool degenerate = true;
191  bool not_on_origin = true;
192  std::list<vpPoint>::const_iterator it_tmp;
193 
194  std::list<vpPoint>::const_iterator it_i, it_j, it_k;
195  for (it_i = listP.begin(); it_i != listP.end(); ++it_i) {
196  if (degenerate == false) {
197  // std::cout << "Found a non degenerate configuration" << std::endl;
198  break;
199  }
200  P1 = *it_i;
201  // Test if point is on origin
202  if ((std::fabs(P1.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
203  (std::fabs(P1.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
204  (std::fabs(P1.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
205  not_on_origin = false;
206  } else {
207  not_on_origin = true;
208  }
209  if (not_on_origin) {
210  it_tmp = it_i;
211  ++it_tmp; // j = i+1
212  for (it_j = it_tmp; it_j != listP.end(); ++it_j) {
213  if (degenerate == false) {
214  // std::cout << "Found a non degenerate configuration" << std::endl;
215  break;
216  }
217  P2 = *it_j;
218  if ((std::fabs(P2.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
219  (std::fabs(P2.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
220  (std::fabs(P2.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
221  not_on_origin = false;
222  } else {
223  not_on_origin = true;
224  }
225  if (not_on_origin) {
226  it_tmp = it_j;
227  ++it_tmp; // k = j+1
228  for (it_k = it_tmp; it_k != listP.end(); ++it_k) {
229  P3 = *it_k;
230  if ((std::fabs(P3.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
231  (std::fabs(P3.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
232  (std::fabs(P3.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  x1 = P1.get_oX();
239  x2 = P2.get_oX();
240  x3 = P3.get_oX();
241 
242  y1 = P1.get_oY();
243  y2 = P2.get_oY();
244  y3 = P3.get_oY();
245 
246  z1 = P1.get_oZ();
247  z2 = P2.get_oZ();
248  z3 = P3.get_oZ();
249 
250  vpColVector a_b(3), b_c(3), cross_prod;
251  a_b[0] = x1 - x2;
252  a_b[1] = y1 - y2;
253  a_b[2] = z1 - z2;
254  b_c[0] = x2 - x3;
255  b_c[1] = y2 - y3;
256  b_c[2] = z2 - z3;
257 
258  cross_prod = vpColVector::crossProd(a_b, b_c);
259  if (cross_prod.sumSquare() <= std::numeric_limits<double>::epsilon())
260  degenerate = true; // points are collinear
261  else
262  degenerate = false;
263  }
264  if (degenerate == false)
265  break;
266  }
267  }
268  }
269  }
270  }
271 
272  if (degenerate) {
273  coplanar_plane_type = 4; // points are collinear
274  return true;
275  }
276 
277  double a = y1 * z2 - y1 * z3 - y2 * z1 + y2 * z3 + y3 * z1 - y3 * z2;
278  double b = -x1 * z2 + x1 * z3 + x2 * z1 - x2 * z3 - x3 * z1 + x3 * z2;
279  double c = x1 * y2 - x1 * y3 - x2 * y1 + x2 * y3 + x3 * y1 - x3 * y2;
280  double d = -x1 * y2 * z3 + x1 * y3 * z2 + x2 * y1 * z3 - x2 * y3 * z1 - x3 * y1 * z2 + x3 * y2 * z1;
281 
282  // std::cout << "a=" << a << " b=" << b << " c=" << c << " d=" << d <<
283  // std::endl;
284  if (std::fabs(b) <= std::numeric_limits<double>::epsilon() &&
285  std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
286  coplanar_plane_type = 1; // ax=d
287  } else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
288  std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
289  coplanar_plane_type = 2; // by=d
290  } else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
291  std::fabs(b) <= std::numeric_limits<double>::epsilon()) {
292  coplanar_plane_type = 3; // cz=d
293  }
294 
295  double D = sqrt(vpMath::sqr(a) + vpMath::sqr(b) + vpMath::sqr(c));
296 
297  for (it = listP.begin(); it != listP.end(); ++it) {
298  P1 = *it;
299  double dist = (a * P1.get_oX() + b * P1.get_oY() + c * P1.get_oZ() + d) / D;
300  // std::cout << "dist= " << dist << std::endl;
301 
302  if (fabs(dist) > distanceToPlaneForCoplanarityTest) {
303  vpDEBUG_TRACE(10, " points are not coplanar ");
304  // TRACE(" points are not coplanar ") ;
305  return false;
306  }
307  }
308 
309  vpDEBUG_TRACE(10, " points are coplanar ");
310  // vpTRACE(" points are coplanar ") ;
311 
312  return true;
313 }
314 
325 {
326  double squared_error = 0;
327  vpPoint P;
328  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
329  P = *it;
330  double x = P.get_x();
331  double y = P.get_y();
332 
333  P.track(cMo);
334 
335  squared_error += vpMath::sqr(x - P.get_x()) + vpMath::sqr(y - P.get_y());
336  }
337  return (squared_error);
338 }
339 
363 {
364  if (npt < 4) {
366  "Not enough point (%d) to compute the pose ", npt));
367  }
368 
369  switch (method) {
370  case DEMENTHON:
372  case DEMENTHON_LOWE: {
373  if (npt < 4) {
375  "Dementhon method cannot be used in that case "
376  "(at least 4 points are required)"
377  "Not enough point (%d) to compute the pose ", npt));
378  }
379 
380  // test si les point 3D sont coplanaires
381  int coplanar_plane_type = 0;
382  bool plan = coplanar(coplanar_plane_type);
383  if (plan == true) {
384  poseDementhonPlan(cMo);
385  } else {
387  }
388  } break;
389  case LAGRANGE:
390  case LAGRANGE_VIRTUAL_VS:
391  case LAGRANGE_LOWE: {
392  // test si les point 3D sont coplanaires
393  int coplanar_plane_type;
394  bool plan = coplanar(coplanar_plane_type);
395 
396  if (plan == true)
397  {
398 
399  if (coplanar_plane_type == 4) {
401  "Lagrange method cannot be used in that case "
402  "(points are collinear)"));
403  }
404  if (npt < 4) {
406  "Lagrange method cannot be used in that case "
407  "(at least 4 points are required). "
408  "Not enough point (%d) to compute the pose ", npt));
409  }
410  poseLagrangePlan(cMo);
411  } else {
412  if (npt < 6) {
414  "Lagrange method cannot be used in that case "
415  "(at least 6 points are required when 3D points are non coplanar). "
416  "Not enough point (%d) to compute the pose ", npt));
417  }
418  poseLagrangeNonPlan(cMo);
419  }
420  } break;
421  case RANSAC:
422  if (npt < 4) {
424  "Ransac method cannot be used in that case "
425  "(at least 4 points are required). "
426  "Not enough point (%d) to compute the pose ", npt));
427  }
428  return poseRansac(cMo, func);
429  break;
430  case LOWE:
431  case VIRTUAL_VS:
432  break;
433  }
434 
435  switch (method) {
436  case LAGRANGE:
437  case DEMENTHON:
438  case RANSAC:
439  break;
440  case VIRTUAL_VS:
441  case LAGRANGE_VIRTUAL_VS:
442  case DEMENTHON_VIRTUAL_VS: {
443  poseVirtualVS(cMo);
444  } break;
445  case LOWE:
446  case LAGRANGE_LOWE:
447  case DEMENTHON_LOWE: {
448  poseLowe(cMo);
449  } break;
450  }
451 
452  // If here, there was no exception thrown so return true
453  return true;
454 }
455 
457 {
458  vpPoint P;
459  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
460  P = *it;
461 
462  std::cout << "3D oP " << P.oP.t();
463  std::cout << "3D cP " << P.cP.t();
464  std::cout << "2D " << P.p.t();
465  }
466 }
467 
478  vpColor col)
479 {
480  vpDisplay::displayFrame(I, cMo, cam, size, col);
481 }
482 
493 {
494  vpDisplay::displayFrame(I, cMo, cam, size, col);
495 }
496 
502 {
503  vpPoint P;
504  vpImagePoint ip;
505  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
506  P = *it;
507  vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip);
508  vpDisplay::displayCross(I, ip, 5, col);
509  // std::cout << "3D oP " << P.oP.t() ;
510  // std::cout << "3D cP " << P.cP.t() ;
511  // std::cout << "2D " << P.p.t() ;
512  }
513 }
514 
520 {
521  vpPoint P;
522  vpImagePoint ip;
523  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
524  P = *it;
525  vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip);
526  vpDisplay::displayCross(I, ip, 5, col);
527  // std::cout << "3D oP " << P.oP.t() ;
528  // std::cout << "3D cP " << P.cP.t() ;
529  // std::cout << "2D " << P.p.t() ;
530  }
531 }
532 
552 double vpPose::poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx, vpCameraParameters &cam,
553  vpHomogeneousMatrix &cMo)
554 {
555 
556  std::vector<double> rectx(4);
557  std::vector<double> recty(4);
558  rectx[0] = 0;
559  recty[0] = 0;
560  rectx[1] = 1;
561  recty[1] = 0;
562  rectx[2] = 1;
563  recty[2] = 1;
564  rectx[3] = 0;
565  recty[3] = 1;
566  std::vector<double> irectx(4);
567  std::vector<double> irecty(4);
568  irectx[0] = (p1.get_x());
569  irecty[0] = (p1.get_y());
570  irectx[1] = (p2.get_x());
571  irecty[1] = (p2.get_y());
572  irectx[2] = (p3.get_x());
573  irecty[2] = (p3.get_y());
574  irectx[3] = (p4.get_x());
575  irecty[3] = (p4.get_y());
576 
577  // calcul de l'homographie
578  vpMatrix H(3, 3);
579  vpHomography hom;
580 
581  // vpHomography::HartleyDLT(rectx,recty,irectx,irecty,hom);
582  vpHomography::HLM(rectx, recty, irectx, irecty, 1, hom);
583  for (unsigned int i = 0; i < 3; i++)
584  for (unsigned int j = 0; j < 3; j++)
585  H[i][j] = hom[i][j];
586  // calcul de s = ||Kh1||/ ||Kh2|| =ratio (length on x axis/ length on y
587  // axis)
588  vpColVector kh1(3);
589  vpColVector kh2(3);
590  vpMatrix K(3, 3);
591  K = cam.get_K();
592  K.eye();
593  vpMatrix Kinv = K.pseudoInverse();
594 
595  vpMatrix KinvH = Kinv * H;
596  kh1 = KinvH.getCol(0);
597  kh2 = KinvH.getCol(1);
598 
599  double s = sqrt(kh1.sumSquare()) / sqrt(kh2.sumSquare());
600 
601  vpMatrix D(3, 3);
602  D.eye();
603  D[1][1] = 1 / s;
604  vpMatrix cHo = H * D;
605 
606  // Calcul de la rotation et de la translation
607  // PoseFromRectangle(p1,p2,p3,p4,1/s,lx,cam,cMo );
608  p1.setWorldCoordinates(0, 0, 0);
609  p2.setWorldCoordinates(lx, 0, 0);
610  p3.setWorldCoordinates(lx, lx / s, 0);
611  p4.setWorldCoordinates(0, lx / s, 0);
612 
613  vpPose P;
614  P.addPoint(p1);
615  P.addPoint(p2);
616  P.addPoint(p3);
617  P.addPoint(p4);
618 
620  return lx / s;
621 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:362
virtual ~vpPose()
Definition: vpPose.cpp:107
double residual
Residual in meter.
Definition: vpPose.h:110
bool coplanar(int &coplanar_plane_type)
Definition: vpPose.cpp:172
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
void init()
Definition: vpPose.cpp:62
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void displayModel(vpImage< unsigned char > &I, vpCameraParameters &cam, vpColor col=vpColor::none)
Definition: vpPose.cpp:501
void addPoints(const std::vector< vpPoint > &lP)
Definition: vpPose.cpp:152
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:424
void track(const vpHomogeneousMatrix &cMo)
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:433
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:108
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:113
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:477
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:552
static double sqr(double x)
Definition: vpMath.h:114
vpRowVector t() const
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:78
Generic class defining intrinsic camera parameters.
vpColVector getCol(const unsigned int j) const
Definition: vpMatrix.cpp:3880
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:426
void printPoint()
Definition: vpPose.cpp:456
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
void clear()
Definition: vpMatrix.h:302
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:431
vpPose()
Definition: vpPose.cpp:94
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:107
double sumSquare() const
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:422
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
vpMatrix get_K() const
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:113
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))
vpPoseMethodType
Methods that could be used to estimate the pose from points.
Definition: vpPose.h:82
bool poseRansac(vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
void setDistanceToPlaneForCoplanarityTest(double d)
Definition: vpPose.cpp:159
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2014
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)
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:137
void eye()
Definition: vpMatrix.cpp:360
vpColVector p
Definition: vpTracker.h:71
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:324
void clearPoint()
Definition: vpPose.cpp:122