Visual Servoing Platform  version 3.5.1 under development (2022-05-28)
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...> &, ArgsDecomposed &&...args)
179  {
180  f(args...);
181  }
182 };
183 
184 template <> struct vpCurrentFeatureBuilderWithTuple<1> {
185  template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
186  typename... ArgsF>
187  static void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsF...),
188  std::tuple<ArgsTuple...> &t, ArgsDecomposed &&...args)
189  {
190  vpCurrentFeatureBuilderWithTuple<0>::buildCurrentFeatureWithTuple(feature, cMo, f, t, feature, args...);
191  }
192 };
193 
194 template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc>
195 void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsFunc...),
196  std::tuple<ArgsTuple...> &t)
197 {
198  vpCurrentFeatureBuilderWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureWithTuple(feature, cMo, f, t);
199 }
200 
201 //#####################################################
202 //## Call un function with a tuple as parameters
203 //## Track all the parameters with the cMo
204 //## Except the first one (must be de "BasicFeature"
205 //## Object Mode
206 //#####################################################
207 
208 template <unsigned int N> struct vpCurrentFeatureBuilderObjectWithTuple {
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  template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
224  typename... ArgsF>
225  static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType & /*feature*/,
226  const vpHomogeneousMatrix & /*cMo*/, RetType (objType::*f)(ArgsF...),
227  std::tuple<ArgsTuple...> &, ArgsDecomposed &&...args)
228  {
229  (obj->*f)(args...);
230  }
231 };
232 
233 template <> struct vpCurrentFeatureBuilderObjectWithTuple<1> {
234  template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
235  typename... ArgsF>
236  static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
237  RetType (objType::*f)(ArgsF...), std::tuple<ArgsTuple...> &t,
238  ArgsDecomposed &&...args)
239  {
240  vpCurrentFeatureBuilderObjectWithTuple<0>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, feature,
241  args...);
242  }
243 };
244 
245 template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc>
246 void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
247  RetType (objType::*f)(ArgsFunc...), std::tuple<ArgsTuple...> &t)
248 {
249  vpCurrentFeatureBuilderObjectWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f,
250  t);
251 }
252 
253 //#################################################
254 //## Call that will be used in our vpPoseFeatures
255 //## to store the specific features.
256 //#################################################
263 class VISP_EXPORT vpPoseSpecificFeature
264 {
265 public:
266  vpPoseSpecificFeature() {}
267  virtual ~vpPoseSpecificFeature() {}
268 
269  virtual vpColVector error() = 0;
270  virtual vpMatrix currentInteraction() = 0;
271  virtual void createDesired() = 0;
272  virtual void createCurrent(const vpHomogeneousMatrix &cMo) = 0;
273 };
274 
275 //#################################################
276 //## Template for all kind of specific features
277 //#################################################
278 
285 template <typename featureType, typename RetType, typename... Args>
286 class vpPoseSpecificFeatureTemplate : public vpPoseSpecificFeature
287 {
288 private:
289  featureType desiredFeature;
290  featureType currentFeature;
291  std::tuple<Args...> *tuple;
292  RetType (*func_ptr)(Args...);
293 
294 public:
295  vpPoseSpecificFeatureTemplate(RetType (*f_ptr)(Args...), Args &&...args)
296  {
297  func_ptr = f_ptr; // std::move(f_ptr);
298  tuple = new std::tuple<Args...>(args...);
299  }
300 
301  virtual ~vpPoseSpecificFeatureTemplate() { delete tuple; }
302 
303  virtual void createDesired() { buildDesiredFeatureWithTuple(desiredFeature, func_ptr, *tuple); }
304 
305  virtual vpColVector error()
306  {
307  // std::cout << "Getting S... : " << std::get<0>(*tuple).get_s() <<
308  // std::endl;
309  return currentFeature.error(desiredFeature);
310  }
311 
312  virtual vpMatrix currentInteraction() { return currentFeature.interaction(); }
313 
314  virtual void createCurrent(const vpHomogeneousMatrix &cMo)
315  {
316  buildCurrentFeatureWithTuple(currentFeature, cMo, func_ptr, *tuple);
317  }
318 };
319 
320 //#################################################
321 //## Template for all kind of specific features
322 //## Object Mode
323 //#################################################
324 
331 template <typename ObjectType, typename featureType, typename RetType, typename... Args>
332 class vpPoseSpecificFeatureTemplateObject : public vpPoseSpecificFeature
333 {
334 private:
335  featureType desiredFeature;
336  featureType currentFeature;
337  std::tuple<Args...> *tuple;
338  RetType (ObjectType::*func_ptr)(Args...);
339  ObjectType *obj;
340 
341 public:
342  vpPoseSpecificFeatureTemplateObject(ObjectType *o, RetType (ObjectType::*f_ptr)(Args...), Args &&...args)
343  {
344  func_ptr = f_ptr; // std::move(f_ptr);
345  tuple = new std::tuple<Args...>(args...);
346  obj = o;
347  }
348 
349  virtual ~vpPoseSpecificFeatureTemplateObject() { delete tuple; }
350 
351  virtual void createDesired() { buildDesiredFeatureObjectWithTuple(obj, desiredFeature, func_ptr, *tuple); }
352 
353  virtual vpColVector error() { return currentFeature.error(desiredFeature); }
354 
355  virtual vpMatrix currentInteraction() { return currentFeature.interaction(); }
356 
357  virtual void createCurrent(const vpHomogeneousMatrix &cMo)
358  {
359  buildCurrentFeatureObjectWithTuple(obj, currentFeature, cMo, func_ptr, *tuple);
360  }
361 };
362 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
363 #endif // (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
364 
375 class VISP_EXPORT vpPoseFeatures
376 {
377 public:
381  typedef enum {
383  ROBUST_VIRTUAL_VS
385 
386 private:
387 #ifndef DOXYGEN_SHOULD_SKIP_THIS
388  template <typename FeatureType, typename FirstParamType> struct vpDuo {
389  FeatureType *desiredFeature;
390  FirstParamType firstParam;
391  vpDuo() : desiredFeature(NULL), firstParam() {}
392  };
393 
394  template <typename FeatureType, typename FirstParamType, typename SecondParamType> struct vpTrio {
395  FeatureType *desiredFeature;
396  FirstParamType firstParam;
397  SecondParamType secondParam;
398 
399  vpTrio() : desiredFeature(NULL), firstParam(), secondParam() {}
400  };
401 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
402 
403  unsigned int maxSize;
404  unsigned int totalSize;
405  unsigned int vvsIterMax;
406  double lambda;
407 
408  bool verbose;
409 
410  bool computeCovariance;
411  vpMatrix covarianceMatrix;
412 
413  // vpFeaturePoint
414  std::vector<vpDuo<vpFeaturePoint, vpPoint> > featurePoint_Point_list;
415  // vpFeaturePoint3D
416  std::vector<vpDuo<vpFeaturePoint3D, vpPoint> > featurePoint3D_Point_list;
417  // vpFeatureVanishingPoint
418  std::vector<vpDuo<vpFeatureVanishingPoint, vpPoint> > featureVanishingPoint_Point_list;
419  std::vector<vpTrio<vpFeatureVanishingPoint, vpLine, vpLine> > featureVanishingPoint_DuoLine_list;
420  // vpFeatureEllipse
421  std::vector<vpDuo<vpFeatureEllipse, vpSphere> > featureEllipse_Sphere_list;
422  std::vector<vpDuo<vpFeatureEllipse, vpCircle> > featureEllipse_Circle_list;
423  // vpFeatureLine
424  std::vector<vpDuo<vpFeatureLine, vpLine> > featureLine_Line_list;
425  std::vector<vpTrio<vpFeatureLine, vpCylinder, int> > featureLine_DuoLineInt_List;
426  // vpFeatureSegment
427  std::vector<vpTrio<vpFeatureSegment, vpPoint, vpPoint> > featureSegment_DuoPoints_list;
428 
429 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
430  // Specific features
431  std::vector<vpPoseSpecificFeature *> featureSpecific_list;
432 #endif
433 
434 public:
435  vpPoseFeatures();
436  virtual ~vpPoseFeatures();
437 
438  // ! Features addition
439  void addFeaturePoint(const vpPoint &);
440 
441  void addFeaturePoint3D(const vpPoint &);
442 
443  void addFeatureVanishingPoint(const vpPoint &);
444  void addFeatureVanishingPoint(const vpLine &, const vpLine &);
445 
446  void addFeatureEllipse(const vpCircle &);
447  void addFeatureEllipse(const vpSphere &);
448 
449  void addFeatureLine(const vpLine &);
450  void addFeatureLine(const vpCylinder &, const int &line);
451 
452  void addFeatureSegment(vpPoint &, vpPoint &);
453 
454 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
455  template <typename RetType, typename... ArgsFunc, typename... Args>
456  void addSpecificFeature(RetType (*fct_ptr)(ArgsFunc...), Args &&...args);
457 
458  template <typename ObjType, typename RetType, typename... ArgsFunc, typename... Args>
459  void addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc...), Args &&...args);
460 #endif
461 
462  void clear();
463 
464  // ! Pose computation
465  void computePose(vpHomogeneousMatrix &cMo, const vpPoseFeaturesMethodType &type = VIRTUAL_VS);
466 
475  {
476  if (!computeCovariance)
477  vpTRACE("Warning : The covariance matrix has not been computed. See "
478  "setCovarianceComputation() to do it.");
479 
480  return covarianceMatrix;
481  }
482 
489  double getLambda() { return lambda; }
490 
497  unsigned int getVVSIterMax() { return vvsIterMax; }
498 
504  void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
505 
512  void setLambda(const double &val) { lambda = val; }
513 
519  void setVVSIterMax(const unsigned int &val) { vvsIterMax = val; }
520 
526  void setVerbose(const bool &mode) { verbose = mode; }
527 
528 private:
529  void error_and_interaction(vpHomogeneousMatrix &cMo, vpColVector &err, vpMatrix &L);
530 
531  void computePoseVVS(vpHomogeneousMatrix &cMo);
532  void computePoseRobustVVS(vpHomogeneousMatrix &cMo);
533 };
534 
535 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
536 
590 template <typename RetType, typename... ArgsFunc, typename... Args>
591 void vpPoseFeatures::addSpecificFeature(RetType (*fct_ptr)(ArgsFunc...), Args &&...args)
592 {
593  typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
594  typedef typename std::remove_reference<featureTypeReference>::type featureType;
595  featureSpecific_list.push_back(
596  new vpPoseSpecificFeatureTemplate<featureType, RetType, ArgsFunc...>(fct_ptr, std::forward<ArgsFunc>(args)...));
597 
598  featureSpecific_list.back()->createDesired();
599 
600  totalSize++;
601  if (featureSpecific_list.size() > maxSize)
602  maxSize = static_cast<unsigned int>(featureSpecific_list.size());
603 }
604 
671 template <typename ObjType, typename RetType, typename... ArgsFunc, typename... Args>
672 void vpPoseFeatures::addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc...), Args &&...args)
673 {
674  typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
675  typedef typename std::remove_reference<featureTypeReference>::type featureType;
676  featureSpecific_list.push_back(new vpPoseSpecificFeatureTemplateObject<ObjType, featureType, RetType, ArgsFunc...>(
677  obj, fct_ptr, std::forward<ArgsFunc>(args)...));
678 
679  featureSpecific_list.back()->createDesired();
680 
681  totalSize++;
682  if (featureSpecific_list.size() > maxSize)
683  maxSize = static_cast<unsigned int>(featureSpecific_list.size());
684 }
685 #endif
686 
687 #endif //#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
688 
689 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
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:82
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
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 a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:91