Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpPoseFeatures.h
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 * See the file LICENSE.txt at the root directory of this source
10 * distribution for additional information about the GNU GPL.
11 *
12 * For using ViSP with software that can not be combined with the GNU
13 * GPL, please contact Inria about acquiring a ViSP Professional
14 * Edition License.
15 *
16 * See https://visp.inria.fr for more information.
17 *
18 * This software was developed at:
19 * Inria Rennes - Bretagne Atlantique
20 * Campus Universitaire de Beaulieu
21 * 35042 Rennes Cedex
22 * France
23 *
24 * If you have questions regarding the use of this file, please contact
25 * Inria at visp@inria.fr
26 *
27 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Description:
31 * Pose computation from any features.
32 */
33
42#ifndef vpPoseFeatures_HH
43#define vpPoseFeatures_HH
44
45#include <visp3/core/vpConfig.h>
46
47#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
48
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>
63
64#include <iostream>
65#include <vector>
66
67#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
68#include <tuple>
69
70#ifndef DOXYGEN_SHOULD_SKIP_THIS
71//#################################################
72//## Call a function with a tuple as parameters
73//#################################################
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,
77 Args &&...args)
78 {
79 vpDesiredFeatureBuilderWithTuple<N - 1>::buildDesiredFeatureWithTuple(feature, f, t, std::get<N - 1>(t), args...);
80 }
81};
82
83template <> struct vpDesiredFeatureBuilderWithTuple<0> {
84 template <typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args>
85 static void buildDesiredFeatureWithTuple(featureType & /* feature */, RetType (*f)(ArgsF...),
86 const std::tuple<ArgsT...> & /* t */, Args &&...args)
87 {
88 f(args...);
89 }
90};
91
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,
95 Args &&...args)
96 {
97 vpDesiredFeatureBuilderWithTuple<0>::buildDesiredFeatureWithTuple(feature, f, t, feature, args...);
98 }
99};
100
101template <typename featureType, typename RetType, typename... Args, typename... ArgsFunc>
102void buildDesiredFeatureWithTuple(featureType &feature, RetType (*f)(ArgsFunc...), std::tuple<Args...> const &t)
103{
104 vpDesiredFeatureBuilderWithTuple<sizeof...(Args)>::buildDesiredFeatureWithTuple(feature, f, t);
105}
106
107//#################################################
108//## Call a function with a tuple as parameters
109//## Object Mode
110//#################################################
111
112template <unsigned int N> struct vpDesiredFeatureBuilderObjectWithTuple {
113 template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
114 typename... Args>
115 static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType (objType::*f)(ArgsF...),
116 const std::tuple<ArgsT...> &t, Args &&...args)
117 {
118 vpDesiredFeatureBuilderObjectWithTuple<N - 1>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t,
119 std::get<N - 1>(t), args...);
120 }
121};
122
123template <> struct vpDesiredFeatureBuilderObjectWithTuple<0> {
124 template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
125 typename... Args>
126 static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType & /*feature*/,
127 RetType (objType::*f)(ArgsF...), const std::tuple<ArgsT...> & /* t */,
128 Args &&...args)
129 {
130 (obj->*f)(args...);
131 }
132};
133
134template <> struct vpDesiredFeatureBuilderObjectWithTuple<1> {
135 template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
136 typename... Args>
137 static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType (objType::*f)(ArgsF...),
138 const std::tuple<ArgsT...> &t, Args &&...args)
139 {
140 vpDesiredFeatureBuilderObjectWithTuple<0>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t, feature, args...);
141 }
142};
143
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)
147{
148 vpDesiredFeatureBuilderObjectWithTuple<sizeof...(Args)>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t);
149}
150
151//#####################################################
152//## Call un function with a tuple as parameters
153//## Track all the parameters with the cMo
154//## Except the first one (must be de "BasicFeature"
155//#####################################################
156
157template <unsigned int N> struct vpCurrentFeatureBuilderWithTuple {
158 template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
159 typename... ArgsF>
160 static void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsF...),
161 std::tuple<ArgsTuple...> &t, ArgsDecomposed &&...args)
162 {
163 auto proj = std::get<N - 1>(t);
164 proj.track(cMo);
165 vpCurrentFeatureBuilderWithTuple<N - 1>::buildCurrentFeatureWithTuple(feature, cMo, f, t, proj, args...);
166 }
167};
168
169template <> struct vpCurrentFeatureBuilderWithTuple<0> {
170 template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
171 typename... ArgsF>
172 static void buildCurrentFeatureWithTuple(featureType & /*feature*/, const vpHomogeneousMatrix & /*cMo*/,
173 RetType (*f)(ArgsF...), std::tuple<ArgsTuple...> &, ArgsDecomposed &&...args)
174 {
175 f(args...);
176 }
177};
178
179template <> struct vpCurrentFeatureBuilderWithTuple<1> {
180 template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
181 typename... ArgsF>
182 static void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsF...),
183 std::tuple<ArgsTuple...> &t, ArgsDecomposed &&...args)
184 {
185 vpCurrentFeatureBuilderWithTuple<0>::buildCurrentFeatureWithTuple(feature, cMo, f, t, feature, args...);
186 }
187};
188
189template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc>
190void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsFunc...),
191 std::tuple<ArgsTuple...> &t)
192{
193 vpCurrentFeatureBuilderWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureWithTuple(feature, cMo, f, t);
194}
195
196//#####################################################
197//## Call un function with a tuple as parameters
198//## Track all the parameters with the cMo
199//## Except the first one (must be de "BasicFeature"
200//## Object Mode
201//#####################################################
202
203template <unsigned int N> struct vpCurrentFeatureBuilderObjectWithTuple {
204 template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
205 typename... ArgsF>
206 static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
207 RetType (objType::*f)(ArgsF...), std::tuple<ArgsTuple...> &t,
208 ArgsDecomposed &&...args)
209 {
210 auto proj = std::get<N - 1>(t);
211 proj.track(cMo);
212 vpCurrentFeatureBuilderObjectWithTuple<N - 1>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, proj,
213 args...);
214 }
215};
216
217template <> struct vpCurrentFeatureBuilderObjectWithTuple<0> {
218 template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
219 typename... ArgsF>
220 static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType & /*feature*/,
221 const vpHomogeneousMatrix & /*cMo*/, RetType (objType::*f)(ArgsF...),
222 std::tuple<ArgsTuple...> &, ArgsDecomposed &&...args)
223 {
224 (obj->*f)(args...);
225 }
226};
227
228template <> struct vpCurrentFeatureBuilderObjectWithTuple<1> {
229 template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
230 typename... ArgsF>
231 static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
232 RetType (objType::*f)(ArgsF...), std::tuple<ArgsTuple...> &t,
233 ArgsDecomposed &&...args)
234 {
235 vpCurrentFeatureBuilderObjectWithTuple<0>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, feature,
236 args...);
237 }
238};
239
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)
243{
244 vpCurrentFeatureBuilderObjectWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f,
245 t);
246}
247
248//#################################################
249//## Call that will be used in our vpPoseFeatures
250//## to store the specific features.
251//#################################################
258class VISP_EXPORT vpPoseSpecificFeature
259{
260public:
261 vpPoseSpecificFeature() {}
262 virtual ~vpPoseSpecificFeature() {}
263
264 virtual vpColVector error() = 0;
265 virtual vpMatrix currentInteraction() = 0;
266 virtual void createDesired() = 0;
267 virtual void createCurrent(const vpHomogeneousMatrix &cMo) = 0;
268};
269
270//#################################################
271//## Template for all kind of specific features
272//#################################################
273
280template <typename featureType, typename RetType, typename... Args>
281class vpPoseSpecificFeatureTemplate : public vpPoseSpecificFeature
282{
283private:
284 featureType desiredFeature;
285 featureType currentFeature;
286 std::tuple<Args...> *tuple;
287 RetType (*func_ptr)(Args...);
288
289public:
290 vpPoseSpecificFeatureTemplate(RetType (*f_ptr)(Args...), Args &&...args)
291 {
292 func_ptr = f_ptr; // std::move(f_ptr);
293 tuple = new std::tuple<Args...>(args...);
294 }
295
296 virtual ~vpPoseSpecificFeatureTemplate() { delete tuple; }
297
298 virtual void createDesired() { buildDesiredFeatureWithTuple(desiredFeature, func_ptr, *tuple); }
299
300 virtual vpColVector error()
301 {
302 // std::cout << "Getting S... : " << std::get<0>(*tuple).get_s() <<
303 // std::endl;
304 return currentFeature.error(desiredFeature);
305 }
306
307 virtual vpMatrix currentInteraction() { return currentFeature.interaction(); }
308
309 virtual void createCurrent(const vpHomogeneousMatrix &cMo)
310 {
311 buildCurrentFeatureWithTuple(currentFeature, cMo, func_ptr, *tuple);
312 }
313};
314
315//#################################################
316//## Template for all kind of specific features
317//## Object Mode
318//#################################################
319
326template <typename ObjectType, typename featureType, typename RetType, typename... Args>
327class vpPoseSpecificFeatureTemplateObject : public vpPoseSpecificFeature
328{
329private:
330 featureType desiredFeature;
331 featureType currentFeature;
332 std::tuple<Args...> *tuple;
333 RetType (ObjectType::*func_ptr)(Args...);
334 ObjectType *obj;
335
336public:
337 vpPoseSpecificFeatureTemplateObject(ObjectType *o, RetType (ObjectType::*f_ptr)(Args...), Args &&...args)
338 {
339 func_ptr = f_ptr; // std::move(f_ptr);
340 tuple = new std::tuple<Args...>(args...);
341 obj = o;
342 }
343
344 virtual ~vpPoseSpecificFeatureTemplateObject() { delete tuple; }
345
346 virtual void createDesired() { buildDesiredFeatureObjectWithTuple(obj, desiredFeature, func_ptr, *tuple); }
347
348 virtual vpColVector error() { return currentFeature.error(desiredFeature); }
349
350 virtual vpMatrix currentInteraction() { return currentFeature.interaction(); }
351
352 virtual void createCurrent(const vpHomogeneousMatrix &cMo)
353 {
354 buildCurrentFeatureObjectWithTuple(obj, currentFeature, cMo, func_ptr, *tuple);
355 }
356};
357#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
358#endif // (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
359
370class VISP_EXPORT vpPoseFeatures
371{
372public:
376 typedef enum {
378 ROBUST_VIRTUAL_VS
379 } vpPoseFeaturesMethodType;
380
381private:
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() {}
387 };
388
389 template <typename FeatureType, typename FirstParamType, typename SecondParamType> struct vpTrio {
390 FeatureType *desiredFeature;
391 FirstParamType firstParam;
392 SecondParamType secondParam;
393
394 vpTrio() : desiredFeature(NULL), firstParam(), secondParam() {}
395 };
396#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
397
398 unsigned int maxSize;
399 unsigned int totalSize;
400 unsigned int vvsIterMax;
401 double lambda;
402
403 bool verbose;
404
405 bool computeCovariance;
406 vpMatrix covarianceMatrix;
407
408 // vpFeaturePoint
409 std::vector<vpDuo<vpFeaturePoint, vpPoint> > featurePoint_Point_list;
410 // vpFeaturePoint3D
411 std::vector<vpDuo<vpFeaturePoint3D, vpPoint> > featurePoint3D_Point_list;
412 // vpFeatureVanishingPoint
413 std::vector<vpDuo<vpFeatureVanishingPoint, vpPoint> > featureVanishingPoint_Point_list;
414 std::vector<vpTrio<vpFeatureVanishingPoint, vpLine, vpLine> > featureVanishingPoint_DuoLine_list;
415 // vpFeatureEllipse
416 std::vector<vpDuo<vpFeatureEllipse, vpSphere> > featureEllipse_Sphere_list;
417 std::vector<vpDuo<vpFeatureEllipse, vpCircle> > featureEllipse_Circle_list;
418 // vpFeatureLine
419 std::vector<vpDuo<vpFeatureLine, vpLine> > featureLine_Line_list;
420 std::vector<vpTrio<vpFeatureLine, vpCylinder, int> > featureLine_DuoLineInt_List;
421 // vpFeatureSegment
422 std::vector<vpTrio<vpFeatureSegment, vpPoint, vpPoint> > featureSegment_DuoPoints_list;
423
424#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
425 // Specific features
426 std::vector<vpPoseSpecificFeature *> featureSpecific_list;
427#endif
428
429public:
431 virtual ~vpPoseFeatures();
432
433 // ! Features addition
434 void addFeaturePoint(const vpPoint &);
435
436 void addFeaturePoint3D(const vpPoint &);
437
438 void addFeatureVanishingPoint(const vpPoint &);
439 void addFeatureVanishingPoint(const vpLine &, const vpLine &);
440
441 void addFeatureEllipse(const vpCircle &);
442 void addFeatureEllipse(const vpSphere &);
443
444 void addFeatureLine(const vpLine &);
445 void addFeatureLine(const vpCylinder &, const int &line);
446
447 void addFeatureSegment(vpPoint &, vpPoint &);
448
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);
452
453 template <typename ObjType, typename RetType, typename... ArgsFunc, typename... Args>
454 void addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc...), Args &&...args);
455#endif
456
457 void clear();
458
459 // ! Pose computation
460 void computePose(vpHomogeneousMatrix &cMo, const vpPoseFeaturesMethodType &type = VIRTUAL_VS);
461
470 {
471 if (!computeCovariance)
472 vpTRACE("Warning : The covariance matrix has not been computed. See "
473 "setCovarianceComputation() to do it.");
474
475 return covarianceMatrix;
476 }
477
484 double getLambda() { return lambda; }
485
492 unsigned int getVVSIterMax() { return vvsIterMax; }
493
499 void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
500
507 void setLambda(const double &val) { lambda = val; }
508
514 void setVVSIterMax(const unsigned int &val) { vvsIterMax = val; }
515
521 void setVerbose(const bool &mode) { verbose = mode; }
522
523private:
524 void error_and_interaction(vpHomogeneousMatrix &cMo, vpColVector &err, vpMatrix &L);
525
526 void computePoseVVS(vpHomogeneousMatrix &cMo);
527 void computePoseRobustVVS(vpHomogeneousMatrix &cMo);
528};
529
530#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
585template <typename RetType, typename... ArgsFunc, typename... Args>
586void vpPoseFeatures::addSpecificFeature(RetType (*fct_ptr)(ArgsFunc...), Args &&...args)
587{
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)...));
592
593 featureSpecific_list.back()->createDesired();
594
595 totalSize++;
596 if (featureSpecific_list.size() > maxSize)
597 maxSize = static_cast<unsigned int>(featureSpecific_list.size());
598}
599
666template <typename ObjType, typename RetType, typename... ArgsFunc, typename... Args>
667void vpPoseFeatures::addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc...), Args &&...args)
668{
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)...));
673
674 featureSpecific_list.back()->createDesired();
675
676 totalSize++;
677 if (featureSpecific_list.size() > maxSize)
678 maxSize = static_cast<unsigned int>(featureSpecific_list.size());
679}
680#endif
681
682#endif //#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
683
684#endif
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition vpCircle.h:87
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...
Definition vpCylinder.h:98
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...
Definition vpLine.h:100
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
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...
Definition vpSphere.h:78
#define vpTRACE
Definition vpDebug.h:411