Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpMeSite.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
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 http://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  * Authors:
34  * Eric Marchand
35  * Andrew Comport
36  * Aurelien Yol
37  *
38  *****************************************************************************/
39 
47 #include <visp3/me/vpMeSite.h>
48 #include <visp3/me/vpMe.h>
49 #include <visp3/core/vpTrackingException.h>
50 #include <stdlib.h>
51 #include <cmath> // std::fabs
52 #include <limits> // numeric_limits
53 #include <visp3/me/vpMeSite.h>
54 
55 
56 #ifndef DOXYGEN_SHOULD_SKIP_THIS
57 static
58 bool horsImage(int i , int j, int half, int rows, int cols)
59 {
60  //return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j > (cols - half - 3) )) ;
61  int half_1 = half + 1;
62  int half_3 = half + 3;
63  //return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j > (cols - half - 3) )) ;
64  return( (0 < (half_1 - i) ) || ( (i - rows + half_3) > 0 ) || ( 0 < (half_1 -j) ) || ( (j - cols + half_3) > 0 ) ) ;
65 }
66 #endif
67 
68 void
70 {
71  // Site components
72  alpha = 0.0 ;
73  convlt = 0.0 ;
74  weight=-1;
75 
76  selectDisplay = NONE ;
77 
78  // Pixel components
79  i = 0 ;
80  j = 0 ;
81  v = 0 ;
82  ifloat =i ;
83  jfloat = j ;
84  i_1 = 0 ;
85  j_1 = 0 ;
86 
87  mask_sign = 1 ;
88 
89  normGradient = 0;
90 
91  state = NO_SUPPRESSION;
92 
93 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
94  suppress = 0;
95 #endif
96 }
97 
99  : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.),
100  convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
101 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
102  , suppress(0)
103 #endif
104 {
105 }
106 
107 vpMeSite::vpMeSite(double ip, double jp)
108  : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.),
109  convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
110 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
111  , suppress(0)
112 #endif
113 {
114  i = vpMath::round(ip) ;
115  j = vpMath::round(jp) ;
116  ifloat = ip ;
117  jfloat = jp ;
118 }
119 
124  : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.),
125  convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
126 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
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
136 vpMeSite::init(double ip, double jp, double alphap)
137 {
138  // Note: keep old value of convlt, suppress and contrast
139  selectDisplay = NONE ;
140 
141  ifloat = ip;
142  i= vpMath::round(ip);
143  jfloat = jp ;
144  j = vpMath::round(jp);
145  alpha = alphap ;
146  mask_sign =1 ;
147 
148  v = 0 ;
149  i_1 = 0 ;
150  j_1 = 0 ;
151 
152 }
153 
154 // initialise with convolution
155 void
156 vpMeSite::init(double ip, double jp, double alphap, double convltp)
157 {
158  selectDisplay = NONE ;
159  ifloat = ip ;
160  i= (int)ip ;
161  jfloat = jp ;
162  j =(int)jp ;
163  alpha = alphap ;
164  convlt = convltp;
165  mask_sign =1 ;
166 
167  v = 0 ;
168  i_1 = 0 ;
169  j_1 = 0 ;
170 }
171 // initialise with convolution and sign
172 void
173 vpMeSite::init(double ip, double jp, double alphap, double convltp, int sign)
174 {
175  selectDisplay = NONE ;
176  ifloat = ip ;
177  i= (int)ip ;
178  jfloat = jp ;
179  j =(int)jp ;
180  alpha = alphap ;
181  convlt = convltp;
182  mask_sign = sign ;
183 
184  v = 0 ;
185  i_1 = 0 ;
186  j_1 = 0 ;
187 }
188 
190 {
191  i = m.i;
192  j = m.j;
193  i_1 = m.i_1;
194  j_1 = m.j_1;
195  ifloat = m.ifloat;
196  jfloat = m.jfloat;
197  v = m.v;
198  mask_sign = m.mask_sign;
199  alpha = m.alpha;
200  convlt = m.convlt;
202  weight = m.weight;
203  selectDisplay = m.selectDisplay;
204  state = m.state;
205 
206 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
207  suppress = m.suppress;
208 #endif
209 
210  return *this ;
211 }
212 
213 
214 // ===================================================================
222 // ===================================================================
223 
224 vpMeSite*
226 {
227 
228  int k ;
229 
230  int n;
231  vpMeSite *list_query_pixels ;
232  list_query_pixels = NULL ;
233 
234  unsigned int range_ = static_cast<unsigned int>(range);
235  // Size of query list includes the point on the line
236  list_query_pixels = new vpMeSite[2 * range_ + 1] ;
237 
238  // range : +/- the range within which the pixel's
239  //correspondent will be sought
240 
241  double salpha = sin(alpha);
242  double calpha = cos(alpha);
243  n = 0 ;
244  vpImagePoint ip;
245 
246  for(k = -range ; k <= range ; k++)
247  {
248  double ii = (ifloat+k*salpha);
249  double jj = (jfloat+k*calpha);
250 
251  // Display
252  if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RANGE)) {
253  ip.set_i( ii );
254  ip.set_j( jj );
256  }
257 
258  // Copy parent's convolution
259  vpMeSite pel ;
260  pel.init(ii, jj, alpha, convlt,mask_sign) ;
261  pel.setDisplay(selectDisplay) ;// Display
262 
263  // Add site to the query list
264  list_query_pixels[n] = pel ;
265  n++ ;
266  }
267 
268  return(list_query_pixels) ;
269 }
270 
271 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
272 // ===================================================================
280 // ===================================================================
281 void
282 vpMeSite::getSign(const vpImage<unsigned char> &I, const int range)
283 {
284 
285  int k ;
286 
287  double salpha = sin(alpha);
288  double calpha = cos(alpha);
289 
290  //First extremity
291  k = -range ;
292  unsigned int i1 = static_cast<unsigned int>(vpMath::round(ifloat+k*salpha));
293  unsigned int j1 = static_cast<unsigned int>(vpMath::round(jfloat+k*calpha));
294 
295  //Second extremity
296  k = range ;
297  unsigned int i2 = static_cast<unsigned int>(vpMath::round(ifloat+k*salpha));
298  unsigned int j2 = static_cast<unsigned int>(vpMath::round(jfloat+k*calpha));
299 
300  // TODO: Here check if i1,j1,i2,j2 > 0 else ??
301  if (I[i1][j1] > I[i2][j2]) mask_sign = 1 ; else mask_sign = -1 ;
302 }
303 #endif
304 
305 // Specific function for ME
306 double
308 {
309  int half;
310  int height_ = static_cast<int>(I.getHeight());
311  int width_ = static_cast<int>(I.getWidth());
312 
313  double conv = 0.0 ;
314  unsigned int msize = me->getMaskSize();
315  half = (static_cast<int>(msize) - 1) >> 1 ;
316 
317  if(horsImage( i , j , half + me->getStrip() , height_, width_))
318  {
319  conv = 0.0 ;
320  i = 0 ; j = 0 ;
321  }
322  else
323  {
324  // Calculate tangent angle from normal
325  double theta = alpha+M_PI/2;
326  // Move tangent angle to within 0->M_PI for a positive
327  // mask index
328  while (theta<0) theta += M_PI;
329  while (theta>M_PI) theta -= M_PI;
330 
331  // Convert radians to degrees
332  int thetadeg = vpMath::round(theta * 180 / M_PI) ;
333 
334  if(abs(thetadeg) == 180 )
335  {
336  thetadeg= 0 ;
337  }
338 
339  unsigned int index_mask = (unsigned int)(thetadeg/(double)me->getAngleStep());
340 
341  unsigned int i_ = static_cast<unsigned int>(i);
342  unsigned int j_ = static_cast<unsigned int>(j);
343  unsigned int half_ = static_cast<unsigned int>(half);
344 
345  unsigned int ihalf = i_-half_ ;
346  unsigned int jhalf = j_-half_ ;
347 
348  for(unsigned int a = 0 ; a < msize ; a++ )
349  {
350  unsigned int ihalfa = ihalf+a ;
351  for(unsigned int b = 0 ; b < msize ; b++ )
352  {
353  conv += mask_sign* me->getMask()[index_mask][a][b] *
354  // I(i-half+a,j-half+b) ;
355  I(ihalfa,jhalf+b) ;
356  }
357  }
358 
359  }
360 
361  return(conv) ;
362 }
363 
364 
373 void
375  const vpMe *me,
376  const bool test_contraste)
377 {
378  // vpMeSite *list_query_pixels ;
379  // int max_rank =0 ;
380  // int max_rank1=0 ;
381  // int max_rank2 = 0;
382  // double convolution = 0 ;
383  // double max_convolution = 0 ;
384  // double max = 0 ;
385  // double contraste = 0;
386  // // vpDisplay::display(I) ;
387  // // vpERROR_TRACE("getclcik %d",me->getRange()) ;
388  // // vpDisplay::getClick(I) ;
389  //
390  // // range = +/- range of pixels within which the correspondent
391  // // of the current pixel will be sought
392  // int range = me->getRange() ;
393  //
394  // // std::cout << i << " " << j<<" " << range << " " << suppress << std::endl ;
395  // list_query_pixels = getQueryList(I, range) ;
396  //
397  // double contraste_max = 1 + me->getMu2();
398  // double contraste_min = 1 - me->getMu1();
399  //
400  // // array in which likelihood ratios will be stored
401  // double *likelihood= new double[ 2 * range + 1 ] ;
402  //
403  // int ii_1 = i ;
404  // int jj_1 = j ;
405  // i_1 = i ;
406  // j_1 = j ;
407  // double threshold;
408  // threshold = me->getThreshold();
409  //
410  // // std::cout <<"---------------------"<<std::endl ;
411  // for(int n = 0 ; n < 2 * range + 1 ; n++)
412  // {
413  //
414  // // convolution results
415  // convolution = list_query_pixels[n].convolution(I, me) ;
416  //
417  // // luminance ratio of reference pixel to potential correspondent pixel
418  // // the luminance must be similar, hence the ratio value should
419  // // lay between, for instance, 0.5 and 1.5 (parameter tolerance)
420  // if( test_contraste )
421  // {
422  // // Include this to eliminate temporal calculation
423  // if (convlt==0)
424  // {
425  // std::cout << "vpMeSite::track : Division by zero convlt = 0" << std::endl ;
426  // delete []list_query_pixels ;
427  // delete []likelihood;
428  // throw(vpTrackingException(vpTrackingException::initializationError,
429  // "Division by zero")) ;
430  // }
431  //
432  // // contraste = fabs(convolution / convlt) ;
433  // contraste = convolution / convlt ;
434  // // likelihood ratios
435  // if((contraste > contraste_min) && (contraste < contraste_max))
436  // likelihood[n] = fabs(convolution + convlt ) ;
437  // else
438  // likelihood[n] = 0 ;
439  // }
440  // else
441  // likelihood[n] = fabs(2*convolution) ;
442  //
443  //
444  // // establishment of the maximal likelihood ratios's rank
445  // // in the array, the value of the likelihood ratio can now be
446  // // referenced by its rank in the array
447  // if (likelihood[n] > max)
448  // {
449  // max_convolution= convolution;
450  // max = likelihood[n] ;
451  // max_rank = n ;
452  // max_rank2 = max_rank1;
453  // max_rank1 = max_rank;
454  // }
455  //
456  // }
457  //
458  // // test on the likelihood threshold if threshold==-1 then
459  // // the me->threshold is selected
460  //
461  // vpImagePoint ip;
462  //
463  // // if (test_contrast)
464  // if(max > threshold)
465  // {
466  // if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
467  // {
468  // ip.set_i( list_query_pixels[max_rank].i );
469  // ip.set_j( list_query_pixels[max_rank].j );
470  // vpDisplay::displayPoint(I, ip, vpColor::red);
471  // }
472  //
473  // *this = list_query_pixels[max_rank] ;//The vpMeSite is replaced by the vpMeSite of max likelihood
474  // normGradient = vpMath::sqr(max_convolution);
475  //
476  // convlt = max_convolution;
477  // i_1 = ii_1; //list_query_pixels[max_rank].i ;
478  // j_1 = jj_1; //list_query_pixels[max_rank].j ;
479  // delete []list_query_pixels ;
480  // delete []likelihood;
481  // }
482  // else //none of the query sites is better than the threshold
483  // {
484  // if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
485  // {
486  // ip.set_i( list_query_pixels[max_rank].i );
487  // ip.set_j( list_query_pixels[max_rank].j );
488  // vpDisplay::displayPoint(I, ip, vpColor::green);
489  // }
490  // normGradient = 0 ;
491  // if(max == 0)
492  // suppress = 1; // contrast suppression
493  // else
494  // suppress = 2; // threshold suppression
495  //
496  // delete []list_query_pixels ;
497  // delete []likelihood; // modif portage
498  // }
499 
500  vpMeSite *list_query_pixels ;
501  int max_rank =-1 ;
502  // int max_rank1=-1 ;
503  // int max_rank2 = -1;
504  double max_convolution = 0 ;
505  double max = 0 ;
506  double contraste = 0;
507  // vpDisplay::display(I) ;
508  // vpERROR_TRACE("getclcik %d",me->range) ;
509  // vpDisplay::getClick(I) ;
510 
511  // range = +/- range of pixels within which the correspondent
512  // of the current pixel will be sought
513  unsigned int range = me->getRange() ;
514 
515  // std::cout << i << " " << j<<" " << range << " " << suppress << std::endl ;
516  list_query_pixels = getQueryList(I, (int)range) ;
517 
518  double contraste_max = 1 + me->getMu2();
519  double contraste_min = 1 - me->getMu1();
520 
521  // array in which likelihood ratios will be stored
522  double *likelihood= new double[ 2 * range + 1 ] ;
523 
524  int ii_1 = i ;
525  int jj_1 = j ;
526  i_1 = i ;
527  j_1 = j ;
528  double threshold;
529  threshold = me->getThreshold() ;
530  double diff = 1e6;
531 
532  // std::cout <<"---------------------"<<std::endl ;
533  for(unsigned int n = 0 ; n < 2 * range + 1 ; n++)
534  {
535  // convolution results
536  double convolution_ = list_query_pixels[n].convolution(I, me) ;
537 
538  // luminance ratio of reference pixel to potential correspondent pixel
539  // the luminance must be similar, hence the ratio value should
540  // lay between, for instance, 0.5 and 1.5 (parameter tolerance)
541  if( test_contraste )
542  {
543  likelihood[n] = fabs(convolution_ + convlt );
544  if (likelihood[n]> threshold)
545  {
546  contraste = convolution_ / convlt;
547  if((contraste > contraste_min) && (contraste < contraste_max) && fabs(1-contraste) < diff)
548  {
549  diff = fabs(1-contraste);
550  max_convolution= convolution_;
551  max = likelihood[n] ;
552  max_rank = (int)n ;
553  // max_rank2 = max_rank1;
554  // max_rank1 = max_rank;
555  }
556  }
557  }
558 
559  else
560  {
561  likelihood[n] = fabs(2*convolution_) ;
562  if (likelihood[n] > max && likelihood[n] > threshold)
563  {
564  max_convolution= convolution_;
565  max = likelihood[n] ;
566  max_rank = (int)n ;
567  // max_rank2 = max_rank1;
568  // max_rank1 = max_rank;
569  }
570  }
571  }
572 
573  // test on the likelihood threshold if threshold==-1 then
574  // the me->threshold is selected
575 
576  vpImagePoint ip;
577 
578  // if (test_contrast)
579  if(max_rank >= 0)
580  {
581  if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
582  {
583  ip.set_i( list_query_pixels[max_rank].i );
584  ip.set_j( list_query_pixels[max_rank].j );
586  }
587 
588  *this = list_query_pixels[max_rank] ;//The vpMeSite2 is replaced by the vpMeSite2 of max likelihood
589  normGradient = vpMath::sqr(max_convolution);
590 
591  convlt = max_convolution;
592  i_1 = ii_1; //list_query_pixels[max_rank].i ;
593  j_1 = jj_1; //list_query_pixels[max_rank].j ;
594  delete []list_query_pixels ;
595  delete []likelihood;
596  }
597  else //none of the query sites is better than the threshold
598  {
599  if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
600  {
601  ip.set_i( list_query_pixels[0].i );
602  ip.set_j( list_query_pixels[0].j );
604  }
605  normGradient = 0 ;
606  //if(contraste != 0)
607  if(std::fabs(contraste) > std::numeric_limits<double>::epsilon())
608  state = CONSTRAST; // contrast suppression
609  else
610  state = THRESHOLD; // threshold suppression
611 
612  delete []list_query_pixels ;
613  delete []likelihood; // modif portage
614  }
615 }
616 
618 {
619  return((m.i != i) || (m.j != j)) ;
620 
621 }
622 
623 VISP_EXPORT std::ostream& operator<<(std::ostream& os, vpMeSite& vpMeS)
624 {
625 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
626  return (os << "Alpha: " << vpMeS.alpha
627  << " Convolution: " << vpMeS.convlt
628  << " Flag: " << vpMeS.suppress
629  << " Weight: " << vpMeS.weight );
630 #else
631  return (os << "Alpha: " << vpMeS.alpha
632  << " Convolution: " << vpMeS.convlt
633  << " Weight: " << vpMeS.weight );
634 #endif
635 }
636 
638 {
640 }
641 
642 //Static functions
643 
659 void vpMeSite::display(const vpImage<unsigned char>& I, const double &i, const double &j, const vpMeSiteState &state)
660 {
661  switch(state)
662  {
663  case NO_SUPPRESSION:
665  break;
666 
667  case CONSTRAST:
669  break;
670 
671  case THRESHOLD:
673  break;
674 
675  case M_ESTIMATOR:
677  break;
678 
679  case TOO_NEAR:
681  break;
682 
683  default:
685  }
686 }
687 
703 void vpMeSite::display(const vpImage<vpRGBa>& I, const double &i, const double &j, const vpMeSiteState &state)
704 {
705  switch(state)
706  {
707  case NO_SUPPRESSION:
709  break;
710 
711  case CONSTRAST:
713  break;
714 
715  case THRESHOLD:
717  break;
718 
719  case M_ESTIMATOR:
721  break;
722 
723  case TOO_NEAR:
725  break;
726 
727  default:
729  }
730 }
unsigned int getRange() const
Definition: vpMe.h:167
unsigned int getMaskSize() const
Definition: vpMe.h:131
void init()
Definition: vpMeSite.cpp:69
unsigned int getWidth() const
Definition: vpImage.h:226
double jfloat
Definition: vpMeSite.h:96
void display(const vpImage< unsigned char > &I)
Definition: vpMeSite.cpp:637
double getMu1() const
Definition: vpMe.h:143
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
Definition: vpMeSite.cpp:307
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
Definition: vpMeSite.h:72
double alpha
Definition: vpMeSite.h:100
int i
Definition: vpMeSite.h:94
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
Definition: vpMe.h:59
int mask_sign
Definition: vpMeSite.h:98
double normGradient
Definition: vpMeSite.h:104
static const vpColor green
Definition: vpColor.h:166
static int round(const double x)
Definition: vpMath.h:249
static const vpColor red
Definition: vpColor.h:163
vpMeSite()
Definition: vpMeSite.cpp:98
double ifloat
Definition: vpMeSite.h:96
int suppress
Definition: vpMeSite.h:253
int getStrip() const
Definition: vpMe.h:173
double getThreshold() const
Definition: vpMe.h:180
void set_i(const double ii)
Definition: vpImagePoint.h:163
unsigned int getAngleStep() const
Definition: vpMe.h:104
static const vpColor cyan
Definition: vpColor.h:172
static double sqr(double x)
Definition: vpMath.h:110
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:148
double convlt
Definition: vpMeSite.h:102
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:267
unsigned char v
Definition: vpMeSite.h:97
vpMeSite * getQueryList(const vpImage< unsigned char > &I, const int range)
Definition: vpMeSite.cpp:225
vp_deprecated void getSign(const vpImage< unsigned char > &I, const int range)
Definition: vpMeSite.cpp:282
int j_1
Definition: vpMeSite.h:95
vpMatrix * getMask() const
Definition: vpMe.h:110
void set_j(const double jj)
Definition: vpImagePoint.h:174
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
double getMu2() const
Definition: vpMe.h:149
int j
Definition: vpMeSite.h:94
double weight
Definition: vpMeSite.h:106
vpMeSite & operator=(const vpMeSite &m)
Definition: vpMeSite.cpp:189
void track(const vpImage< unsigned char > &im, const vpMe *me, const bool test_contraste=true)
Definition: vpMeSite.cpp:374
unsigned int getHeight() const
Definition: vpImage.h:175
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:95
static const vpColor yellow
Definition: vpColor.h:171
static const vpColor purple
Definition: vpColor.h:174
int operator!=(const vpMeSite &m)
Definition: vpMeSite.cpp:617
vpMeSiteState
Definition: vpMeSite.h:83
static const vpColor blue
Definition: vpColor.h:169