Visual Servoing Platform  version 3.2.1 under development (2019-10-23) under development (2019-10-23)
vpFeatureMoment.cpp
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  * Base for all moment features
33  *
34  * Authors:
35  * Filip Novotny
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpMath.h>
40 #include <visp3/core/vpMoment.h>
41 #include <visp3/core/vpMomentDatabase.h>
42 #include <visp3/visual_features/vpFeatureMoment.h>
43 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
44 
45 #include <visp3/core/vpException.h>
46 #include <visp3/visual_features/vpFeatureException.h>
47 
48 #include <vector>
49 #include <visp3/core/vpDebug.h>
50 
51 class vpBasicFeature;
52 
57 {
58  // feature dimension
59  /*
60  * The dimension of the visual feature is set according to the size of the
61  * vpMoment associated to it. This partly explains why vpFeatureMomentBasic
62  * cannot be used directly as a visual feature.
63  */
64  if (this->moment != NULL)
65  dim_s = (unsigned int)this->moment->get().size();
66  else
67  dim_s = 0;
68 
69  nbParameters = 1;
70 
71  // memory allocation
72  s.resize(dim_s);
73  for (unsigned int i = 0; i < dim_s; i++)
74  s[i] = 0;
75 
76  if (flags == NULL)
77  flags = new bool[nbParameters];
78  for (unsigned int i = 0; i < nbParameters; i++)
79  flags[i] = false;
80 }
81 
85 int vpFeatureMoment::getDimension(unsigned int select) const
86 {
87  int dim = 0;
88 
89  for (unsigned int i = 0; i < dim_s; ++i)
90  if (vpBasicFeature::FEATURE_LINE[i] & select)
91  dim++;
92 
93  return dim;
94 }
95 
99 void vpFeatureMoment::print(unsigned int select) const
100 {
101  for (unsigned int i = 0; i < dim_s; ++i) {
102  if (vpBasicFeature::FEATURE_LINE[i] & select) {
103  std::cout << s[i] << ",";
104  }
105  }
106 
107  std::cout << std::endl;
108 }
109 
115  unsigned int thickness) const
116 {
117  // visual representation of a moment doesn't often make sense
118  (void)cam;
119  (void)I;
120  (void)color;
121  (void)thickness;
122 }
123 
128 void vpFeatureMoment::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
129  unsigned int thickness) const
130 {
131  (void)cam;
132  (void)I;
133  (void)color;
134  (void)thickness;
135 }
136 
153 void vpFeatureMoment::update(double A_, double B_, double C_)
154 {
155  this->A = A_;
156  this->B = B_;
157  this->C = C_;
158 
159  if (moment == NULL) {
160  bool found;
161  this->moment = &(moments.get(momentName(), found));
162  if (!found)
163  throw vpException(vpException::notInitialized, "Moment not found for feature");
164  }
165  nbParameters = 1;
166  if (this->moment != NULL) {
167  dim_s = (unsigned int)this->moment->get().size();
168 
169  s.resize(dim_s);
170 
171  for (unsigned int i = 0; i < dim_s; i++)
172  s[i] = this->moment->get()[i];
173 
174  if (flags == NULL)
175  flags = new bool[nbParameters];
176  for (unsigned int i = 0; i < nbParameters; i++)
177  flags[i] = false;
178  } else
179  dim_s = 0;
180 
182 }
183 
200 {
201  vpMatrix L(0, 0);
202 
203  for (unsigned int i = 0; i < dim_s; ++i) {
204  if (vpBasicFeature::FEATURE_LINE[i] & select) {
206  }
207  }
208 
209  return L;
210 }
211 
221 {
224  feat->dim_s = dim_s;
225  feat->nbParameters = nbParameters;
226  // memory allocation
227  feat->s.resize(dim_s);
228  for (unsigned int i = 0; i < dim_s; i++)
229  feat->s[i] = this->s[i];
230 
231  feat->flags = new bool[(unsigned int)nbParameters];
232  for (unsigned int i = 0; i < (unsigned int)nbParameters; i++)
233  feat->flags[i] = flags[i];
234 
235  return feat;
236 }
237 
245 {
246  if (strlen(name()) >= 255) {
247  throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the moment name"));
248  }
249 
250  std::strcpy(_name, name());
251  this->featureMomentsDataBase = &featureMoments;
252 
253  featureMoments.add(*this, _name);
254 }
255 
257 
259 
260 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpFeatureMoment &featM)
261 {
262  /*
263  A const_cast is forced here since interaction() defined in vpBasicFeature()
264  is not const But introducing const in vpBasicFeature() can break a lot of
265  client code
266  */
267  vpMatrix Lcomplete((unsigned int)featM.getDimension(),
268  6); // 6 corresponds to 6velocities in standard interaction matrix
269  Lcomplete = const_cast<vpFeatureMoment &>(featM).interaction(vpBasicFeature::FEATURE_ALL);
270  Lcomplete.matlabPrint(os);
271  return os;
272 }
273 
281 void vpFeatureMoment::printDependencies(std::ostream &os) const
282 {
283  os << " WARNING : Falling back to base class version of "
284  "printDependencies() in vpFeatureMoment. To prevent that, this has "
285  "to be implemented in the derived classes!"
286  << std::endl;
287 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
virtual const char * name() const =0
const vpMoment * moment
virtual void compute_interaction(void)
vpBasicFeature * duplicate() const
void linkTo(vpFeatureMomentDatabase &featureMoments)
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
const vpMoment & get(const char *type, bool &found) const
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4800
unsigned int dim_s
Dimension of the visual feature.
error that can be emited by ViSP classes.
Definition: vpException.h:71
std::vector< vpMatrix > interaction_matrices
This class defines a generic feature used for moment feature duplication.
const std::vector< double > & get() const
Definition: vpMoment.h:155
virtual ~vpFeatureMoment()
virtual const char * momentName() const =0
class that defines what is a visual feature
int getDimension(unsigned int select=FEATURE_ALL) const
std::ostream & matlabPrint(std::ostream &os) const
Definition: vpMatrix.cpp:4642
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpFeatureMoment &featM)
This class defines shared system methods/attributes for 2D moment features but no functional code...
Memory allocation error.
Definition: vpException.h:88
Generic class defining intrinsic camera parameters.
vpMomentDatabase & moments
void update(double A, double B, double C)
static const unsigned int FEATURE_LINE[32]
vpFeatureMomentDatabase * featureMomentsDataBase
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
void print(const unsigned int select=FEATURE_ALL) const
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector s
State of the visual feature.
This class allows to register all feature moments (implemented in vpFeatureMoment... classes) so they can access each other according to their dependencies.
virtual void printDependencies(std::ostream &os) const
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310