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