42#ifndef vpPoseFeatures_HH
43#define vpPoseFeatures_HH
45#include <visp3/core/vpConfig.h>
47#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
49#include <visp3/core/vpCircle.h>
50#include <visp3/core/vpCylinder.h>
51#include <visp3/core/vpDebug.h>
52#include <visp3/core/vpException.h>
53#include <visp3/core/vpExponentialMap.h>
54#include <visp3/core/vpForwardProjection.h>
55#include <visp3/core/vpLine.h>
56#include <visp3/core/vpPoint.h>
57#include <visp3/core/vpRobust.h>
58#include <visp3/core/vpSphere.h>
59#include <visp3/visual_features/vpBasicFeature.h>
60#include <visp3/visual_features/vpFeatureBuilder.h>
61#include <visp3/visual_features/vpFeatureEllipse.h>
62#include <visp3/visual_features/vpFeaturePoint.h>
67#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
70#ifndef DOXYGEN_SHOULD_SKIP_THIS
74template <
unsigned int N>
struct vpDesiredFeatureBuilderWithTuple {
75 template <
typename featureType,
typename RetType,
typename... ArgsF,
typename... ArgsT,
typename... Args>
76 static void buildDesiredFeatureWithTuple(featureType &feature, RetType (*f)(ArgsF...),
const std::tuple<ArgsT...> &t,
79 vpDesiredFeatureBuilderWithTuple<N - 1>::buildDesiredFeatureWithTuple(feature, f, t, std::get<N - 1>(t), args...);
83template <>
struct vpDesiredFeatureBuilderWithTuple<0> {
84 template <
typename featureType,
typename RetType,
typename... ArgsF,
typename... ArgsT,
typename... Args>
85 static void buildDesiredFeatureWithTuple(featureType & , RetType (*f)(ArgsF...),
86 const std::tuple<ArgsT...> & , Args &&...args)
92template <>
struct vpDesiredFeatureBuilderWithTuple<1> {
93 template <
typename featureType,
typename RetType,
typename... ArgsF,
typename... ArgsT,
typename... Args>
94 static void buildDesiredFeatureWithTuple(featureType &feature, RetType (*f)(ArgsF...),
const std::tuple<ArgsT...> &t,
97 vpDesiredFeatureBuilderWithTuple<0>::buildDesiredFeatureWithTuple(feature, f, t, feature, args...);
101template <
typename featureType,
typename RetType,
typename... Args,
typename... ArgsFunc>
102void buildDesiredFeatureWithTuple(featureType &feature, RetType (*f)(ArgsFunc...), std::tuple<Args...>
const &t)
104 vpDesiredFeatureBuilderWithTuple<
sizeof...(Args)>::buildDesiredFeatureWithTuple(feature, f, t);
112template <
unsigned int N>
struct vpDesiredFeatureBuilderObjectWithTuple {
113 template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsF,
typename... ArgsT,
115 static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType (objType::*f)(ArgsF...),
116 const std::tuple<ArgsT...> &t, Args &&...args)
118 vpDesiredFeatureBuilderObjectWithTuple<N - 1>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t,
119 std::get<N - 1>(t), args...);
123template <>
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...> & ,
134template <>
struct vpDesiredFeatureBuilderObjectWithTuple<1> {
135 template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsF,
typename... ArgsT,
137 static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType (objType::*f)(ArgsF...),
138 const std::tuple<ArgsT...> &t, Args &&...args)
140 vpDesiredFeatureBuilderObjectWithTuple<0>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t, feature, args...);
144template <
typename objType,
typename featureType,
typename RetType,
typename... Args,
typename... ArgsFunc>
145void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType (objType::*f)(ArgsFunc...),
146 std::tuple<Args...> const &t)
148 vpDesiredFeatureBuilderObjectWithTuple<
sizeof...(Args)>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t);
157template <
unsigned int N>
struct vpCurrentFeatureBuilderWithTuple {
158 template <
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
160 static void buildCurrentFeatureWithTuple(featureType &feature,
const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsF...),
161 std::tuple<ArgsTuple...> &t, ArgsDecomposed &&...args)
163 auto proj = std::get<N - 1>(t);
165 vpCurrentFeatureBuilderWithTuple<N - 1>::buildCurrentFeatureWithTuple(feature, cMo, f, t, proj, args...);
169template <>
struct vpCurrentFeatureBuilderWithTuple<0> {
170 template <
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
173 RetType (*f)(ArgsF...), std::tuple<ArgsTuple...> &, ArgsDecomposed &&...args)
179template <>
struct vpCurrentFeatureBuilderWithTuple<1> {
180 template <
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
182 static void buildCurrentFeatureWithTuple(featureType &feature,
const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsF...),
183 std::tuple<ArgsTuple...> &t, ArgsDecomposed &&...args)
185 vpCurrentFeatureBuilderWithTuple<0>::buildCurrentFeatureWithTuple(feature, cMo, f, t, feature, args...);
189template <
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsFunc>
190void buildCurrentFeatureWithTuple(featureType &feature,
const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsFunc...),
191 std::tuple<ArgsTuple...> &t)
193 vpCurrentFeatureBuilderWithTuple<
sizeof...(ArgsTuple)>::buildCurrentFeatureWithTuple(feature, cMo, f, t);
203template <
unsigned int N>
struct vpCurrentFeatureBuilderObjectWithTuple {
204 template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
206 static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature,
const vpHomogeneousMatrix &cMo,
207 RetType (objType::*f)(ArgsF...), std::tuple<ArgsTuple...> &t,
208 ArgsDecomposed &&...args)
210 auto proj = std::get<N - 1>(t);
212 vpCurrentFeatureBuilderObjectWithTuple<N - 1>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, proj,
217template <>
struct vpCurrentFeatureBuilderObjectWithTuple<0> {
218 template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
220 static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType & ,
222 std::tuple<ArgsTuple...> &, ArgsDecomposed &&...args)
228template <>
struct vpCurrentFeatureBuilderObjectWithTuple<1> {
229 template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsDecomposed,
231 static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature,
const vpHomogeneousMatrix &cMo,
232 RetType (objType::*f)(ArgsF...), std::tuple<ArgsTuple...> &t,
233 ArgsDecomposed &&...args)
235 vpCurrentFeatureBuilderObjectWithTuple<0>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, feature,
240template <
typename objType,
typename featureType,
typename RetType,
typename... ArgsTuple,
typename... ArgsFunc>
241void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature,
const vpHomogeneousMatrix &cMo,
242 RetType (objType::*f)(ArgsFunc...), std::tuple<ArgsTuple...> &t)
244 vpCurrentFeatureBuilderObjectWithTuple<
sizeof...(ArgsTuple)>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f,
258class VISP_EXPORT vpPoseSpecificFeature
261 vpPoseSpecificFeature() {}
262 virtual ~vpPoseSpecificFeature() {}
265 virtual vpMatrix currentInteraction() = 0;
266 virtual void createDesired() = 0;
280template <
typename featureType,
typename RetType,
typename... Args>
281class vpPoseSpecificFeatureTemplate :
public vpPoseSpecificFeature
284 featureType desiredFeature;
285 featureType currentFeature;
286 std::tuple<Args...> *tuple;
287 RetType (*func_ptr)(Args...);
290 vpPoseSpecificFeatureTemplate(RetType (*f_ptr)(Args...), Args &&...args)
293 tuple =
new std::tuple<Args...>(args...);
296 virtual ~vpPoseSpecificFeatureTemplate() {
delete tuple; }
298 virtual void createDesired() { buildDesiredFeatureWithTuple(desiredFeature, func_ptr, *tuple); }
304 return currentFeature.error(desiredFeature);
307 virtual vpMatrix currentInteraction() {
return currentFeature.interaction(); }
311 buildCurrentFeatureWithTuple(currentFeature, cMo, func_ptr, *tuple);
326template <
typename ObjectType,
typename featureType,
typename RetType,
typename... Args>
327class vpPoseSpecificFeatureTemplateObject :
public vpPoseSpecificFeature
330 featureType desiredFeature;
331 featureType currentFeature;
332 std::tuple<Args...> *tuple;
333 RetType (ObjectType::*func_ptr)(Args...);
337 vpPoseSpecificFeatureTemplateObject(ObjectType *o, RetType (ObjectType::*f_ptr)(Args...), Args &&...args)
340 tuple =
new std::tuple<Args...>(args...);
344 virtual ~vpPoseSpecificFeatureTemplateObject() {
delete tuple; }
346 virtual void createDesired() { buildDesiredFeatureObjectWithTuple(obj, desiredFeature, func_ptr, *tuple); }
348 virtual vpColVector error() {
return currentFeature.error(desiredFeature); }
350 virtual vpMatrix currentInteraction() {
return currentFeature.interaction(); }
354 buildCurrentFeatureObjectWithTuple(obj, currentFeature, cMo, func_ptr, *tuple);
379 } vpPoseFeaturesMethodType;
382#ifndef DOXYGEN_SHOULD_SKIP_THIS
383 template <
typename FeatureType,
typename FirstParamType>
struct vpDuo {
384 FeatureType *desiredFeature;
385 FirstParamType firstParam;
386 vpDuo() : desiredFeature(NULL), firstParam() {}
389 template <
typename FeatureType,
typename FirstParamType,
typename SecondParamType>
struct vpTrio {
390 FeatureType *desiredFeature;
391 FirstParamType firstParam;
392 SecondParamType secondParam;
394 vpTrio() : desiredFeature(NULL), firstParam(), secondParam() {}
398 unsigned int maxSize;
399 unsigned int totalSize;
400 unsigned int vvsIterMax;
405 bool computeCovariance;
409 std::vector<vpDuo<vpFeaturePoint, vpPoint> > featurePoint_Point_list;
411 std::vector<vpDuo<vpFeaturePoint3D, vpPoint> > featurePoint3D_Point_list;
413 std::vector<vpDuo<vpFeatureVanishingPoint, vpPoint> > featureVanishingPoint_Point_list;
414 std::vector<vpTrio<vpFeatureVanishingPoint, vpLine, vpLine> > featureVanishingPoint_DuoLine_list;
416 std::vector<vpDuo<vpFeatureEllipse, vpSphere> > featureEllipse_Sphere_list;
417 std::vector<vpDuo<vpFeatureEllipse, vpCircle> > featureEllipse_Circle_list;
419 std::vector<vpDuo<vpFeatureLine, vpLine> > featureLine_Line_list;
420 std::vector<vpTrio<vpFeatureLine, vpCylinder, int> > featureLine_DuoLineInt_List;
422 std::vector<vpTrio<vpFeatureSegment, vpPoint, vpPoint> > featureSegment_DuoPoints_list;
424#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
426 std::vector<vpPoseSpecificFeature *> featureSpecific_list;
434 void addFeaturePoint(
const vpPoint &);
436 void addFeaturePoint3D(
const vpPoint &);
438 void addFeatureVanishingPoint(
const vpPoint &);
439 void addFeatureVanishingPoint(
const vpLine &,
const vpLine &);
441 void addFeatureEllipse(
const vpCircle &);
442 void addFeatureEllipse(
const vpSphere &);
444 void addFeatureLine(
const vpLine &);
445 void addFeatureLine(
const vpCylinder &,
const int &line);
449#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
450 template <
typename RetType,
typename... ArgsFunc,
typename... Args>
451 void addSpecificFeature(RetType (*fct_ptr)(ArgsFunc...), Args &&...args);
453 template <
typename ObjType,
typename RetType,
typename... ArgsFunc,
typename... Args>
454 void addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc...), Args &&...args);
460 void computePose(
vpHomogeneousMatrix &cMo,
const vpPoseFeaturesMethodType &type = VIRTUAL_VS);
471 if (!computeCovariance)
472 vpTRACE(
"Warning : The covariance matrix has not been computed. See "
473 "setCovarianceComputation() to do it.");
475 return covarianceMatrix;
530#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
585template <
typename RetType,
typename... ArgsFunc,
typename... Args>
588 typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
589 typedef typename std::remove_reference<featureTypeReference>::type featureType;
590 featureSpecific_list.push_back(
591 new vpPoseSpecificFeatureTemplate<featureType, RetType, ArgsFunc...>(fct_ptr, std::forward<ArgsFunc>(args)...));
593 featureSpecific_list.back()->createDesired();
596 if (featureSpecific_list.size() > maxSize)
597 maxSize =
static_cast<unsigned int>(featureSpecific_list.size());
666template <
typename ObjType,
typename RetType,
typename... ArgsFunc,
typename... Args>
669 typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
670 typedef typename std::remove_reference<featureTypeReference>::type featureType;
671 featureSpecific_list.push_back(
new vpPoseSpecificFeatureTemplateObject<ObjType, featureType, RetType, ArgsFunc...>(
672 obj, fct_ptr, std::forward<ArgsFunc>(args)...));
674 featureSpecific_list.back()->createDesired();
677 if (featureSpecific_list.size() > maxSize)
678 maxSize =
static_cast<unsigned int>(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...