Visual Servoing Platform  version 3.6.1 under development (2023-12-07)
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  // 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  m_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  m_state = NO_SUPPRESSION;
79 }
80 
82  : i(0), j(0), ifloat(0), jfloat(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0),
83  weight(1), m_selectDisplay(NONE), m_state(NO_SUPPRESSION)
84 { }
85 
86 vpMeSite::vpMeSite(double ip, double jp)
87  : i(0), j(0), ifloat(0), jfloat(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0),
88  weight(1), m_selectDisplay(NONE), m_state(NO_SUPPRESSION)
89 {
90  i = vpMath::round(ip);
91  j = vpMath::round(jp);
92  ifloat = ip;
93  jfloat = jp;
94 }
95 
97  : i(0), j(0), ifloat(0), jfloat(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0),
98  weight(1), m_selectDisplay(NONE), m_state(NO_SUPPRESSION)
99 {
100  *this = mesite;
101 }
102 
103 // More an Update than init
104 // For points in meter form (to avoid approximations)
105 void vpMeSite::init(double ip, double jp, double alphap)
106 {
107  // Note: keep old value of convlt and contrast
108  m_selectDisplay = NONE;
109 
110  ifloat = ip;
111  i = vpMath::round(ip);
112  jfloat = jp;
113  j = vpMath::round(jp);
114  alpha = alphap;
115  mask_sign = 1;
116 }
117 
118 // initialise with convolution()
119 void vpMeSite::init(double ip, double jp, double alphap, double convltp)
120 {
121  m_selectDisplay = NONE;
122  ifloat = ip;
123  i = (int)ip;
124  jfloat = jp;
125  j = (int)jp;
126  alpha = alphap;
127  convlt = convltp;
128  mask_sign = 1;
129 }
130 
131 // initialise with convolution and sign
132 void vpMeSite::init(double ip, double jp, double alphap, double convltp, int sign)
133 {
134  m_selectDisplay = NONE;
135  ifloat = ip;
136  i = (int)ip;
137  jfloat = jp;
138  j = (int)jp;
139  alpha = alphap;
140  convlt = convltp;
141  mask_sign = sign;
142 }
143 
145 {
146  i = m.i;
147  j = m.j;
148  ifloat = m.ifloat;
149  jfloat = m.jfloat;
150  mask_sign = m.mask_sign;
151  alpha = m.alpha;
152  convlt = m.convlt;
154  weight = m.weight;
155  m_selectDisplay = m.m_selectDisplay;
156  m_state = m.m_state;
157 
158  return *this;
159 }
160 
162 {
163  unsigned int range_ = static_cast<unsigned int>(range);
164  // Size of query list includes the point on the line
165  vpMeSite *list_query_pixels = new vpMeSite[2 * range_ + 1];
166 
167  // range : +/- the range within which the pixel's
168  // correspondent will be sought
169 
170  double salpha = sin(alpha);
171  double calpha = cos(alpha);
172  int n = 0;
173  vpImagePoint ip;
174 
175  for (int k = -range; k <= range; k++) {
176  double ii = (ifloat + k * salpha);
177  double jj = (jfloat + k * calpha);
178 
179  // Display
180  if ((m_selectDisplay == RANGE_RESULT) || (m_selectDisplay == RANGE)) {
181  ip.set_i(ii);
182  ip.set_j(jj);
184  }
185 
186  // Copy parent's convolution
187  vpMeSite pel;
188  pel.init(ii, jj, alpha, convlt, mask_sign);
189  pel.setDisplay(m_selectDisplay); // Display
190 
191  // Add site to the query list
192  list_query_pixels[n] = pel;
193  n++;
194  }
195 
196  return (list_query_pixels);
197 }
198 
199 // Specific function for ME
201 {
202  int half;
203  int height_ = static_cast<int>(I.getHeight());
204  int width_ = static_cast<int>(I.getWidth());
205 
206  double conv = 0.0;
207  unsigned int msize = me->getMaskSize();
208  half = (static_cast<int>(msize) - 1) >> 1;
209 
210  if (horsImage(i, j, half + me->getStrip(), height_, width_)) {
211  conv = 0.0;
212  i = 0;
213  j = 0;
214  }
215  else {
216  // Calculate tangent angle from normal
217  double theta = alpha + M_PI / 2;
218  // Move tangent angle to within 0->M_PI for a positive
219  // mask index
220  while (theta < 0)
221  theta += M_PI;
222  while (theta > M_PI)
223  theta -= M_PI;
224 
225  // Convert radians to degrees
226  int thetadeg = vpMath::round(theta * 180 / M_PI);
227 
228  if (abs(thetadeg) == 180) {
229  thetadeg = 0;
230  }
231 
232  unsigned int index_mask = (unsigned int)(thetadeg / (double)me->getAngleStep());
233 
234  unsigned int i_ = static_cast<unsigned int>(i);
235  unsigned int j_ = static_cast<unsigned int>(j);
236  unsigned int half_ = static_cast<unsigned int>(half);
237 
238  unsigned int ihalf = i_ - half_;
239  unsigned int jhalf = j_ - half_;
240 
241  for (unsigned int a = 0; a < msize; a++) {
242  unsigned int ihalfa = ihalf + a;
243  for (unsigned int b = 0; b < msize; b++) {
244  conv += mask_sign * me->getMask()[index_mask][a][b] * I(ihalfa, jhalf + b);
245  }
246  }
247  }
248 
249  return (conv);
250 }
251 
252 void vpMeSite::track(const vpImage<unsigned char> &I, const vpMe *me, bool test_contrast)
253 {
254  int max_rank = -1;
255  double max_convolution = 0;
256  double max = 0;
257  double contrast = 0;
258 
259  // range = +/- range of pixels within which the correspondent
260  // of the current pixel will be sought
261  unsigned int range = me->getRange();
262 
263  vpMeSite *list_query_pixels = getQueryList(I, (int)range);
264 
265  double contrast_max = 1 + me->getMu2();
266  double contrast_min = 1 - me->getMu1();
267 
268  // array in which likelihood ratios will be stored
269  double *likelihood = new double[2 * range + 1];
270 
271  double threshold;
273  threshold = 2.0 * me->getThreshold();
274  }
275  else {
276  double n_d = me->getMaskSize();
277  threshold = me->getThreshold() / (100.0 * n_d * trunc(n_d / 2.0));
278  }
279 
280  if (test_contrast) {
281  double diff = 1e6;
282  for (unsigned int n = 0; n < 2 * range + 1; n++) {
283  // convolution results
284  double convolution_ = list_query_pixels[n].convolution(I, me);
285 
286  // luminance ratio of reference pixel to potential correspondent pixel
287  // the luminance must be similar, hence the ratio value should
288  // lay between, for instance, 0.5 and 1.5 (parameter tolerance)
289  likelihood[n] = fabs(convolution_ + convlt);
290  if (likelihood[n] > threshold) {
291  contrast = convolution_ / convlt;
292  if ((contrast > contrast_min) && (contrast < contrast_max) && fabs(1 - contrast) < diff) {
293  diff = fabs(1 - contrast);
294  max_convolution = convolution_;
295  max = likelihood[n];
296  max_rank = (int)n;
297  }
298  }
299  }
300  }
301  else { // test on contrast only
302  for (unsigned int n = 0; n < 2 * range + 1; n++) {
303  // convolution results
304  double convolution_ = list_query_pixels[n].convolution(I, me);
305  likelihood[n] = fabs(2 * convolution_);
306  if (likelihood[n] > max && likelihood[n] > threshold) {
307  max_convolution = convolution_;
308  max = likelihood[n];
309  max_rank = (int)n;
310  }
311  }
312  }
313 
314  vpImagePoint ip;
315 
316  if (max_rank >= 0) {
317  if ((m_selectDisplay == RANGE_RESULT) || (m_selectDisplay == RESULT)) {
318  ip.set_i(list_query_pixels[max_rank].i);
319  ip.set_j(list_query_pixels[max_rank].j);
321  }
322 
323  *this = list_query_pixels[max_rank]; // The vpMeSite2 is replaced by the
324  // vpMeSite2 of max likelihood
325  normGradient = vpMath::sqr(max_convolution);
326 
327  convlt = max_convolution;
328 
329  delete[] list_query_pixels;
330  delete[] likelihood;
331  }
332  else // none of the query sites is better than the threshold
333  {
334  if ((m_selectDisplay == RANGE_RESULT) || (m_selectDisplay == RESULT)) {
335  ip.set_i(list_query_pixels[0].i);
336  ip.set_j(list_query_pixels[0].j);
338  }
339  normGradient = 0;
340  if (std::fabs(contrast) > std::numeric_limits<double>::epsilon())
341  m_state = CONTRAST; // contrast suppression
342  else
343  m_state = THRESHOLD; // threshold suppression
344 
345  delete[] list_query_pixels;
346  delete[] likelihood; // modif portage
347  }
348 }
349 
350 int vpMeSite::operator!=(const vpMeSite &m) { return ((m.i != i) || (m.j != j)); }
351 
352 VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpMeSite &vpMeS)
353 {
354  return (os << "Alpha: " << vpMeS.alpha << " Convolution: " << vpMeS.convlt << " Weight: " << vpMeS.weight);
355 }
356 
358 
360 
361 // Static functions
362 
363 void vpMeSite::display(const vpImage<unsigned char> &I, const double &i, const double &j, const vpMeSiteState &state)
364 {
365  switch (state) {
366  case NO_SUPPRESSION:
368  break;
369 
370  case CONTRAST:
372  break;
373 
374  case THRESHOLD:
376  break;
377 
378  case M_ESTIMATOR:
380  break;
381 
382  case TOO_NEAR:
384  break;
385 
386  default:
388  }
389 }
390 
391 void vpMeSite::display(const vpImage<vpRGBa> &I, const double &i, const double &j, const vpMeSiteState &state)
392 {
393  switch (state) {
394  case NO_SUPPRESSION:
396  break;
397 
398  case CONTRAST:
400  break;
401 
402  case THRESHOLD:
404  break;
405 
406  case M_ESTIMATOR:
408  break;
409 
410  case TOO_NEAR:
412  break;
413 
414  default:
416  }
417 }
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:504
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:294
void set_i(double ii)
Definition: vpImagePoint.h:283
unsigned int getWidth() const
Definition: vpImage.h:240
unsigned int getHeight() const
Definition: vpImage.h:182
static double sqr(double x)
Definition: vpMath.h:201
static int round(double x)
Definition: vpMath.h:403
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:97
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:204
double weight
Uncertainty of point given as a probability between 0 and 1.
Definition: vpMeSite.h:111
int operator!=(const vpMeSite &m)
Definition: vpMeSite.cpp:350
void display(const vpImage< unsigned char > &I)
Definition: vpMeSite.cpp:357
double ifloat
Floating coordinates along i of a site.
Definition: vpMeSite.h:99
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
Definition: vpMeSite.cpp:200
@ NONE
Not displayed.
Definition: vpMeSite.h:72
@ RANGE_RESULT
Definition: vpMeSite.h:75
@ RESULT
Definition: vpMeSite.h:74
@ RANGE
Definition: vpMeSite.h:73
int i
Coordinate along i of a site.
Definition: vpMeSite.h:95
int mask_sign
Mask sign.
Definition: vpMeSite.h:103
vpMeSite & operator=(const vpMeSite &m)
Definition: vpMeSite.cpp:144
double normGradient
Convolution of Site in previous image.
Definition: vpMeSite.h:109
double alpha
Angle of tangent at site.
Definition: vpMeSite.h:105
void init()
Definition: vpMeSite.cpp:59
double jfloat
Floating coordinates along j of a site.
Definition: vpMeSite.h:101
vpMeSite * getQueryList(const vpImage< unsigned char > &I, const int range)
Definition: vpMeSite.cpp:161
double convlt
Convolution of Site in previous image.
Definition: vpMeSite.h:107
vpMeSite()
Definition: vpMeSite.cpp:81
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_likelihood=true)
Definition: vpMeSite.cpp:252
Definition: vpMe.h:120
vpLikelihoodThresholdType getLikelihoodThresholdType() const
Definition: vpMe.h:305
unsigned int getAngleStep() const
Definition: vpMe.h:198
double getMu1() const
Definition: vpMe.h:245
int getStrip() const
Definition: vpMe.h:287
double getMu2() const
Definition: vpMe.h:252
double getThreshold() const
Definition: vpMe.h:296
unsigned int getMaskSize() const
Definition: vpMe.h:230
vpMatrix * getMask() const
Definition: vpMe.h:205
unsigned int getRange() const
Definition: vpMe.h:273
@ NORMALIZED_THRESHOLD
Definition: vpMe.h:131