Visual Servoing Platform  version 3.6.1 under development (2024-05-26)
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
47 static bool horsImage(int i, int j, int half, int rows, int cols)
48 {
49  int half_1 = half + 1;
50  int half_3 = half + 3;
51  return ((0 < (half_1 - i)) || (((i - rows) + half_3) > 0) || (0 < (half_1 - j)) || (((j - cols) + half_3) > 0));
52 }
53 #endif
54 
56 {
57  // Site components
58  m_alpha = 0.0;
59  m_convlt = 0.0;
60  m_weight = -1;
61  m_contrastThreshold = 10000.0;
62 
63  m_selectDisplay = NONE;
64 
65  // Pixel components
66  m_i = 0;
67  m_j = 0;
68  m_ifloat = m_i;
69  m_jfloat = m_j;
70 
71  m_mask_sign = 1;
72 
73  m_normGradient = 0;
74 
75  m_state = NO_SUPPRESSION;
76 }
77 
79  : m_i(0), m_j(0), m_ifloat(0), m_jfloat(0), m_mask_sign(1), m_alpha(0.), m_convlt(0.), m_normGradient(0),
80  m_weight(1), m_contrastThreshold(10000.0), m_selectDisplay(NONE), m_state(NO_SUPPRESSION)
81 { }
82 
83 vpMeSite::vpMeSite(const double &ip, const double &jp)
84  : m_i(0), m_j(0), m_ifloat(0), m_jfloat(0), m_mask_sign(1), m_alpha(0.), m_convlt(0.), m_normGradient(0),
85  m_weight(1), m_contrastThreshold(10000.0), m_selectDisplay(NONE), m_state(NO_SUPPRESSION)
86 {
87  m_i = vpMath::round(ip);
88  m_j = vpMath::round(jp);
89  m_ifloat = ip;
90  m_jfloat = jp;
91 }
92 
94  : m_i(0), m_j(0), m_ifloat(0), m_jfloat(0), m_mask_sign(1), m_alpha(0.), m_convlt(0.), m_normGradient(0),
95  m_weight(1), m_contrastThreshold(10000.0), m_selectDisplay(NONE), m_state(NO_SUPPRESSION)
96 {
97  *this = mesite;
98 }
99 
100 // More an Update than init
101 // For points in meter form (to avoid approximations)
102 void vpMeSite::init(const double &ip, const double &jp, const double &alphap)
103 {
104  // Note: keep old value of m_convlt, contrast and threshold
105  m_selectDisplay = NONE;
106 
107  m_ifloat = ip;
108  m_i = vpMath::round(ip);
109  m_jfloat = jp;
110  m_j = vpMath::round(jp);
111  m_alpha = alphap;
112  m_mask_sign = 1;
113 }
114 
115 // initialise with convolution()
116 void vpMeSite::init(const double &ip, const double &jp, const double &alphap, const double &convltp)
117 {
118  m_selectDisplay = NONE;
119  m_ifloat = ip;
120  m_i = static_cast<int>(ip);
121  m_jfloat = jp;
122  m_j = static_cast<int>(jp);
123  m_alpha = alphap;
124  m_convlt = convltp;
125  m_mask_sign = 1;
126 }
127 
128 // initialise with convolution and sign
129 void vpMeSite::init(const double &ip, const double &jp, const double &alphap, const double &convltp, const int &sign)
130 {
131  m_selectDisplay = NONE;
132  m_ifloat = ip;
133  m_i = static_cast<int>(ip);
134  m_jfloat = jp;
135  m_j = static_cast<int>(jp);
136  m_alpha = alphap;
137  m_convlt = convltp;
138  m_mask_sign = sign;
139 }
140 
141 // initialise with convolution and sign
142 void vpMeSite::init(const double &ip, const double &jp, const double &alphap, const double &convltp, const int &sign, const double &contrastThreshold)
143 {
144  m_selectDisplay = NONE;
145  m_ifloat = ip;
146  m_i = static_cast<int>(ip);
147  m_jfloat = jp;
148  m_j = static_cast<int>(jp);
149  m_alpha = alphap;
150  m_convlt = convltp;
151  m_mask_sign = sign;
152  m_contrastThreshold = contrastThreshold;
153 }
154 
156 {
157  m_i = m.m_i;
158  m_j = m.m_j;
159  m_ifloat = m.m_ifloat;
160  m_jfloat = m.m_jfloat;
162  m_alpha = m.m_alpha;
163  m_convlt = m.m_convlt;
165  m_weight = m.m_weight;
167  m_selectDisplay = m.m_selectDisplay;
168  m_state = m.m_state;
169 
170  return *this;
171 }
172 
173 vpMeSite *vpMeSite::getQueryList(const vpImage<unsigned char> &I, const int &range) const
174 {
175  unsigned int range_ = static_cast<unsigned int>(range);
176  // Size of query list includes the point on the line
177  vpMeSite *list_query_pixels = new vpMeSite[(2 * range_) + 1];
178 
179  // range : +/- the range within which the pixel's
180  // correspondent will be sought
181 
182  double salpha = sin(m_alpha);
183  double calpha = cos(m_alpha);
184  int n = 0;
185  vpImagePoint ip;
186 
187  for (int k = -range; k <= range; ++k) {
188  double ii = m_ifloat + (k * salpha);
189  double jj = m_jfloat + (k * calpha);
190 
191  // Display
192  if ((m_selectDisplay == RANGE_RESULT) || (m_selectDisplay == RANGE)) {
193  ip.set_i(ii);
194  ip.set_j(jj);
196  }
197 
198  // Copy parent's convolution
199  vpMeSite pel;
201  pel.setDisplay(m_selectDisplay); // Display
202 
203  // Add site to the query list
204  list_query_pixels[n] = pel;
205  ++n;
206  }
207 
208  return list_query_pixels;
209 }
210 
211 // Specific function for ME
213 {
214  int half;
215  int height_ = static_cast<int>(I.getHeight());
216  int width_ = static_cast<int>(I.getWidth());
217 
218  double conv = 0.0;
219  unsigned int msize = me->getMaskSize();
220  half = static_cast<int>((msize - 1) >> 1);
221 
222  if (horsImage(m_i, m_j, half + me->getStrip(), height_, width_)) {
223  conv = 0.0;
224  m_i = 0;
225  m_j = 0;
226  }
227  else {
228  // Calculate tangent angle from normal
229  double theta = m_alpha + (M_PI / 2);
230  // Move tangent angle to within 0->M_PI for a positive
231  // mask index
232  while (theta < 0) {
233  theta += M_PI;
234  }
235  while (theta > M_PI) {
236  theta -= M_PI;
237  }
238 
239  // Convert radians to degrees
240  int thetadeg = vpMath::round((theta * 180) / M_PI);
241 
242  const int flatAngle = 180;
243  if (abs(thetadeg) == flatAngle) {
244  thetadeg = 0;
245  }
246 
247  unsigned int index_mask = static_cast<unsigned int>(thetadeg / static_cast<double>(me->getAngleStep()));
248 
249  unsigned int i_ = static_cast<unsigned int>(m_i);
250  unsigned int j_ = static_cast<unsigned int>(m_j);
251  unsigned int half_ = static_cast<unsigned int>(half);
252 
253  unsigned int ihalf = i_ - half_;
254  unsigned int jhalf = j_ - half_;
255 
256  for (unsigned int a = 0; a < msize; ++a) {
257  unsigned int ihalfa = ihalf + a;
258  for (unsigned int b = 0; b < msize; ++b) {
259  conv += m_mask_sign * me->getMask()[index_mask][a][b] * I(ihalfa, jhalf + b);
260  }
261  }
262  }
263 
264  return conv;
265 }
266 
267 void vpMeSite::track(const vpImage<unsigned char> &I, const vpMe *me, const bool &test_contrast)
268 {
269  int max_rank = -1;
270  double max_convolution = 0;
271  double max = 0;
272  double contrast = 0;
273 
274  // range = +/- range of pixels within which the correspondent
275  // of the current pixel will be sought
276  unsigned int range = me->getRange();
277 
278  vpMeSite *list_query_pixels = getQueryList(I, static_cast<int>(range));
279 
280  double contrast_max = 1 + me->getMu2();
281  double contrast_min = 1 - me->getMu1();
282 
283  // array in which likelihood ratios will be stored
284  double *likelihood = new double[(2 * range) + 1];
285 
286  if (test_contrast) {
287  double diff = 1e6;
288  for (unsigned int n = 0; n < ((2 * range) + 1); ++n) {
289  // convolution results
290  double convolution_ = list_query_pixels[n].convolution(I, me);
291  double threshold = list_query_pixels[n].getContrastThreshold();
292 
294  threshold = 2.0 * threshold;
295  }
296  else {
297  double n_d = me->getMaskSize();
298  threshold = threshold / (100.0 * n_d * trunc(n_d / 2.0));
299  }
300 
301  // luminance ratio of reference pixel to potential correspondent pixel
302  // the luminance must be similar, hence the ratio value should
303  // lay between, for instance, 0.5 and 1.5 (parameter tolerance)
304  likelihood[n] = fabs(convolution_ + m_convlt);
305 
306  if (likelihood[n] > threshold) {
307  contrast = convolution_ / m_convlt;
308  if ((contrast > contrast_min) && (contrast < contrast_max) && (fabs(1 - contrast) < diff)) {
309  diff = fabs(1 - contrast);
310  max_convolution = convolution_;
311  max = likelihood[n];
312  max_rank = static_cast<int>(n);
313  }
314  }
315  }
316  }
317  else { // test on contrast only
318  for (unsigned int n = 0; n < ((2 * range) + 1); ++n) {
319  double threshold = list_query_pixels[n].getContrastThreshold();
320 
322  threshold = 2.0 * threshold;
323  }
324  else {
325  double n_d = me->getMaskSize();
326  threshold = threshold / (100.0 * n_d * trunc(n_d / 2.0));
327  }
328 
329  // convolution results
330  double convolution_ = list_query_pixels[n].convolution(I, me);
331  likelihood[n] = fabs(2 * convolution_);
332  if ((likelihood[n] > max) && (likelihood[n] > threshold)) {
333  max_convolution = convolution_;
334  max = likelihood[n];
335  max_rank = static_cast<int>(n);
336  }
337  }
338  }
339 
340  vpImagePoint ip;
341 
342  if (max_rank >= 0) {
343  if ((m_selectDisplay == RANGE_RESULT) || (m_selectDisplay == RESULT)) {
344  ip.set_i(list_query_pixels[max_rank].m_i);
345  ip.set_j(list_query_pixels[max_rank].m_j);
347  }
348 
349  *this = list_query_pixels[max_rank]; // The vpMeSite2 is replaced by the
350  // vpMeSite2 of max likelihood
351  m_normGradient = vpMath::sqr(max_convolution);
352 
353  m_convlt = max_convolution;
354 
355  delete[] list_query_pixels;
356  delete[] likelihood;
357  }
358  else // none of the query sites is better than the threshold
359  {
360  if ((m_selectDisplay == RANGE_RESULT) || (m_selectDisplay == RESULT)) {
361  ip.set_i(list_query_pixels[0].m_i);
362  ip.set_j(list_query_pixels[0].m_j);
364  }
365  m_normGradient = 0;
366  if (std::fabs(contrast) > std::numeric_limits<double>::epsilon()) {
367  m_state = CONTRAST; // contrast suppression
368  }
369  else {
370  m_state = THRESHOLD; // threshold suppression
371  }
372 
373  delete[] list_query_pixels;
374  delete[] likelihood; // modif portage
375  }
376 }
377 
378 int vpMeSite::operator!=(const vpMeSite &m) { return ((m.m_i != m_i) || (m.m_j != m_j)); }
379 
380 VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpMeSite &vpMeS)
381 {
382  return (os << "Alpha: " << vpMeS.m_alpha << " Convolution: " << vpMeS.m_convlt << " Weight: " << vpMeS.m_weight << " Threshold: " << vpMeS.m_contrastThreshold);
383 }
384 
386 
388 
389 // Static functions
390 
391 void vpMeSite::display(const vpImage<unsigned char> &I, const double &i, const double &j, const vpMeSiteState &state)
392 {
393  const unsigned int crossSize = 3;
394  switch (state) {
395  case NO_SUPPRESSION:
396  vpDisplay::displayCross(I, vpImagePoint(i, j), crossSize, vpColor::green, 1);
397  break;
398 
399  case CONTRAST:
400  vpDisplay::displayCross(I, vpImagePoint(i, j), crossSize, vpColor::blue, 1);
401  break;
402 
403  case THRESHOLD:
404  vpDisplay::displayCross(I, vpImagePoint(i, j), crossSize, vpColor::purple, 1);
405  break;
406 
407  case M_ESTIMATOR:
408  vpDisplay::displayCross(I, vpImagePoint(i, j), crossSize, vpColor::red, 1);
409  break;
410 
411  case OUTSIDE_ROI_MASK:
412  vpDisplay::displayCross(I, vpImagePoint(i, j), crossSize, vpColor::cyan, 1);
413  break;
414 
415  default:
416  vpDisplay::displayCross(I, vpImagePoint(i, j), crossSize, vpColor::yellow, 1);
417  }
418 }
419 
420 void vpMeSite::display(const vpImage<vpRGBa> &I, const double &i, const double &j, const vpMeSiteState &state)
421 {
422  switch (state) {
423  case NO_SUPPRESSION:
425  break;
426 
427  case CONTRAST:
429  break;
430 
431  case THRESHOLD:
433  break;
434 
435  case M_ESTIMATOR:
437  break;
438 
439  case OUTSIDE_ROI_MASK:
441  break;
442 
443  default:
445  }
446 }
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:603
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 ...
Definition: vpImagePoint.h:82
void set_j(double jj)
Definition: vpImagePoint.h:305
void set_i(double ii)
Definition: vpImagePoint.h:294
unsigned int getWidth() const
Definition: vpImage.h:245
unsigned int getHeight() const
Definition: vpImage.h:184
static double sqr(double x)
Definition: vpMath.h:201
static int round(double x)
Definition: vpMath.h:405
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:65
int m_mask_sign
Mask sign.
Definition: vpMeSite.h:104
vpMeSiteState
Definition: vpMeSite.h:82
@ OUTSIDE_ROI_MASK
Point is outside the region of interest mask, but retained in the ME list.
Definition: vpMeSite.h:92
@ THRESHOLD
Point not tracked due to the likelihood that is below the threshold, but retained in the ME list.
Definition: vpMeSite.h:88
@ CONTRAST
Point not tracked due to a contrast problem, but retained in the ME list.
Definition: vpMeSite.h:84
@ M_ESTIMATOR
Point detected as an outlier during virtual visual-servoing.
Definition: vpMeSite.h:89
@ NO_SUPPRESSION
Point successfully tracked.
Definition: vpMeSite.h:83
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:247
int operator!=(const vpMeSite &m)
Definition: vpMeSite.cpp:378
void display(const vpImage< unsigned char > &I)
Definition: vpMeSite.cpp:385
double m_ifloat
Subpixel coordinates along i of a site.
Definition: vpMeSite.h:100
vpMeSite * getQueryList(const vpImage< unsigned char > &I, const int &range) const
Definition: vpMeSite.cpp:173
double m_normGradient
Convolution of Site in previous image.
Definition: vpMeSite.h:110
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
Definition: vpMeSite.cpp:212
double getContrastThreshold() const
Definition: vpMeSite.h:304
double m_convlt
Convolution of Site in previous image.
Definition: vpMeSite.h:108
double m_alpha
Angle of tangent at site.
Definition: vpMeSite.h:106
@ NONE
Not displayed.
Definition: vpMeSite.h:72
@ RANGE_RESULT
Definition: vpMeSite.h:75
@ RESULT
Definition: vpMeSite.h:74
@ RANGE
Definition: vpMeSite.h:73
vpMeSite & operator=(const vpMeSite &m)
Definition: vpMeSite.cpp:155
double m_contrastThreshold
Old likelihood ratio threshold (to be avoided) or easy-to-use normalized threshold: minimal contrast.
Definition: vpMeSite.h:114
void init()
Definition: vpMeSite.cpp:55
int m_j
Integer coordinates along j of a site.
Definition: vpMeSite.h:98
int m_i
Integer coordinate along i of a site.
Definition: vpMeSite.h:96
double m_jfloat
Subpixel coordinates along j of a site.
Definition: vpMeSite.h:102
vpMeSite()
Definition: vpMeSite.cpp:78
double m_weight
Uncertainty of point given as a probability between 0 and 1.
Definition: vpMeSite.h:112
void track(const vpImage< unsigned char > &I, const vpMe *me, const bool &test_contrast=true)
Definition: vpMeSite.cpp:267
Definition: vpMe.h:124
vpLikelihoodThresholdType getLikelihoodThresholdType() const
Definition: vpMe.h:317
unsigned int getAngleStep() const
Definition: vpMe.h:183
double getMu1() const
Definition: vpMe.h:230
int getStrip() const
Definition: vpMe.h:272
double getMu2() const
Definition: vpMe.h:237
unsigned int getMaskSize() const
Definition: vpMe.h:215
vpMatrix * getMask() const
Definition: vpMe.h:190
unsigned int getRange() const
Definition: vpMe.h:258
@ NORMALIZED_THRESHOLD
Definition: vpMe.h:135