Visual Servoing Platform  version 3.3.1 under development (2020-12-02)
vpPoseFeatures.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Pose computation from any features.
33  *
34  * Authors:
35  * Aurelien Yol
36  *
37  *****************************************************************************/
38 
47 #ifndef vpPoseFeatures_HH
48 #define vpPoseFeatures_HH
49 
50 #include <visp3/core/vpConfig.h>
51 
52 #ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
53 
54 #include <visp3/core/vpCircle.h>
55 #include <visp3/core/vpCylinder.h>
56 #include <visp3/core/vpDebug.h>
57 #include <visp3/core/vpException.h>
58 #include <visp3/core/vpExponentialMap.h>
59 #include <visp3/core/vpForwardProjection.h>
60 #include <visp3/core/vpLine.h>
61 #include <visp3/core/vpPoint.h>
62 #include <visp3/core/vpRobust.h>
63 #include <visp3/core/vpSphere.h>
64 #include <visp3/visual_features/vpBasicFeature.h>
65 #include <visp3/visual_features/vpFeatureBuilder.h>
66 #include <visp3/visual_features/vpFeatureEllipse.h>
67 #include <visp3/visual_features/vpFeaturePoint.h>
68 
69 #include <iostream>
70 #include <vector>
71 
72 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
73 #include <tuple>
74 
75 #ifndef DOXYGEN_SHOULD_SKIP_THIS
76 //#################################################
77 //## Call a function with a tuple as parameters
78 //#################################################
79 template <unsigned int N> struct vpDesiredFeatureBuilderWithTuple {
80  template <typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args>
81  static void buildDesiredFeatureWithTuple(featureType &feature, RetType (*f)(ArgsF...), const std::tuple<ArgsT...> &t,
82  Args &&... args)
83  {
84  vpDesiredFeatureBuilderWithTuple<N - 1>::buildDesiredFeatureWithTuple(feature, f, t, std::get<N - 1>(t), args...);
85  }
86 };
87 
88 template <> struct vpDesiredFeatureBuilderWithTuple<0> {
89  template <typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args>
90  static void buildDesiredFeatureWithTuple(featureType & /* feature */, RetType (*f)(ArgsF...),
91  const std::tuple<ArgsT...> & /* t */, Args &&... args)
92  {
93  f(args...);
94  }
95 };
96 
97 template <> struct vpDesiredFeatureBuilderWithTuple<1> {
98  template <typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args>
99  static void buildDesiredFeatureWithTuple(featureType &feature, RetType (*f)(ArgsF...), const std::tuple<ArgsT...> &t,
100  Args &&... args)
101  {
102  vpDesiredFeatureBuilderWithTuple<0>::buildDesiredFeatureWithTuple(feature, f, t, feature, args...);
103  }
104 };
105 
106 template <typename featureType, typename RetType, typename... Args, typename... ArgsFunc>
107 void buildDesiredFeatureWithTuple(featureType &feature, RetType (*f)(ArgsFunc...), std::tuple<Args...> const &t)
108 {
109  vpDesiredFeatureBuilderWithTuple<sizeof...(Args)>::buildDesiredFeatureWithTuple(feature, f, t);
110 }
111 
112 //#################################################
113 //## Call a function with a tuple as parameters
114 //## Object Mode
115 //#################################################
116 
117 template <unsigned int N> struct vpDesiredFeatureBuilderObjectWithTuple {
118  template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
119  typename... Args>
120  static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType (objType::*f)(ArgsF...),
121  const std::tuple<ArgsT...> &t, Args &&... args)
122  {
123  vpDesiredFeatureBuilderObjectWithTuple<N - 1>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t,
124  std::get<N - 1>(t), args...);
125  }
126 };
127 
128 template <> struct vpDesiredFeatureBuilderObjectWithTuple<0> {
129  template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
130  typename... Args>
131  static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType & /*feature*/,
132  RetType (objType::*f)(ArgsF...), const std::tuple<ArgsT...> & /* t */,
133  Args &&... args)
134  {
135  (obj->*f)(args...);
136  }
137 };
138 
139 template <> struct vpDesiredFeatureBuilderObjectWithTuple<1> {
140  template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
141  typename... Args>
142  static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType (objType::*f)(ArgsF...),
143  const std::tuple<ArgsT...> &t, Args &&... args)
144  {
145  vpDesiredFeatureBuilderObjectWithTuple<0>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t, feature, args...);
146  }
147 };
148 
149 template <typename objType, typename featureType, typename RetType, typename... Args, typename... ArgsFunc>
150 void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType (objType::*f)(ArgsFunc...),
151  std::tuple<Args...> const &t)
152 {
153  vpDesiredFeatureBuilderObjectWithTuple<sizeof...(Args)>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t);
154 }
155 
156 //#####################################################
157 //## Call un function with a tuple as parameters
158 //## Track all the parameters with the cMo
159 //## Except the first one (must be de "BasicFeature"
160 //#####################################################
161 
162 template <unsigned int N> struct vpCurrentFeatureBuilderWithTuple {
163  template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
164  typename... ArgsF>
165  static void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsF...),
166  std::tuple<ArgsTuple...> &t, ArgsDecomposed &&... args)
167  {
168  auto proj = std::get<N - 1>(t);
169  proj.track(cMo);
170  vpCurrentFeatureBuilderWithTuple<N - 1>::buildCurrentFeatureWithTuple(feature, cMo, f, t, proj, args...);
171  }
172 };
173 
174 template <> struct vpCurrentFeatureBuilderWithTuple<0> {
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...> &,
179  ArgsDecomposed &&... args)
180  {
181  f(args...);
182  }
183 };
184 
185 template <> struct vpCurrentFeatureBuilderWithTuple<1> {
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  template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
211  typename... ArgsF>
212  static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
213  RetType (objType::*f)(ArgsF...), std::tuple<ArgsTuple...> &t,
214  ArgsDecomposed &&... args)
215  {
216  auto proj = std::get<N - 1>(t);
217  proj.track(cMo);
218  vpCurrentFeatureBuilderObjectWithTuple<N - 1>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, proj,
219  args...);
220  }
221 };
222 
223 template <> struct vpCurrentFeatureBuilderObjectWithTuple<0> {
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  template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
236  typename... ArgsF>
237  static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
238  RetType (objType::*f)(ArgsF...), std::tuple<ArgsTuple...> &t,
239  ArgsDecomposed &&... args)
240  {
241  vpCurrentFeatureBuilderObjectWithTuple<0>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, feature,
242  args...);
243  }
244 };
245 
246 template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc>
247 void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
248  RetType (objType::*f)(ArgsFunc...), std::tuple<ArgsTuple...> &t)
249 {
250  vpCurrentFeatureBuilderObjectWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f,
251  t);
252 }
253 
254 //#################################################
255 //## Call that will be used in our vpPoseFeatures
256 //## to store the specific features.
257 //#################################################
264 class VISP_EXPORT vpPoseSpecificFeature
265 {
266 public:
267  vpPoseSpecificFeature() {}
268  virtual ~vpPoseSpecificFeature() {}
269 
270  virtual vpColVector error() = 0;
271  virtual vpMatrix currentInteraction() = 0;
272  virtual void createDesired() = 0;
273  virtual void createCurrent(const vpHomogeneousMatrix &cMo) = 0;
274 };
275 
276 //#################################################
277 //## Template for all kind of specific features
278 //#################################################
279 
286 template <typename featureType, typename RetType, typename... Args>
287 class vpPoseSpecificFeatureTemplate : public vpPoseSpecificFeature
288 {
289 private:
290  featureType desiredFeature;
291  featureType currentFeature;
292  std::tuple<Args...> *tuple;
293  RetType (*func_ptr)(Args...);
294 
295 public:
296  vpPoseSpecificFeatureTemplate(RetType (*f_ptr)(Args...), Args &&... args)
297  {
298  func_ptr = f_ptr; // std::move(f_ptr);
299  tuple = new std::tuple<Args...>(args...);
300  }
301 
302  virtual ~vpPoseSpecificFeatureTemplate() { delete tuple; }
303 
304  virtual void createDesired() { buildDesiredFeatureWithTuple(desiredFeature, func_ptr, *tuple); }
305 
306  virtual vpColVector error()
307  {
308  // std::cout << "Getting S... : " << std::get<0>(*tuple).get_s() <<
309  // std::endl;
310  return currentFeature.error(desiredFeature);
311  }
312 
313  virtual vpMatrix currentInteraction() { return currentFeature.interaction(); }
314 
315  virtual void createCurrent(const vpHomogeneousMatrix &cMo)
316  {
317  buildCurrentFeatureWithTuple(currentFeature, cMo, func_ptr, *tuple);
318  }
319 };
320 
321 //#################################################
322 //## Template for all kind of specific features
323 //## Object Mode
324 //#################################################
325 
332 template <typename ObjectType, typename featureType, typename RetType, typename... Args>
333 class vpPoseSpecificFeatureTemplateObject : public vpPoseSpecificFeature
334 {
335 private:
336  featureType desiredFeature;
337  featureType currentFeature;
338  std::tuple<Args...> *tuple;
339  RetType (ObjectType::*func_ptr)(Args...);
340  ObjectType *obj;
341 
342 public:
343  vpPoseSpecificFeatureTemplateObject(ObjectType *o, RetType (ObjectType::*f_ptr)(Args...), Args &&... args)
344  {
345  func_ptr = f_ptr; // std::move(f_ptr);
346  tuple = new std::tuple<Args...>(args...);
347  obj = o;
348  }
349 
350  virtual ~vpPoseSpecificFeatureTemplateObject() { delete tuple; }
351 
352  virtual void createDesired() { buildDesiredFeatureObjectWithTuple(obj, desiredFeature, func_ptr, *tuple); }
353 
354  virtual vpColVector error() { return currentFeature.error(desiredFeature); }
355 
356  virtual vpMatrix currentInteraction() { return currentFeature.interaction(); }
357 
358  virtual void createCurrent(const vpHomogeneousMatrix &cMo)
359  {
360  buildCurrentFeatureObjectWithTuple(obj, currentFeature, cMo, func_ptr, *tuple);
361  }
362 };
363 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
364 #endif // (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
365 
376 class VISP_EXPORT vpPoseFeatures
377 {
378 public:
382  typedef enum {
384  ROBUST_VIRTUAL_VS
386 
387 private:
388 #ifndef DOXYGEN_SHOULD_SKIP_THIS
389  template <typename FeatureType, typename FirstParamType> struct vpDuo {
390  FeatureType *desiredFeature;
391  FirstParamType firstParam;
392  vpDuo() : desiredFeature(NULL), firstParam() {}
393  };
394 
395  template <typename FeatureType, typename FirstParamType, typename SecondParamType> struct vpTrio {
396  FeatureType *desiredFeature;
397  FirstParamType firstParam;
398  SecondParamType secondParam;
399 
400  vpTrio() : desiredFeature(NULL), firstParam(), secondParam() {}
401  };
402 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
403 
404  unsigned int maxSize;
405  unsigned int totalSize;
406  unsigned int vvsIterMax;
407  double lambda;
408 
409  bool verbose;
410 
411  bool computeCovariance;
412  vpMatrix covarianceMatrix;
413 
414  // vpFeaturePoint
415  std::vector<vpDuo<vpFeaturePoint, vpPoint> > featurePoint_Point_list;
416  // vpFeaturePoint3D
417  std::vector<vpDuo<vpFeaturePoint3D, vpPoint> > featurePoint3D_Point_list;
418  // vpFeatureVanishingPoint
419  std::vector<vpDuo<vpFeatureVanishingPoint, vpPoint> > featureVanishingPoint_Point_list;
420  std::vector<vpTrio<vpFeatureVanishingPoint, vpLine, vpLine> > featureVanishingPoint_DuoLine_list;
421  // vpFeatureEllipse
422  std::vector<vpDuo<vpFeatureEllipse, vpSphere> > featureEllipse_Sphere_list;
423  std::vector<vpDuo<vpFeatureEllipse, vpCircle> > featureEllipse_Circle_list;
424  // vpFeatureLine
425  std::vector<vpDuo<vpFeatureLine, vpLine> > featureLine_Line_list;
426  std::vector<vpTrio<vpFeatureLine, vpCylinder, int> > featureLine_DuoLineInt_List;
427  // vpFeatureSegment
428  std::vector<vpTrio<vpFeatureSegment, vpPoint, vpPoint> > featureSegment_DuoPoints_list;
429 
430 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
431  // Specific features
432  std::vector<vpPoseSpecificFeature *> featureSpecific_list;
433 #endif
434 
435 public:
436  vpPoseFeatures();
437  virtual ~vpPoseFeatures();
438 
439  // ! Features addition
440  void addFeaturePoint(const vpPoint &);
441 
442  void addFeaturePoint3D(const vpPoint &);
443 
444  void addFeatureVanishingPoint(const vpPoint &);
445  void addFeatureVanishingPoint(const vpLine &, const vpLine &);
446 
447  void addFeatureEllipse(const vpCircle &);
448  void addFeatureEllipse(const vpSphere &);
449 
450  void addFeatureLine(const vpLine &);
451  void addFeatureLine(const vpCylinder &, const int &line);
452 
453  void addFeatureSegment(vpPoint &, vpPoint &);
454 
455 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
456  template <typename RetType, typename... ArgsFunc, typename... Args>
457  void addSpecificFeature(RetType (*fct_ptr)(ArgsFunc...), Args &&... args);
458 
459  template <typename ObjType, typename RetType, typename... ArgsFunc, typename... Args>
460  void addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc...), Args &&... args);
461 #endif
462 
463  void clear();
464 
465  // ! Pose computation
466  void computePose(vpHomogeneousMatrix &cMo, const vpPoseFeaturesMethodType &type = VIRTUAL_VS);
467 
476  {
477  if (!computeCovariance)
478  vpTRACE("Warning : The covariance matrix has not been computed. See "
479  "setCovarianceComputation() to do it.");
480 
481  return covarianceMatrix;
482  }
483 
490  double getLambda() { return lambda; }
491 
498  unsigned int getVVSIterMax() { return vvsIterMax; }
499 
505  void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
506 
513  void setLambda(const double &val) { lambda = val; }
514 
520  void setVVSIterMax(const unsigned int &val) { vvsIterMax = val; }
521 
527  void setVerbose(const bool &mode) { verbose = mode; }
528 
529 private:
530  void error_and_interaction(vpHomogeneousMatrix &cMo, vpColVector &err, vpMatrix &L);
531 
532  void computePoseVVS(vpHomogeneousMatrix &cMo);
533  void computePoseRobustVVS(vpHomogeneousMatrix &cMo);
534 };
535 
536 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
537 
591 template <typename RetType, typename... ArgsFunc, typename... Args>
592 void vpPoseFeatures::addSpecificFeature(RetType (*fct_ptr)(ArgsFunc...), Args &&... args)
593 {
594  typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
595  typedef typename std::remove_reference<featureTypeReference>::type featureType;
596  featureSpecific_list.push_back(
597  new vpPoseSpecificFeatureTemplate<featureType, RetType, ArgsFunc...>(fct_ptr, std::forward<ArgsFunc>(args)...));
598 
599  featureSpecific_list.back()->createDesired();
600 
601  totalSize++;
602  if (featureSpecific_list.size() > maxSize)
603  maxSize = static_cast<unsigned int>(featureSpecific_list.size());
604 }
605 
672 template <typename ObjType, typename RetType, typename... ArgsFunc, typename... Args>
673 void vpPoseFeatures::addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc...), Args &&... args)
674 {
675  typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
676  typedef typename std::remove_reference<featureTypeReference>::type featureType;
677  featureSpecific_list.push_back(new vpPoseSpecificFeatureTemplateObject<ObjType, featureType, RetType, ArgsFunc...>(
678  obj, fct_ptr, std::forward<ArgsFunc>(args)...));
679 
680  featureSpecific_list.back()->createDesired();
681 
682  totalSize++;
683  if (featureSpecific_list.size() > maxSize)
684  maxSize = static_cast<unsigned int>(featureSpecific_list.size());
685 }
686 #endif
687 
688 #endif //#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
689 
690 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
void setVerbose(const bool &mode)
void setVVSIterMax(const unsigned int &val)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setLambda(const double &val)
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
Definition: vpSphere.h:83
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition: vpLine.h:104
#define vpTRACE
Definition: vpDebug.h:416
vpMatrix getCovarianceMatrix() const
double getLambda()
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition: vpCylinder.h:102
void setCovarianceComputation(const bool &flag)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
unsigned int getVVSIterMax()
Tools for pose computation from any feature.This class allows to estimate a pose by virtual visual se...
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:91
void addSpecificFeature(RetType(*fct_ptr)(ArgsFunc...), Args &&... args)