ViSP  2.8.0
vpPose.h
1 /****************************************************************************
2  *
3  * $Id: vpPose.h 4303 2013-07-04 14:14:00Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Pose computation.
36  *
37  * Authors:
38  * Eric Marchand
39  * Francois Chaumette
40  * Aurelien Yol
41  *
42  *****************************************************************************/
43 
52 #ifndef vpPOSE_HH
53 #define vpPOSE_HH
54 
55 #include <visp/vpHomogeneousMatrix.h>
56 #include <visp/vpHomography.h>
57 #include <visp/vpPoint.h>
58 #include <visp/vpRGBa.h>
59 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
60 # include <visp/vpList.h>
61 #endif
62 
63 #include <math.h>
64 #include <list>
65 #include <vector>
66 
78 class VISP_EXPORT vpPose
79 {
80 public:
81  typedef enum
82  {
85  LOWE ,
91  LAGRANGE_VIRTUAL_VS
92  } vpPoseMethodType;
93 
94  unsigned int npt ;
95  std::list<vpPoint> listP ;
96 
97  double residual ;
98 
99 protected :
100  double lambda ;
101 
102 private:
103  int vvsIterMax ;
104  vpPoint *c3d ;
107  bool computeCovariance;
109  vpMatrix covarianceMatrix;
110 
111  unsigned int ransacNbInlierConsensus;
112  int ransacMaxTrials;
113  std::vector<vpPoint> ransacInliers;
114  double ransacThreshold;
115 
116 protected:
117  double computeResidualDementhon(const vpHomogeneousMatrix &cMo) ;
118 
119  // method used in poseDementhonPlan()
120  int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo) ;
121 
122 public:
124  vpPose() ;
126  virtual ~vpPose() ;
128  void addPoint(const vpPoint& P) ;
130  void clearPoint() ;
131 
133  void computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo) ;
136  double computeResidual(const vpHomogeneousMatrix &cMo) const ;
138  bool coplanar(int &coplanar_plane_type) ;
139  void displayModel(vpImage<unsigned char> &I,
140  vpCameraParameters &cam,
141  vpColor col=vpColor::none) ;
142  void displayModel(vpImage<vpRGBa> &I,
143  vpCameraParameters &cam,
144  vpColor col=vpColor::none) ;
146  void init() ;
148  void poseDementhonPlan(vpHomogeneousMatrix &cMo) ;
150  void poseDementhonNonPlan(vpHomogeneousMatrix &cMo) ;
152  void poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type=0) ;
154  void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) ;
157  void poseLowe(vpHomogeneousMatrix & cMo) ;
159  void poseRansac(vpHomogeneousMatrix & cMo) ;
161  void poseVirtualVSrobust(vpHomogeneousMatrix & cMo) ;
163  void poseVirtualVS(vpHomogeneousMatrix & cMo) ;
164  void printPoint() ;
165  void setDistanceToPlaneForCoplanarityTest(double d) ;
166  void setLambda(double a) { lambda = a ; }
167  void setVvsIterMax(int nb) { vvsIterMax = nb ; }
168 
169  void setRansacNbInliersToReachConsensus(const unsigned int &nbC){ ransacNbInlierConsensus = nbC; }
170  void setRansacThreshold(const double &t){ ransacThreshold = t; }
171  void setRansacMaxTrials(const int &rM){ ransacMaxTrials = rM; }
172  unsigned int getRansacNbInliers(){ return (unsigned int) ransacInliers.size(); }
173  std::vector<vpPoint> getRansacInliers(){ return ransacInliers; }
174 
180  void setCovarianceComputation(const bool& flag) { computeCovariance = flag; }
181 
190  if(!computeCovariance)
191  vpTRACE("Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it.");
192 
193  return covarianceMatrix;
194  }
195 
196  static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo,
197  vpCameraParameters &cam, double size,
198  vpColor col=vpColor::none) ;
199  static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo,
200  vpCameraParameters &cam, double size,
201  vpColor col=vpColor::none) ;
202  static double poseFromRectangle(vpPoint &p1,vpPoint &p2,
203  vpPoint &p3,vpPoint &p4,
204  double lx, vpCameraParameters & cam,
205  vpHomogeneousMatrix & cMo) ;
206 
207  static void findMatch(std::vector<vpPoint> &p2D,
208  std::vector<vpPoint> &p3D,
209  const unsigned int &numberOfInlierToReachAConsensus,
210  const double &threshold,
211  unsigned int &ninliers,
212  std::vector<vpPoint> &listInliers,
213  vpHomogeneousMatrix &cMo,
214  const int &maxNbTrials = 10000);
215 
216 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
217 private:
221  static void initRansac(const unsigned int n,
222  const double *x, const double *y,
223  const unsigned int m,
224  const double *X, const double *Y, const double *Z,
225  vpColVector &data) ;
226 
227 public:
228  static void computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M) ;
229 
230  static double computeResidual(const vpColVector &x, const vpColVector &M, vpColVector &d) ;
231 
232  static bool degenerateConfiguration(vpColVector &x, unsigned int *ind) ;
233 
234  static void ransac(const unsigned int n,
235  const double *x, const double *y,
236  const unsigned int m,
237  const double *X, const double *Y, const double *Z,
238  const int numberOfInlierToReachAConsensus,
239  const double threshold,
240  unsigned int &ninliers,
241  vpColVector &xi, vpColVector &yi,
242  vpColVector &Xi, vpColVector &Yi, vpColVector &Zi,
243  vpHomogeneousMatrix &cMo, const int maxNbTrials = 10000) ;
244 
245  vp_deprecated static void ransac(const unsigned int n,
246  const vpPoint *p,
247  const unsigned int m,
248  const vpPoint *P,
249  const int numberOfInlierToReachAConsensus,
250  const double threshold,
251  unsigned int &ninliers,
252  std::list<vpPoint> &Pi,
253  vpHomogeneousMatrix &cMo, const int maxNbTrials = 10000) ;
254 
255  vp_deprecated static void ransac(std::list<vpPoint> &p,
256  std::list<vpPoint> &P,
257  const int numberOfInlierToReachAConsensus,
258  const double threshold,
259  unsigned int &ninliers,
260  std::list<vpPoint> &lPi,
261  vpHomogeneousMatrix &cMo, const int maxNbTrials = 10000) ;
262 #endif
263 } ;
264 
265 
266 #endif
267 
268 
269 /*
270  * Local variables:
271  * c-basic-offset: 2
272  * End:
273  */
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
double residual
compute the residual in meter
Definition: vpPose.h:97
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpTRACE
Definition: vpDebug.h:401
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
static const vpColor none
Definition: vpColor.h:179
void setRansacThreshold(const double &t)
Definition: vpPose.h:170
std::list< vpPoint > listP
array of point (use here class vpPoint)
Definition: vpPose.h:95
Class that defines what is a point.
Definition: vpPoint.h:65
double lambda
parameters use for the virtual visual servoing approach
Definition: vpPose.h:100
std::vector< vpPoint > getRansacInliers()
Definition: vpPose.h:173
Class used for pose computation from N points (pose from point only).
Definition: vpPose.h:78
Generic class defining intrinsic camera parameters.
double distanceToPlaneForCoplanarityTest
Definition: vpPose.h:145
vpMatrix getCovarianceMatrix() const
Definition: vpPose.h:189
void setVvsIterMax(int nb)
Definition: vpPose.h:167
unsigned int npt
number of point used in pose computation
Definition: vpPose.h:94
void setRansacMaxTrials(const int &rM)
Definition: vpPose.h:171
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:169
void setLambda(double a)
Definition: vpPose.h:166
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
unsigned int getRansacNbInliers()
Definition: vpPose.h:172
vpPoseMethodType
Definition: vpPose.h:81
void setCovarianceComputation(const bool &flag)
Definition: vpPose.h:180