Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpPoseFeatures.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
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 https://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 from any features.
32  */
33 
39 #ifndef vpPoseFeatures_HH
40 #define vpPoseFeatures_HH
41 
42 #include <visp3/core/vpConfig.h>
43 
44 #ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
45 
46 #include <visp3/core/vpCircle.h>
47 #include <visp3/core/vpCylinder.h>
48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpException.h>
50 #include <visp3/core/vpExponentialMap.h>
51 #include <visp3/core/vpForwardProjection.h>
52 #include <visp3/core/vpLine.h>
53 #include <visp3/core/vpPoint.h>
54 #include <visp3/core/vpRobust.h>
55 #include <visp3/core/vpSphere.h>
56 #include <visp3/visual_features/vpBasicFeature.h>
57 #include <visp3/visual_features/vpFeatureBuilder.h>
58 #include <visp3/visual_features/vpFeatureEllipse.h>
59 #include <visp3/visual_features/vpFeaturePoint.h>
60 
61 #include <iostream>
62 #include <vector>
63 
64 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
65 #include <tuple>
66 
67 #ifndef DOXYGEN_SHOULD_SKIP_THIS
68 //#################################################
69 //## Call a function with a tuple as parameters
70 //#################################################
71 template <unsigned int N> struct vpDesiredFeatureBuilderWithTuple
72 {
73  template <typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args>
74  static void buildDesiredFeatureWithTuple(featureType &feature, RetType(*f)(ArgsF...), const std::tuple<ArgsT...> &t,
75  Args &&...args)
76  {
77  vpDesiredFeatureBuilderWithTuple<N - 1>::buildDesiredFeatureWithTuple(feature, f, t, std::get<N - 1>(t), args...);
78  }
79 };
80 
81 template <> struct vpDesiredFeatureBuilderWithTuple<0>
82 {
83  template <typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args>
84  static void buildDesiredFeatureWithTuple(featureType & /* feature */, RetType(*f)(ArgsF...),
85  const std::tuple<ArgsT...> & /* t */, Args &&...args)
86  {
87  f(args...);
88  }
89 };
90 
91 template <> struct vpDesiredFeatureBuilderWithTuple<1>
92 {
93  template <typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args>
94  static void buildDesiredFeatureWithTuple(featureType &feature, RetType(*f)(ArgsF...), const std::tuple<ArgsT...> &t,
95  Args &&...args)
96  {
97  vpDesiredFeatureBuilderWithTuple<0>::buildDesiredFeatureWithTuple(feature, f, t, feature, args...);
98  }
99 };
100 
101 template <typename featureType, typename RetType, typename... Args, typename... ArgsFunc>
102 void buildDesiredFeatureWithTuple(featureType &feature, RetType(*f)(ArgsFunc...), std::tuple<Args...> const &t)
103 {
104  vpDesiredFeatureBuilderWithTuple<sizeof...(Args)>::buildDesiredFeatureWithTuple(feature, f, t);
105 }
106 
107 //#################################################
108 //## Call a function with a tuple as parameters
109 //## Object Mode
110 //#################################################
111 
112 template <unsigned int N> struct vpDesiredFeatureBuilderObjectWithTuple
113 {
114  template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
115  typename... Args>
116  static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType(objType:: *f)(ArgsF...),
117  const std::tuple<ArgsT...> &t, Args &&...args)
118  {
119  vpDesiredFeatureBuilderObjectWithTuple<N - 1>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t,
120  std::get<N - 1>(t), args...);
121  }
122 };
123 
124 template <> struct vpDesiredFeatureBuilderObjectWithTuple<0>
125 {
126  template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
127  typename... Args>
128  static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType & /*feature*/,
129  RetType(objType:: *f)(ArgsF...), const std::tuple<ArgsT...> & /* t */,
130  Args &&...args)
131  {
132  (obj->*f)(args...);
133  }
134 };
135 
136 template <> struct vpDesiredFeatureBuilderObjectWithTuple<1>
137 {
138  template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
139  typename... Args>
140  static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType(objType:: *f)(ArgsF...),
141  const std::tuple<ArgsT...> &t, Args &&...args)
142  {
143  vpDesiredFeatureBuilderObjectWithTuple<0>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t, feature, args...);
144  }
145 };
146 
147 template <typename objType, typename featureType, typename RetType, typename... Args, typename... ArgsFunc>
148 void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType(objType:: *f)(ArgsFunc...),
149  std::tuple<Args...> const &t)
150 {
151  vpDesiredFeatureBuilderObjectWithTuple<sizeof...(Args)>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t);
152 }
153 
154 //#####################################################
155 //## Call un function with a tuple as parameters
156 //## Track all the parameters with the cMo
157 //## Except the first one (must be de "BasicFeature"
158 //#####################################################
159 
160 template <unsigned int N> struct vpCurrentFeatureBuilderWithTuple
161 {
162  template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
163  typename... ArgsF>
164  static void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType(*f)(ArgsF...),
165  std::tuple<ArgsTuple...> &t, ArgsDecomposed &&...args)
166  {
167  auto proj = std::get<N - 1>(t);
168  proj.track(cMo);
169  vpCurrentFeatureBuilderWithTuple<N - 1>::buildCurrentFeatureWithTuple(feature, cMo, f, t, proj, args...);
170  }
171 };
172 
173 template <> struct vpCurrentFeatureBuilderWithTuple<0>
174 {
175  template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
176  typename... ArgsF>
177  static void buildCurrentFeatureWithTuple(featureType & /*feature*/, const vpHomogeneousMatrix & /*cMo*/,
178  RetType(*f)(ArgsF...), std::tuple<ArgsTuple...> &, ArgsDecomposed &&...args)
179  {
180  f(args...);
181  }
182 };
183 
184 template <> struct vpCurrentFeatureBuilderWithTuple<1>
185 {
186  template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
187  typename... ArgsF>
188  static void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType(*f)(ArgsF...),
189  std::tuple<ArgsTuple...> &t, ArgsDecomposed &&...args)
190  {
191  vpCurrentFeatureBuilderWithTuple<0>::buildCurrentFeatureWithTuple(feature, cMo, f, t, feature, args...);
192  }
193 };
194 
195 template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc>
196 void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType(*f)(ArgsFunc...),
197  std::tuple<ArgsTuple...> &t)
198 {
199  vpCurrentFeatureBuilderWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureWithTuple(feature, cMo, f, t);
200 }
201 
202 //#####################################################
203 //## Call un function with a tuple as parameters
204 //## Track all the parameters with the cMo
205 //## Except the first one (must be de "BasicFeature"
206 //## Object Mode
207 //#####################################################
208 
209 template <unsigned int N> struct vpCurrentFeatureBuilderObjectWithTuple
210 {
211  template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
212  typename... ArgsF>
213  static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
214  RetType(objType:: *f)(ArgsF...), std::tuple<ArgsTuple...> &t,
215  ArgsDecomposed &&...args)
216  {
217  auto proj = std::get<N - 1>(t);
218  proj.track(cMo);
219  vpCurrentFeatureBuilderObjectWithTuple<N - 1>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, proj,
220  args...);
221  }
222 };
223 
224 template <> struct vpCurrentFeatureBuilderObjectWithTuple<0>
225 {
226  template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
227  typename... ArgsF>
228  static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType & /*feature*/,
229  const vpHomogeneousMatrix & /*cMo*/, RetType(objType:: *f)(ArgsF...),
230  std::tuple<ArgsTuple...> &, ArgsDecomposed &&...args)
231  {
232  (obj->*f)(args...);
233  }
234 };
235 
236 template <> struct vpCurrentFeatureBuilderObjectWithTuple<1>
237 {
238  template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
239  typename... ArgsF>
240  static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
241  RetType(objType:: *f)(ArgsF...), std::tuple<ArgsTuple...> &t,
242  ArgsDecomposed &&...args)
243  {
244  vpCurrentFeatureBuilderObjectWithTuple<0>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, feature,
245  args...);
246  }
247 };
248 
249 template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc>
250 void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
251  RetType(objType:: *f)(ArgsFunc...), std::tuple<ArgsTuple...> &t)
252 {
253  vpCurrentFeatureBuilderObjectWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f,
254  t);
255 }
256 
257 //#################################################
258 //## Call that will be used in our vpPoseFeatures
259 //## to store the specific features.
260 //#################################################
267 class VISP_EXPORT vpPoseSpecificFeature
268 {
269 public:
270  vpPoseSpecificFeature() { }
271  virtual ~vpPoseSpecificFeature() { }
272 
273  virtual vpColVector error() = 0;
274  virtual vpMatrix currentInteraction() = 0;
275  virtual void createDesired() = 0;
276  virtual void createCurrent(const vpHomogeneousMatrix &cMo) = 0;
277 };
278 
279 //#################################################
280 //## Template for all kind of specific features
281 //#################################################
282 
289 template <typename featureType, typename RetType, typename... Args>
290 class vpPoseSpecificFeatureTemplate : public vpPoseSpecificFeature
291 {
292 private:
293  featureType m_desiredFeature;
294  featureType m_currentFeature;
295  std::tuple<Args...> *m_tuple;
296  RetType(*func_ptr)(Args...);
297 
298 public:
299  vpPoseSpecificFeatureTemplate(RetType(*f_ptr)(Args...), Args &&...args)
300  {
301  func_ptr = f_ptr; // std::move(f_ptr);
302  m_tuple = new std::tuple<Args...>(args...);
303  }
304  virtual ~vpPoseSpecificFeatureTemplate() vp_override
305  {
306  delete m_tuple;
307  }
308 
309  virtual void createDesired() vp_override
310  {
311  buildDesiredFeatureWithTuple(m_desiredFeature, func_ptr, *m_tuple);
312  }
313 
314  virtual vpColVector error() vp_override
315  {
316  // std::cout << "Getting S... : " << std::get<0>(*tuple).get_s() <<
317  // std::endl;
318  return m_currentFeature.error(m_desiredFeature);
319  }
320 
321  virtual vpMatrix currentInteraction() vp_override
322  {
323  return m_currentFeature.interaction();
324  }
325 
326  virtual void createCurrent(const vpHomogeneousMatrix &cMo) vp_override
327  {
328  buildCurrentFeatureWithTuple(m_currentFeature, cMo, func_ptr, *m_tuple);
329  }
330 };
331 
332 //#################################################
333 //## Template for all kind of specific features
334 //## Object Mode
335 //#################################################
336 
343 template <typename ObjectType, typename featureType, typename RetType, typename... Args>
344 class vpPoseSpecificFeatureTemplateObject : public vpPoseSpecificFeature
345 {
346 private:
347  featureType m_desiredFeature;
348  featureType m_currentFeature;
349  std::tuple<Args...> *m_tuple;
350  RetType(ObjectType:: *func_ptr)(Args...);
351  ObjectType *m_obj;
352 
353 public:
354  vpPoseSpecificFeatureTemplateObject(ObjectType *o, RetType(ObjectType:: *f_ptr)(Args...), Args &&...args)
355  {
356  func_ptr = f_ptr; // std::move(f_ptr);
357  m_tuple = new std::tuple<Args...>(args...);
358  m_obj = o;
359  }
360 
361  virtual ~vpPoseSpecificFeatureTemplateObject() vp_override
362  {
363  delete m_tuple;
364  }
365 
366  virtual void createDesired() vp_override
367  {
368  buildDesiredFeatureObjectWithTuple(m_obj, m_desiredFeature, func_ptr, *m_tuple);
369  }
370 
371  virtual vpColVector error() vp_override
372  {
373  return m_currentFeature.error(m_desiredFeature);
374  }
375 
376  virtual vpMatrix currentInteraction() vp_override
377  {
378  return m_currentFeature.interaction();
379  }
380 
381  virtual void createCurrent(const vpHomogeneousMatrix &cMo) vp_override
382  {
383  buildCurrentFeatureObjectWithTuple(m_obj, m_currentFeature, cMo, func_ptr, *m_tuple);
384  }
385 };
386 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
387 #endif // (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
388 
399 class VISP_EXPORT vpPoseFeatures
400 {
401 public:
405  typedef enum
406  {
408  ROBUST_VIRTUAL_VS
409  } vpPoseFeaturesMethodType;
410 
414  vpPoseFeatures();
415 
419  virtual ~vpPoseFeatures();
420 
427  void addFeaturePoint(const vpPoint &p);
428 
435  void addFeaturePoint3D(const vpPoint &p);
436 
443  void addFeatureVanishingPoint(const vpPoint &p);
444 
452  void addFeatureVanishingPoint(const vpLine &l1, const vpLine &l2);
453 
460  void addFeatureEllipse(const vpCircle &);
461 
468  void addFeatureEllipse(const vpSphere &);
469 
476  void addFeatureLine(const vpLine &);
477 
486  void addFeatureLine(const vpCylinder &, const int &line);
487 
495  void addFeatureSegment(vpPoint &, vpPoint &);
496 
497 #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher
501  template <typename RetType, typename... ArgsFunc, typename... Args>
502  void addSpecificFeature(RetType(*fct_ptr)(ArgsFunc...), Args &&...args);
503 
507  template <typename ObjType, typename RetType, typename... ArgsFunc, typename... Args>
508  void addSpecificFeature(ObjType *obj, RetType(ObjType:: *fct_ptr)(ArgsFunc...), Args &&...args);
509 #endif
510 
514  void clear();
515 
529  void computePose(vpHomogeneousMatrix &cMo, const vpPoseFeaturesMethodType &type = VIRTUAL_VS);
530 
539  {
540  if (!m_computeCovariance)
541  vpTRACE("Warning : The covariance matrix has not been computed. See "
542  "setCovarianceComputation() to do it.");
543 
544  return m_covarianceMatrix;
545  }
546 
553  double getLambda() { return m_lambda; }
554 
561  unsigned int getVVSIterMax() { return m_vvsIterMax; }
562 
568  void setCovarianceComputation(const bool &flag) { m_computeCovariance = flag; }
569 
576  void setLambda(const double &val) { m_lambda = val; }
577 
583  void setVVSIterMax(const unsigned int &val) { m_vvsIterMax = val; }
584 
590  void setVerbose(const bool &mode) { m_verbose = mode; }
591 
592 private:
593 #ifndef DOXYGEN_SHOULD_SKIP_THIS
594  template <typename FeatureType, typename FirstParamType> struct vpDuo
595  {
596  FeatureType *desiredFeature;
597  FirstParamType firstParam;
598  vpDuo() : desiredFeature(nullptr), firstParam() { }
599  };
600 
601  template <typename FeatureType, typename FirstParamType, typename SecondParamType> struct vpTrio
602  {
603  FeatureType *desiredFeature;
604  FirstParamType firstParam;
605  SecondParamType secondParam;
606 
607  vpTrio() : desiredFeature(nullptr), firstParam(), secondParam() { }
608  };
609 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
610 
611  unsigned int m_maxSize;
612  unsigned int m_totalSize;
613  unsigned int m_vvsIterMax;
614  double m_lambda;
615 
616  bool m_verbose;
617 
618  bool m_computeCovariance;
619  vpMatrix m_covarianceMatrix;
620 
621  // vpFeaturePoint
622  std::vector<vpDuo<vpFeaturePoint, vpPoint> > m_featurePoint_Point_list;
623  // vpFeaturePoint3D
624  std::vector<vpDuo<vpFeaturePoint3D, vpPoint> > m_featurePoint3D_Point_list;
625  // vpFeatureVanishingPoint
626  std::vector<vpDuo<vpFeatureVanishingPoint, vpPoint> > m_featureVanishingPoint_Point_list;
627  std::vector<vpTrio<vpFeatureVanishingPoint, vpLine, vpLine> > m_featureVanishingPoint_DuoLine_list;
628  // vpFeatureEllipse
629  std::vector<vpDuo<vpFeatureEllipse, vpSphere> > m_featureEllipse_Sphere_list;
630  std::vector<vpDuo<vpFeatureEllipse, vpCircle> > m_featureEllipse_Circle_list;
631  // vpFeatureLine
632  std::vector<vpDuo<vpFeatureLine, vpLine> > m_featureLine_Line_list;
633  std::vector<vpTrio<vpFeatureLine, vpCylinder, int> > m_featureLine_DuoLineInt_List;
634  // vpFeatureSegment
635  std::vector<vpTrio<vpFeatureSegment, vpPoint, vpPoint> > m_featureSegment_DuoPoints_list;
636 
637 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
638  // Specific features
639  std::vector<vpPoseSpecificFeature *> m_featureSpecific_list;
640 #endif
641 
649  void error_and_interaction(vpHomogeneousMatrix &cMo, vpColVector &err, vpMatrix &L);
650 
658  void computePoseVVS(vpHomogeneousMatrix &cMo);
659 
666  void computePoseRobustVVS(vpHomogeneousMatrix &cMo);
667 };
668 
669 #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher
722 template <typename RetType, typename... ArgsFunc, typename... Args>
723 void vpPoseFeatures::addSpecificFeature(RetType(*fct_ptr)(ArgsFunc...), Args &&...args)
724 {
725  typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
726  typedef typename std::remove_reference<featureTypeReference>::type featureType;
727  m_featureSpecific_list.push_back(
728  new vpPoseSpecificFeatureTemplate<featureType, RetType, ArgsFunc...>(fct_ptr, std::forward<ArgsFunc>(args)...));
729 
730  m_featureSpecific_list.back()->createDesired();
731 
732  m_totalSize++;
733  if (m_featureSpecific_list.size() > m_maxSize)
734  m_maxSize = static_cast<unsigned int>(m_featureSpecific_list.size());
735 }
736 
801 template <typename ObjType, typename RetType, typename... ArgsFunc, typename... Args>
802 void vpPoseFeatures::addSpecificFeature(ObjType *obj, RetType(ObjType:: *fct_ptr)(ArgsFunc...), Args &&...args)
803 {
804  typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
805  typedef typename std::remove_reference<featureTypeReference>::type featureType;
806  m_featureSpecific_list.push_back(new vpPoseSpecificFeatureTemplateObject<ObjType, featureType, RetType, ArgsFunc...>(
807  obj, fct_ptr, std::forward<ArgsFunc>(args)...));
808 
809  m_featureSpecific_list.back()->createDesired();
810 
811  m_totalSize++;
812  if (m_featureSpecific_list.size() > m_maxSize)
813  m_maxSize = static_cast<unsigned int>(m_featureSpecific_list.size());
814 }
815 #endif
816 #endif //#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
817 
818 #endif
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:86
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition: vpCylinder.h:99
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition: vpLine.h:101
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
Tools for pose computation from any feature.
void setVerbose(const bool &mode)
void setVVSIterMax(const unsigned int &val)
double getLambda()
void setCovarianceComputation(const bool &flag)
unsigned int getVVSIterMax()
void setLambda(const double &val)
vpMatrix getCovarianceMatrix() const
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
Definition: vpSphere.h:78
#define vpTRACE
Definition: vpDebug.h:405