Visual Servoing Platform  version 3.2.1 under development (2019-08-08)
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  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  void init();
226  void poseDementhonPlan(vpHomogeneousMatrix &cMo);
227  void poseDementhonNonPlan(vpHomogeneousMatrix &cMo);
228  void poseLagrangePlan(vpHomogeneousMatrix &cMo);
229  void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo);
230  void poseLowe(vpHomogeneousMatrix &cMo);
231  bool poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
232  void poseVirtualVSrobust(vpHomogeneousMatrix &cMo);
233  void poseVirtualVS(vpHomogeneousMatrix &cMo);
234  void printPoint();
235  void setDistanceToPlaneForCoplanarityTest(double d);
236  void setLambda(double a) { lambda = a; }
237  void setVvsEpsilon(const double eps)
238  {
239  if (eps >= 0) {
240  vvsEpsilon = eps;
241  } else {
242  throw vpException(vpException::badValue, "Epsilon value must be >= 0.");
243  }
244  }
245  void setVvsIterMax(int nb) { vvsIterMax = nb; }
246 
247  void setRansacNbInliersToReachConsensus(const unsigned int &nbC) { ransacNbInlierConsensus = nbC; }
248  void setRansacThreshold(const double &t)
249  {
250  // Test whether or not t is > 0
251  if (t > std::numeric_limits<double>::epsilon()) {
252  ransacThreshold = t;
253  } else {
254  throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
255  }
256  }
257  void setRansacMaxTrials(const int &rM) { ransacMaxTrials = rM; }
258  unsigned int getRansacNbInliers() const { return (unsigned int)ransacInliers.size(); }
259  std::vector<unsigned int> getRansacInlierIndex() const { return ransacInlierIndex; }
260  std::vector<vpPoint> getRansacInliers() const { return ransacInliers; }
261 
268  void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
269 
280  {
281  if (!computeCovariance)
282  vpTRACE("Warning : The covariance matrix has not been computed. See "
283  "setCovarianceComputation() to do it.");
284 
285  return covarianceMatrix;
286  }
287 
299  inline void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag) { ransacFlag = flag; }
300 
306  inline int getNbParallelRansacThreads() const { return nbParallelRansacThreads; }
307 
316  inline void setNbParallelRansacThreads(const int nb) { nbParallelRansacThreads = nb; }
317 
323  inline bool getUseParallelRansac() const { return useParallelRansac; }
324 
330  inline void setUseParallelRansac(const bool use) { useParallelRansac = use; }
331 
337  std::vector<vpPoint> getPoints() const
338  {
339  std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
340  return vectorOfPoints;
341  }
342 
343  static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
344  vpColor col = vpColor::none);
345  static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
346  vpColor col = vpColor::none);
347  static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx,
349 
350  static int computeRansacIterations(double probability, double epsilon, const int sampleSize = 4,
351  int maxIterations = 2000);
352 
353  static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
354  const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
355  unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
356  const int &maxNbTrials=10000, const bool useParallelRansac=true, const unsigned int nthreads=0,
357  bool (*func)(const vpHomogeneousMatrix &)=NULL);
358 };
359 
360 #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
std::vector< vpPoint > getPoints() const
Definition: vpPose.h:337
double residual
Residual in meter.
Definition: vpPose.h:110
bool getUseParallelRansac() const
Definition: vpPose.h:323
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
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:248
std::vector< unsigned int > getRansacInlierIndex() const
Definition: vpPose.h:259
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:108
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
int getNbParallelRansacThreads() const
Definition: vpPose.h:306
unsigned int getRansacNbInliers() const
Definition: vpPose.h:258
#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:316
Generic class defining intrinsic camera parameters.
vpMatrix getCovarianceMatrix() const
Definition: vpPose.h:279
void setVvsIterMax(int nb)
Definition: vpPose.h:245
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:107
void setRansacMaxTrials(const int &rM)
Definition: vpPose.h:257
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:247
void setLambda(double a)
Definition: vpPose.h:236
void setUseParallelRansac(const bool use)
Definition: vpPose.h:330
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
Definition: vpPose.h:299
vpPoseMethodType
Methods that could be used to estimate the pose from points.
Definition: vpPose.h:82
void setVvsEpsilon(const double eps)
Definition: vpPose.h:237
std::vector< vpPoint > getRansacInliers() const
Definition: vpPose.h:260
void setCovarianceComputation(const bool &flag)
Definition: vpPose.h:268
RANSAC_FILTER_FLAGS
Definition: vpPose.h:101