Visual Servoing Platform  version 3.5.1 under development (2023-05-30)
vpMeSite.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  * Moving edges.
33  *
34 *****************************************************************************/
35 
41 #include <cmath> // std::fabs
42 #include <limits> // numeric_limits
43 #include <stdlib.h>
44 #include <visp3/core/vpTrackingException.h>
45 #include <visp3/me/vpMe.h>
46 #include <visp3/me/vpMeSite.h>
47 
48 #ifndef DOXYGEN_SHOULD_SKIP_THIS
49 static bool horsImage(int i, int j, int half, int rows, int cols)
50 {
51  // return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j
52  // > (cols - half - 3) )) ;
53  int half_1 = half + 1;
54  int half_3 = half + 3;
55  // return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j
56  // > (cols - half - 3) )) ;
57  return ((0 < (half_1 - i)) || ((i - rows + half_3) > 0) || (0 < (half_1 - j)) || ((j - cols + half_3) > 0));
58 }
59 #endif
60 
62 {
63  // Site components
64  alpha = 0.0;
65  convlt = 0.0;
66  weight = -1;
67 
68  selectDisplay = NONE;
69 
70  // Pixel components
71  i = 0;
72  j = 0;
73  v = 0;
74  ifloat = i;
75  jfloat = j;
76  i_1 = 0;
77  j_1 = 0;
78 
79  mask_sign = 1;
80 
81  normGradient = 0;
82 
83  state = NO_SUPPRESSION;
84 
85 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
86  suppress = 0;
87 #endif
88 }
89 
91  : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0),
92  weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
93 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
94  ,
95  suppress(0)
96 #endif
97 { }
98 
99 vpMeSite::vpMeSite(double ip, double jp)
100  : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0),
101  weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
102 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
103  ,
104  suppress(0)
105 #endif
106 {
107  i = vpMath::round(ip);
108  j = vpMath::round(jp);
109  ifloat = ip;
110  jfloat = jp;
111 }
112 
117  : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0),
118  weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
119 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
120  ,
121  suppress(0)
122 #endif
123 {
124  *this = mesite;
125 }
126 
127 // More an Update than init
128 // For points in meter form (to avoid approximations)
129 void vpMeSite::init(double ip, double jp, double alphap)
130 {
131  // Note: keep old value of convlt, suppress and contrast
132  selectDisplay = NONE;
133 
134  ifloat = ip;
135  i = vpMath::round(ip);
136  jfloat = jp;
137  j = vpMath::round(jp);
138  alpha = alphap;
139  mask_sign = 1;
140 
141  v = 0;
142  i_1 = 0;
143  j_1 = 0;
144 }
145 
146 // initialise with convolution
147 void vpMeSite::init(double ip, double jp, double alphap, double convltp)
148 {
149  selectDisplay = NONE;
150  ifloat = ip;
151  i = (int)ip;
152  jfloat = jp;
153  j = (int)jp;
154  alpha = alphap;
155  convlt = convltp;
156  mask_sign = 1;
157 
158  v = 0;
159  i_1 = 0;
160  j_1 = 0;
161 }
162 // initialise with convolution and sign
163 void vpMeSite::init(double ip, double jp, double alphap, double convltp, int sign)
164 {
165  selectDisplay = NONE;
166  ifloat = ip;
167  i = (int)ip;
168  jfloat = jp;
169  j = (int)jp;
170  alpha = alphap;
171  convlt = convltp;
172  mask_sign = sign;
173 
174  v = 0;
175  i_1 = 0;
176  j_1 = 0;
177 }
178 
180 {
181  i = m.i;
182  j = m.j;
183  i_1 = m.i_1;
184  j_1 = m.j_1;
185  ifloat = m.ifloat;
186  jfloat = m.jfloat;
187  v = m.v;
188  mask_sign = m.mask_sign;
189  alpha = m.alpha;
190  convlt = m.convlt;
192  weight = m.weight;
193  selectDisplay = m.selectDisplay;
194  state = m.state;
195 
196 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
197  suppress = m.suppress;
198 #endif
199 
200  return *this;
201 }
202 
203 // ===================================================================
211  // ===================================================================
212 
214 {
215  unsigned int range_ = static_cast<unsigned int>(range);
216  // Size of query list includes the point on the line
217  vpMeSite *list_query_pixels = new vpMeSite[2 * range_ + 1];
218 
219  // range : +/- the range within which the pixel's
220  // correspondent will be sought
221 
222  double salpha = sin(alpha);
223  double calpha = cos(alpha);
224  int n = 0;
225  vpImagePoint ip;
226 
227  for (int k = -range; k <= range; k++) {
228  double ii = (ifloat + k * salpha);
229  double jj = (jfloat + k * calpha);
230 
231  // Display
232  if ((selectDisplay == RANGE_RESULT) || (selectDisplay == RANGE)) {
233  ip.set_i(ii);
234  ip.set_j(jj);
236  }
237 
238  // Copy parent's convolution
239  vpMeSite pel;
240  pel.init(ii, jj, alpha, convlt, mask_sign);
241  pel.setDisplay(selectDisplay); // Display
242 
243  // Add site to the query list
244  list_query_pixels[n] = pel;
245  n++;
246  }
247 
248  return (list_query_pixels);
249 }
250 
251 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
252 // ===================================================================
260  // ===================================================================
261 void vpMeSite::getSign(const vpImage<unsigned char> &I, const int range)
262 {
263 
264  int k;
265 
266  double salpha = sin(alpha);
267  double calpha = cos(alpha);
268 
269  // First extremity
270  k = -range;
271  unsigned int i1 = static_cast<unsigned int>(vpMath::round(ifloat + k * salpha));
272  unsigned int j1 = static_cast<unsigned int>(vpMath::round(jfloat + k * calpha));
273 
274  // Second extremity
275  k = range;
276  unsigned int i2 = static_cast<unsigned int>(vpMath::round(ifloat + k * salpha));
277  unsigned int j2 = static_cast<unsigned int>(vpMath::round(jfloat + k * calpha));
278 
279  // TODO: Here check if i1,j1,i2,j2 > 0 else ??
280  if (I[i1][j1] > I[i2][j2])
281  mask_sign = 1;
282  else
283  mask_sign = -1;
284 }
285 #endif
286 
287 // Specific function for ME
289 {
290  int half;
291  int height_ = static_cast<int>(I.getHeight());
292  int width_ = static_cast<int>(I.getWidth());
293 
294  double conv = 0.0;
295  unsigned int msize = me->getMaskSize();
296  half = (static_cast<int>(msize) - 1) >> 1;
297 
298  if (horsImage(i, j, half + me->getStrip(), height_, width_)) {
299  conv = 0.0;
300  i = 0;
301  j = 0;
302  }
303  else {
304  // Calculate tangent angle from normal
305  double theta = alpha + M_PI / 2;
306  // Move tangent angle to within 0->M_PI for a positive
307  // mask index
308  while (theta < 0)
309  theta += M_PI;
310  while (theta > M_PI)
311  theta -= M_PI;
312 
313  // Convert radians to degrees
314  int thetadeg = vpMath::round(theta * 180 / M_PI);
315 
316  if (abs(thetadeg) == 180) {
317  thetadeg = 0;
318  }
319 
320  unsigned int index_mask = (unsigned int)(thetadeg / (double)me->getAngleStep());
321 
322  unsigned int i_ = static_cast<unsigned int>(i);
323  unsigned int j_ = static_cast<unsigned int>(j);
324  unsigned int half_ = static_cast<unsigned int>(half);
325 
326  unsigned int ihalf = i_ - half_;
327  unsigned int jhalf = j_ - half_;
328 
329  for (unsigned int a = 0; a < msize; a++) {
330  unsigned int ihalfa = ihalf + a;
331  for (unsigned int b = 0; b < msize; b++) {
332  conv += mask_sign * me->getMask()[index_mask][a][b] *
333  // I(i-half+a,j-half+b) ;
334  I(ihalfa, jhalf + b);
335  }
336  }
337  }
338 
339  return (conv);
340 }
341 
350 void vpMeSite::track(const vpImage<unsigned char> &I, const vpMe *me, bool test_contrast)
351 {
352  // vpMeSite *list_query_pixels ;
353  // int max_rank =0 ;
354  // int max_rank1=0 ;
355  // int max_rank2 = 0;
356  // double convolution = 0 ;
357  // double max_convolution = 0 ;
358  // double max = 0 ;
359  // double contrast = 0;
360  // // vpDisplay::display(I) ;
361  // // vpERROR_TRACE("getclcik %d",me->getRange()) ;
362  // // vpDisplay::getClick(I) ;
363  //
364  // // range = +/- range of pixels within which the correspondent
365  // // of the current pixel will be sought
366  // int range = me->getRange() ;
367  //
368  // // std::cout << i << " " << j<<" " << range << " " << suppress <<
369  // std::endl ; list_query_pixels = getQueryList(I, range) ;
370  //
371  // double contrast_max = 1 + me->getMu2();
372  // double contrast_min = 1 - me->getMu1();
373  //
374  // // array in which likelihood ratios will be stored
375  // double *likelihood= new double[ 2 * range + 1 ] ;
376  //
377  // int ii_1 = i ;
378  // int jj_1 = j ;
379  // i_1 = i ;
380  // j_1 = j ;
381  // double threshold;
382  // threshold = me->getThreshold();
383  //
384  // // std::cout <<"---------------------"<<std::endl ;
385  // for(int n = 0 ; n < 2 * range + 1 ; n++)
386  // {
387  //
388  // // convolution results
389  // convolution = list_query_pixels[n].convolution(I, me) ;
390  //
391  // // luminance ratio of reference pixel to potential correspondent
392  // pixel
393  // // the luminance must be similar, hence the ratio value should
394  // // lay between, for instance, 0.5 and 1.5 (parameter tolerance)
395  // if( test_contrast )
396  // {
397  // // Include this to eliminate temporal calculation
398  // if (convlt==0)
399  // {
400  // std::cout << "vpMeSite::track : Division by zero convlt = 0" <<
401  // std::endl ; delete []list_query_pixels ; delete
402  // []likelihood;
403  // throw(vpTrackingException(vpTrackingException::initializationError,
404  // "Division by zero")) ;
405  // }
406  //
407  // // contrast = fabs(convolution / convlt) ;
408  // contrast = convolution / convlt ;
409  // // likelihood ratios
410  // if((contrast > contrast_min) && (contrast < contrast_max))
411  // likelihood[n] = fabs(convolution + convlt ) ;
412  // else
413  // likelihood[n] = 0 ;
414  // }
415  // else
416  // likelihood[n] = fabs(2*convolution) ;
417  //
418  //
419  // // establishment of the maximal likelihood ratios's rank
420  // // in the array, the value of the likelihood ratio can now be
421  // // referenced by its rank in the array
422  // if (likelihood[n] > max)
423  // {
424  // max_convolution= convolution;
425  // max = likelihood[n] ;
426  // max_rank = n ;
427  // max_rank2 = max_rank1;
428  // max_rank1 = max_rank;
429  // }
430  //
431  // }
432  //
433  // // test on the likelihood threshold if threshold==-1 then
434  // // the me->threshold is selected
435  //
436  // vpImagePoint ip;
437  //
438  // // if (test_contrast)
439  // if(max > threshold)
440  // {
441  // if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
442  // {
443  // ip.set_i( list_query_pixels[max_rank].i );
444  // ip.set_j( list_query_pixels[max_rank].j );
445  // vpDisplay::displayPoint(I, ip, vpColor::red);
446  // }
447  //
448  // *this = list_query_pixels[max_rank] ;//The vpMeSite is replaced by
449  // the vpMeSite of max likelihood normGradient =
450  // vpMath::sqr(max_convolution);
451  //
452  // convlt = max_convolution;
453  // i_1 = ii_1; //list_query_pixels[max_rank].i ;
454  // j_1 = jj_1; //list_query_pixels[max_rank].j ;
455  // delete []list_query_pixels ;
456  // delete []likelihood;
457  // }
458  // else //none of the query sites is better than the threshold
459  // {
460  // if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
461  // {
462  // ip.set_i( list_query_pixels[max_rank].i );
463  // ip.set_j( list_query_pixels[max_rank].j );
464  // vpDisplay::displayPoint(I, ip, vpColor::green);
465  // }
466  // normGradient = 0 ;
467  // if(max == 0)
468  // suppress = 1; // contrast suppression
469  // else
470  // suppress = 2; // threshold suppression
471  //
472  // delete []list_query_pixels ;
473  // delete []likelihood; // modif portage
474  // }
475 
476  int max_rank = -1;
477  // int max_rank1=-1 ;
478  // int max_rank2 = -1;
479  double max_convolution = 0;
480  double max = 0;
481  double contrast = 0;
482  // vpDisplay::display(I) ;
483  // vpERROR_TRACE("getclcik %d",me->range) ;
484  // vpDisplay::getClick(I) ;
485 
486  // range = +/- range of pixels within which the correspondent
487  // of the current pixel will be sought
488  unsigned int range = me->getRange();
489 
490  // std::cout << i << " " << j<<" " << range << " " << suppress <<
491  // std::endl ;
492  vpMeSite *list_query_pixels = getQueryList(I, (int)range);
493 
494  double contrast_max = 1 + me->getMu2();
495  double contrast_min = 1 - me->getMu1();
496 
497  // array in which likelihood ratios will be stored
498  double *likelihood = new double[2 * range + 1];
499 
500  int ii_1 = i;
501  int jj_1 = j;
502  i_1 = i;
503  j_1 = j;
504  double threshold;
505  threshold = me->getThreshold();
506  double diff = 1e6;
507 
508  // std::cout <<"---------------------"<<std::endl ;
509  for (unsigned int n = 0; n < 2 * range + 1; n++) {
510  // convolution results
511  double convolution_ = list_query_pixels[n].convolution(I, me);
512 
513  // luminance ratio of reference pixel to potential correspondent pixel
514  // the luminance must be similar, hence the ratio value should
515  // lay between, for instance, 0.5 and 1.5 (parameter tolerance)
516  if (test_contrast) {
517  likelihood[n] = fabs(convolution_ + convlt);
518  if (likelihood[n] > threshold) {
519  contrast = convolution_ / convlt;
520  if ((contrast > contrast_min) && (contrast < contrast_max) && fabs(1 - contrast) < diff) {
521  diff = fabs(1 - contrast);
522  max_convolution = convolution_;
523  max = likelihood[n];
524  max_rank = (int)n;
525  // max_rank2 = max_rank1;
526  // max_rank1 = max_rank;
527  }
528  }
529  }
530 
531  else {
532  likelihood[n] = fabs(2 * convolution_);
533  if (likelihood[n] > max && likelihood[n] > threshold) {
534  max_convolution = convolution_;
535  max = likelihood[n];
536  max_rank = (int)n;
537  // max_rank2 = max_rank1;
538  // max_rank1 = max_rank;
539  }
540  }
541  }
542 
543  // test on the likelihood threshold if threshold==-1 then
544  // the me->threshold is selected
545 
546  vpImagePoint ip;
547 
548  // if (test_contrast)
549  if (max_rank >= 0) {
550  if ((selectDisplay == RANGE_RESULT) || (selectDisplay == RESULT)) {
551  ip.set_i(list_query_pixels[max_rank].i);
552  ip.set_j(list_query_pixels[max_rank].j);
554  }
555 
556  *this = list_query_pixels[max_rank]; // The vpMeSite2 is replaced by the
557  // vpMeSite2 of max likelihood
558  normGradient = vpMath::sqr(max_convolution);
559 
560  convlt = max_convolution;
561  i_1 = ii_1; // list_query_pixels[max_rank].i ;
562  j_1 = jj_1; // list_query_pixels[max_rank].j ;
563  delete [] list_query_pixels;
564  delete [] likelihood;
565  }
566  else // none of the query sites is better than the threshold
567  {
568  if ((selectDisplay == RANGE_RESULT) || (selectDisplay == RESULT)) {
569  ip.set_i(list_query_pixels[0].i);
570  ip.set_j(list_query_pixels[0].j);
572  }
573  normGradient = 0;
574  // if(contrast != 0)
575  if (std::fabs(contrast) > std::numeric_limits<double>::epsilon())
576  state = CONTRAST; // contrast suppression
577  else
578  state = THRESHOLD; // threshold suppression
579 
580  delete [] list_query_pixels;
581  delete [] likelihood; // modif portage
582  }
583 }
584 
585 int vpMeSite::operator!=(const vpMeSite &m) { return ((m.i != i) || (m.j != j)); }
586 
587 VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpMeSite &vpMeS)
588 {
589 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
590  return (os << "Alpha: " << vpMeS.alpha << " Convolution: " << vpMeS.convlt << " Flag: " << vpMeS.suppress
591  << " Weight: " << vpMeS.weight);
592 #else
593  return (os << "Alpha: " << vpMeS.alpha << " Convolution: " << vpMeS.convlt << " Weight: " << vpMeS.weight);
594 #endif
595 }
596 
598 
600 
601 // Static functions
602 
621 void vpMeSite::display(const vpImage<unsigned char> &I, const double &i, const double &j, const vpMeSiteState &state)
622 {
623  switch (state) {
624  case NO_SUPPRESSION:
626  break;
627 
628  case CONTRAST:
630  break;
631 
632  case THRESHOLD:
634  break;
635 
636  case M_ESTIMATOR:
638  break;
639 
640  case TOO_NEAR:
642  break;
643 
644  default:
646  }
647 }
648 
667 void vpMeSite::display(const vpImage<vpRGBa> &I, const double &i, const double &j, const vpMeSiteState &state)
668 {
669  switch (state) {
670  case NO_SUPPRESSION:
672  break;
673 
674  case CONTRAST:
676  break;
677 
678  case THRESHOLD:
680  break;
681 
682  case M_ESTIMATOR:
684  break;
685 
686  case TOO_NEAR:
688  break;
689 
690  default:
692  }
693 }
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:499
static const vpColor red
Definition: vpColor.h:217
static const vpColor cyan
Definition: vpColor.h:226
static const vpColor blue
Definition: vpColor.h:223
static const vpColor purple
Definition: vpColor.h:228
static const vpColor yellow
Definition: vpColor.h:225
static const vpColor green
Definition: vpColor.h:220
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:89
void set_j(double jj)
Definition: vpImagePoint.h:309
void set_i(double ii)
Definition: vpImagePoint.h:298
unsigned int getWidth() const
Definition: vpImage.h:247
unsigned int getHeight() const
Definition: vpImage.h:189
static double sqr(double x)
Definition: vpMath.h:122
static int round(double x)
Definition: vpMath.h:321
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:67
int j
Definition: vpMeSite.h:86
vpMeSiteState
Moving-edge site state.
Definition: vpMeSite.h:73
@ TOO_NEAR
Point removed because too near image borders.
Definition: vpMeSite.h:81
@ THRESHOLD
Point removed due to a threshold problem.
Definition: vpMeSite.h:79
@ CONTRAST
Point removed due to a contrast problem.
Definition: vpMeSite.h:75
@ M_ESTIMATOR
Point removed during virtual visual-servoing because considered as an outlier.
Definition: vpMeSite.h:80
@ NO_SUPPRESSION
Point used by the tracker.
Definition: vpMeSite.h:74
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:138
double weight
Definition: vpMeSite.h:98
int operator!=(const vpMeSite &m)
Definition: vpMeSite.cpp:585
void display(const vpImage< unsigned char > &I)
Definition: vpMeSite.cpp:597
double ifloat
Definition: vpMeSite.h:88
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
Definition: vpMeSite.cpp:288
@ NONE
Definition: vpMeSite.h:69
@ RANGE_RESULT
Definition: vpMeSite.h:69
@ RESULT
Definition: vpMeSite.h:69
@ RANGE
Definition: vpMeSite.h:69
int i
Definition: vpMeSite.h:86
int mask_sign
Definition: vpMeSite.h:90
vpMeSite & operator=(const vpMeSite &m)
Definition: vpMeSite.cpp:179
int suppress
Definition: vpMeSite.h:248
vp_deprecated void getSign(const vpImage< unsigned char > &I, const int range)
Definition: vpMeSite.cpp:261
int j_1
Definition: vpMeSite.h:87
double normGradient
Definition: vpMeSite.h:96
double alpha
Definition: vpMeSite.h:92
void init()
Definition: vpMeSite.cpp:61
double jfloat
Definition: vpMeSite.h:88
vpMeSite * getQueryList(const vpImage< unsigned char > &I, const int range)
Definition: vpMeSite.cpp:213
double convlt
Definition: vpMeSite.h:94
unsigned char v
Definition: vpMeSite.h:89
int i_1
Definition: vpMeSite.h:87
vpMeSite()
Definition: vpMeSite.cpp:90
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_contrast=true)
Definition: vpMeSite.cpp:350
Definition: vpMe.h:119
unsigned int getAngleStep() const
Definition: vpMe.h:172
double getMu1() const
Definition: vpMe.h:213
int getStrip() const
Definition: vpMe.h:243
double getMu2() const
Definition: vpMe.h:219
double getThreshold() const
Definition: vpMe.h:252
unsigned int getMaskSize() const
Definition: vpMe.h:200
vpMatrix * getMask() const
Definition: vpMe.h:178
unsigned int getRange() const
Definition: vpMe.h:237