Visual Servoing Platform  version 3.4.1 under development (2022-01-26)
vpPose.h
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  * Aurelien Yol
38  *
39  *****************************************************************************/
40 
46 #ifndef _vpPose_h_
47 #define _vpPose_h_
48 
49 #include <visp3/core/vpHomogeneousMatrix.h>
50 #include <visp3/core/vpPoint.h>
51 #include <visp3/core/vpRGBa.h>
52 #include <visp3/vision/vpHomography.h>
53 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
54 #include <visp3/core/vpList.h>
55 #endif
56 #include <visp3/core/vpThread.h>
57 
58 #include <list>
59 #include <math.h>
60 #include <vector>
61 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
62 #include <atomic>
63 #endif
64 
65 #include <visp3/core/vpUniRand.h>
66 
80 class VISP_EXPORT vpPose
81 {
82 public:
84  typedef enum {
87  LOWE,
91  LAGRANGE_LOWE,
93  DEMENTHON_LOWE,
95  VIRTUAL_VS,
97  DEMENTHON_VIRTUAL_VS,
99  LAGRANGE_VIRTUAL_VS
102 
106  CHECK_DEGENERATE_POINTS
107  };
108 
109  unsigned int npt;
110  std::list<vpPoint> listP;
111 
112  double residual;
113 
114 protected:
115  double lambda;
116 
117 private:
119  int vvsIterMax;
121  std::vector<vpPoint> c3d;
123  bool computeCovariance;
125  vpMatrix covarianceMatrix;
128  unsigned int ransacNbInlierConsensus;
130  int ransacMaxTrials;
132  std::vector<vpPoint> ransacInliers;
134  std::vector<unsigned int> ransacInlierIndex;
136  double ransacThreshold;
139  double distanceToPlaneForCoplanarityTest;
141  RANSAC_FILTER_FLAGS ransacFlag;
144  std::vector<vpPoint> listOfPoints;
146  bool useParallelRansac;
148  int nbParallelRansacThreads;
151  double vvsEpsilon;
152 
153  // For parallel RANSAC
154  class RansacFunctor
155  {
156  public:
157  RansacFunctor(const vpHomogeneousMatrix &cMo_, unsigned int ransacNbInlierConsensus_, const int ransacMaxTrials_,
158  double ransacThreshold_, unsigned int initial_seed_, bool checkDegeneratePoints_,
159  const std::vector<vpPoint> &listOfUniquePoints_, bool (*func_)(const vpHomogeneousMatrix &))
160  : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false),
161  m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), m_ransacMaxTrials(ransacMaxTrials_),
162  m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_),
163  m_uniRand(initial_seed_)
164  {
165  }
166 
167  void operator()() { m_foundSolution = poseRansacImpl(); }
168 
169  // Access the return value.
170  bool getResult() const { return m_foundSolution; }
171 
172  std::vector<unsigned int> getBestConsensus() const { return m_best_consensus; }
173 
174  vpHomogeneousMatrix getEstimatedPose() const { return m_cMo; }
175 
176  unsigned int getNbInliers() const { return m_nbInliers; }
177 
178  private:
179  std::vector<unsigned int> m_best_consensus;
180  bool m_checkDegeneratePoints;
181  vpHomogeneousMatrix m_cMo;
182  bool m_foundSolution;
183  bool (*m_func)(const vpHomogeneousMatrix &);
184  std::vector<vpPoint> m_listOfUniquePoints;
185  unsigned int m_nbInliers;
186  int m_ransacMaxTrials;
187  unsigned int m_ransacNbInlierConsensus;
188  double m_ransacThreshold;
189  vpUniRand m_uniRand;
190 
191  bool poseRansacImpl();
192  };
193 
194 protected:
195  double computeResidualDementhon(const vpHomogeneousMatrix &cMo);
196 
197  // method used in poseDementhonPlan()
198  int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo);
199 
200 public:
201  vpPose();
202  vpPose(const std::vector<vpPoint> &lP);
203  virtual ~vpPose();
204  void addPoint(const vpPoint &P);
205  void addPoints(const std::vector<vpPoint> &lP);
206  void clearPoint();
207 
208  bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
209  double computeResidual(const vpHomogeneousMatrix &cMo) const;
210  bool coplanar(int &coplanar_plane_type);
211  void displayModel(vpImage<unsigned char> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
212  void displayModel(vpImage<vpRGBa> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
213 
214  void poseDementhonPlan(vpHomogeneousMatrix &cMo);
215  void poseDementhonNonPlan(vpHomogeneousMatrix &cMo);
216  void poseLagrangePlan(vpHomogeneousMatrix &cMo);
217  void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo);
218  void poseLowe(vpHomogeneousMatrix &cMo);
219  bool poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
220  void poseVirtualVSrobust(vpHomogeneousMatrix &cMo);
221  void poseVirtualVS(vpHomogeneousMatrix &cMo);
222  void printPoint();
223  void setDistanceToPlaneForCoplanarityTest(double d);
224  void setLambda(double a) { lambda = a; }
225  void setVvsEpsilon(const double eps)
226  {
227  if (eps >= 0) {
228  vvsEpsilon = eps;
229  } else {
230  throw vpException(vpException::badValue, "Epsilon value must be >= 0.");
231  }
232  }
233  void setVvsIterMax(int nb) { vvsIterMax = nb; }
234 
235  void setRansacNbInliersToReachConsensus(const unsigned int &nbC) { ransacNbInlierConsensus = nbC; }
236  void setRansacThreshold(const double &t)
237  {
238  // Test whether or not t is > 0
239  if (t > std::numeric_limits<double>::epsilon()) {
240  ransacThreshold = t;
241  } else {
242  throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
243  }
244  }
245  void setRansacMaxTrials(const int &rM) { ransacMaxTrials = rM; }
246  unsigned int getRansacNbInliers() const { return (unsigned int)ransacInliers.size(); }
247  std::vector<unsigned int> getRansacInlierIndex() const { return ransacInlierIndex; }
248  std::vector<vpPoint> getRansacInliers() const { return ransacInliers; }
249 
256  void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
257 
268  {
269  if (!computeCovariance)
270  vpTRACE("Warning : The covariance matrix has not been computed. See "
271  "setCovarianceComputation() to do it.");
272 
273  return covarianceMatrix;
274  }
275 
287  inline void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag) { ransacFlag = flag; }
288 
294  inline int getNbParallelRansacThreads() const { return nbParallelRansacThreads; }
295 
304  inline void setNbParallelRansacThreads(int nb) { nbParallelRansacThreads = nb; }
305 
311  inline bool getUseParallelRansac() const { return useParallelRansac; }
312 
318  inline void setUseParallelRansac(bool use) { useParallelRansac = use; }
319 
325  std::vector<vpPoint> getPoints() const
326  {
327  std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
328  return vectorOfPoints;
329  }
330 
331  static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
332  vpColor col = vpColor::none);
333  static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
334  vpColor col = vpColor::none);
335  static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx,
337 
338  static int computeRansacIterations(double probability, double epsilon, const int sampleSize = 4,
339  int maxIterations = 2000);
340 
341  static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
342  const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
343  unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
344  const int &maxNbTrials = 10000, bool useParallelRansac = true, unsigned int nthreads = 0,
345  bool (*func)(const vpHomogeneousMatrix &) = NULL);
346 
347  static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap, const std::vector<vpImagePoint> &corners,
348  const vpCameraParameters &colorIntrinsics,
349  const std::vector<vpPoint> &point3d, vpHomogeneousMatrix &cMo,
350  double *confidence_index = NULL);
351 
352  static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap,
353  const std::vector<std::vector<vpImagePoint> > &corners,
354  const vpCameraParameters &colorIntrinsics,
355  const std::vector<std::vector<vpPoint> > &point3d,
356  vpHomogeneousMatrix &cMo, double *confidence_index = NULL,
357  bool coplanar_points = true);
358 
359 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
360 
364  vp_deprecated void init();
366 #endif
367 };
368 
369 #endif
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
double residual
Residual in meter.
Definition: vpPose.h:112
std::vector< unsigned int > getRansacInlierIndex() const
Definition: vpPose.h:247
std::vector< vpPoint > getPoints() const
Definition: vpPose.h:325
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
bool getUseParallelRansac() const
Definition: vpPose.h:311
static const vpColor none
Definition: vpColor.h:229
error that can be emited by ViSP classes.
Definition: vpException.h:71
void setRansacThreshold(const double &t)
Definition: vpPose.h:236
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:110
std::vector< vpPoint > getRansacInliers() const
Definition: vpPose.h:248
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
double lambda
parameters use for the virtual visual servoing approach
Definition: vpPose.h:115
vpMatrix getCovarianceMatrix() const
Definition: vpPose.h:267
int getNbParallelRansacThreads() const
Definition: vpPose.h:294
void setNbParallelRansacThreads(int nb)
Definition: vpPose.h:304
#define vpTRACE
Definition: vpDebug.h:416
void setUseParallelRansac(bool use)
Definition: vpPose.h:318
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:80
Generic class defining intrinsic camera parameters.
void setVvsIterMax(int nb)
Definition: vpPose.h:233
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:109
void setRansacMaxTrials(const int &rM)
Definition: vpPose.h:245
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:235
void setLambda(double a)
Definition: vpPose.h:224
unsigned int getRansacNbInliers() const
Definition: vpPose.h:246
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
Definition: vpPose.h:287
vpPoseMethodType
Methods that could be used to estimate the pose from points.
Definition: vpPose.h:84
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:100
void setVvsEpsilon(const double eps)
Definition: vpPose.h:225
void setCovarianceComputation(const bool &flag)
Definition: vpPose.h:256
RANSAC_FILTER_FLAGS
Definition: vpPose.h:103