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