Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpBasicFeature.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  * Visual feature.
33  *
34  * Authors:
35  * Eric Marchand
36  * Nicolas Mansard
37  *
38  *****************************************************************************/
39 
40 #include <visp3/visual_features/vpBasicFeature.h>
41 
42 const unsigned int vpBasicFeature::FEATURE_LINE[32] = {
43  (unsigned int)(1 << 0), (unsigned int)(1 << 1), (unsigned int)(1 << 2), (unsigned int)(1 << 3),
44  (unsigned int)(1 << 4), (unsigned int)(1 << 5), (unsigned int)(1 << 6), (unsigned int)(1 << 7),
45  (unsigned int)(1 << 8), (unsigned int)(1 << 9), (unsigned int)(1 << 10), (unsigned int)(1 << 11),
46  (unsigned int)(1 << 12), (unsigned int)(1 << 13), (unsigned int)(1 << 14), (unsigned int)(1 << 15),
47  (unsigned int)(1 << 16), (unsigned int)(1 << 17), (unsigned int)(1 << 18), (unsigned int)(1 << 19),
48  (unsigned int)(1 << 20), (unsigned int)(1 << 21), (unsigned int)(1 << 22), (unsigned int)(1 << 23),
49  (unsigned int)(1 << 24), (unsigned int)(1 << 25), (unsigned int)(1 << 26), (unsigned int)(1 << 27),
50  (unsigned int)(1 << 28), (unsigned int)(1 << 29), (unsigned int)(1 << 30), (unsigned int)(1 << 31)};
51 
59 vpBasicFeature::vpBasicFeature() : s(), dim_s(0), flags(NULL), nbParameters(0), deallocate(vpBasicFeature::user) {}
60 
65 {
66  if (flags != NULL) {
67  delete[] flags;
68  flags = NULL;
69  }
70 }
71 
76  : s(), dim_s(0), flags(NULL), nbParameters(0), deallocate(vpBasicFeature::user)
77 {
78  *this = f;
79 }
80 
85 {
86  s = f.s;
87  dim_s = f.dim_s;
90  if (flags)
91  delete[] flags;
92  flags = new bool[nbParameters];
93  for (unsigned int i = 0; i < nbParameters; i++)
94  flags[i] = f.flags[i];
95 
96  return (*this);
97 }
98 
100 unsigned int vpBasicFeature::getDimension(unsigned int select) const
101 {
102  unsigned int dim = 0;
103  if (dim_s > 31)
104  return dim_s;
105  for (unsigned int i = 0; i < s.getRows(); i++) {
106  // printf("%x %x %d \n",select, featureLine[i], featureLine[i] & select);
107  if (FEATURE_LINE[i] & select)
108  dim += 1;
109  }
110  return dim;
111 }
112 
114 vpColVector vpBasicFeature::get_s(const unsigned int select) const
115 {
116  vpColVector state(0), stateLine(1);
117  // if s is higher than the possible selections (photometry), send back the
118  // whole vector
119  if (dim_s > 31)
120  return s;
121 
122  for (unsigned int i = 0; i < dim_s; ++i) {
123  if (FEATURE_LINE[i] & select) {
124  stateLine[0] = s[i];
125  state.stack(stateLine);
126  }
127  }
128  return state;
129 }
130 
132 {
133  if (flags != NULL) {
134  for (unsigned int i = 0; i < nbParameters; i++)
135  flags[i] = false;
136  }
137 }
138 
142 {
143  if (flags != NULL) {
144  for (unsigned int i = 0; i < nbParameters; i++)
145  flags[i] = true;
146  }
147 }
148 
151 vpColVector vpBasicFeature::error(const vpBasicFeature &s_star, const unsigned int select)
152 {
153  vpColVector e(0), eLine(1);
154  if (dim_s <= 31) {
155  for (unsigned int i = 0; i < dim_s; ++i) {
156  if (FEATURE_LINE[i] & select) {
157  eLine[0] = s[i] - s_star[i];
158  e.stack(eLine);
159  // std::cout << "dim_s <= 31"<<std::endl;
160  }
161  }
162  } else {
163  e.resize(dim_s);
164  vpColVector sd = s_star.get_s();
165  e = s - sd;
166  }
167 
168  return e;
169 }
170 
171 /*
172  * Local variables:
173  * c-basic-offset: 4
174  * End:
175  */
vpBasicFeature & operator=(const vpBasicFeature &f)
unsigned int getDimension(const unsigned int select=FEATURE_ALL) const
Get the feature vector dimension.
unsigned int dim_s
Dimension of the visual feature.
virtual vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
class that defines what is a visual feature
unsigned int getRows() const
Definition: vpArray2D.h:156
virtual ~vpBasicFeature()
static const unsigned int FEATURE_LINE[32]
vpBasicFeatureDeallocatorType deallocate
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void stack(double d)
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector s
State of the visual feature.