47 #ifndef vpPoseFeatures_HH 48 #define vpPoseFeatures_HH 50 #include <visp3/core/vpConfig.h> 52 #ifdef VISP_HAVE_MODULE_VISUAL_FEATURES 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> 72 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) 75 #ifndef DOXYGEN_SHOULD_SKIP_THIS 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,
84 vpDesiredFeatureBuilderWithTuple<N - 1>::buildDesiredFeatureWithTuple(feature, f, t, std::get<N - 1>(t), args...);
88 template <>
struct vpDesiredFeatureBuilderWithTuple<0> {
89 template <
typename featureType,
typename RetType,
typename... ArgsF,
typename... ArgsT,
typename... Args>
90 static void buildDesiredFeatureWithTuple(featureType & , RetType (*f)(ArgsF...),
91 const std::tuple<ArgsT...> & , Args &&...args)
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,
102 vpDesiredFeatureBuilderWithTuple<0>::buildDesiredFeatureWithTuple(feature, f, t, feature, args...);
106 template <
typename featureType,
typename RetType,
typename... Args,
typename... ArgsFunc>
107 void buildDesiredFeatureWithTuple(featureType &feature, RetType (*f)(ArgsFunc...), std::tuple<Args...>
const &t)
109 vpDesiredFeatureBuilderWithTuple<
sizeof...(Args)>::buildDesiredFeatureWithTuple(feature, f, t);
117 template <
unsigned int N>
struct vpDesiredFeatureBuilderObjectWithTuple {
118 template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsF,
typename... ArgsT,
120 static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType (objType::*f)(ArgsF...),
121 const std::tuple<ArgsT...> &t, Args &&...args)
123 vpDesiredFeatureBuilderObjectWithTuple<N - 1>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t,
124 std::get<N - 1>(t), args...);
128 template <>
struct vpDesiredFeatureBuilderObjectWithTuple<0> {
129 template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsF,
typename... ArgsT,
131 static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType & ,
132 RetType (objType::*f)(ArgsF...),
const std::tuple<ArgsT...> & ,
139 template <>
struct vpDesiredFeatureBuilderObjectWithTuple<1> {
140 template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsF,
typename... ArgsT,
142 static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType (objType::*f)(ArgsF...),
143 const std::tuple<ArgsT...> &t, Args &&...args)
145 vpDesiredFeatureBuilderObjectWithTuple<0>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t, feature, args...);
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)
153 vpDesiredFeatureBuilderObjectWithTuple<
sizeof...(Args)>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t);
162 template <
unsigned int N>
struct vpCurrentFeatureBuilderWithTuple {
163 template <
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
165 static void buildCurrentFeatureWithTuple(featureType &feature,
const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsF...),
166 std::tuple<ArgsTuple...> &t, ArgsDecomposed &&...args)
168 auto proj = std::get<N - 1>(t);
170 vpCurrentFeatureBuilderWithTuple<N - 1>::buildCurrentFeatureWithTuple(feature, cMo, f, t, proj, args...);
174 template <>
struct vpCurrentFeatureBuilderWithTuple<0> {
175 template <
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
178 RetType (*f)(ArgsF...), std::tuple<ArgsTuple...> &, ArgsDecomposed &&...args)
184 template <>
struct vpCurrentFeatureBuilderWithTuple<1> {
185 template <
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
187 static void buildCurrentFeatureWithTuple(featureType &feature,
const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsF...),
188 std::tuple<ArgsTuple...> &t, ArgsDecomposed &&...args)
190 vpCurrentFeatureBuilderWithTuple<0>::buildCurrentFeatureWithTuple(feature, cMo, f, t, feature, args...);
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)
198 vpCurrentFeatureBuilderWithTuple<
sizeof...(ArgsTuple)>::buildCurrentFeatureWithTuple(feature, cMo, f, t);
208 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> {
223 template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
225 static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType & ,
227 std::tuple<ArgsTuple...> &, ArgsDecomposed &&...args)
233 template <>
struct vpCurrentFeatureBuilderObjectWithTuple<1> {
234 template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
236 static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature,
const vpHomogeneousMatrix &cMo,
237 RetType (objType::*f)(ArgsF...), std::tuple<ArgsTuple...> &t,
238 ArgsDecomposed &&...args)
240 vpCurrentFeatureBuilderObjectWithTuple<0>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, feature,
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)
249 vpCurrentFeatureBuilderObjectWithTuple<
sizeof...(ArgsTuple)>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f,
263 class VISP_EXPORT vpPoseSpecificFeature
266 vpPoseSpecificFeature() {}
267 virtual ~vpPoseSpecificFeature() {}
270 virtual vpMatrix currentInteraction() = 0;
271 virtual void createDesired() = 0;
285 template <
typename featureType,
typename RetType,
typename... Args>
286 class vpPoseSpecificFeatureTemplate :
public vpPoseSpecificFeature
289 featureType desiredFeature;
290 featureType currentFeature;
291 std::tuple<Args...> *tuple;
292 RetType (*func_ptr)(Args...);
295 vpPoseSpecificFeatureTemplate(RetType (*f_ptr)(Args...), Args &&...args)
298 tuple =
new std::tuple<Args...>(args...);
301 virtual ~vpPoseSpecificFeatureTemplate() {
delete tuple; }
303 virtual void createDesired() { buildDesiredFeatureWithTuple(desiredFeature, func_ptr, *tuple); }
309 return currentFeature.error(desiredFeature);
312 virtual vpMatrix currentInteraction() {
return currentFeature.interaction(); }
316 buildCurrentFeatureWithTuple(currentFeature, cMo, func_ptr, *tuple);
331 template <
typename ObjectType,
typename featureType,
typename RetType,
typename... Args>
332 class vpPoseSpecificFeatureTemplateObject :
public vpPoseSpecificFeature
335 featureType desiredFeature;
336 featureType currentFeature;
337 std::tuple<Args...> *tuple;
338 RetType (ObjectType::*func_ptr)(Args...);
342 vpPoseSpecificFeatureTemplateObject(ObjectType *o, RetType (ObjectType::*f_ptr)(Args...), Args &&...args)
345 tuple =
new std::tuple<Args...>(args...);
349 virtual ~vpPoseSpecificFeatureTemplateObject() {
delete tuple; }
351 virtual void createDesired() { buildDesiredFeatureObjectWithTuple(obj, desiredFeature, func_ptr, *tuple); }
353 virtual vpColVector error() {
return currentFeature.error(desiredFeature); }
355 virtual vpMatrix currentInteraction() {
return currentFeature.interaction(); }
359 buildCurrentFeatureObjectWithTuple(obj, currentFeature, cMo, func_ptr, *tuple);
362 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS 363 #endif // (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) 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() {}
394 template <
typename FeatureType,
typename FirstParamType,
typename SecondParamType>
struct vpTrio {
395 FeatureType *desiredFeature;
396 FirstParamType firstParam;
397 SecondParamType secondParam;
399 vpTrio() : desiredFeature(NULL), firstParam(), secondParam() {}
401 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS 403 unsigned int maxSize;
404 unsigned int totalSize;
405 unsigned int vvsIterMax;
410 bool computeCovariance;
414 std::vector<vpDuo<vpFeaturePoint, vpPoint> > featurePoint_Point_list;
416 std::vector<vpDuo<vpFeaturePoint3D, vpPoint> > featurePoint3D_Point_list;
418 std::vector<vpDuo<vpFeatureVanishingPoint, vpPoint> > featureVanishingPoint_Point_list;
419 std::vector<vpTrio<vpFeatureVanishingPoint, vpLine, vpLine> > featureVanishingPoint_DuoLine_list;
421 std::vector<vpDuo<vpFeatureEllipse, vpSphere> > featureEllipse_Sphere_list;
422 std::vector<vpDuo<vpFeatureEllipse, vpCircle> > featureEllipse_Circle_list;
424 std::vector<vpDuo<vpFeatureLine, vpLine> > featureLine_Line_list;
425 std::vector<vpTrio<vpFeatureLine, vpCylinder, int> > featureLine_DuoLineInt_List;
427 std::vector<vpTrio<vpFeatureSegment, vpPoint, vpPoint> > featureSegment_DuoPoints_list;
429 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) 431 std::vector<vpPoseSpecificFeature *> featureSpecific_list;
439 void addFeaturePoint(
const vpPoint &);
441 void addFeaturePoint3D(
const vpPoint &);
443 void addFeatureVanishingPoint(
const vpPoint &);
444 void addFeatureVanishingPoint(
const vpLine &,
const vpLine &);
446 void addFeatureEllipse(
const vpCircle &);
447 void addFeatureEllipse(
const vpSphere &);
449 void addFeatureLine(
const vpLine &);
450 void addFeatureLine(
const vpCylinder &,
const int &line);
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);
458 template <
typename ObjType,
typename RetType,
typename... ArgsFunc,
typename... Args>
459 void addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc...), Args &&...args);
465 void computePose(
vpHomogeneousMatrix &cMo,
const vpPoseFeaturesMethodType &type = VIRTUAL_VS);
476 if (!computeCovariance)
477 vpTRACE(
"Warning : The covariance matrix has not been computed. See " 478 "setCovarianceComputation() to do it.");
480 return covarianceMatrix;
535 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) 590 template <
typename RetType,
typename... ArgsFunc,
typename... Args>
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)...));
598 featureSpecific_list.back()->createDesired();
601 if (featureSpecific_list.size() > maxSize)
602 maxSize = static_cast<unsigned int>(featureSpecific_list.size());
671 template <
typename ObjType,
typename RetType,
typename... ArgsFunc,
typename... Args>
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)...));
679 featureSpecific_list.back()->createDesired();
682 if (featureSpecific_list.size() > maxSize)
683 maxSize = static_cast<unsigned int>(featureSpecific_list.size());
687 #endif //#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES Implementation of a matrix and operations on matrices.
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...
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
vpMatrix getCovarianceMatrix() const
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
void setCovarianceComputation(const bool &flag)
Implementation of column vector and the associated operations.
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...