Visual Servoing Platform  version 3.6.1 under development (2024-06-12)
vpFeatureLuminance.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 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  * Luminance feature.
32  */
33 
41 #include <visp3/core/vpDisplay.h>
42 #include <visp3/core/vpException.h>
43 #include <visp3/core/vpHomogeneousMatrix.h>
44 #include <visp3/core/vpImageConvert.h>
45 #include <visp3/core/vpImageFilter.h>
46 #include <visp3/core/vpMatrix.h>
47 #include <visp3/core/vpPixelMeterConversion.h>
48 
49 #include <visp3/visual_features/vpFeatureLuminance.h>
50 
52 
54 
59 {
60  if (flags == nullptr)
61  flags = new bool[nbParameters];
62  for (unsigned int i = 0; i < nbParameters; i++)
63  flags[i] = false;
64 
65  // default value Z (1 meters)
66  Z = 1;
67 
68  firstTimeIn = 0;
69 
70  nbr = nbc = 0;
71 }
72 
73 void vpFeatureLuminance::init(unsigned int _nbr, unsigned int _nbc, double _Z)
74 {
75  init();
76 
77  nbr = _nbr;
78  nbc = _nbc;
79 
80  if ((nbr < 2 * bord) || (nbc < 2 * bord)) {
81  throw vpException(vpException::dimensionError, "border is too important compared to number of row or column.");
82  }
83 
84  // number of feature = nb column x nb lines in the images
85  dim_s = (nbr - 2 * bord) * (nbc - 2 * bord);
86 
87  s.resize(dim_s);
88 
89  if (pixInfo != nullptr)
90  delete[] pixInfo;
91 
92  pixInfo = new vpLuminance[dim_s];
93 
94  Z = _Z;
95 }
96 
100 vpFeatureLuminance::vpFeatureLuminance() : Z(1), nbr(0), nbc(0), bord(DEFAULT_BORDER), pixInfo(nullptr), firstTimeIn(0), cam()
101 {
102  nbParameters = 1;
103  dim_s = 0;
104  if (flags != nullptr) {
105  delete[] flags;
106  }
107  flags = nullptr;
108 
109  init();
110 }
111 
116  : vpBasicFeature(f), Z(1), nbr(0), nbc(0), bord(DEFAULT_BORDER), pixInfo(nullptr), firstTimeIn(0), cam()
117 {
118  *this = f;
119 }
120 
125 {
126  Z = f.Z;
127  nbr = f.nbr;
128  nbc = f.nbc;
129  bord = f.bord;
131  cam = f.cam;
132  dim_s = f.dim_s;
133  if (pixInfo)
134  delete[] pixInfo;
135  pixInfo = new vpLuminance[dim_s];
136  for (unsigned int i = 0; i < dim_s; i++)
137  pixInfo[i] = f.pixInfo[i];
138  s.resize(dim_s);
139  return (*this);
140 }
141 
146 {
147  if (pixInfo != nullptr) {
148  delete[] pixInfo;
149  }
150 }
151 
159 {
160  this->Z = Z_;
161  flags[0] = true;
162 }
163 
170 double vpFeatureLuminance::get_Z() const { return Z; }
171 unsigned int vpFeatureLuminance::getBorder() const { return bord; }
172 
174 
175 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
182 {
183  build(I);
184 }
185 #endif
186 
193 {
194  unsigned int l = 0;
195  double Ix, Iy;
196 
197  double px = cam.get_px();
198  double py = cam.get_py();
199 
200  if (firstTimeIn == 0) {
201  firstTimeIn = 1;
202  l = 0;
203  for (unsigned int i = bord; i < nbr - bord; i++) {
204  for (unsigned int j = bord; j < nbc - bord; j++) {
205 
206  double x = 0, y = 0;
208 
209  pixInfo[l].x = x;
210  pixInfo[l].y = y;
211  pixInfo[l].Z = Z;
212 
213  ++l;
214  }
215  }
216  }
217 
218  l = 0;
219 
220  for (unsigned int i = bord; i < (nbr - bord); ++i) {
221  for (unsigned int j = bord; j < (nbc - bord); ++j) {
222  Ix = px * vpImageFilter::derivativeFilterX(I, i, j);
223  Iy = py * vpImageFilter::derivativeFilterY(I, i, j);
224 
225  // Calcul de Z
226  pixInfo[l].I = I[i][j];
227  s[l] = I[i][j];
228  pixInfo[l].Ix = Ix;
229  pixInfo[l].Iy = Iy;
230 
231  ++l;
232  }
233  }
234  return *this;
235 }
236 
242 {
243  L.resize(dim_s, 6);
244 
245  for (unsigned int m = 0; m < L.getRows(); m++) {
246  double Ix = pixInfo[m].Ix;
247  double Iy = pixInfo[m].Iy;
248 
249  double x = pixInfo[m].x;
250  double y = pixInfo[m].y;
251  double Zinv = 1 / pixInfo[m].Z;
252 
253  {
254  L[m][0] = Ix * Zinv;
255  L[m][1] = Iy * Zinv;
256  L[m][2] = -(x * Ix + y * Iy) * Zinv;
257  L[m][3] = -Ix * x * y - (1 + y * y) * Iy;
258  L[m][4] = (1 + x * x) * Ix + Iy * x * y;
259  L[m][5] = Iy * x - Ix * y;
260  }
261  }
262 }
263 
268 vpMatrix vpFeatureLuminance::interaction(const unsigned int /* select */)
269 {
270  /* static */ vpMatrix L; // warning C4640: 'L' : construction of local
271  // static object is not thread-safe
272  interaction(L);
273  return L;
274 }
275 
284 {
285  e.resize(dim_s);
286 
287  for (unsigned int i = 0; i < dim_s; i++) {
288  e[i] = s[i] - s_star[i];
289  }
290 }
291 
299 vpColVector vpFeatureLuminance::error(const vpBasicFeature &s_star, const unsigned int /* select */)
300 {
301  /* static */ vpColVector e; // warning C4640: 'e' : construction of local
302  // static object is not thread-safe
303 
304  error(s_star, e);
305 
306  return e;
307 }
308 
314 void vpFeatureLuminance::print(const unsigned int /* select */) const
315 {
316  static int firsttime = 0;
317 
318  if (firsttime == 0) {
319  firsttime = 1;
320  vpERROR_TRACE("not implemented");
321  // Do not throw and error since it is not subject
322  // to produce a failure
323  }
324 }
325 
332  const vpColor & /* color */, unsigned int /* thickness */) const
333 {
334  static int firsttime = 0;
335 
336  if (firsttime == 0) {
337  firsttime = 1;
338  vpERROR_TRACE("not implemented");
339  // Do not throw and error since it is not subject
340  // to produce a failure
341  }
342 }
343 
349 void vpFeatureLuminance::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
350  const vpColor & /* color */, unsigned int /* thickness */) const
351 {
352  static int firsttime = 0;
353 
354  if (firsttime == 0) {
355  firsttime = 1;
356  vpERROR_TRACE("not implemented");
357  // Do not throw and error since it is not subject
358  // to produce a failure
359  }
360 }
361 
373 {
374  vpFeatureLuminance *feature = new vpFeatureLuminance;
375  return feature;
376 }
377 
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:171
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1066
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:153
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
Class that defines the image luminance visual feature.
unsigned int nbr
Number of rows.
vpFeatureLuminance * duplicate() const vp_override
static const int DEFAULT_BORDER
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
void setCameraParameters(const vpCameraParameters &_cam)
vpCameraParameters cam
virtual ~vpFeatureLuminance() vp_override
Destructor.
vpLuminance * pixInfo
Store the image (as a vector with intensity and gradient I, Ix, Iy)
vpFeatureLuminance & build(vpImage< unsigned char > &I)
unsigned int getBorder() const
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:151
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
#define vpERROR_TRACE
Definition: vpDebug.h:385