Visual Servoing Platform  version 3.2.1 under development (2019-10-18) under development (2019-10-18)
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 
78 class VISP_EXPORT vpPose
79 {
80 public:
82  typedef enum {
85  LOWE,
89  LAGRANGE_LOWE,
91  DEMENTHON_LOWE,
93  VIRTUAL_VS,
95  DEMENTHON_VIRTUAL_VS,
97  LAGRANGE_VIRTUAL_VS
100 
104  CHECK_DEGENERATE_POINTS
105  };
106 
107  unsigned int npt;
108  std::list<vpPoint> listP;
109 
110  double residual;
111 
112 protected:
113  double lambda;
114 
115 private:
117  int vvsIterMax;
119  std::vector<vpPoint> c3d;
121  bool computeCovariance;
123  vpMatrix covarianceMatrix;
126  unsigned int ransacNbInlierConsensus;
128  int ransacMaxTrials;
130  std::vector<vpPoint> ransacInliers;
132  std::vector<unsigned int> ransacInlierIndex;
134  double ransacThreshold;
137  double distanceToPlaneForCoplanarityTest;
139  RANSAC_FILTER_FLAGS ransacFlag;
142  std::vector<vpPoint> listOfPoints;
144  bool useParallelRansac;
146  int nbParallelRansacThreads;
149  double vvsEpsilon;
150 
151  // For parallel RANSAC
152  class RansacFunctor
153  {
154  public:
155  RansacFunctor(const vpHomogeneousMatrix &cMo_, const unsigned int ransacNbInlierConsensus_,
156  const int ransacMaxTrials_, const double ransacThreshold_, const unsigned int initial_seed_,
157  const bool checkDegeneratePoints_, const std::vector<vpPoint> &listOfUniquePoints_,
158  bool (*func_)(const vpHomogeneousMatrix &)
159  #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
160  , std::atomic<bool> &abort
161  #endif
162  )
163  :
164  #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
165  m_abort(abort),
166  #endif
167  m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false),
168  m_func(func_), m_initial_seed(initial_seed_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0),
169  m_ransacMaxTrials(ransacMaxTrials_), m_ransacNbInlierConsensus(ransacNbInlierConsensus_),
170  m_ransacThreshold(ransacThreshold_)
171  {
172 #if (defined(_WIN32) && (defined(_MSC_VER) || defined(__MINGW32__)) || defined(ANDROID))
173  (void)initial_seed_;
174 #endif
175  }
176 
177  void operator()() { m_foundSolution = poseRansacImpl(); }
178 
179  // Access the return value.
180  bool getResult() const { return m_foundSolution; }
181 
182  std::vector<unsigned int> getBestConsensus() const { return m_best_consensus; }
183 
184  vpHomogeneousMatrix getEstimatedPose() const { return m_cMo; }
185 
186  unsigned int getNbInliers() const { return m_nbInliers; }
187 
188  private:
189 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
190  std::atomic<bool> &m_abort;
191 #endif
192  std::vector<unsigned int> m_best_consensus;
193  bool m_checkDegeneratePoints;
194  vpHomogeneousMatrix m_cMo;
195  bool m_foundSolution;
196  bool (*m_func)(const vpHomogeneousMatrix &);
197  unsigned int m_initial_seed;
198  std::vector<vpPoint> m_listOfUniquePoints;
199  unsigned int m_nbInliers;
200  int m_ransacMaxTrials;
201  unsigned int m_ransacNbInlierConsensus;
202  double m_ransacThreshold;
203 
204  bool poseRansacImpl();
205  };
206 
207 protected:
208  double computeResidualDementhon(const vpHomogeneousMatrix &cMo);
209 
210  // method used in poseDementhonPlan()
211  int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo);
212 
213 public:
214  vpPose();
215  vpPose(const std::vector<vpPoint>& lP);
216  virtual ~vpPose();
217  void addPoint(const vpPoint &P);
218  void addPoints(const std::vector<vpPoint> &lP);
219  void clearPoint();
220 
221  bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
222  double computeResidual(const vpHomogeneousMatrix &cMo) const;
223  bool coplanar(int &coplanar_plane_type);
224  void displayModel(vpImage<unsigned char> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
225  void displayModel(vpImage<vpRGBa> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
226 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
227 
231  vp_deprecated void init();
233 #endif
234  void poseDementhonPlan(vpHomogeneousMatrix &cMo);
235  void poseDementhonNonPlan(vpHomogeneousMatrix &cMo);
236  void poseLagrangePlan(vpHomogeneousMatrix &cMo);
237  void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo);
238  void poseLowe(vpHomogeneousMatrix &cMo);
239  bool poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
240  void poseVirtualVSrobust(vpHomogeneousMatrix &cMo);
241  void poseVirtualVS(vpHomogeneousMatrix &cMo);
242  void printPoint();
243  void setDistanceToPlaneForCoplanarityTest(double d);
244  void setLambda(double a) { lambda = a; }
245  void setVvsEpsilon(const double eps)
246  {
247  if (eps >= 0) {
248  vvsEpsilon = eps;
249  } else {
250  throw vpException(vpException::badValue, "Epsilon value must be >= 0.");
251  }
252  }
253  void setVvsIterMax(int nb) { vvsIterMax = nb; }
254 
255  void setRansacNbInliersToReachConsensus(const unsigned int &nbC) { ransacNbInlierConsensus = nbC; }
256  void setRansacThreshold(const double &t)
257  {
258  // Test whether or not t is > 0
259  if (t > std::numeric_limits<double>::epsilon()) {
260  ransacThreshold = t;
261  } else {
262  throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
263  }
264  }
265  void setRansacMaxTrials(const int &rM) { ransacMaxTrials = rM; }
266  unsigned int getRansacNbInliers() const { return (unsigned int)ransacInliers.size(); }
267  std::vector<unsigned int> getRansacInlierIndex() const { return ransacInlierIndex; }
268  std::vector<vpPoint> getRansacInliers() const { return ransacInliers; }
269 
276  void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
277 
288  {
289  if (!computeCovariance)
290  vpTRACE("Warning : The covariance matrix has not been computed. See "
291  "setCovarianceComputation() to do it.");
292 
293  return covarianceMatrix;
294  }
295 
307  inline void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag) { ransacFlag = flag; }
308 
314  inline int getNbParallelRansacThreads() const { return nbParallelRansacThreads; }
315 
324  inline void setNbParallelRansacThreads(const int nb) { nbParallelRansacThreads = nb; }
325 
331  inline bool getUseParallelRansac() const { return useParallelRansac; }
332 
338  inline void setUseParallelRansac(const bool use) { useParallelRansac = use; }
339 
345  std::vector<vpPoint> getPoints() const
346  {
347  std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
348  return vectorOfPoints;
349  }
350 
351  static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
352  vpColor col = vpColor::none);
353  static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
354  vpColor col = vpColor::none);
355  static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx,
357 
358  static int computeRansacIterations(double probability, double epsilon, const int sampleSize = 4,
359  int maxIterations = 2000);
360 
361  static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
362  const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
363  unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
364  const int &maxNbTrials=10000, const bool useParallelRansac=true, const unsigned int nthreads=0,
365  bool (*func)(const vpHomogeneousMatrix &)=NULL);
366 };
367 
368 #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:110
std::vector< unsigned int > getRansacInlierIndex() const
Definition: vpPose.h:267
std::vector< vpPoint > getPoints() const
Definition: vpPose.h:345
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:331
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:256
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:108
std::vector< vpPoint > getRansacInliers() const
Definition: vpPose.h:268
Class that defines what is a point.
Definition: vpPoint.h:58
double lambda
parameters use for the virtual visual servoing approach
Definition: vpPose.h:113
vpMatrix getCovarianceMatrix() const
Definition: vpPose.h:287
int getNbParallelRansacThreads() const
Definition: vpPose.h:314
#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:78
void setNbParallelRansacThreads(const int nb)
Definition: vpPose.h:324
Generic class defining intrinsic camera parameters.
void setVvsIterMax(int nb)
Definition: vpPose.h:253
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:107
void setRansacMaxTrials(const int &rM)
Definition: vpPose.h:265
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:255
void setLambda(double a)
Definition: vpPose.h:244
void setUseParallelRansac(const bool use)
Definition: vpPose.h:338
unsigned int getRansacNbInliers() const
Definition: vpPose.h:266
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
Definition: vpPose.h:307
vpPoseMethodType
Methods that could be used to estimate the pose from points.
Definition: vpPose.h:82
void setVvsEpsilon(const double eps)
Definition: vpPose.h:245
void setCovarianceComputation(const bool &flag)
Definition: vpPose.h:276
RANSAC_FILTER_FLAGS
Definition: vpPose.h:101