39 #ifndef vpPoseFeatures_HH
40 #define vpPoseFeatures_HH
42 #include <visp3/core/vpConfig.h>
44 #if defined(VISP_HAVE_MODULE_VISUAL_FEATURES) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
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>
65 #ifndef DOXYGEN_SHOULD_SKIP_THIS
69 template <
unsigned int N>
struct vpDesiredFeatureBuilderWithTuple
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,
75 vpDesiredFeatureBuilderWithTuple<N - 1>::buildDesiredFeatureWithTuple(feature, f, t, std::get<N - 1>(t), args...);
79 template <>
struct vpDesiredFeatureBuilderWithTuple<0>
81 template <
typename featureType,
typename RetType,
typename... ArgsF,
typename... ArgsT,
typename... Args>
82 static void buildDesiredFeatureWithTuple(featureType & , RetType(*f)(ArgsF...),
83 const std::tuple<ArgsT...> & , Args &&...args)
89 template <>
struct vpDesiredFeatureBuilderWithTuple<1>
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,
95 vpDesiredFeatureBuilderWithTuple<0>::buildDesiredFeatureWithTuple(feature, f, t, feature, args...);
99 template <
typename featureType,
typename RetType,
typename... Args,
typename... ArgsFunc>
100 void buildDesiredFeatureWithTuple(featureType &feature, RetType(*f)(ArgsFunc...), std::tuple<Args...>
const &t)
102 vpDesiredFeatureBuilderWithTuple<
sizeof...(Args)>::buildDesiredFeatureWithTuple(feature, f, t);
110 template <
unsigned int N>
struct vpDesiredFeatureBuilderObjectWithTuple
112 template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsF,
typename... ArgsT,
114 static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType(objType:: *f)(ArgsF...),
115 const std::tuple<ArgsT...> &t, Args &&...args)
117 vpDesiredFeatureBuilderObjectWithTuple<N - 1>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t,
118 std::get<N - 1>(t), args...);
122 template <>
struct vpDesiredFeatureBuilderObjectWithTuple<0>
124 template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsF,
typename... ArgsT,
126 static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType & ,
127 RetType(objType:: *f)(ArgsF...),
const std::tuple<ArgsT...> & ,
134 template <>
struct vpDesiredFeatureBuilderObjectWithTuple<1>
136 template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsF,
typename... ArgsT,
138 static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType(objType:: *f)(ArgsF...),
139 const std::tuple<ArgsT...> &t, Args &&...args)
141 vpDesiredFeatureBuilderObjectWithTuple<0>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t, feature, args...);
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)
149 vpDesiredFeatureBuilderObjectWithTuple<
sizeof...(Args)>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t);
158 template <
unsigned int N>
struct vpCurrentFeatureBuilderWithTuple
160 template <
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
162 static void buildCurrentFeatureWithTuple(featureType &feature,
const vpHomogeneousMatrix &cMo, RetType(*f)(ArgsF...),
163 std::tuple<ArgsTuple...> &t, ArgsDecomposed &&...args)
165 auto proj = std::get<N - 1>(t);
167 vpCurrentFeatureBuilderWithTuple<N - 1>::buildCurrentFeatureWithTuple(feature, cMo, f, t, proj, args...);
171 template <>
struct vpCurrentFeatureBuilderWithTuple<0>
173 template <
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
176 RetType(*f)(ArgsF...), std::tuple<ArgsTuple...> &, ArgsDecomposed &&...args)
182 template <>
struct vpCurrentFeatureBuilderWithTuple<1>
184 template <
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
186 static void buildCurrentFeatureWithTuple(featureType &feature,
const vpHomogeneousMatrix &cMo, RetType(*f)(ArgsF...),
187 std::tuple<ArgsTuple...> &t, ArgsDecomposed &&...args)
189 vpCurrentFeatureBuilderWithTuple<0>::buildCurrentFeatureWithTuple(feature, cMo, f, t, feature, args...);
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)
197 vpCurrentFeatureBuilderWithTuple<
sizeof...(ArgsTuple)>::buildCurrentFeatureWithTuple(feature, cMo, f, t);
207 template <
unsigned int N>
struct vpCurrentFeatureBuilderObjectWithTuple
209 template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
211 static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature,
const vpHomogeneousMatrix &cMo,
212 RetType(objType:: *f)(ArgsF...), std::tuple<ArgsTuple...> &t,
213 ArgsDecomposed &&...args)
215 auto proj = std::get<N - 1>(t);
217 vpCurrentFeatureBuilderObjectWithTuple<N - 1>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, proj,
222 template <>
struct vpCurrentFeatureBuilderObjectWithTuple<0>
224 template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
226 static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType & ,
228 std::tuple<ArgsTuple...> &, ArgsDecomposed &&...args)
234 template <>
struct vpCurrentFeatureBuilderObjectWithTuple<1>
236 template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
238 static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature,
const vpHomogeneousMatrix &cMo,
239 RetType(objType:: *f)(ArgsF...), std::tuple<ArgsTuple...> &t,
240 ArgsDecomposed &&...args)
242 vpCurrentFeatureBuilderObjectWithTuple<0>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, feature,
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)
251 vpCurrentFeatureBuilderObjectWithTuple<
sizeof...(ArgsTuple)>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f,
265 class VISP_EXPORT vpPoseSpecificFeature
268 vpPoseSpecificFeature() { }
269 virtual ~vpPoseSpecificFeature() { }
272 virtual vpMatrix currentInteraction() = 0;
273 virtual void createDesired() = 0;
287 template <
typename featureType,
typename RetType,
typename... Args>
288 class vpPoseSpecificFeatureTemplate :
public vpPoseSpecificFeature
291 featureType m_desiredFeature;
292 featureType m_currentFeature;
293 std::tuple<Args...> *m_tuple;
294 RetType(*func_ptr)(Args...);
297 vpPoseSpecificFeatureTemplate(RetType(*f_ptr)(Args...), Args &&...args)
300 m_tuple =
new std::tuple<Args...>(args...);
302 virtual ~vpPoseSpecificFeatureTemplate() vp_override
307 virtual void createDesired() vp_override
309 buildDesiredFeatureWithTuple(m_desiredFeature, func_ptr, *m_tuple);
316 return m_currentFeature.error(m_desiredFeature);
319 virtual vpMatrix currentInteraction() vp_override
321 return m_currentFeature.interaction();
326 buildCurrentFeatureWithTuple(m_currentFeature, cMo, func_ptr, *m_tuple);
341 template <
typename ObjectType,
typename featureType,
typename RetType,
typename... Args>
342 class vpPoseSpecificFeatureTemplateObject :
public vpPoseSpecificFeature
345 featureType m_desiredFeature;
346 featureType m_currentFeature;
347 std::tuple<Args...> *m_tuple;
348 RetType(ObjectType:: *func_ptr)(Args...);
352 vpPoseSpecificFeatureTemplateObject(ObjectType *o, RetType(ObjectType:: *f_ptr)(Args...), Args &&...args)
355 m_tuple =
new std::tuple<Args...>(args...);
359 virtual ~vpPoseSpecificFeatureTemplateObject() vp_override
364 virtual void createDesired() vp_override
366 buildDesiredFeatureObjectWithTuple(m_obj, m_desiredFeature, func_ptr, *m_tuple);
371 return m_currentFeature.error(m_desiredFeature);
374 virtual vpMatrix currentInteraction() vp_override
376 return m_currentFeature.interaction();
381 buildCurrentFeatureObjectWithTuple(m_obj, m_currentFeature, cMo, func_ptr, *m_tuple);
406 } vpPoseFeaturesMethodType;
424 void addFeaturePoint(
const vpPoint &p);
432 void addFeaturePoint3D(
const vpPoint &p);
440 void addFeatureVanishingPoint(
const vpPoint &p);
449 void addFeatureVanishingPoint(
const vpLine &l1,
const vpLine &l2);
457 void addFeatureEllipse(
const vpCircle &);
465 void addFeatureEllipse(
const vpSphere &);
473 void addFeatureLine(
const vpLine &);
483 void addFeatureLine(
const vpCylinder &,
const int &line);
497 template <
typename RetType,
typename... ArgsFunc,
typename... Args>
498 void addSpecificFeature(RetType(*fct_ptr)(ArgsFunc...), Args &&...args);
503 template <
typename ObjType,
typename RetType,
typename... ArgsFunc,
typename... Args>
504 void addSpecificFeature(ObjType *obj, RetType(ObjType:: *fct_ptr)(ArgsFunc...), Args &&...args);
524 void computePose(
vpHomogeneousMatrix &cMo,
const vpPoseFeaturesMethodType &type = VIRTUAL_VS);
535 if (!m_computeCovariance)
536 vpTRACE(
"Warning : The covariance matrix has not been computed. See "
537 "setCovarianceComputation() to do it.");
539 return m_covarianceMatrix;
588 #ifndef DOXYGEN_SHOULD_SKIP_THIS
589 template <
typename FeatureType,
typename FirstParamType>
struct vpDuo
591 FeatureType *desiredFeature;
592 FirstParamType firstParam;
593 vpDuo() : desiredFeature(nullptr), firstParam() { }
596 template <
typename FeatureType,
typename FirstParamType,
typename SecondParamType>
struct vpTrio
598 FeatureType *desiredFeature;
599 FirstParamType firstParam;
600 SecondParamType secondParam;
602 vpTrio() : desiredFeature(nullptr), firstParam(), secondParam() { }
606 unsigned int m_maxSize;
607 unsigned int m_totalSize;
608 unsigned int m_vvsIterMax;
613 bool m_computeCovariance;
617 std::vector<vpDuo<vpFeaturePoint, vpPoint> > m_featurePoint_Point_list;
619 std::vector<vpDuo<vpFeaturePoint3D, vpPoint> > m_featurePoint3D_Point_list;
621 std::vector<vpDuo<vpFeatureVanishingPoint, vpPoint> > m_featureVanishingPoint_Point_list;
622 std::vector<vpTrio<vpFeatureVanishingPoint, vpLine, vpLine> > m_featureVanishingPoint_DuoLine_list;
624 std::vector<vpDuo<vpFeatureEllipse, vpSphere> > m_featureEllipse_Sphere_list;
625 std::vector<vpDuo<vpFeatureEllipse, vpCircle> > m_featureEllipse_Circle_list;
627 std::vector<vpDuo<vpFeatureLine, vpLine> > m_featureLine_Line_list;
628 std::vector<vpTrio<vpFeatureLine, vpCylinder, int> > m_featureLine_DuoLineInt_List;
630 std::vector<vpTrio<vpFeatureSegment, vpPoint, vpPoint> > m_featureSegment_DuoPoints_list;
632 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
634 std::vector<vpPoseSpecificFeature *> m_featureSpecific_list;
716 template <
typename RetType,
typename... ArgsFunc,
typename... Args>
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)...));
724 m_featureSpecific_list.back()->createDesired();
727 if (m_featureSpecific_list.size() > m_maxSize)
728 m_maxSize =
static_cast<unsigned int>(m_featureSpecific_list.size());
795 template <
typename ObjType,
typename RetType,
typename... ArgsFunc,
typename... Args>
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)...));
803 m_featureSpecific_list.back()->createDesired();
806 if (m_featureSpecific_list.size() > m_maxSize)
807 m_maxSize =
static_cast<unsigned int>(m_featureSpecific_list.size());
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Implementation of column vector and the associated operations.
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
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...
Implementation of a matrix and operations on matrices.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Tools for pose computation from any feature.
void setVerbose(const bool &mode)
void setVVSIterMax(const unsigned int &val)
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...