ViSP  2.9.0
vpPoseFeatures.h
1 /****************************************************************************
2  *
3  * $Id: vpPoseFeatures.h 4660 2014-02-09 14:13:27Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 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 from any features.
36  *
37  * Authors:
38  * Aurelien Yol
39  *
40  *****************************************************************************/
41 
50 #ifndef vpPoseFeatures_HH
51 #define vpPoseFeatures_HH
52 
53 #include <visp/vpDebug.h>
54 #include <visp/vpException.h>
55 #include <visp/vpExponentialMap.h>
56 #include <visp/vpFeatureBuilder.h>
57 #include <visp/vpBasicFeature.h>
58 #include <visp/vpFeaturePoint.h>
59 #include <visp/vpFeatureEllipse.h>
60 #include <visp/vpRobust.h>
61 #include <visp/vpForwardProjection.h>
62 #include <visp/vpPoint.h>
63 #include <visp/vpCircle.h>
64 #include <visp/vpSphere.h>
65 #include <visp/vpLine.h>
66 #include <visp/vpCylinder.h>
67 
68 #include <vector>
69 #include <iostream>
70 
71 #ifdef VISP_HAVE_CPP11_COMPATIBILITY
72 #include <tuple>
73 
74 #ifndef DOXYGEN_SHOULD_SKIP_THIS
75 //#################################################
76 //## Call a function with a tuple as parameters
77 //#################################################
78 template < unsigned int N >
79 struct vpDesiredFeatureBuilderWithTuple
80 {
81  template < typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
82  static void buildDesiredFeatureWithTuple( featureType &feature,
83  RetType (*f)( ArgsF... ),
84  const std::tuple<ArgsT...>& t,
85  Args &&... args )
86  {
87  vpDesiredFeatureBuilderWithTuple<N-1>::buildDesiredFeatureWithTuple( feature, f, t, std::get<N-1>( t ), args... );
88  }
89 };
90 
91 template <>
92 struct vpDesiredFeatureBuilderWithTuple<0>
93 {
94  template < typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
95  static void buildDesiredFeatureWithTuple( featureType &/* feature */,
96  RetType (*f)( ArgsF... ),
97  const std::tuple<ArgsT...>& /* t */,
98  Args&&... args )
99  {
100  f( args... );
101  }
102 };
103 
104 template <>
105 struct vpDesiredFeatureBuilderWithTuple<1>
106 {
107  template < typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
108  static void buildDesiredFeatureWithTuple( featureType &feature,
109  RetType (*f)( ArgsF... ),
110  const std::tuple<ArgsT...>& t,
111  Args&&... args )
112  {
113  vpDesiredFeatureBuilderWithTuple<0>::buildDesiredFeatureWithTuple( feature, f, t, feature, args... );
114  }
115 };
116 
117 template < typename featureType, typename RetType, typename... Args, typename... ArgsFunc >
118 void buildDesiredFeatureWithTuple( featureType &feature,
119  RetType (*f)(ArgsFunc...),
120  std::tuple<Args...> const& t )
121 {
122  vpDesiredFeatureBuilderWithTuple<sizeof...(Args)>::buildDesiredFeatureWithTuple( feature, f, t );
123 }
124 
125 //#################################################
126 //## Call a function with a tuple as parameters
127 //## Object Mode
128 //#################################################
129 
130 template < unsigned int N >
131 struct vpDesiredFeatureBuilderObjectWithTuple
132 {
133  template < typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
134  static void buildDesiredFeatureObjectWithTuple( objType *obj, featureType &feature,
135  RetType (objType::*f)( ArgsF... ),
136  const std::tuple<ArgsT...>& t,
137  Args &&... args )
138  {
139  vpDesiredFeatureBuilderObjectWithTuple<N-1>::buildDesiredFeatureObjectWithTuple( obj, feature, f, t, std::get<N-1>( t ), args... );
140  }
141 };
142 
143 template <>
144 struct vpDesiredFeatureBuilderObjectWithTuple<0>
145 {
146  template < typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
147  static void buildDesiredFeatureObjectWithTuple( objType *obj, featureType & /*feature*/,
148  RetType (objType::*f)( ArgsF... ),
149  const std::tuple<ArgsT...>& /* t */,
150  Args&&... args )
151  {
152  (obj->*f)( args... );
153  }
154 };
155 
156 template <>
157 struct vpDesiredFeatureBuilderObjectWithTuple<1>
158 {
159  template < typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
160  static void buildDesiredFeatureObjectWithTuple( objType *obj, featureType &feature,
161  RetType (objType::*f)( ArgsF... ),
162  const std::tuple<ArgsT...>& t,
163  Args&&... args )
164  {
165  vpDesiredFeatureBuilderObjectWithTuple<0>::buildDesiredFeatureObjectWithTuple( obj, feature, f, t, feature, args... );
166  }
167 };
168 
169 template < typename objType, typename featureType, typename RetType, typename... Args, typename... ArgsFunc >
170 void buildDesiredFeatureObjectWithTuple( objType *obj, featureType &feature,
171  RetType (objType::*f)(ArgsFunc...),
172  std::tuple<Args...> const& t )
173 {
174  vpDesiredFeatureBuilderObjectWithTuple<sizeof...(Args)>::buildDesiredFeatureObjectWithTuple( obj, feature, f, t );
175 }
176 
177 //#####################################################
178 //## Call un function with a tuple as parameters
179 //## Track all the parameters with the cMo
180 //## Except the first one (must be de "BasicFeature"
181 //#####################################################
182 
183 template < unsigned int N >
184 struct vpCurrentFeatureBuilderWithTuple
185 {
186  template < typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
187  static void buildCurrentFeatureWithTuple( featureType &feature,
188  const vpHomogeneousMatrix &cMo,
189  RetType (*f)(ArgsF...), std::tuple<ArgsTuple...>& t,
190  ArgsDecomposed &&... args )
191  {
192  auto proj = std::get<N-1>( t );
193  proj.track(cMo);
194  vpCurrentFeatureBuilderWithTuple<N-1>::buildCurrentFeatureWithTuple( feature, cMo, f, t, proj, args... );
195  }
196 };
197 
198 template <>
199 struct vpCurrentFeatureBuilderWithTuple<0>
200 {
201  template < typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
202  static void buildCurrentFeatureWithTuple( featureType &/*feature*/,
203  const vpHomogeneousMatrix & /*cMo*/,
204  RetType (*f)(ArgsF...),
205  std::tuple<ArgsTuple...>&,
206  ArgsDecomposed &&... args )
207  {
208  f( args... );
209  }
210 };
211 
212 template <>
213 struct vpCurrentFeatureBuilderWithTuple<1>
214 {
215  template < typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
216  static void buildCurrentFeatureWithTuple( featureType &feature,
217  const vpHomogeneousMatrix &cMo,
218  RetType (*f)(ArgsF...),
219  std::tuple<ArgsTuple...>&t,
220  ArgsDecomposed &&... args )
221  {
222  vpCurrentFeatureBuilderWithTuple<0>::buildCurrentFeatureWithTuple( feature, cMo, f, t, feature, args... );
223  }
224 };
225 
226 template < typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc >
227 void buildCurrentFeatureWithTuple( featureType &feature,
228  const vpHomogeneousMatrix &cMo,
229  RetType (*f)(ArgsFunc...),
230  std::tuple<ArgsTuple...> &t )
231 {
232  vpCurrentFeatureBuilderWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureWithTuple( feature, cMo, f, t );
233 }
234 
235 //#####################################################
236 //## Call un function with a tuple as parameters
237 //## Track all the parameters with the cMo
238 //## Except the first one (must be de "BasicFeature"
239 //## Object Mode
240 //#####################################################
241 
242 template < unsigned int N >
243 struct vpCurrentFeatureBuilderObjectWithTuple
244 {
245  template < typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
246  static void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &feature,
247  const vpHomogeneousMatrix &cMo,
248  RetType (objType::*f)(ArgsF...),
249  std::tuple<ArgsTuple...>& t,
250  ArgsDecomposed &&... args )
251  {
252  auto proj = std::get<N-1>( t );
253  proj.track(cMo);
254  vpCurrentFeatureBuilderObjectWithTuple<N-1>::buildCurrentFeatureObjectWithTuple( obj, feature, cMo, f, t, proj, args... );
255  }
256 };
257 
258 template <>
259 struct vpCurrentFeatureBuilderObjectWithTuple<0>
260 {
261  template < typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
262  static void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &/*feature*/,
263  const vpHomogeneousMatrix &/*cMo*/,
264  RetType (objType::*f)(ArgsF...),
265  std::tuple<ArgsTuple...>&,
266  ArgsDecomposed &&... args )
267  {
268  (obj->*f)( args... );
269  }
270 };
271 
272 template <>
273 struct vpCurrentFeatureBuilderObjectWithTuple<1>
274 {
275  template < typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
276  static void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &feature,
277  const vpHomogeneousMatrix &cMo,
278  RetType (objType::*f)(ArgsF...),
279  std::tuple<ArgsTuple...>&t,
280  ArgsDecomposed &&... args )
281  {
282  vpCurrentFeatureBuilderObjectWithTuple<0>::buildCurrentFeatureObjectWithTuple( obj, feature, cMo, f, t, feature, args... );
283  }
284 };
285 
286 template < typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc >
287 void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &feature,
288  const vpHomogeneousMatrix &cMo,
289  RetType (objType::*f)(ArgsFunc...),
290  std::tuple<ArgsTuple...> &t )
291 {
292  vpCurrentFeatureBuilderObjectWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureObjectWithTuple( obj, feature, cMo, f, t );
293 }
294 
295 //#################################################
296 //## Call that will be used in our vpPoseFeatures
297 //## to store the specific features.
298 //#################################################
304 class VISP_EXPORT vpPoseSpecificFeature
305 {
306 public:
307  vpPoseSpecificFeature(){}
308  virtual ~vpPoseSpecificFeature(){};
309 
310  virtual vpColVector error() = 0;
311  virtual vpMatrix currentInteraction() = 0;
312  virtual void createDesired() = 0;
313  virtual void createCurrent(const vpHomogeneousMatrix &cMo) = 0;
314 };
315 
316 //#################################################
317 //## Template for all kind of specific features
318 //#################################################
319 
325 template< typename featureType, typename RetType, typename ...Args >
326 class vpPoseSpecificFeatureTemplate : public vpPoseSpecificFeature
327 {
328 private:
329  featureType desiredFeature;
330  featureType currentFeature;
331  std::tuple<Args...> *tuple;
332  RetType (*func_ptr)(Args...);
333 
334 public:
335  vpPoseSpecificFeatureTemplate(RetType (*f_ptr)(Args...), Args &&...args)
336  {
337  func_ptr = f_ptr; //std::move(f_ptr);
338  tuple = new std::tuple<Args...>(args...);
339  }
340 
341  virtual ~vpPoseSpecificFeatureTemplate()
342  {
343  delete tuple;
344  };
345 
346  virtual void createDesired(){
347  buildDesiredFeatureWithTuple(desiredFeature, func_ptr, *tuple);
348  }
349 
350  virtual vpColVector error(){
351  //std::cout << "Getting S... : " << std::get<0>(*tuple).get_s() << std::endl;
352  return currentFeature.error(desiredFeature);
353  }
354 
355  virtual vpMatrix currentInteraction(){
356  return currentFeature.interaction();
357  }
358 
359  virtual void createCurrent(const vpHomogeneousMatrix &cMo){
360  buildCurrentFeatureWithTuple(currentFeature, cMo, func_ptr, *tuple);
361  }
362 };
363 
364 //#################################################
365 //## Template for all kind of specific features
366 //## Object Mode
367 //#################################################
368 
374 template< typename ObjectType, typename featureType, typename RetType, typename ...Args >
375 class vpPoseSpecificFeatureTemplateObject : public vpPoseSpecificFeature
376 {
377 private:
378  featureType desiredFeature;
379  featureType currentFeature;
380  std::tuple<Args...> *tuple;
381  RetType (ObjectType::*func_ptr)(Args...);
382  ObjectType* obj;
383 
384 public:
385  vpPoseSpecificFeatureTemplateObject(ObjectType *o, RetType (ObjectType::*f_ptr)(Args...), Args &&...args)
386  {
387  func_ptr = f_ptr; //std::move(f_ptr);
388  tuple = new std::tuple<Args...>(args...);
389  obj = o;
390  }
391 
392  virtual ~vpPoseSpecificFeatureTemplateObject()
393  {
394  delete tuple;
395  };
396 
397  virtual void createDesired(){
398  buildDesiredFeatureObjectWithTuple(obj, desiredFeature, func_ptr, *tuple);
399  }
400 
401  virtual vpColVector error(){
402  return currentFeature.error(desiredFeature);
403  }
404 
405  virtual vpMatrix currentInteraction(){
406  return currentFeature.interaction();
407  }
408 
409  virtual void createCurrent(const vpHomogeneousMatrix &cMo){
410  buildCurrentFeatureObjectWithTuple(obj, currentFeature, cMo, func_ptr, *tuple);
411  }
412 };
413 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
414 #endif //VISP_HAVE_CPP11_COMPATIBILITY
415 
424 class VISP_EXPORT vpPoseFeatures
425 {
426 public:
430  typedef enum
431  {
433  ROBUST_VIRTUAL_VS
434  } vpPoseFeaturesMethodType;
435 
436 private:
437 
438 #ifndef DOXYGEN_SHOULD_SKIP_THIS
439  template<typename FeatureType, typename FirstParamType>
440  struct vpDuo{
441  FeatureType *desiredFeature;
442  FirstParamType firstParam;
443  vpDuo() : desiredFeature(NULL), firstParam() {}
444  };
445 
446  template<typename FeatureType, typename FirstParamType, typename SecondParamType>
447  struct vpTrio{
448  FeatureType *desiredFeature;
449  FirstParamType firstParam;
450  SecondParamType secondParam;
451 
452  vpTrio() : desiredFeature(NULL), firstParam(), secondParam() {}
453  };
454 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
455 
456  unsigned int maxSize;
457  unsigned int totalSize;
458  unsigned int vvsIterMax;
459  double lambda;
460 
461  bool verbose;
462 
463  bool computeCovariance;
464  vpMatrix covarianceMatrix;
465 
466  //vpFeaturePoint
467  std::vector<vpDuo<vpFeaturePoint,vpPoint> > featurePoint_Point_list;
468  //vpFeaturePoint3D
469  std::vector<vpDuo<vpFeaturePoint3D,vpPoint> > featurePoint3D_Point_list;
470  //vpFeatureVanishingPoint
471  std::vector<vpDuo<vpFeatureVanishingPoint,vpPoint> > featureVanishingPoint_Point_list;
472  std::vector<vpTrio<vpFeatureVanishingPoint,vpLine,vpLine> > featureVanishingPoint_DuoLine_list;
473  //vpFeatureEllipse
474  std::vector<vpDuo<vpFeatureEllipse,vpSphere> > featureEllipse_Sphere_list;
475  std::vector<vpDuo<vpFeatureEllipse,vpCircle> > featureEllipse_Circle_list;
476  //vpFeatureLine
477  std::vector<vpDuo<vpFeatureLine,vpLine> > featureLine_Line_list;
478  std::vector<vpTrio<vpFeatureLine,vpCylinder,int> > featureLine_DuoLineInt_List;
479  //vpFeatureSegment
480  std::vector<vpTrio<vpFeatureSegment,vpPoint,vpPoint> > featureSegment_DuoPoints_list;
481 
482 #ifdef VISP_HAVE_CPP11_COMPATIBILITY
483  //Specific features
484  std::vector<vpPoseSpecificFeature*> featureSpecific_list;
485 #endif
486 
487 public:
488 
489  vpPoseFeatures();
490  virtual ~vpPoseFeatures();
491 
492  // ! Features addition
493  void addFeaturePoint(const vpPoint&);
494 
495  void addFeaturePoint3D(const vpPoint&);
496 
497  void addFeatureVanishingPoint(const vpPoint&);
498  void addFeatureVanishingPoint(const vpLine&, const vpLine&);
499 
500  void addFeatureEllipse(const vpCircle&);
501  void addFeatureEllipse(const vpSphere&);
502 
503  void addFeatureLine(const vpLine&);
504  void addFeatureLine(const vpCylinder&, const int &line);
505 
506  void addFeatureSegment(vpPoint &, vpPoint&);
507 
508 
509 #ifdef VISP_HAVE_CPP11_COMPATIBILITY
510  template<typename RetType, typename ...ArgsFunc, typename ...Args>
511  void addSpecificFeature(RetType (*fct_ptr)(ArgsFunc ...), Args &&...args);
512 
513  template<typename ObjType, typename RetType, typename ...ArgsFunc, typename ...Args>
514  void addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc ...), Args &&...args);
515 #endif
516 
517  void clear();
518 
519  // ! Pose computation
520  void computePose(vpHomogeneousMatrix & cMo, const vpPoseFeaturesMethodType &type = VIRTUAL_VS);
521 
528  if(!computeCovariance)
529  vpTRACE("Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it.");
530 
531  return covarianceMatrix;
532  }
533 
539  double getLambda(){ return lambda; }
540 
546  unsigned int getVVSIterMax(){ return vvsIterMax; }
547 
553  void setCovarianceComputation(const bool& flag) { computeCovariance = flag; }
554 
560  void setLambda(const double &val){ lambda = val; }
561 
567  void setVVSIterMax(const unsigned int &val){ vvsIterMax = val; }
568 
574  void setVerbose(const bool &mode){ verbose = mode; }
575 
576 
577 private:
578  void error_and_interaction(vpHomogeneousMatrix & cMo, vpColVector &err, vpMatrix &L);
579 
580  void computePoseVVS(vpHomogeneousMatrix & cMo);
581  void computePoseRobustVVS(vpHomogeneousMatrix & cMo);
582 };
583 
584 #ifdef VISP_HAVE_CPP11_COMPATIBILITY
585 
638 template< typename RetType, typename ...ArgsFunc, typename ...Args>
639 void vpPoseFeatures::addSpecificFeature(RetType (*fct_ptr)(ArgsFunc ...), Args &&...args)
640 {
641  typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
642  typedef typename std::remove_reference<featureTypeReference>::type featureType;
643  featureSpecific_list.push_back(
644  new vpPoseSpecificFeatureTemplate< featureType, RetType, ArgsFunc... >(fct_ptr,std::forward<ArgsFunc>(args)...)
645  );
646 
647  featureSpecific_list.back()->createDesired();
648 
649  totalSize++;
650  if(featureSpecific_list.size() > maxSize)
651  maxSize = featureSpecific_list.size();
652 }
653 
717 template< typename ObjType, typename RetType, typename ...ArgsFunc, typename ...Args>
718 void vpPoseFeatures::addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc ...), Args &&...args)
719 {
720  typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
721  typedef typename std::remove_reference<featureTypeReference>::type featureType;
722  featureSpecific_list.push_back(
723  new vpPoseSpecificFeatureTemplateObject< ObjType, featureType, RetType, ArgsFunc... >(obj, fct_ptr,std::forward<ArgsFunc>(args)...)
724  );
725 
726  featureSpecific_list.back()->createDesired();
727 
728  totalSize++;
729  if(featureSpecific_list.size() > maxSize)
730  maxSize = featureSpecific_list.size();
731 }
732 #endif
733 
734 
735 #endif
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
void setVerbose(const bool &mode)
void setVVSIterMax(const unsigned int &val)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpTRACE
Definition: vpDebug.h:418
void setLambda(const double &val)
Class that defines what is a sphere.
Definition: vpSphere.h:64
Class that defines what is a point.
Definition: vpPoint.h:65
Class that defines a line in the object frame, the camera frame and the image plane. All the parameters must be set in meter.
Definition: vpLine.h:124
double getLambda()
Class that defines what is a cylinder.
Definition: vpCylinder.h:97
void setCovarianceComputation(const bool &flag)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
vpMatrix getCovarianceMatrix() const
void addSpecificFeature(RetType(*fct_ptr)(ArgsFunc...), Args &&...args)
unsigned int getVVSIterMax()
Tools for pose computation from any feature.This class allows to estimate a pose by virtual visual se...
Class that defines what is a circle.
Definition: vpCircle.h:61