Visual Servoing Platform  version 3.0.0
vpPose.h
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Pose computation.
32  *
33  * Authors:
34  * Eric Marchand
35  * Francois Chaumette
36  * Aurelien Yol
37  *
38  *****************************************************************************/
39 
48 #ifndef vpPOSE_HH
49 #define vpPOSE_HH
50 
51 #include <visp3/core/vpHomogeneousMatrix.h>
52 #include <visp3/vision/vpHomography.h>
53 #include <visp3/core/vpPoint.h>
54 #include <visp3/core/vpRGBa.h>
55 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
56 # include <visp3/core/vpList.h>
57 #endif
58 
59 #include <math.h>
60 #include <list>
61 #include <vector>
62 
74 class VISP_EXPORT vpPose
75 {
76 public:
77  typedef enum
78  {
81  LOWE ,
87  LAGRANGE_VIRTUAL_VS
88  } vpPoseMethodType;
89 
90  unsigned int npt ;
91  std::list<vpPoint> listP ;
92 
93  double residual ;
94 
95 protected :
96  double lambda ;
97 
98 private:
99  int vvsIterMax ;
100  std::vector<vpPoint> c3d ;
103  bool computeCovariance;
105  vpMatrix covarianceMatrix;
106 
107  unsigned int ransacNbInlierConsensus;
108  int ransacMaxTrials;
109  std::vector<vpPoint> ransacInliers;
110  std::vector<unsigned int> ransacInlierIndex;
111  double ransacThreshold;
112 
113 protected:
114  double computeResidualDementhon(const vpHomogeneousMatrix &cMo) ;
115 
116  // method used in poseDementhonPlan()
117  int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo) ;
118 
119 public:
120  // constructor
121  vpPose() ;
123  virtual ~vpPose() ;
125  void addPoint(const vpPoint& P) ;
127  void clearPoint() ;
128 
130  bool computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo, bool (*func)(vpHomogeneousMatrix *)=NULL) ;
133  double computeResidual(const vpHomogeneousMatrix &cMo) const ;
135  bool coplanar(int &coplanar_plane_type) ;
136  void displayModel(vpImage<unsigned char> &I,
137  vpCameraParameters &cam,
138  vpColor col=vpColor::none) ;
139  void displayModel(vpImage<vpRGBa> &I,
140  vpCameraParameters &cam,
141  vpColor col=vpColor::none) ;
143  void init() ;
145  void poseDementhonPlan(vpHomogeneousMatrix &cMo) ;
147  void poseDementhonNonPlan(vpHomogeneousMatrix &cMo) ;
149  void poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type=0) ;
151  void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) ;
154  void poseLowe(vpHomogeneousMatrix & cMo) ;
156  bool poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMatrix *)=NULL) ;
158  void poseVirtualVSrobust(vpHomogeneousMatrix & cMo) ;
160  void poseVirtualVS(vpHomogeneousMatrix & cMo) ;
161  void printPoint() ;
162  void setDistanceToPlaneForCoplanarityTest(double d) ;
163  void setLambda(double a) { lambda = a ; }
164  void setVvsIterMax(int nb) { vvsIterMax = nb ; }
165 
166  void setRansacNbInliersToReachConsensus(const unsigned int &nbC){ ransacNbInlierConsensus = nbC; }
167  void setRansacThreshold(const double &t) {
168  //Test whether or not t is > 0
169  if(t > 0) {
170  ransacThreshold = t;
171  } else {
172  throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
173  }
174  }
175  void setRansacMaxTrials(const int &rM){ ransacMaxTrials = rM; }
176  unsigned int getRansacNbInliers() const { return (unsigned int) ransacInliers.size(); }
177  std::vector<unsigned int> getRansacInlierIndex() const{ return ransacInlierIndex; }
178  std::vector<vpPoint> getRansacInliers() const{ return ransacInliers; }
179 
185  void setCovarianceComputation(const bool& flag) { computeCovariance = flag; }
186 
195  if(!computeCovariance)
196  vpTRACE("Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it.");
197 
198  return covarianceMatrix;
199  }
200 
201  static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo,
202  vpCameraParameters &cam, double size,
203  vpColor col=vpColor::none) ;
204  static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo,
205  vpCameraParameters &cam, double size,
206  vpColor col=vpColor::none) ;
207  static double poseFromRectangle(vpPoint &p1,vpPoint &p2,
208  vpPoint &p3,vpPoint &p4,
209  double lx, vpCameraParameters & cam,
210  vpHomogeneousMatrix & cMo) ;
211 
212  static void findMatch(std::vector<vpPoint> &p2D,
213  std::vector<vpPoint> &p3D,
214  const unsigned int &numberOfInlierToReachAConsensus,
215  const double &threshold,
216  unsigned int &ninliers,
217  std::vector<vpPoint> &listInliers,
218  vpHomogeneousMatrix &cMo,
219  const int &maxNbTrials = 10000);
220 } ;
221 
222 
223 #endif
224 
225 
226 /*
227  * Local variables:
228  * c-basic-offset: 2
229  * End:
230  */
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
double residual
compute the residual in meter
Definition: vpPose.h:93
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
static const vpColor none
Definition: vpColor.h:175
error that can be emited by ViSP classes.
Definition: vpException.h:73
void setRansacThreshold(const double &t)
Definition: vpPose.h:167
std::vector< unsigned int > getRansacInlierIndex() const
Definition: vpPose.h:177
std::list< vpPoint > listP
array of point (use here class vpPoint)
Definition: vpPose.h:91
Class that defines what is a point.
Definition: vpPoint.h:59
double lambda
parameters use for the virtual visual servoing approach
Definition: vpPose.h:96
unsigned int getRansacNbInliers() const
Definition: vpPose.h:176
#define vpTRACE
Definition: vpDebug.h:414
Class used for pose computation from N points (pose from point only).
Definition: vpPose.h:74
Generic class defining intrinsic camera parameters.
double distanceToPlaneForCoplanarityTest
Definition: vpPose.h:142
vpMatrix getCovarianceMatrix() const
Definition: vpPose.h:194
void setVvsIterMax(int nb)
Definition: vpPose.h:164
unsigned int npt
number of point used in pose computation
Definition: vpPose.h:90
void setRansacMaxTrials(const int &rM)
Definition: vpPose.h:175
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:166
void setLambda(double a)
Definition: vpPose.h:163
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpPoseMethodType
Definition: vpPose.h:77
std::vector< vpPoint > getRansacInliers() const
Definition: vpPose.h:178
void setCovarianceComputation(const bool &flag)
Definition: vpPose.h:185