Visual Servoing Platform  version 3.6.1 under development (2024-04-16)
vpFeatureMoment.cpp
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  * Base for all moment features
32  */
33 
34 #include <visp3/core/vpMath.h>
35 #include <visp3/core/vpMoment.h>
36 #include <visp3/core/vpMomentDatabase.h>
37 #include <visp3/visual_features/vpFeatureMoment.h>
38 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
39 
40 #include <visp3/core/vpException.h>
41 #include <visp3/visual_features/vpFeatureException.h>
42 
43 #include <vector>
44 #include <visp3/core/vpDebug.h>
45 
46 class vpBasicFeature;
47 
52 {
53  // feature dimension
54  /*
55  * The dimension of the visual feature is set according to the size of the
56  * vpMoment associated to it. This partly explains why vpFeatureMomentBasic
57  * cannot be used directly as a visual feature.
58  */
59  if (this->moment != nullptr)
60  dim_s = (unsigned int)this->moment->get().size();
61  else
62  dim_s = 0;
63 
64  nbParameters = 1;
65 
66  // memory allocation
67  s.resize(dim_s);
68  for (unsigned int i = 0; i < dim_s; i++)
69  s[i] = 0;
70 
71  if (flags == nullptr)
72  flags = new bool[nbParameters];
73  for (unsigned int i = 0; i < nbParameters; i++)
74  flags[i] = false;
75 }
76 
80 int vpFeatureMoment::getDimension(unsigned int select) const
81 {
82  int dim = 0;
83 
84  for (unsigned int i = 0; i < dim_s; ++i)
85  if (vpBasicFeature::FEATURE_LINE[i] & select)
86  dim++;
87 
88  return dim;
89 }
90 
94 void vpFeatureMoment::print(unsigned int select) const
95 {
96  for (unsigned int i = 0; i < dim_s; ++i) {
97  if (vpBasicFeature::FEATURE_LINE[i] & select) {
98  std::cout << s[i] << ",";
99  }
100  }
101 
102  std::cout << std::endl;
103 }
104 
110  unsigned int thickness) const
111 {
112  // visual representation of a moment doesn't often make sense
113  (void)cam;
114  (void)I;
115  (void)color;
116  (void)thickness;
117 }
118 
122 void vpFeatureMoment::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
123  unsigned int thickness) const
124 {
125  (void)cam;
126  (void)I;
127  (void)color;
128  (void)thickness;
129 }
130 
147 void vpFeatureMoment::update(double A_, double B_, double C_)
148 {
149  this->A = A_;
150  this->B = B_;
151  this->C = C_;
152 
153  if (moment == nullptr) {
154  bool found;
155  this->moment = &(moments.get(momentName(), found));
156  if (!found)
157  throw vpException(vpException::notInitialized, "Moment not found for feature");
158  }
159  nbParameters = 1;
160  if (this->moment != nullptr) {
161  dim_s = (unsigned int)this->moment->get().size();
162 
163  s.resize(dim_s);
164 
165  for (unsigned int i = 0; i < dim_s; i++)
166  s[i] = this->moment->get()[i];
167 
168  if (flags == nullptr)
169  flags = new bool[nbParameters];
170  for (unsigned int i = 0; i < nbParameters; i++)
171  flags[i] = false;
172  }
173  else
174  dim_s = 0;
175 
177 }
178 
195 {
196  vpMatrix L(0, 0);
197 
198  for (unsigned int i = 0; i < dim_s; ++i) {
199  if (vpBasicFeature::FEATURE_LINE[i] & select) {
200  L.stack(interaction_matrices[i]);
201  }
202  }
203 
204  return L;
205 }
206 
217 {
220  feat->dim_s = dim_s;
221  feat->nbParameters = nbParameters;
222  // memory allocation
223  feat->s.resize(dim_s);
224  for (unsigned int i = 0; i < dim_s; i++)
225  feat->s[i] = this->s[i];
226 
227  feat->flags = new bool[(unsigned int)nbParameters];
228  for (unsigned int i = 0; i < (unsigned int)nbParameters; i++)
229  feat->flags[i] = flags[i];
230 
231  return feat;
232 }
233 
241 {
242  m_name = name();
243  this->featureMomentsDataBase = &featureMoments;
244 
245  featureMoments.add(*this, m_name);
246 }
247 
249 
250 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpFeatureMoment &featM)
251 {
252  /*
253  * - A static_cast is forced here since interaction() defined in vpBasicFeature()
254  * is not const. But introducing const in vpBasicFeature() can break a lot of
255  * client code.
256  * - 6 corresponds to 6 velocities in standard interaction matrix
257  */
258  vpMatrix Lcomplete(static_cast<unsigned int>(featM.getDimension()), 6);
259  Lcomplete = const_cast<vpFeatureMoment &>(featM).interaction(vpBasicFeature::FEATURE_ALL);
260  Lcomplete.matlabPrint(os);
261  return os;
262 }
263 
271 void vpFeatureMoment::printDependencies(std::ostream &os) const
272 {
273  os << " WARNING : Falling back to base class version of "
274  "printDependencies() in vpFeatureMoment. To prevent that, this has "
275  "to be implemented in the derived classes!"
276  << std::endl;
277 }
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:587
class that defines what is a visual feature
vpColVector s
State of the visual feature.
static const unsigned int FEATURE_LINE[32]
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
virtual vpMatrix interaction(unsigned int select=FEATURE_ALL)=0
Compute the interaction matrix from a subset of the possible features.
unsigned int dim_s
Dimension of the visual feature.
Generic class defining intrinsic camera parameters.
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:86
This class allows to register all feature moments (implemented in vpFeatureMoment....
This class defines shared system methods/attributes for 2D moment features but no functional code....
std::string m_name
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const vp_override
virtual void printDependencies(std::ostream &os) const
std::vector< vpMatrix > interaction_matrices
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
void print(unsigned int select=FEATURE_ALL) const vp_override
vpFeatureMomentDatabase * featureMomentsDataBase
void init(void) vp_override
vpMomentDatabase & moments
vpBasicFeature * duplicate() const vp_override
int getDimension(unsigned int select=FEATURE_ALL) const
const vpMoment * moment
virtual const std::string name() const =0
void update(double A, double B, double C)
virtual const std::string momentName() const =0
virtual void compute_interaction(void)
void linkTo(vpFeatureMomentDatabase &featureMoments)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
std::ostream & matlabPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5501
const vpMoment & get(const std::string &moment_name, bool &found) const
This class defines a generic feature used for moment feature duplication.
const std::vector< double > & get() const
Definition: vpMoment.h:149