Visual Servoing Platform  version 3.2.1 under development (2019-12-07)
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_, const unsigned int ransacNbInlierConsensus_,
158  const int ransacMaxTrials_, const double ransacThreshold_, const unsigned int initial_seed_,
159  const bool checkDegeneratePoints_, const std::vector<vpPoint> &listOfUniquePoints_,
160  bool (*func_)(const vpHomogeneousMatrix &)
161  #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
162  , std::atomic<bool> &abort
163  #endif
164  )
165  :
166  #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
167  m_abort(abort),
168  #endif
169  m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false),
170  m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0),
171  m_ransacMaxTrials(ransacMaxTrials_), m_ransacNbInlierConsensus(ransacNbInlierConsensus_),
172  m_ransacThreshold(ransacThreshold_), m_uniRand(initial_seed_)
173  {
174  }
175 
176  void operator()() { m_foundSolution = poseRansacImpl(); }
177 
178  // Access the return value.
179  bool getResult() const { return m_foundSolution; }
180 
181  std::vector<unsigned int> getBestConsensus() const { return m_best_consensus; }
182 
183  vpHomogeneousMatrix getEstimatedPose() const { return m_cMo; }
184 
185  unsigned int getNbInliers() const { return m_nbInliers; }
186 
187  private:
188 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
189  std::atomic<bool> &m_abort;
190 #endif
191  std::vector<unsigned int> m_best_consensus;
192  bool m_checkDegeneratePoints;
193  vpHomogeneousMatrix m_cMo;
194  bool m_foundSolution;
195  bool (*m_func)(const vpHomogeneousMatrix &);
196  std::vector<vpPoint> m_listOfUniquePoints;
197  unsigned int m_nbInliers;
198  int m_ransacMaxTrials;
199  unsigned int m_ransacNbInlierConsensus;
200  double m_ransacThreshold;
201  vpUniRand m_uniRand;
202 
203  bool poseRansacImpl();
204  };
205 
206 protected:
207  double computeResidualDementhon(const vpHomogeneousMatrix &cMo);
208 
209  // method used in poseDementhonPlan()
210  int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo);
211 
212 public:
213  vpPose();
214  vpPose(const std::vector<vpPoint>& lP);
215  virtual ~vpPose();
216  void addPoint(const vpPoint &P);
217  void addPoints(const std::vector<vpPoint> &lP);
218  void clearPoint();
219 
220  bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
221  double computeResidual(const vpHomogeneousMatrix &cMo) const;
222  bool coplanar(int &coplanar_plane_type);
223  void displayModel(vpImage<unsigned char> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
224  void displayModel(vpImage<vpRGBa> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
225 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
226 
230  vp_deprecated void init();
232 #endif
233  void poseDementhonPlan(vpHomogeneousMatrix &cMo);
234  void poseDementhonNonPlan(vpHomogeneousMatrix &cMo);
235  void poseLagrangePlan(vpHomogeneousMatrix &cMo);
236  void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo);
237  void poseLowe(vpHomogeneousMatrix &cMo);
238  bool poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
239  void poseVirtualVSrobust(vpHomogeneousMatrix &cMo);
240  void poseVirtualVS(vpHomogeneousMatrix &cMo);
241  void printPoint();
242  void setDistanceToPlaneForCoplanarityTest(double d);
243  void setLambda(double a) { lambda = a; }
244  void setVvsEpsilon(const double eps)
245  {
246  if (eps >= 0) {
247  vvsEpsilon = eps;
248  } else {
249  throw vpException(vpException::badValue, "Epsilon value must be >= 0.");
250  }
251  }
252  void setVvsIterMax(int nb) { vvsIterMax = nb; }
253 
254  void setRansacNbInliersToReachConsensus(const unsigned int &nbC) { ransacNbInlierConsensus = nbC; }
255  void setRansacThreshold(const double &t)
256  {
257  // Test whether or not t is > 0
258  if (t > std::numeric_limits<double>::epsilon()) {
259  ransacThreshold = t;
260  } else {
261  throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
262  }
263  }
264  void setRansacMaxTrials(const int &rM) { ransacMaxTrials = rM; }
265  unsigned int getRansacNbInliers() const { return (unsigned int)ransacInliers.size(); }
266  std::vector<unsigned int> getRansacInlierIndex() const { return ransacInlierIndex; }
267  std::vector<vpPoint> getRansacInliers() const { return ransacInliers; }
268 
275  void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
276 
287  {
288  if (!computeCovariance)
289  vpTRACE("Warning : The covariance matrix has not been computed. See "
290  "setCovarianceComputation() to do it.");
291 
292  return covarianceMatrix;
293  }
294 
306  inline void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag) { ransacFlag = flag; }
307 
313  inline int getNbParallelRansacThreads() const { return nbParallelRansacThreads; }
314 
323  inline void setNbParallelRansacThreads(const int nb) { nbParallelRansacThreads = nb; }
324 
330  inline bool getUseParallelRansac() const { return useParallelRansac; }
331 
337  inline void setUseParallelRansac(const bool use) { useParallelRansac = use; }
338 
344  std::vector<vpPoint> getPoints() const
345  {
346  std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
347  return vectorOfPoints;
348  }
349 
350  static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
351  vpColor col = vpColor::none);
352  static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
353  vpColor col = vpColor::none);
354  static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx,
356 
357  static int computeRansacIterations(double probability, double epsilon, const int sampleSize = 4,
358  int maxIterations = 2000);
359 
360  static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
361  const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
362  unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
363  const int &maxNbTrials=10000, const bool useParallelRansac=true, const unsigned int nthreads=0,
364  bool (*func)(const vpHomogeneousMatrix &)=NULL);
365 };
366 
367 #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:164
double residual
Residual in meter.
Definition: vpPose.h:112
std::vector< unsigned int > getRansacInlierIndex() const
Definition: vpPose.h:266
std::vector< vpPoint > getPoints() const
Definition: vpPose.h:344
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
bool getUseParallelRansac() const
Definition: vpPose.h:330
static const vpColor none
Definition: vpColor.h:192
error that can be emited by ViSP classes.
Definition: vpException.h:71
void setRansacThreshold(const double &t)
Definition: vpPose.h:255
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:110
std::vector< vpPoint > getRansacInliers() const
Definition: vpPose.h:267
Class that defines what is a point.
Definition: vpPoint.h:58
double lambda
parameters use for the virtual visual servoing approach
Definition: vpPose.h:115
vpMatrix getCovarianceMatrix() const
Definition: vpPose.h:286
int getNbParallelRansacThreads() const
Definition: vpPose.h:313
#define vpTRACE
Definition: vpDebug.h:416
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:80
void setNbParallelRansacThreads(const int nb)
Definition: vpPose.h:323
Generic class defining intrinsic camera parameters.
void setVvsIterMax(int nb)
Definition: vpPose.h:252
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:109
void setRansacMaxTrials(const int &rM)
Definition: vpPose.h:264
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:254
void setLambda(double a)
Definition: vpPose.h:243
void setUseParallelRansac(const bool use)
Definition: vpPose.h:337
unsigned int getRansacNbInliers() const
Definition: vpPose.h:265
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
Definition: vpPose.h:306
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:94
void setVvsEpsilon(const double eps)
Definition: vpPose.h:244
void setCovarianceComputation(const bool &flag)
Definition: vpPose.h:275
RANSAC_FILTER_FLAGS
Definition: vpPose.h:103