Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
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 #if defined(VISP_HAVE_MODULE_VISUAL_FEATURES) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
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 #include <tuple>
64 
65 #ifndef DOXYGEN_SHOULD_SKIP_THIS
66 //#################################################
67 //## Call a function with a tuple as parameters
68 //#################################################
69 template <unsigned int N> struct vpDesiredFeatureBuilderWithTuple
70 {
71  template <typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args>
72  static void buildDesiredFeatureWithTuple(featureType &feature, RetType(*f)(ArgsF...), const std::tuple<ArgsT...> &t,
73  Args &&...args)
74  {
75  vpDesiredFeatureBuilderWithTuple<N - 1>::buildDesiredFeatureWithTuple(feature, f, t, std::get<N - 1>(t), args...);
76  }
77 };
78 
79 template <> struct vpDesiredFeatureBuilderWithTuple<0>
80 {
81  template <typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args>
82  static void buildDesiredFeatureWithTuple(featureType & /* feature */, RetType(*f)(ArgsF...),
83  const std::tuple<ArgsT...> & /* t */, Args &&...args)
84  {
85  f(args...);
86  }
87 };
88 
89 template <> struct vpDesiredFeatureBuilderWithTuple<1>
90 {
91  template <typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args>
92  static void buildDesiredFeatureWithTuple(featureType &feature, RetType(*f)(ArgsF...), const std::tuple<ArgsT...> &t,
93  Args &&...args)
94  {
95  vpDesiredFeatureBuilderWithTuple<0>::buildDesiredFeatureWithTuple(feature, f, t, feature, args...);
96  }
97 };
98 
99 template <typename featureType, typename RetType, typename... Args, typename... ArgsFunc>
100 void buildDesiredFeatureWithTuple(featureType &feature, RetType(*f)(ArgsFunc...), std::tuple<Args...> const &t)
101 {
102  vpDesiredFeatureBuilderWithTuple<sizeof...(Args)>::buildDesiredFeatureWithTuple(feature, f, t);
103 }
104 
105 //#################################################
106 //## Call a function with a tuple as parameters
107 //## Object Mode
108 //#################################################
109 
110 template <unsigned int N> struct vpDesiredFeatureBuilderObjectWithTuple
111 {
112  template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
113  typename... Args>
114  static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType(objType:: *f)(ArgsF...),
115  const std::tuple<ArgsT...> &t, Args &&...args)
116  {
117  vpDesiredFeatureBuilderObjectWithTuple<N - 1>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t,
118  std::get<N - 1>(t), args...);
119  }
120 };
121 
122 template <> struct vpDesiredFeatureBuilderObjectWithTuple<0>
123 {
124  template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
125  typename... Args>
126  static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType & /*feature*/,
127  RetType(objType:: *f)(ArgsF...), const std::tuple<ArgsT...> & /* t */,
128  Args &&...args)
129  {
130  (obj->*f)(args...);
131  }
132 };
133 
134 template <> struct vpDesiredFeatureBuilderObjectWithTuple<1>
135 {
136  template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
137  typename... Args>
138  static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType(objType:: *f)(ArgsF...),
139  const std::tuple<ArgsT...> &t, Args &&...args)
140  {
141  vpDesiredFeatureBuilderObjectWithTuple<0>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t, feature, args...);
142  }
143 };
144 
145 template <typename objType, typename featureType, typename RetType, typename... Args, typename... ArgsFunc>
146 void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType(objType:: *f)(ArgsFunc...),
147  std::tuple<Args...> const &t)
148 {
149  vpDesiredFeatureBuilderObjectWithTuple<sizeof...(Args)>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t);
150 }
151 
152 //#####################################################
153 //## Call un function with a tuple as parameters
154 //## Track all the parameters with the cMo
155 //## Except the first one (must be de "BasicFeature"
156 //#####################################################
157 
158 template <unsigned int N> struct vpCurrentFeatureBuilderWithTuple
159 {
160  template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
161  typename... ArgsF>
162  static void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType(*f)(ArgsF...),
163  std::tuple<ArgsTuple...> &t, ArgsDecomposed &&...args)
164  {
165  auto proj = std::get<N - 1>(t);
166  proj.track(cMo);
167  vpCurrentFeatureBuilderWithTuple<N - 1>::buildCurrentFeatureWithTuple(feature, cMo, f, t, proj, args...);
168  }
169 };
170 
171 template <> struct vpCurrentFeatureBuilderWithTuple<0>
172 {
173  template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
174  typename... ArgsF>
175  static void buildCurrentFeatureWithTuple(featureType & /*feature*/, const vpHomogeneousMatrix & /*cMo*/,
176  RetType(*f)(ArgsF...), std::tuple<ArgsTuple...> &, ArgsDecomposed &&...args)
177  {
178  f(args...);
179  }
180 };
181 
182 template <> struct vpCurrentFeatureBuilderWithTuple<1>
183 {
184  template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
185  typename... ArgsF>
186  static void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType(*f)(ArgsF...),
187  std::tuple<ArgsTuple...> &t, ArgsDecomposed &&...args)
188  {
189  vpCurrentFeatureBuilderWithTuple<0>::buildCurrentFeatureWithTuple(feature, cMo, f, t, feature, args...);
190  }
191 };
192 
193 template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc>
194 void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType(*f)(ArgsFunc...),
195  std::tuple<ArgsTuple...> &t)
196 {
197  vpCurrentFeatureBuilderWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureWithTuple(feature, cMo, f, t);
198 }
199 
200 //#####################################################
201 //## Call un function with a tuple as parameters
202 //## Track all the parameters with the cMo
203 //## Except the first one (must be de "BasicFeature"
204 //## Object Mode
205 //#####################################################
206 
207 template <unsigned int N> struct vpCurrentFeatureBuilderObjectWithTuple
208 {
209  template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
210  typename... ArgsF>
211  static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
212  RetType(objType:: *f)(ArgsF...), std::tuple<ArgsTuple...> &t,
213  ArgsDecomposed &&...args)
214  {
215  auto proj = std::get<N - 1>(t);
216  proj.track(cMo);
217  vpCurrentFeatureBuilderObjectWithTuple<N - 1>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, proj,
218  args...);
219  }
220 };
221 
222 template <> struct vpCurrentFeatureBuilderObjectWithTuple<0>
223 {
224  template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
225  typename... ArgsF>
226  static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType & /*feature*/,
227  const vpHomogeneousMatrix & /*cMo*/, RetType(objType:: *f)(ArgsF...),
228  std::tuple<ArgsTuple...> &, ArgsDecomposed &&...args)
229  {
230  (obj->*f)(args...);
231  }
232 };
233 
234 template <> struct vpCurrentFeatureBuilderObjectWithTuple<1>
235 {
236  template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
237  typename... ArgsF>
238  static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
239  RetType(objType:: *f)(ArgsF...), std::tuple<ArgsTuple...> &t,
240  ArgsDecomposed &&...args)
241  {
242  vpCurrentFeatureBuilderObjectWithTuple<0>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, feature,
243  args...);
244  }
245 };
246 
247 template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc>
248 void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
249  RetType(objType:: *f)(ArgsFunc...), std::tuple<ArgsTuple...> &t)
250 {
251  vpCurrentFeatureBuilderObjectWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f,
252  t);
253 }
254 
255 //#################################################
256 //## Call that will be used in our vpPoseFeatures
257 //## to store the specific features.
258 //#################################################
265 class VISP_EXPORT vpPoseSpecificFeature
266 {
267 public:
268  vpPoseSpecificFeature() { }
269  virtual ~vpPoseSpecificFeature() { }
270 
271  virtual vpColVector error() = 0;
272  virtual vpMatrix currentInteraction() = 0;
273  virtual void createDesired() = 0;
274  virtual void createCurrent(const vpHomogeneousMatrix &cMo) = 0;
275 };
276 
277 //#################################################
278 //## Template for all kind of specific features
279 //#################################################
280 
287 template <typename featureType, typename RetType, typename... Args>
288 class vpPoseSpecificFeatureTemplate : public vpPoseSpecificFeature
289 {
290 private:
291  featureType m_desiredFeature;
292  featureType m_currentFeature;
293  std::tuple<Args...> *m_tuple;
294  RetType(*func_ptr)(Args...);
295 
296 public:
297  vpPoseSpecificFeatureTemplate(RetType(*f_ptr)(Args...), Args &&...args)
298  {
299  func_ptr = f_ptr; // std::move(f_ptr);
300  m_tuple = new std::tuple<Args...>(args...);
301  }
302  virtual ~vpPoseSpecificFeatureTemplate() vp_override
303  {
304  delete m_tuple;
305  }
306 
307  virtual void createDesired() vp_override
308  {
309  buildDesiredFeatureWithTuple(m_desiredFeature, func_ptr, *m_tuple);
310  }
311 
312  virtual vpColVector error() vp_override
313  {
314  // std::cout << "Getting S... : " << std::get<0>(*tuple).get_s() <<
315  // std::endl;
316  return m_currentFeature.error(m_desiredFeature);
317  }
318 
319  virtual vpMatrix currentInteraction() vp_override
320  {
321  return m_currentFeature.interaction();
322  }
323 
324  virtual void createCurrent(const vpHomogeneousMatrix &cMo) vp_override
325  {
326  buildCurrentFeatureWithTuple(m_currentFeature, cMo, func_ptr, *m_tuple);
327  }
328 };
329 
330 //#################################################
331 //## Template for all kind of specific features
332 //## Object Mode
333 //#################################################
334 
341 template <typename ObjectType, typename featureType, typename RetType, typename... Args>
342 class vpPoseSpecificFeatureTemplateObject : public vpPoseSpecificFeature
343 {
344 private:
345  featureType m_desiredFeature;
346  featureType m_currentFeature;
347  std::tuple<Args...> *m_tuple;
348  RetType(ObjectType:: *func_ptr)(Args...);
349  ObjectType *m_obj;
350 
351 public:
352  vpPoseSpecificFeatureTemplateObject(ObjectType *o, RetType(ObjectType:: *f_ptr)(Args...), Args &&...args)
353  {
354  func_ptr = f_ptr; // std::move(f_ptr);
355  m_tuple = new std::tuple<Args...>(args...);
356  m_obj = o;
357  }
358 
359  virtual ~vpPoseSpecificFeatureTemplateObject() vp_override
360  {
361  delete m_tuple;
362  }
363 
364  virtual void createDesired() vp_override
365  {
366  buildDesiredFeatureObjectWithTuple(m_obj, m_desiredFeature, func_ptr, *m_tuple);
367  }
368 
369  virtual vpColVector error() vp_override
370  {
371  return m_currentFeature.error(m_desiredFeature);
372  }
373 
374  virtual vpMatrix currentInteraction() vp_override
375  {
376  return m_currentFeature.interaction();
377  }
378 
379  virtual void createCurrent(const vpHomogeneousMatrix &cMo) vp_override
380  {
381  buildCurrentFeatureObjectWithTuple(m_obj, m_currentFeature, cMo, func_ptr, *m_tuple);
382  }
383 };
384 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
385 
396 class VISP_EXPORT vpPoseFeatures
397 {
398 public:
402  typedef enum
403  {
405  ROBUST_VIRTUAL_VS
406  } vpPoseFeaturesMethodType;
407 
411  vpPoseFeatures();
412 
416  virtual ~vpPoseFeatures();
417 
424  void addFeaturePoint(const vpPoint &p);
425 
432  void addFeaturePoint3D(const vpPoint &p);
433 
440  void addFeatureVanishingPoint(const vpPoint &p);
441 
449  void addFeatureVanishingPoint(const vpLine &l1, const vpLine &l2);
450 
457  void addFeatureEllipse(const vpCircle &);
458 
465  void addFeatureEllipse(const vpSphere &);
466 
473  void addFeatureLine(const vpLine &);
474 
483  void addFeatureLine(const vpCylinder &, const int &line);
484 
492  void addFeatureSegment(vpPoint &, vpPoint &);
493 
497  template <typename RetType, typename... ArgsFunc, typename... Args>
498  void addSpecificFeature(RetType(*fct_ptr)(ArgsFunc...), Args &&...args);
499 
503  template <typename ObjType, typename RetType, typename... ArgsFunc, typename... Args>
504  void addSpecificFeature(ObjType *obj, RetType(ObjType:: *fct_ptr)(ArgsFunc...), Args &&...args);
505 
509  void clear();
510 
524  void computePose(vpHomogeneousMatrix &cMo, const vpPoseFeaturesMethodType &type = VIRTUAL_VS);
525 
534  {
535  if (!m_computeCovariance)
536  vpTRACE("Warning : The covariance matrix has not been computed. See "
537  "setCovarianceComputation() to do it.");
538 
539  return m_covarianceMatrix;
540  }
541 
548  double getLambda() { return m_lambda; }
549 
556  unsigned int getVVSIterMax() { return m_vvsIterMax; }
557 
563  void setCovarianceComputation(const bool &flag) { m_computeCovariance = flag; }
564 
571  void setLambda(const double &val) { m_lambda = val; }
572 
578  void setVVSIterMax(const unsigned int &val) { m_vvsIterMax = val; }
579 
585  void setVerbose(const bool &mode) { m_verbose = mode; }
586 
587 private:
588 #ifndef DOXYGEN_SHOULD_SKIP_THIS
589  template <typename FeatureType, typename FirstParamType> struct vpDuo
590  {
591  FeatureType *desiredFeature;
592  FirstParamType firstParam;
593  vpDuo() : desiredFeature(nullptr), firstParam() { }
594  };
595 
596  template <typename FeatureType, typename FirstParamType, typename SecondParamType> struct vpTrio
597  {
598  FeatureType *desiredFeature;
599  FirstParamType firstParam;
600  SecondParamType secondParam;
601 
602  vpTrio() : desiredFeature(nullptr), firstParam(), secondParam() { }
603  };
604 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
605 
606  unsigned int m_maxSize;
607  unsigned int m_totalSize;
608  unsigned int m_vvsIterMax;
609  double m_lambda;
610 
611  bool m_verbose;
612 
613  bool m_computeCovariance;
614  vpMatrix m_covarianceMatrix;
615 
616  // vpFeaturePoint
617  std::vector<vpDuo<vpFeaturePoint, vpPoint> > m_featurePoint_Point_list;
618  // vpFeaturePoint3D
619  std::vector<vpDuo<vpFeaturePoint3D, vpPoint> > m_featurePoint3D_Point_list;
620  // vpFeatureVanishingPoint
621  std::vector<vpDuo<vpFeatureVanishingPoint, vpPoint> > m_featureVanishingPoint_Point_list;
622  std::vector<vpTrio<vpFeatureVanishingPoint, vpLine, vpLine> > m_featureVanishingPoint_DuoLine_list;
623  // vpFeatureEllipse
624  std::vector<vpDuo<vpFeatureEllipse, vpSphere> > m_featureEllipse_Sphere_list;
625  std::vector<vpDuo<vpFeatureEllipse, vpCircle> > m_featureEllipse_Circle_list;
626  // vpFeatureLine
627  std::vector<vpDuo<vpFeatureLine, vpLine> > m_featureLine_Line_list;
628  std::vector<vpTrio<vpFeatureLine, vpCylinder, int> > m_featureLine_DuoLineInt_List;
629  // vpFeatureSegment
630  std::vector<vpTrio<vpFeatureSegment, vpPoint, vpPoint> > m_featureSegment_DuoPoints_list;
631 
632 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
633  // Specific features
634  std::vector<vpPoseSpecificFeature *> m_featureSpecific_list;
635 #endif
636 
644  void error_and_interaction(vpHomogeneousMatrix &cMo, vpColVector &err, vpMatrix &L);
645 
653  void computePoseVVS(vpHomogeneousMatrix &cMo);
654 
661  void computePoseRobustVVS(vpHomogeneousMatrix &cMo);
662 };
663 
716 template <typename RetType, typename... ArgsFunc, typename... Args>
717 void vpPoseFeatures::addSpecificFeature(RetType(*fct_ptr)(ArgsFunc...), Args &&...args)
718 {
719  typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
720  typedef typename std::remove_reference<featureTypeReference>::type featureType;
721  m_featureSpecific_list.push_back(
722  new vpPoseSpecificFeatureTemplate<featureType, RetType, ArgsFunc...>(fct_ptr, std::forward<ArgsFunc>(args)...));
723 
724  m_featureSpecific_list.back()->createDesired();
725 
726  m_totalSize++;
727  if (m_featureSpecific_list.size() > m_maxSize)
728  m_maxSize = static_cast<unsigned int>(m_featureSpecific_list.size());
729 }
730 
795 template <typename ObjType, typename RetType, typename... ArgsFunc, typename... Args>
796 void vpPoseFeatures::addSpecificFeature(ObjType *obj, RetType(ObjType:: *fct_ptr)(ArgsFunc...), Args &&...args)
797 {
798  typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
799  typedef typename std::remove_reference<featureTypeReference>::type featureType;
800  m_featureSpecific_list.push_back(new vpPoseSpecificFeatureTemplateObject<ObjType, featureType, RetType, ArgsFunc...>(
801  obj, fct_ptr, std::forward<ArgsFunc>(args)...));
802 
803  m_featureSpecific_list.back()->createDesired();
804 
805  m_totalSize++;
806  if (m_featureSpecific_list.size() > m_maxSize)
807  m_maxSize = static_cast<unsigned int>(m_featureSpecific_list.size());
808 }
809 #endif
810 
811 #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 addSpecificFeature(RetType(*fct_ptr)(ArgsFunc...), Args &&...args)
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