Visual Servoing Platform  version 3.3.1 under development (2020-08-10)
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_,
158  const int ransacMaxTrials_, double ransacThreshold_, unsigned int initial_seed_,
159  bool checkDegeneratePoints_, const std::vector<vpPoint> &listOfUniquePoints_,
160  bool (*func_)(const vpHomogeneousMatrix &))
161  :
162  m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false),
163  m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0),
164  m_ransacMaxTrials(ransacMaxTrials_), m_ransacNbInlierConsensus(ransacNbInlierConsensus_),
165  m_ransacThreshold(ransacThreshold_), m_uniRand(initial_seed_)
166  {
167  }
168 
169  void operator()() { m_foundSolution = poseRansacImpl(); }
170 
171  // Access the return value.
172  bool getResult() const { return m_foundSolution; }
173 
174  std::vector<unsigned int> getBestConsensus() const { return m_best_consensus; }
175 
176  vpHomogeneousMatrix getEstimatedPose() const { return m_cMo; }
177 
178  unsigned int getNbInliers() const { return m_nbInliers; }
179 
180  private:
181  std::vector<unsigned int> m_best_consensus;
182  bool m_checkDegeneratePoints;
183  vpHomogeneousMatrix m_cMo;
184  bool m_foundSolution;
185  bool (*m_func)(const vpHomogeneousMatrix &);
186  std::vector<vpPoint> m_listOfUniquePoints;
187  unsigned int m_nbInliers;
188  int m_ransacMaxTrials;
189  unsigned int m_ransacNbInlierConsensus;
190  double m_ransacThreshold;
191  vpUniRand m_uniRand;
192 
193  bool poseRansacImpl();
194  };
195 
196 protected:
197  double computeResidualDementhon(const vpHomogeneousMatrix &cMo);
198 
199  // method used in poseDementhonPlan()
200  int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo);
201 
202 public:
203  vpPose();
204  vpPose(const std::vector<vpPoint>& lP);
205  virtual ~vpPose();
206  void addPoint(const vpPoint &P);
207  void addPoints(const std::vector<vpPoint> &lP);
208  void clearPoint();
209 
210  bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
211  double computeResidual(const vpHomogeneousMatrix &cMo) const;
212  bool coplanar(int &coplanar_plane_type);
213  void displayModel(vpImage<unsigned char> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
214  void displayModel(vpImage<vpRGBa> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
215 
216  void poseDementhonPlan(vpHomogeneousMatrix &cMo);
217  void poseDementhonNonPlan(vpHomogeneousMatrix &cMo);
218  void poseLagrangePlan(vpHomogeneousMatrix &cMo);
219  void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo);
220  void poseLowe(vpHomogeneousMatrix &cMo);
221  bool poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
222  void poseVirtualVSrobust(vpHomogeneousMatrix &cMo);
223  void poseVirtualVS(vpHomogeneousMatrix &cMo);
224  void printPoint();
225  void setDistanceToPlaneForCoplanarityTest(double d);
226  void setLambda(double a) { lambda = a; }
227  void setVvsEpsilon(const double eps)
228  {
229  if (eps >= 0) {
230  vvsEpsilon = eps;
231  } else {
232  throw vpException(vpException::badValue, "Epsilon value must be >= 0.");
233  }
234  }
235  void setVvsIterMax(int nb) { vvsIterMax = nb; }
236 
237  void setRansacNbInliersToReachConsensus(const unsigned int &nbC) { ransacNbInlierConsensus = nbC; }
238  void setRansacThreshold(const double &t)
239  {
240  // Test whether or not t is > 0
241  if (t > std::numeric_limits<double>::epsilon()) {
242  ransacThreshold = t;
243  } else {
244  throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
245  }
246  }
247  void setRansacMaxTrials(const int &rM) { ransacMaxTrials = rM; }
248  unsigned int getRansacNbInliers() const { return (unsigned int)ransacInliers.size(); }
249  std::vector<unsigned int> getRansacInlierIndex() const { return ransacInlierIndex; }
250  std::vector<vpPoint> getRansacInliers() const { return ransacInliers; }
251 
258  void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
259 
270  {
271  if (!computeCovariance)
272  vpTRACE("Warning : The covariance matrix has not been computed. See "
273  "setCovarianceComputation() to do it.");
274 
275  return covarianceMatrix;
276  }
277 
289  inline void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag) { ransacFlag = flag; }
290 
296  inline int getNbParallelRansacThreads() const { return nbParallelRansacThreads; }
297 
306  inline void setNbParallelRansacThreads(int nb) { nbParallelRansacThreads = nb; }
307 
313  inline bool getUseParallelRansac() const { return useParallelRansac; }
314 
320  inline void setUseParallelRansac(bool use) { useParallelRansac = use; }
321 
327  std::vector<vpPoint> getPoints() const
328  {
329  std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
330  return vectorOfPoints;
331  }
332 
333  static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
334  vpColor col = vpColor::none);
335  static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
336  vpColor col = vpColor::none);
337  static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx,
339 
340  static int computeRansacIterations(double probability, double epsilon, const int sampleSize = 4,
341  int maxIterations = 2000);
342 
343  static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
344  const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
345  unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
346  const int &maxNbTrials=10000, bool useParallelRansac=true, unsigned int nthreads=0,
347  bool (*func)(const vpHomogeneousMatrix &) = NULL);
348 
349  static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap, const std::vector<vpImagePoint> &corners,
350  const vpCameraParameters &colorIntrinsics, const std::vector<vpPoint> &point3d, vpHomogeneousMatrix &cMo,
351  double *confidence_index = NULL);
352 
353 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
354 
358  vp_deprecated void init();
360 #endif
361 };
362 
363 #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:156
double residual
Residual in meter.
Definition: vpPose.h:112
std::vector< unsigned int > getRansacInlierIndex() const
Definition: vpPose.h:249
std::vector< vpPoint > getPoints() const
Definition: vpPose.h:327
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:313
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:238
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:110
std::vector< vpPoint > getRansacInliers() const
Definition: vpPose.h:250
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:269
int getNbParallelRansacThreads() const
Definition: vpPose.h:296
void setNbParallelRansacThreads(int nb)
Definition: vpPose.h:306
#define vpTRACE
Definition: vpDebug.h:416
void setUseParallelRansac(bool use)
Definition: vpPose.h:320
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:235
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:109
void setRansacMaxTrials(const int &rM)
Definition: vpPose.h:247
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:237
void setLambda(double a)
Definition: vpPose.h:226
unsigned int getRansacNbInliers() const
Definition: vpPose.h:248
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
Definition: vpPose.h:289
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:227
void setCovarianceComputation(const bool &flag)
Definition: vpPose.h:258
RANSAC_FILTER_FLAGS
Definition: vpPose.h:103