Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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 == NULL)
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
71void 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 != NULL)
88 delete[] pixInfo;
89
90 pixInfo = new vpLuminance[dim_s];
91
92 Z = _Z;
93}
94
98vpFeatureLuminance::vpFeatureLuminance() : Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), firstTimeIn(0), cam()
99{
100 nbParameters = 1;
101 dim_s = 0;
102 flags = NULL;
103
104 init();
105}
106
111 : vpBasicFeature(f), Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), 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 != NULL)
141 delete[] pixInfo;
142}
143
151{
152 this->Z = Z_;
153 flags[0] = true;
154}
155
162double vpFeatureLuminance::get_Z() const { return Z; }
163
165
172{
173 unsigned int l = 0;
174 double Ix, Iy;
175
176 double px = cam.get_px();
177 double py = cam.get_py();
178
179 if (firstTimeIn == 0) {
180 firstTimeIn = 1;
181 l = 0;
182 for (unsigned int i = bord; i < nbr - bord; i++) {
183 // cout << i << endl ;
184 for (unsigned int j = bord; j < nbc - bord; j++) {
185 double x = 0, y = 0;
187
188 pixInfo[l].x = x;
189 pixInfo[l].y = y;
190
191 pixInfo[l].Z = Z;
192
193 l++;
194 }
195 }
196 }
197
198 l = 0;
199 for (unsigned int i = bord; i < nbr - bord; i++) {
200 // cout << i << endl ;
201 for (unsigned int j = bord; j < nbc - bord; j++) {
202 // cout << dim_s <<" " <<l <<" " <<i << " " << j <<endl ;
203 Ix = px * vpImageFilter::derivativeFilterX(I, i, j);
204 Iy = py * vpImageFilter::derivativeFilterY(I, i, j);
205
206 // Calcul de Z
207
208 pixInfo[l].I = I[i][j];
209 s[l] = I[i][j];
210 pixInfo[l].Ix = Ix;
211 pixInfo[l].Iy = Iy;
212
213 l++;
214 }
215 }
216}
217
224{
225 L.resize(dim_s, 6);
226
227 for (unsigned int m = 0; m < L.getRows(); m++) {
228 double Ix = pixInfo[m].Ix;
229 double Iy = pixInfo[m].Iy;
230
231 double x = pixInfo[m].x;
232 double y = pixInfo[m].y;
233 double Zinv = 1 / pixInfo[m].Z;
234
235 {
236 L[m][0] = Ix * Zinv;
237 L[m][1] = Iy * Zinv;
238 L[m][2] = -(x * Ix + y * Iy) * Zinv;
239 L[m][3] = -Ix * x * y - (1 + y * y) * Iy;
240 L[m][4] = (1 + x * x) * Ix + Iy * x * y;
241 L[m][5] = Iy * x - Ix * y;
242 }
243 }
244}
245
250vpMatrix vpFeatureLuminance::interaction(const unsigned int /* select */)
251{
252 /* static */ vpMatrix L; // warning C4640: 'L' : construction of local
253 // static object is not thread-safe
254 interaction(L);
255 return L;
256}
257
266{
267 e.resize(dim_s);
268
269 for (unsigned int i = 0; i < dim_s; i++) {
270 e[i] = s[i] - s_star[i];
271 }
272}
273
281vpColVector vpFeatureLuminance::error(const vpBasicFeature &s_star, const unsigned int /* select */)
282{
283 /* static */ vpColVector e; // warning C4640: 'e' : construction of local
284 // static object is not thread-safe
285
286 error(s_star, e);
287
288 return e;
289}
290
296void vpFeatureLuminance::print(const unsigned int /* select */) const
297{
298 static int firsttime = 0;
299
300 if (firsttime == 0) {
301 firsttime = 1;
302 vpERROR_TRACE("not implemented");
303 // Do not throw and error since it is not subject
304 // to produce a failure
305 }
306}
307
314 const vpColor & /* color */, unsigned int /* thickness */) 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
331void vpFeatureLuminance::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
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
355{
357 return feature;
358}
359
360/*
361 * Local variables:
362 * c-basic-offset: 2
363 * End:
364 */
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.
void resize(unsigned int i, bool flagNullify=true)
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.
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)
static double derivativeFilterX(const vpImage< T > &I, unsigned int r, unsigned int c)
Definition of the vpImage class member functions.
Definition vpImage.h:135
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
#define vpERROR_TRACE
Definition vpDebug.h:388