Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
vpFeatureLuminance.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  * Luminance feature.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpDisplay.h>
40 #include <visp3/core/vpException.h>
41 #include <visp3/core/vpHomogeneousMatrix.h>
42 #include <visp3/core/vpImageConvert.h>
43 #include <visp3/core/vpImageFilter.h>
44 #include <visp3/core/vpMatrix.h>
45 #include <visp3/core/vpPixelMeterConversion.h>
46 
47 #include <visp3/visual_features/vpFeatureLuminance.h>
48 
60 {
61  if (flags == NULL)
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 != NULL)
91  delete[] pixInfo;
92 
93  pixInfo = new vpLuminance[dim_s];
94 
95  Z = _Z;
96 }
97 
102 {
103  nbParameters = 1;
104  dim_s = 0;
105  flags = NULL;
106 
107  init();
108 }
109 
114  : vpBasicFeature(f), Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), firstTimeIn(0), cam()
115 {
116  *this = f;
117 }
118 
123 {
124  Z = f.Z;
125  nbr = f.nbr;
126  nbc = f.nbc;
127  bord = f.bord;
129  cam = f.cam;
130  if (pixInfo)
131  delete[] pixInfo;
132  pixInfo = new vpLuminance[dim_s];
133  for (unsigned int i = 0; i < dim_s; i++)
134  pixInfo[i] = f.pixInfo[i];
135  return (*this);
136 }
137 
142 {
143  if (pixInfo != NULL)
144  delete[] pixInfo;
145 }
146 
153 void vpFeatureLuminance::set_Z(const double Z_)
154 {
155  this->Z = Z_;
156  flags[0] = true;
157 }
158 
165 double vpFeatureLuminance::get_Z() const { return Z; }
166 
168 
175 {
176  unsigned int l = 0;
177  double Ix, Iy;
178 
179  double px = cam.get_px();
180  double py = cam.get_py();
181 
182  if (firstTimeIn == 0) {
183  firstTimeIn = 1;
184  l = 0;
185  for (unsigned int i = bord; i < nbr - bord; i++) {
186  // cout << i << endl ;
187  for (unsigned int j = bord; j < nbc - bord; j++) {
188  double x = 0, y = 0;
190 
191  pixInfo[l].x = x;
192  pixInfo[l].y = y;
193 
194  pixInfo[l].Z = Z;
195 
196  l++;
197  }
198  }
199  }
200 
201  l = 0;
202  for (unsigned int i = bord; i < nbr - bord; i++) {
203  // cout << i << endl ;
204  for (unsigned int j = bord; j < nbc - bord; j++) {
205  // cout << dim_s <<" " <<l <<" " <<i << " " << j <<endl ;
206  Ix = px * vpImageFilter::derivativeFilterX(I, i, j);
207  Iy = py * vpImageFilter::derivativeFilterY(I, i, j);
208 
209  // Calcul de Z
210 
211  pixInfo[l].I = I[i][j];
212  s[l] = I[i][j];
213  pixInfo[l].Ix = Ix;
214  pixInfo[l].Iy = Iy;
215 
216  l++;
217  }
218  }
219 }
220 
227 {
228  L.resize(dim_s, 6);
229 
230  for (unsigned int m = 0; m < L.getRows(); m++) {
231  double Ix = pixInfo[m].Ix;
232  double Iy = pixInfo[m].Iy;
233 
234  double x = pixInfo[m].x;
235  double y = pixInfo[m].y;
236  double Zinv = 1 / pixInfo[m].Z;
237 
238  {
239  L[m][0] = Ix * Zinv;
240  L[m][1] = Iy * Zinv;
241  L[m][2] = -(x * Ix + y * Iy) * Zinv;
242  L[m][3] = -Ix * x * y - (1 + y * y) * Iy;
243  L[m][4] = (1 + x * x) * Ix + Iy * x * y;
244  L[m][5] = Iy * x - Ix * y;
245  }
246  }
247 }
248 
253 vpMatrix vpFeatureLuminance::interaction(const unsigned int /* select */)
254 {
255  /* static */ vpMatrix L; // warning C4640: 'L' : construction of local
256  // static object is not thread-safe
257  interaction(L);
258  return L;
259 }
260 
269 {
270  e.resize(dim_s);
271 
272  for (unsigned int i = 0; i < dim_s; i++) {
273  e[i] = s[i] - s_star[i];
274  }
275 }
276 
284 vpColVector vpFeatureLuminance::error(const vpBasicFeature &s_star, const unsigned int /* select */)
285 {
286  /* static */ vpColVector e; // warning C4640: 'e' : construction of local
287  // static object is not thread-safe
288 
289  error(s_star, e);
290 
291  return e;
292 }
293 
299 void vpFeatureLuminance::print(const unsigned int /* select */) const
300 {
301  static int firsttime = 0;
302 
303  if (firsttime == 0) {
304  firsttime = 1;
305  vpERROR_TRACE("not implemented");
306  // Do not throw and error since it is not subject
307  // to produce a failure
308  }
309 }
310 
317  const vpColor & /* color */, unsigned int /* thickness */) const
318 {
319  static int firsttime = 0;
320 
321  if (firsttime == 0) {
322  firsttime = 1;
323  vpERROR_TRACE("not implemented");
324  // Do not throw and error since it is not subject
325  // to produce a failure
326  }
327 }
328 
334 void vpFeatureLuminance::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
335  const vpColor & /* color */, unsigned int /* thickness */) const
336 {
337  static int firsttime = 0;
338 
339  if (firsttime == 0) {
340  firsttime = 1;
341  vpERROR_TRACE("not implemented");
342  // Do not throw and error since it is not subject
343  // to produce a failure
344  }
345 }
346 
358 {
359  vpFeatureLuminance *feature = new vpFeatureLuminance;
360  return feature;
361 }
362 
363 /*
364  * Local variables:
365  * c-basic-offset: 2
366  * End:
367  */
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
virtual ~vpFeatureLuminance()
Destructor.
vpLuminance * pixInfo
Store the image (as a vector with intensity and gradient I, Ix, Iy)
void print(const unsigned int select=FEATURE_ALL) const
unsigned int bord
Border size.
static double derivativeFilterY(const vpImage< T > &I, const unsigned int r, const unsigned int c)
Definition: vpImageFilter.h:96
void buildFrom(vpImage< unsigned char > &I)
#define vpERROR_TRACE
Definition: vpDebug.h:393
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void setCameraParameters(vpCameraParameters &_cam)
unsigned int dim_s
Dimension of the visual feature.
error that can be emited by ViSP classes.
Definition: vpException.h:71
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
double get_py() const
unsigned int nbr
Number of rows.
class that defines what is a visual feature
vpFeatureLuminance * duplicate() const
void set_Z(const double Z)
Class that defines the image luminance visual feature.
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
vpFeatureLuminance & operator=(const vpFeatureLuminance &f)
Generic class defining intrinsic camera parameters.
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
unsigned int getRows() const
Definition: vpArray2D.h:289
double get_px() const
vpCameraParameters cam
unsigned int nbc
Number of column.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
static double derivativeFilterX(const vpImage< T > &I, const unsigned int r, const unsigned int c)
Definition: vpImageFilter.h:82
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector s
State of the visual feature.
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310