Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMeSite.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2023 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 * Moving edges.
32 */
33
39#include <cmath> // std::fabs
40#include <limits> // numeric_limits
41#include <stdlib.h>
42#include <visp3/core/vpTrackingException.h>
43#include <visp3/me/vpMe.h>
44#include <visp3/me/vpMeSite.h>
45
46#ifndef DOXYGEN_SHOULD_SKIP_THIS
47static bool horsImage(int i, int j, int half, int rows, int cols)
48{
49 // return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j
50 // > (cols - half - 3) )) ;
51 int half_1 = half + 1;
52 int half_3 = half + 3;
53 // return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j
54 // > (cols - half - 3) )) ;
55 return ((0 < (half_1 - i)) || ((i - rows + half_3) > 0) || (0 < (half_1 - j)) || ((j - cols + half_3) > 0));
56}
57#endif
58
60{
61 // Site components
62 alpha = 0.0;
63 convlt = 0.0;
64 weight = -1;
65
66 selectDisplay = NONE;
67
68 // Pixel components
69 i = 0;
70 j = 0;
71 ifloat = i;
72 jfloat = j;
73
74 mask_sign = 1;
75
76 normGradient = 0;
77
78 state = NO_SUPPRESSION;
79
80#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
81 suppress = 0;
82#endif
83}
84
86 : i(0), j(0), ifloat(0), jfloat(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0),
87 weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
88#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
89 ,
90 suppress(0)
91#endif
92{ }
93
94vpMeSite::vpMeSite(double ip, double jp)
95 : i(0), j(0), ifloat(0), jfloat(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0),
96 weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
97#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
98 ,
99 suppress(0)
100#endif
101{
102 i = vpMath::round(ip);
103 j = vpMath::round(jp);
104 ifloat = ip;
105 jfloat = jp;
106}
107
109 : i(0), j(0), ifloat(0), jfloat(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0),
110 weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
111#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
112 ,
113 suppress(0)
114#endif
115{
116 *this = mesite;
117}
118
119// More an Update than init
120// For points in meter form (to avoid approximations)
121void vpMeSite::init(double ip, double jp, double alphap)
122{
123 // Note: keep old value of convlt, suppress and contrast
124 selectDisplay = NONE;
125
126 ifloat = ip;
127 i = vpMath::round(ip);
128 jfloat = jp;
129 j = vpMath::round(jp);
130 alpha = alphap;
131 mask_sign = 1;
132}
133
134// initialise with convolution()
135void vpMeSite::init(double ip, double jp, double alphap, double convltp)
136{
137 selectDisplay = NONE;
138 ifloat = ip;
139 i = (int)ip;
140 jfloat = jp;
141 j = (int)jp;
142 alpha = alphap;
143 convlt = convltp;
144 mask_sign = 1;
145}
146
147// initialise with convolution and sign
148void vpMeSite::init(double ip, double jp, double alphap, double convltp, int sign)
149{
150 selectDisplay = NONE;
151 ifloat = ip;
152 i = (int)ip;
153 jfloat = jp;
154 j = (int)jp;
155 alpha = alphap;
156 convlt = convltp;
157 mask_sign = sign;
158}
159
161{
162 i = m.i;
163 j = m.j;
164 ifloat = m.ifloat;
165 jfloat = m.jfloat;
167 alpha = m.alpha;
168 convlt = m.convlt;
170 weight = m.weight;
171 selectDisplay = m.selectDisplay;
172 state = m.state;
173
174#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
175 suppress = m.suppress;
176#endif
177
178 return *this;
179}
180
182{
183 unsigned int range_ = static_cast<unsigned int>(range);
184 // Size of query list includes the point on the line
185 vpMeSite *list_query_pixels = new vpMeSite[2 * range_ + 1];
186
187 // range : +/- the range within which the pixel's
188 // correspondent will be sought
189
190 double salpha = sin(alpha);
191 double calpha = cos(alpha);
192 int n = 0;
193 vpImagePoint ip;
194
195 for (int k = -range; k <= range; k++) {
196 double ii = (ifloat + k * salpha);
197 double jj = (jfloat + k * calpha);
198
199 // Display
200 if ((selectDisplay == RANGE_RESULT) || (selectDisplay == RANGE)) {
201 ip.set_i(ii);
202 ip.set_j(jj);
204 }
205
206 // Copy parent's convolution
207 vpMeSite pel;
208 pel.init(ii, jj, alpha, convlt, mask_sign);
209 pel.setDisplay(selectDisplay); // Display
210
211 // Add site to the query list
212 list_query_pixels[n] = pel;
213 n++;
214 }
215
216 return (list_query_pixels);
217}
218
219#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
228void vpMeSite::getSign(const vpImage<unsigned char> &I, const int range)
229{
230 int k;
231
232 double salpha = sin(alpha);
233 double calpha = cos(alpha);
234
235 // First extremity
236 k = -range;
237 unsigned int i1 = static_cast<unsigned int>(vpMath::round(ifloat + k * salpha));
238 unsigned int j1 = static_cast<unsigned int>(vpMath::round(jfloat + k * calpha));
239
240 // Second extremity
241 k = range;
242 unsigned int i2 = static_cast<unsigned int>(vpMath::round(ifloat + k * salpha));
243 unsigned int j2 = static_cast<unsigned int>(vpMath::round(jfloat + k * calpha));
244
245 if (I[i1][j1] > I[i2][j2])
246 mask_sign = 1;
247 else
248 mask_sign = -1;
249}
250#endif
251
252// Specific function for ME
254{
255 int half;
256 int height_ = static_cast<int>(I.getHeight());
257 int width_ = static_cast<int>(I.getWidth());
258
259 double conv = 0.0;
260 unsigned int msize = me->getMaskSize();
261 half = (static_cast<int>(msize) - 1) >> 1;
262
263 if (horsImage(i, j, half + me->getStrip(), height_, width_)) {
264 conv = 0.0;
265 i = 0;
266 j = 0;
267 }
268 else {
269 // Calculate tangent angle from normal
270 double theta = alpha + M_PI / 2;
271 // Move tangent angle to within 0->M_PI for a positive
272 // mask index
273 while (theta < 0)
274 theta += M_PI;
275 while (theta > M_PI)
276 theta -= M_PI;
277
278 // Convert radians to degrees
279 int thetadeg = vpMath::round(theta * 180 / M_PI);
280
281 if (abs(thetadeg) == 180) {
282 thetadeg = 0;
283 }
284
285 unsigned int index_mask = (unsigned int)(thetadeg / (double)me->getAngleStep());
286
287 unsigned int i_ = static_cast<unsigned int>(i);
288 unsigned int j_ = static_cast<unsigned int>(j);
289 unsigned int half_ = static_cast<unsigned int>(half);
290
291 unsigned int ihalf = i_ - half_;
292 unsigned int jhalf = j_ - half_;
293
294 for (unsigned int a = 0; a < msize; a++) {
295 unsigned int ihalfa = ihalf + a;
296 for (unsigned int b = 0; b < msize; b++) {
297 conv += mask_sign * me->getMask()[index_mask][a][b] * I(ihalfa, jhalf + b);
298 }
299 }
300 }
301
302 return (conv);
303}
304
305void vpMeSite::track(const vpImage<unsigned char> &I, const vpMe *me, bool test_contrast)
306{
307 int max_rank = -1;
308 double max_convolution = 0;
309 double max = 0;
310 double contrast = 0;
311
312 // range = +/- range of pixels within which the correspondent
313 // of the current pixel will be sought
314 unsigned int range = me->getRange();
315
316 vpMeSite *list_query_pixels = getQueryList(I, (int)range);
317
318 double contrast_max = 1 + me->getMu2();
319 double contrast_min = 1 - me->getMu1();
320
321 // array in which likelihood ratios will be stored
322 double *likelihood = new double[2 * range + 1];
323
324 double threshold;
326 threshold = 2.0 * me->getThreshold();
327 }
328 else {
329 double n_d = me->getMaskSize();
330 threshold = me->getThreshold() / (100.0 * n_d * trunc(n_d / 2.0));
331 }
332
333 if (test_contrast) {
334 double diff = 1e6;
335 for (unsigned int n = 0; n < 2 * range + 1; n++) {
336 // convolution results
337 double convolution_ = list_query_pixels[n].convolution(I, me);
338
339 // luminance ratio of reference pixel to potential correspondent pixel
340 // the luminance must be similar, hence the ratio value should
341 // lay between, for instance, 0.5 and 1.5 (parameter tolerance)
342 likelihood[n] = fabs(convolution_ + convlt);
343 if (likelihood[n] > threshold) {
344 contrast = convolution_ / convlt;
345 if ((contrast > contrast_min) && (contrast < contrast_max) && fabs(1 - contrast) < diff) {
346 diff = fabs(1 - contrast);
347 max_convolution = convolution_;
348 max = likelihood[n];
349 max_rank = (int)n;
350 }
351 }
352 }
353 }
354 else { // test on contrast only
355 for (unsigned int n = 0; n < 2 * range + 1; n++) {
356 // convolution results
357 double convolution_ = list_query_pixels[n].convolution(I, me);
358 likelihood[n] = fabs(2 * convolution_);
359 if (likelihood[n] > max && likelihood[n] > threshold) {
360 max_convolution = convolution_;
361 max = likelihood[n];
362 max_rank = (int)n;
363 }
364 }
365 }
366
367 vpImagePoint ip;
368
369 if (max_rank >= 0) {
370 if ((selectDisplay == RANGE_RESULT) || (selectDisplay == RESULT)) {
371 ip.set_i(list_query_pixels[max_rank].i);
372 ip.set_j(list_query_pixels[max_rank].j);
374 }
375
376 *this = list_query_pixels[max_rank]; // The vpMeSite2 is replaced by the
377 // vpMeSite2 of max likelihood
378 normGradient = vpMath::sqr(max_convolution);
379
380 convlt = max_convolution;
381
382 delete[] list_query_pixels;
383 delete[] likelihood;
384 }
385 else // none of the query sites is better than the threshold
386 {
387 if ((selectDisplay == RANGE_RESULT) || (selectDisplay == RESULT)) {
388 ip.set_i(list_query_pixels[0].i);
389 ip.set_j(list_query_pixels[0].j);
391 }
392 normGradient = 0;
393 if (std::fabs(contrast) > std::numeric_limits<double>::epsilon())
394 state = CONTRAST; // contrast suppression
395 else
396 state = THRESHOLD; // threshold suppression
397
398 delete[] list_query_pixels;
399 delete[] likelihood; // modif portage
400 }
401}
402
403int vpMeSite::operator!=(const vpMeSite &m) { return ((m.i != i) || (m.j != j)); }
404
405VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpMeSite &vpMeS)
406{
407#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
408 return (os << "Alpha: " << vpMeS.alpha << " Convolution: " << vpMeS.convlt << " Flag: " << vpMeS.suppress
409 << " Weight: " << vpMeS.weight);
410#else
411 return (os << "Alpha: " << vpMeS.alpha << " Convolution: " << vpMeS.convlt << " Weight: " << vpMeS.weight);
412#endif
413}
414
416
418
419// Static functions
420
421void vpMeSite::display(const vpImage<unsigned char> &I, const double &i, const double &j, const vpMeSiteState &state)
422{
423 switch (state) {
424 case NO_SUPPRESSION:
426 break;
427
428 case CONTRAST:
430 break;
431
432 case THRESHOLD:
434 break;
435
436 case M_ESTIMATOR:
438 break;
439
440 case TOO_NEAR:
442 break;
443
444 default:
446 }
447}
448
449void vpMeSite::display(const vpImage<vpRGBa> &I, const double &i, const double &j, const vpMeSiteState &state)
450{
451 switch (state) {
452 case NO_SUPPRESSION:
454 break;
455
456 case CONTRAST:
458 break;
459
460 case THRESHOLD:
462 break;
463
464 case M_ESTIMATOR:
466 break;
467
468 case TOO_NEAR:
470 break;
471
472 default:
474 }
475}
static const vpColor red
Definition vpColor.h:211
static const vpColor cyan
Definition vpColor.h:220
static const vpColor blue
Definition vpColor.h:217
static const vpColor purple
Definition vpColor.h:222
static const vpColor yellow
Definition vpColor.h:219
static const vpColor green
Definition vpColor.h:214
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_j(double jj)
void set_i(double ii)
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
static double sqr(double x)
Definition vpMath.h:124
static int round(double x)
Definition vpMath.h:323
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition vpMeSite.h:65
int j
Coordinates along j of a site.
Definition vpMeSite.h:98
vpMeSiteState
Definition vpMeSite.h:82
@ TOO_NEAR
Point removed because too near image borders.
Definition vpMeSite.h:90
@ THRESHOLD
Point removed due to a threshold problem.
Definition vpMeSite.h:88
@ CONTRAST
Point removed due to a contrast problem.
Definition vpMeSite.h:84
@ M_ESTIMATOR
Point removed during virtual visual-servoing because considered as an outlier.
Definition vpMeSite.h:89
@ NO_SUPPRESSION
Point used by the tracker.
Definition vpMeSite.h:83
void setDisplay(vpMeSiteDisplayType select)
Definition vpMeSite.h:210
double weight
Uncertainty of point given as a probability between 0 and 1.
Definition vpMeSite.h:112
int operator!=(const vpMeSite &m)
Definition vpMeSite.cpp:403
void display(const vpImage< unsigned char > &I)
Definition vpMeSite.cpp:415
double ifloat
Floating coordinates along i of a site.
Definition vpMeSite.h:100
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
Definition vpMeSite.cpp:253
@ NONE
Not displayed.
Definition vpMeSite.h:72
@ RANGE_RESULT
Definition vpMeSite.h:75
int i
Coordinate along i of a site.
Definition vpMeSite.h:96
int mask_sign
Mask sign.
Definition vpMeSite.h:104
vpMeSite & operator=(const vpMeSite &m)
Definition vpMeSite.cpp:160
int suppress
Definition vpMeSite.h:366
vp_deprecated void getSign(const vpImage< unsigned char > &I, const int range)
Definition vpMeSite.cpp:228
double normGradient
Convolution of Site in previous image.
Definition vpMeSite.h:110
double alpha
Angle of tangent at site.
Definition vpMeSite.h:106
void init()
Definition vpMeSite.cpp:59
double jfloat
Floating coordinates along j of a site.
Definition vpMeSite.h:102
vpMeSite * getQueryList(const vpImage< unsigned char > &I, const int range)
Definition vpMeSite.cpp:181
double convlt
Convolution of Site in previous image.
Definition vpMeSite.h:108
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_likelihood=true)
Definition vpMeSite.cpp:305
Definition vpMe.h:122
vpLikelihoodThresholdType getLikelihoodThresholdType() const
Definition vpMe.h:297
unsigned int getAngleStep() const
Definition vpMe.h:208
vpMatrix * getMask() const
Definition vpMe.h:214
double getMu1() const
Definition vpMe.h:249
int getStrip() const
Definition vpMe.h:279
double getMu2() const
Definition vpMe.h:255
double getThreshold() const
Definition vpMe.h:288
unsigned int getMaskSize() const
Definition vpMe.h:236
unsigned int getRange() const
Definition vpMe.h:273
@ NORMALIZED_THRESHOLD
Easy-to-use normalized likelihood threshold corresponding to the minimal luminance contrast to consid...
Definition vpMe.h:132