Visual Servoing Platform version 3.5.0
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
74void 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
101vpFeatureLuminance::vpFeatureLuminance() : Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), firstTimeIn(0), cam()
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
154{
155 this->Z = Z_;
156 flags[0] = true;
157}
158
165double 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
253vpMatrix 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
284vpColVector 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
299void 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
334void 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{
360 return feature;
361}
362
363/*
364 * Local variables:
365 * c-basic-offset: 2
366 * End:
367 */
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:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
Class that defines the image luminance visual feature.
unsigned int nbr
Number of rows.
void setCameraParameters(vpCameraParameters &_cam)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
vpCameraParameters cam
vpLuminance * pixInfo
Store the image (as a vector with intensity and gradient I, Ix, Iy)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
unsigned int nbc
Number of column.
unsigned int bord
Border size.
virtual ~vpFeatureLuminance()
Destructor.
vpFeatureLuminance & operator=(const vpFeatureLuminance &f)
void buildFrom(vpImage< unsigned char > &I)
vpMatrix interaction(unsigned int select=FEATURE_ALL)
vpFeatureLuminance * duplicate() const
void print(unsigned int select=FEATURE_ALL) const
static double derivativeFilterY(const vpImage< T > &I, unsigned int r, unsigned int c)
Definition: vpImageFilter.h:96
static double derivativeFilterX(const vpImage< T > &I, unsigned int r, unsigned int c)
Definition: vpImageFilter.h:82
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
#define vpERROR_TRACE
Definition: vpDebug.h:393