Visual Servoing Platform  version 3.6.1 under development (2024-11-14)
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/vpDebug.h>
42 #include <visp3/core/vpDisplay.h>
43 #include <visp3/core/vpException.h>
44 #include <visp3/core/vpHomogeneousMatrix.h>
45 #include <visp3/core/vpImageConvert.h>
46 #include <visp3/core/vpImageFilter.h>
47 #include <visp3/core/vpMatrix.h>
48 #include <visp3/core/vpPixelMeterConversion.h>
49 
50 #include <visp3/visual_features/vpFeatureLuminance.h>
51 
52 BEGIN_VISP_NAMESPACE
53 
55 
60 {
61  if (flags == nullptr)
62  flags = new bool[nbParameters];
63  for (unsigned int i = 0; i < nbParameters; i++)
64  flags[i] = false;
65 
66  // default value Z (1 meters)
67  Z = 1;
68 
69  firstTimeIn = 0;
70 
71  nbr = nbc = 0;
72 }
73 
74 void vpFeatureLuminance::init(unsigned int _nbr, unsigned int _nbc, double _Z)
75 {
76  init();
77 
78  nbr = _nbr;
79  nbc = _nbc;
80 
81  if ((nbr < 2 * bord) || (nbc < 2 * bord)) {
82  throw vpException(vpException::dimensionError, "border is too important compared to number of row or column.");
83  }
84 
85  // number of feature = nb column x nb lines in the images
86  dim_s = (nbr - 2 * bord) * (nbc - 2 * bord);
87 
88  s.resize(dim_s);
89 
90  if (pixInfo != nullptr)
91  delete[] pixInfo;
92 
93  pixInfo = new vpLuminance[dim_s];
94 
95  Z = _Z;
96 }
97 
101 vpFeatureLuminance::vpFeatureLuminance() : Z(1), nbr(0), nbc(0), bord(DEFAULT_BORDER), pixInfo(nullptr), firstTimeIn(0), cam()
102 {
103  nbParameters = 1;
104  dim_s = 0;
105  if (flags != nullptr) {
106  delete[] flags;
107  }
108  flags = nullptr;
109 
110  init();
111 }
112 
117  : vpBasicFeature(f), Z(1), nbr(0), nbc(0), bord(DEFAULT_BORDER), pixInfo(nullptr), firstTimeIn(0), cam()
118 {
119  *this = f;
120 }
121 
126 {
127  Z = f.Z;
128  nbr = f.nbr;
129  nbc = f.nbc;
130  bord = f.bord;
132  cam = f.cam;
133  dim_s = f.dim_s;
134  if (pixInfo)
135  delete[] pixInfo;
136  pixInfo = new vpLuminance[dim_s];
137  for (unsigned int i = 0; i < dim_s; i++)
138  pixInfo[i] = f.pixInfo[i];
139  s.resize(dim_s);
140  return (*this);
141 }
142 
147 {
148  if (pixInfo != nullptr) {
149  delete[] pixInfo;
150  }
151 }
152 
160 {
161  this->Z = Z_;
162  flags[0] = true;
163 }
164 
171 double vpFeatureLuminance::get_Z() const { return Z; }
172 unsigned int vpFeatureLuminance::getBorder() const { return bord; }
173 
175 
182 {
183  unsigned int l = 0;
184  double Ix, Iy;
185 
186  double px = cam.get_px();
187  double py = cam.get_py();
188 
189  if (firstTimeIn == 0) {
190  firstTimeIn = 1;
191  l = 0;
192  for (unsigned int i = bord; i < nbr - bord; i++) {
193  for (unsigned int j = bord; j < nbc - bord; j++) {
194 
195  double x = 0, y = 0;
197 
198  pixInfo[l].x = x;
199  pixInfo[l].y = y;
200  pixInfo[l].Z = Z;
201 
202  ++l;
203  }
204  }
205  }
206 
207  l = 0;
208 
209  for (unsigned int i = bord; i < (nbr - bord); ++i) {
210  for (unsigned int j = bord; j < (nbc - bord); ++j) {
211  Ix = px * vpImageFilter::derivativeFilterX(I, i, j);
212  Iy = py * vpImageFilter::derivativeFilterY(I, i, j);
213 
214  // Calcul de Z
215  pixInfo[l].I = I[i][j];
216  s[l] = I[i][j];
217  pixInfo[l].Ix = Ix;
218  pixInfo[l].Iy = Iy;
219 
220  ++l;
221  }
222  }
223  return *this;
224 }
225 
231 {
232  L.resize(dim_s, 6);
233 
234  for (unsigned int m = 0; m < L.getRows(); m++) {
235  double Ix = pixInfo[m].Ix;
236  double Iy = pixInfo[m].Iy;
237 
238  double x = pixInfo[m].x;
239  double y = pixInfo[m].y;
240  double Zinv = 1 / pixInfo[m].Z;
241 
242  {
243  L[m][0] = Ix * Zinv;
244  L[m][1] = Iy * Zinv;
245  L[m][2] = -(x * Ix + y * Iy) * Zinv;
246  L[m][3] = -Ix * x * y - (1 + y * y) * Iy;
247  L[m][4] = (1 + x * x) * Ix + Iy * x * y;
248  L[m][5] = Iy * x - Ix * y;
249  }
250  }
251 }
252 
257 vpMatrix vpFeatureLuminance::interaction(const unsigned int /* select */)
258 {
259  /* static */ vpMatrix L; // warning C4640: 'L' : construction of local
260  // static object is not thread-safe
261  interaction(L);
262  return L;
263 }
264 
273 {
274  e.resize(dim_s);
275 
276  for (unsigned int i = 0; i < dim_s; i++) {
277  e[i] = s[i] - s_star[i];
278  }
279 }
280 
288 vpColVector vpFeatureLuminance::error(const vpBasicFeature &s_star, const unsigned int /* select */)
289 {
290  /* static */ vpColVector e; // warning C4640: 'e' : construction of local
291  // static object is not thread-safe
292 
293  error(s_star, e);
294 
295  return e;
296 }
297 
303 void vpFeatureLuminance::print(const unsigned int /* select */) const
304 {
305  static int firsttime = 0;
306 
307  if (firsttime == 0) {
308  firsttime = 1;
309  vpERROR_TRACE("not implemented");
310  // Do not throw and error since it is not subject
311  // to produce a failure
312  }
313 }
314 
321  const vpColor & /* color */, unsigned int /* thickness */) const
322 {
323  static int firsttime = 0;
324 
325  if (firsttime == 0) {
326  firsttime = 1;
327  vpERROR_TRACE("not implemented");
328  // Do not throw and error since it is not subject
329  // to produce a failure
330  }
331 }
332 
338 void vpFeatureLuminance::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
339  const vpColor & /* color */, unsigned int /* thickness */) const
340 {
341  static int firsttime = 0;
342 
343  if (firsttime == 0) {
344  firsttime = 1;
345  vpERROR_TRACE("not implemented");
346  // Do not throw and error since it is not subject
347  // to produce a failure
348  }
349 }
350 
362 {
363  vpFeatureLuminance *feature = new vpFeatureLuminance;
364  return feature;
365 }
366 
367 END_VISP_NAMESPACE
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:191
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
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.
vpFeatureLuminance & buildFrom(vpImage< unsigned char > &I)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) VP_OVERRIDE
void init() VP_OVERRIDE
vpMatrix interaction(unsigned int select=FEATURE_ALL) VP_OVERRIDE
unsigned int nbr
Number of rows.
static const int DEFAULT_BORDER
void print(unsigned int select=FEATURE_ALL) const VP_OVERRIDE
void setCameraParameters(const vpCameraParameters &_cam)
vpCameraParameters cam
vpLuminance * pixInfo
Store the image (as a vector with intensity and gradient I, Ix, Iy)
vpFeatureLuminance * duplicate() const VP_OVERRIDE
virtual ~vpFeatureLuminance() VP_OVERRIDE
Destructor.
unsigned int getBorder() const
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const VP_OVERRIDE
unsigned int nbc
Number of column.
unsigned int bord
Border size.
vpFeatureLuminance & operator=(const vpFeatureLuminance &f)
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:169
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)