Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpFeatureLuminance.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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  * Luminance feature.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/core/vpDisplay.h>
37 #include <visp3/core/vpException.h>
38 #include <visp3/core/vpHomogeneousMatrix.h>
39 #include <visp3/core/vpImageConvert.h>
40 #include <visp3/core/vpImageFilter.h>
41 #include <visp3/core/vpMatrix.h>
42 #include <visp3/core/vpPixelMeterConversion.h>
43 
44 #include <visp3/visual_features/vpFeatureLuminance.h>
45 
57 {
58  if (flags == nullptr)
59  flags = new bool[nbParameters];
60  for (unsigned int i = 0; i < nbParameters; i++)
61  flags[i] = false;
62 
63  // default value Z (1 meters)
64  Z = 1;
65 
66  firstTimeIn = 0;
67 
68  nbr = nbc = 0;
69 }
70 
71 void vpFeatureLuminance::init(unsigned int _nbr, unsigned int _nbc, double _Z)
72 {
73  init();
74 
75  nbr = _nbr;
76  nbc = _nbc;
77 
78  if ((nbr < 2 * bord) || (nbc < 2 * bord)) {
79  throw vpException(vpException::dimensionError, "border is too important compared to number of row or column.");
80  }
81 
82  // number of feature = nb column x nb lines in the images
83  dim_s = (nbr - 2 * bord) * (nbc - 2 * bord);
84 
85  s.resize(dim_s);
86 
87  if (pixInfo != nullptr)
88  delete[] pixInfo;
89 
90  pixInfo = new vpLuminance[dim_s];
91 
92  Z = _Z;
93 }
94 
98 vpFeatureLuminance::vpFeatureLuminance() : Z(1), nbr(0), nbc(0), bord(10), pixInfo(nullptr), firstTimeIn(0), cam()
99 {
100  nbParameters = 1;
101  dim_s = 0;
102  flags = nullptr;
103 
104  init();
105 }
106 
111  : vpBasicFeature(f), Z(1), nbr(0), nbc(0), bord(10), pixInfo(nullptr), firstTimeIn(0), cam()
112 {
113  *this = f;
114 }
115 
120 {
121  Z = f.Z;
122  nbr = f.nbr;
123  nbc = f.nbc;
124  bord = f.bord;
126  cam = f.cam;
127  if (pixInfo)
128  delete[] pixInfo;
129  pixInfo = new vpLuminance[dim_s];
130  for (unsigned int i = 0; i < dim_s; i++)
131  pixInfo[i] = f.pixInfo[i];
132  return (*this);
133 }
134 
139 {
140  if (pixInfo != nullptr) {
141  delete[] pixInfo;
142  }
143 }
144 
152 {
153  this->Z = Z_;
154  flags[0] = true;
155 }
156 
163 double vpFeatureLuminance::get_Z() const { return Z; }
164 
166 
173 {
174  unsigned int l = 0;
175  double Ix, Iy;
176 
177  double px = cam.get_px();
178  double py = cam.get_py();
179 
180  if (firstTimeIn == 0) {
181  firstTimeIn = 1;
182  l = 0;
183  for (unsigned int i = bord; i < nbr - bord; i++) {
184  // cout << i << endl ;
185  for (unsigned int j = bord; j < nbc - bord; j++) {
186  double x = 0, y = 0;
188 
189  pixInfo[l].x = x;
190  pixInfo[l].y = y;
191 
192  pixInfo[l].Z = Z;
193 
194  l++;
195  }
196  }
197  }
198 
199  l = 0;
200  for (unsigned int i = bord; i < nbr - bord; i++) {
201  // cout << i << endl ;
202  for (unsigned int j = bord; j < nbc - bord; j++) {
203  // cout << dim_s <<" " <<l <<" " <<i << " " << j <<endl ;
204  Ix = px * vpImageFilter::derivativeFilterX(I, i, j);
205  Iy = py * vpImageFilter::derivativeFilterY(I, i, j);
206 
207  // Calcul de Z
208 
209  pixInfo[l].I = I[i][j];
210  s[l] = I[i][j];
211  pixInfo[l].Ix = Ix;
212  pixInfo[l].Iy = Iy;
213 
214  l++;
215  }
216  }
217 }
218 
225 {
226  L.resize(dim_s, 6);
227 
228  for (unsigned int m = 0; m < L.getRows(); m++) {
229  double Ix = pixInfo[m].Ix;
230  double Iy = pixInfo[m].Iy;
231 
232  double x = pixInfo[m].x;
233  double y = pixInfo[m].y;
234  double Zinv = 1 / pixInfo[m].Z;
235 
236  {
237  L[m][0] = Ix * Zinv;
238  L[m][1] = Iy * Zinv;
239  L[m][2] = -(x * Ix + y * Iy) * Zinv;
240  L[m][3] = -Ix * x * y - (1 + y * y) * Iy;
241  L[m][4] = (1 + x * x) * Ix + Iy * x * y;
242  L[m][5] = Iy * x - Ix * y;
243  }
244  }
245 }
246 
251 vpMatrix vpFeatureLuminance::interaction(const unsigned int /* select */)
252 {
253  /* static */ vpMatrix L; // warning C4640: 'L' : construction of local
254  // static object is not thread-safe
255  interaction(L);
256  return L;
257 }
258 
267 {
268  e.resize(dim_s);
269 
270  for (unsigned int i = 0; i < dim_s; i++) {
271  e[i] = s[i] - s_star[i];
272  }
273 }
274 
282 vpColVector vpFeatureLuminance::error(const vpBasicFeature &s_star, const unsigned int /* select */)
283 {
284  /* static */ vpColVector e; // warning C4640: 'e' : construction of local
285  // static object is not thread-safe
286 
287  error(s_star, e);
288 
289  return e;
290 }
291 
297 void vpFeatureLuminance::print(const unsigned int /* select */) const
298 {
299  static int firsttime = 0;
300 
301  if (firsttime == 0) {
302  firsttime = 1;
303  vpERROR_TRACE("not implemented");
304  // Do not throw and error since it is not subject
305  // to produce a failure
306  }
307 }
308 
315  const vpColor & /* color */, unsigned int /* thickness */) const
316 {
317  static int firsttime = 0;
318 
319  if (firsttime == 0) {
320  firsttime = 1;
321  vpERROR_TRACE("not implemented");
322  // Do not throw and error since it is not subject
323  // to produce a failure
324  }
325 }
326 
332 void vpFeatureLuminance::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
333  const vpColor & /* color */, unsigned int /* thickness */) const
334 {
335  static int firsttime = 0;
336 
337  if (firsttime == 0) {
338  firsttime = 1;
339  vpERROR_TRACE("not implemented");
340  // Do not throw and error since it is not subject
341  // to produce a failure
342  }
343 }
344 
356 {
357  vpFeatureLuminance *feature = new vpFeatureLuminance;
358  return feature;
359 }
360 
361 /*
362  * Local variables:
363  * c-basic-offset: 2
364  * End:
365  */
class that defines what is a visual feature
vpColVector s
State of the visual feature.
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
unsigned int dim_s
Dimension of the visual feature.
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
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
@ dimensionError
Bad dimension.
Definition: vpException.h:83
Class that defines the image luminance visual feature.
unsigned int nbr
Number of rows.
vpFeatureLuminance * duplicate() const vp_override
void setCameraParameters(vpCameraParameters &_cam)
void init() vp_override
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const vp_override
vpCameraParameters cam
virtual ~vpFeatureLuminance() vp_override
Destructor.
vpLuminance * pixInfo
Store the image (as a vector with intensity and gradient I, Ix, Iy)
void print(unsigned int select=FEATURE_ALL) const vp_override
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
unsigned int nbc
Number of column.
unsigned int bord
Border size.
vpFeatureLuminance & operator=(const vpFeatureLuminance &f)
void buildFrom(vpImage< unsigned char > &I)
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
static double derivativeFilterX(const vpImage< ImageType > &I, unsigned int r, unsigned int c)
static double derivativeFilterY(const vpImage< ImageType > &I, unsigned int r, unsigned int c)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
#define vpERROR_TRACE
Definition: vpDebug.h:382