ViSP  2.9.0
vpMeSite.cpp
1 /****************************************************************************
2  *
3  * $Id: vpMeSite.cpp 4649 2014-02-07 14:57:11Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Moving edges.
36  *
37  * Authors:
38  * Eric Marchand
39  * Andrew Comport
40  * Aurelien Yol
41  *
42  *****************************************************************************/
43 
51 #include <visp/vpMeSite.h>
52 #include <visp/vpMe.h>
53 #include <visp/vpTrackingException.h>
54 #include <stdlib.h>
55 #include <cmath> // std::fabs
56 #include <limits> // numeric_limits
57 #include "vpMeSite.h"
58 
59 
60 #ifndef DOXYGEN_SHOULD_SKIP_THIS
61 static
62 bool horsImage(int i , int j, int half, int rows, int cols)
63 {
64  //return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j > (cols - half - 3) )) ;
65  int half_1 = half + 1;
66  int half_3 = half + 3;
67  //return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j > (cols - half - 3) )) ;
68  return( (0 < (half_1 - i) ) || ( (i - rows + half_3) > 0 ) || ( 0 < (half_1 -j) ) || ( (j - cols + half_3) > 0 ) ) ;
69 }
70 #endif
71 
72 void
74 {
75  // Site components
76  alpha = 0.0 ;
77  convlt = 0.0 ;
78  weight=-1;
79 
80  selectDisplay = NONE ;
81 
82  // Pixel components
83  i = 0 ;
84  j = 0 ;
85  v = 0 ;
86  ifloat =i ;
87  jfloat = j ;
88  i_1 = 0 ;
89  j_1 = 0 ;
90 
91  mask_sign = 1 ;
92 
93  normGradient = 0;
94 
95  state = NO_SUPPRESSION;
96 
97 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
98  suppress = 0;
99 #endif
100 }
101 
103  : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.),
104  convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
105 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
106  , suppress(0)
107 #endif
108 {
109 }
110 
111 vpMeSite::vpMeSite(double ip, double jp)
112  : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.),
113  convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
114 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
115  , suppress(0)
116 #endif
117 {
118  i = vpMath::round(ip) ;
119  j = vpMath::round(jp) ;
120  ifloat = ip ;
121  jfloat = jp ;
122 }
123 
128  : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.),
129  convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
130 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
131  , suppress(0)
132 #endif
133 {
134  *this = mesite;
135 }
136 
137 // More an Update than init
138 // For points in meter form (to avoid approximations)
139 void
140 vpMeSite::init(double ip, double jp, double alphap)
141 {
142  // Note: keep old value of convlt, suppress and contrast
143  selectDisplay = NONE ;
144 
145  ifloat = ip;
146  i= vpMath::round(ip);
147  jfloat = jp ;
148  j = vpMath::round(jp);
149  alpha = alphap ;
150  mask_sign =1 ;
151 
152  v = 0 ;
153  i_1 = 0 ;
154  j_1 = 0 ;
155 
156 }
157 
158 // initialise with convolution
159 void
160 vpMeSite::init(double ip, double jp, double alphap, double convltp)
161 {
162  selectDisplay = NONE ;
163  ifloat = ip ;
164  i= (int)ip ;
165  jfloat = jp ;
166  j =(int)jp ;
167  alpha = alphap ;
168  convlt = convltp;
169  mask_sign =1 ;
170 
171  v = 0 ;
172  i_1 = 0 ;
173  j_1 = 0 ;
174 }
175 // initialise with convolution and sign
176 void
177 vpMeSite::init(double ip, double jp, double alphap, double convltp, int sign)
178 {
179  selectDisplay = NONE ;
180  ifloat = ip ;
181  i= (int)ip ;
182  jfloat = jp ;
183  j =(int)jp ;
184  alpha = alphap ;
185  convlt = convltp;
186  mask_sign = sign ;
187 
188  v = 0 ;
189  i_1 = 0 ;
190  j_1 = 0 ;
191 }
192 
194 {
195  i = m.i;
196  j = m.j;
197  i_1 = m.i_1;
198  j_1 = m.j_1;
199  ifloat = m.ifloat;
200  jfloat = m.jfloat;
201  v = m.v;
202  mask_sign = m.mask_sign;
203  alpha = m.alpha;
204  convlt = m.convlt;
206  weight = m.weight;
207  selectDisplay = m.selectDisplay;
208  state = m.state;
209 
210 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
211  suppress = m.suppress;
212 #endif
213 
214  return *this ;
215 }
216 
217 
218 // ===================================================================
226 // ===================================================================
227 
228 vpMeSite*
230 {
231 
232  int k ;
233 
234  int n;
235  double ii , jj ;
236  vpMeSite *list_query_pixels ;
237  list_query_pixels = NULL ;
238 
239  unsigned int range_ = static_cast<unsigned int>(range);
240  // Size of query list includes the point on the line
241  list_query_pixels = new vpMeSite[2 * range_ + 1] ;
242 
243  // range : +/- the range within which the pixel's
244  //correspondent will be sought
245 
246  double salpha = sin(alpha);
247  double calpha = cos(alpha);
248  n = 0 ;
249  vpImagePoint ip;
250 
251  for(k = -range ; k <= range ; k++)
252  {
253  ii = (ifloat+k*salpha);
254  jj = (jfloat+k*calpha);
255 
256  // Display
257  if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RANGE)) {
258  ip.set_i( ii );
259  ip.set_j( jj );
261  }
262 
263  // Copy parent's convolution
264  vpMeSite pel ;
265  pel.init(ii, jj, alpha, convlt,mask_sign) ;
266  pel.setDisplay(selectDisplay) ;// Display
267 
268  // Add site to the query list
269  list_query_pixels[n] = pel ;
270  n++ ;
271  }
272 
273  return(list_query_pixels) ;
274 }
275 
276 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
277 // ===================================================================
285 // ===================================================================
286 void
287 vpMeSite::getSign(const vpImage<unsigned char> &I, const int range)
288 {
289 
290  int k ;
291 
292  double salpha = sin(alpha);
293  double calpha = cos(alpha);
294 
295  //First extremity
296  k = -range ;
297  unsigned int i1 = static_cast<unsigned int>(vpMath::round(ifloat+k*salpha));
298  unsigned int j1 = static_cast<unsigned int>(vpMath::round(jfloat+k*calpha));
299 
300  //Second extremity
301  k = range ;
302  unsigned int i2 = static_cast<unsigned int>(vpMath::round(ifloat+k*salpha));
303  unsigned int j2 = static_cast<unsigned int>(vpMath::round(jfloat+k*calpha));
304 
305  // TODO: Here check if i1,j1,i2,j2 > 0 else ??
306  if (I[i1][j1] > I[i2][j2]) mask_sign = 1 ; else mask_sign = -1 ;
307 }
308 #endif
309 
310 // Specific function for ME
311 double
313 {
314  int half;
315  unsigned int index_mask ;
316  int height_ = static_cast<int>(I.getHeight());
317  int width_ = static_cast<int>(I.getWidth());
318 
319  double conv = 0.0 ;
320  unsigned int msize = me->getMaskSize();
321  half = (static_cast<int>(msize) - 1) >> 1 ;
322 
323  if(horsImage( i , j , half + me->getStrip() , height_, width_))
324  {
325  conv = 0.0 ;
326  i = 0 ; j = 0 ;
327  }
328  else
329  {
330  // Calculate tangent angle from normal
331  double theta = alpha+M_PI/2;
332  // Move tangent angle to within 0->M_PI for a positive
333  // mask index
334  while (theta<0) theta += M_PI;
335  while (theta>M_PI) theta -= M_PI;
336 
337  // Convert radians to degrees
338  int thetadeg = vpMath::round(theta * 180 / M_PI) ;
339 
340  if(abs(thetadeg) == 180 )
341  {
342  thetadeg= 0 ;
343  }
344 
345  index_mask = (unsigned int)(thetadeg/(double)me->getAngleStep());
346 
347  unsigned int i_ = static_cast<unsigned int>(i);
348  unsigned int j_ = static_cast<unsigned int>(j);
349  unsigned int half_ = static_cast<unsigned int>(half);
350 
351  unsigned int ihalf = i_-half_ ;
352  unsigned int jhalf = j_-half_ ;
353  unsigned int ihalfa ;
354 
355  for(unsigned int a = 0 ; a < msize ; a++ )
356  {
357  ihalfa = ihalf+a ;
358  for(unsigned int b = 0 ; b < msize ; b++ )
359  {
360  conv += mask_sign* me->getMask()[index_mask][a][b] *
361  // I(i-half+a,j-half+b) ;
362  I(ihalfa,jhalf+b) ;
363  }
364  }
365 
366  }
367 
368  return(conv) ;
369 }
370 
371 
380 void
382  const vpMe *me,
383  const bool test_contraste)
384 {
385  // vpMeSite *list_query_pixels ;
386  // int max_rank =0 ;
387  // int max_rank1=0 ;
388  // int max_rank2 = 0;
389  // double convolution = 0 ;
390  // double max_convolution = 0 ;
391  // double max = 0 ;
392  // double contraste = 0;
393  // // vpDisplay::display(I) ;
394  // // vpERROR_TRACE("getclcik %d",me->getRange()) ;
395  // // vpDisplay::getClick(I) ;
396  //
397  // // range = +/- range of pixels within which the correspondent
398  // // of the current pixel will be sought
399  // int range = me->getRange() ;
400  //
401  // // std::cout << i << " " << j<<" " << range << " " << suppress << std::endl ;
402  // list_query_pixels = getQueryList(I, range) ;
403  //
404  // double contraste_max = 1 + me->getMu2();
405  // double contraste_min = 1 - me->getMu1();
406  //
407  // // array in which likelihood ratios will be stored
408  // double *likelihood= new double[ 2 * range + 1 ] ;
409  //
410  // int ii_1 = i ;
411  // int jj_1 = j ;
412  // i_1 = i ;
413  // j_1 = j ;
414  // double threshold;
415  // threshold = me->getThreshold();
416  //
417  // // std::cout <<"---------------------"<<std::endl ;
418  // for(int n = 0 ; n < 2 * range + 1 ; n++)
419  // {
420  //
421  // // convolution results
422  // convolution = list_query_pixels[n].convolution(I, me) ;
423  //
424  // // luminance ratio of reference pixel to potential correspondent pixel
425  // // the luminance must be similar, hence the ratio value should
426  // // lay between, for instance, 0.5 and 1.5 (parameter tolerance)
427  // if( test_contraste )
428  // {
429  // // Include this to eliminate temporal calculation
430  // if (convlt==0)
431  // {
432  // std::cout << "vpMeSite::track : Division by zero convlt = 0" << std::endl ;
433  // delete []list_query_pixels ;
434  // delete []likelihood;
435  // throw(vpTrackingException(vpTrackingException::initializationError,
436  // "Division by zero")) ;
437  // }
438  //
439  // // contraste = fabs(convolution / convlt) ;
440  // contraste = convolution / convlt ;
441  // // likelihood ratios
442  // if((contraste > contraste_min) && (contraste < contraste_max))
443  // likelihood[n] = fabs(convolution + convlt ) ;
444  // else
445  // likelihood[n] = 0 ;
446  // }
447  // else
448  // likelihood[n] = fabs(2*convolution) ;
449  //
450  //
451  // // establishment of the maximal likelihood ratios's rank
452  // // in the array, the value of the likelihood ratio can now be
453  // // referenced by its rank in the array
454  // if (likelihood[n] > max)
455  // {
456  // max_convolution= convolution;
457  // max = likelihood[n] ;
458  // max_rank = n ;
459  // max_rank2 = max_rank1;
460  // max_rank1 = max_rank;
461  // }
462  //
463  // }
464  //
465  // // test on the likelihood threshold if threshold==-1 then
466  // // the me->threshold is selected
467  //
468  // vpImagePoint ip;
469  //
470  // // if (test_contrast)
471  // if(max > threshold)
472  // {
473  // if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
474  // {
475  // ip.set_i( list_query_pixels[max_rank].i );
476  // ip.set_j( list_query_pixels[max_rank].j );
477  // vpDisplay::displayPoint(I, ip, vpColor::red);
478  // }
479  //
480  // *this = list_query_pixels[max_rank] ;//The vpMeSite is replaced by the vpMeSite of max likelihood
481  // normGradient = vpMath::sqr(max_convolution);
482  //
483  // convlt = max_convolution;
484  // i_1 = ii_1; //list_query_pixels[max_rank].i ;
485  // j_1 = jj_1; //list_query_pixels[max_rank].j ;
486  // delete []list_query_pixels ;
487  // delete []likelihood;
488  // }
489  // else //none of the query sites is better than the threshold
490  // {
491  // if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
492  // {
493  // ip.set_i( list_query_pixels[max_rank].i );
494  // ip.set_j( list_query_pixels[max_rank].j );
495  // vpDisplay::displayPoint(I, ip, vpColor::green);
496  // }
497  // normGradient = 0 ;
498  // if(max == 0)
499  // suppress = 1; // contrast suppression
500  // else
501  // suppress = 2; // threshold suppression
502  //
503  // delete []list_query_pixels ;
504  // delete []likelihood; // modif portage
505  // }
506 
507  vpMeSite *list_query_pixels ;
508  int max_rank =-1 ;
509  // int max_rank1=-1 ;
510  // int max_rank2 = -1;
511  double convolution_ = 0 ;
512  double max_convolution = 0 ;
513  double max = 0 ;
514  double contraste = 0;
515  // vpDisplay::display(I) ;
516  // vpERROR_TRACE("getclcik %d",me->range) ;
517  // vpDisplay::getClick(I) ;
518 
519  // range = +/- range of pixels within which the correspondent
520  // of the current pixel will be sought
521  unsigned int range = me->getRange() ;
522 
523  // std::cout << i << " " << j<<" " << range << " " << suppress << std::endl ;
524  list_query_pixels = getQueryList(I, (int)range) ;
525 
526  double contraste_max = 1 + me->getMu2();
527  double contraste_min = 1 - me->getMu1();
528 
529  // array in which likelihood ratios will be stored
530  double *likelihood= new double[ 2 * range + 1 ] ;
531 
532  int ii_1 = i ;
533  int jj_1 = j ;
534  i_1 = i ;
535  j_1 = j ;
536  double threshold;
537  threshold = me->getThreshold() ;
538  double diff = 1e6;
539 
540  // std::cout <<"---------------------"<<std::endl ;
541  for(unsigned int n = 0 ; n < 2 * range + 1 ; n++)
542  {
543  // convolution results
544  convolution_ = list_query_pixels[n].convolution(I, me) ;
545 
546  // luminance ratio of reference pixel to potential correspondent pixel
547  // the luminance must be similar, hence the ratio value should
548  // lay between, for instance, 0.5 and 1.5 (parameter tolerance)
549  if( test_contraste )
550  {
551  likelihood[n] = fabs(convolution_ + convlt );
552  if (likelihood[n]> threshold)
553  {
554  contraste = convolution_ / convlt;
555  if((contraste > contraste_min) && (contraste < contraste_max) && fabs(1-contraste) < diff)
556  {
557  diff = fabs(1-contraste);
558  max_convolution= convolution_;
559  max = likelihood[n] ;
560  max_rank = (int)n ;
561  // max_rank2 = max_rank1;
562  // max_rank1 = max_rank;
563  }
564  }
565  }
566 
567  else
568  {
569  likelihood[n] = fabs(2*convolution_) ;
570  if (likelihood[n] > max && likelihood[n] > threshold)
571  {
572  max_convolution= convolution_;
573  max = likelihood[n] ;
574  max_rank = (int)n ;
575  // max_rank2 = max_rank1;
576  // max_rank1 = max_rank;
577  }
578  }
579  }
580 
581  // test on the likelihood threshold if threshold==-1 then
582  // the me->threshold is selected
583 
584  vpImagePoint ip;
585 
586  // if (test_contrast)
587  if(max_rank >= 0)
588  {
589  if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
590  {
591  ip.set_i( list_query_pixels[max_rank].i );
592  ip.set_j( list_query_pixels[max_rank].j );
594  }
595 
596  *this = list_query_pixels[max_rank] ;//The vpMeSite2 is replaced by the vpMeSite2 of max likelihood
597  normGradient = vpMath::sqr(max_convolution);
598 
599  convlt = max_convolution;
600  i_1 = ii_1; //list_query_pixels[max_rank].i ;
601  j_1 = jj_1; //list_query_pixels[max_rank].j ;
602  delete []list_query_pixels ;
603  delete []likelihood;
604  }
605  else //none of the query sites is better than the threshold
606  {
607  if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
608  {
609  ip.set_i( list_query_pixels[0].i );
610  ip.set_j( list_query_pixels[0].j );
612  }
613  normGradient = 0 ;
614  //if(contraste != 0)
615  if(std::fabs(contraste) > std::numeric_limits<double>::epsilon())
616  state = CONSTRAST; // contrast suppression
617  else
618  state = THRESHOLD; // threshold suppression
619 
620  delete []list_query_pixels ;
621  delete []likelihood; // modif portage
622  }
623 }
624 
626 {
627  return((m.i != i) || (m.j != j)) ;
628 
629 }
630 
631 VISP_EXPORT std::ostream& operator<<(std::ostream& os, vpMeSite& vpMeS)
632 {
633 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
634  return (os << "Alpha: " << vpMeS.alpha
635  << " Convolution: " << vpMeS.convlt
636  << " Flag: " << vpMeS.suppress
637  << " Weight: " << vpMeS.weight );
638 #else
639  return (os << "Alpha: " << vpMeS.alpha
640  << " Convolution: " << vpMeS.convlt
641  << " Weight: " << vpMeS.weight );
642 #endif
643 }
644 
646 {
648 }
649 
650 //Static functions
651 
667 void vpMeSite::display(const vpImage<unsigned char>& I, const double &i, const double &j, const vpMeSiteState &state)
668 {
669  switch(state)
670  {
671  case NO_SUPPRESSION:
673  break;
674 
675  case CONSTRAST:
677  break;
678 
679  case THRESHOLD:
681  break;
682 
683  case M_ESTIMATOR:
685  break;
686 
687  case TOO_NEAR:
689  break;
690 
691  default:
693  }
694 }
695 
711 void vpMeSite::display(const vpImage<vpRGBa>& I, const double &i, const double &j, const vpMeSiteState &state)
712 {
713  switch(state)
714  {
715  case NO_SUPPRESSION:
717  break;
718 
719  case CONSTRAST:
721  break;
722 
723  case THRESHOLD:
725  break;
726 
727  case M_ESTIMATOR:
729  break;
730 
731  case TOO_NEAR:
733  break;
734 
735  default:
737  }
738 }
unsigned int getRange() const
Definition: vpMe.h:236
unsigned int getMaskSize() const
Definition: vpMe.h:164
void init()
Definition: vpMeSite.cpp:73
unsigned int getWidth() const
Definition: vpImage.h:159
double jfloat
Definition: vpMeSite.h:100
void display(const vpImage< unsigned char > &I)
Definition: vpMeSite.cpp:645
double getMu1() const
Definition: vpMe.h:178
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
Definition: vpMeSite.cpp:312
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
Definition: vpMeSite.h:76
double alpha
Definition: vpMeSite.h:104
int i
Definition: vpMeSite.h:98
Contains predetermined masks for sites and holds moving edges tracking parameters.
Definition: vpMe.h:70
int mask_sign
Definition: vpMeSite.h:102
double normGradient
Definition: vpMeSite.h:108
static const vpColor green
Definition: vpColor.h:170
static int round(const double x)
Definition: vpMath.h:228
static const vpColor red
Definition: vpColor.h:167
double ifloat
Definition: vpMeSite.h:100
int suppress
Definition: vpMeSite.h:257
int getStrip() const
Definition: vpMe.h:292
double getThreshold() const
Definition: vpMe.h:306
void set_i(const double ii)
Definition: vpImagePoint.h:158
unsigned int getAngleStep() const
Definition: vpMe.h:250
static const vpColor cyan
Definition: vpColor.h:176
static double sqr(double x)
Definition: vpMath.h:106
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:152
double convlt
Definition: vpMeSite.h:106
unsigned char v
Definition: vpMeSite.h:101
vpMeSite * getQueryList(const vpImage< unsigned char > &I, const int range)
Definition: vpMeSite.cpp:229
vp_deprecated void getSign(const vpImage< unsigned char > &I, const int range)
Definition: vpMeSite.cpp:287
int j_1
Definition: vpMeSite.h:99
vpMatrix * getMask() const
Definition: vpMe.h:116
void set_j(const double jj)
Definition: vpImagePoint.h:169
double getMu2() const
Definition: vpMe.h:192
int j
Definition: vpMeSite.h:98
double weight
Definition: vpMeSite.h:110
vpMeSite & operator=(const vpMeSite &m)
Definition: vpMeSite.cpp:193
void track(const vpImage< unsigned char > &im, const vpMe *me, const bool test_contraste=true)
Definition: vpMeSite.cpp:381
unsigned int getHeight() const
Definition: vpImage.h:150
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:92
int i_1
Definition: vpMeSite.h:99
static const vpColor yellow
Definition: vpColor.h:175
static const vpColor purple
Definition: vpColor.h:178
int operator!=(const vpMeSite &m)
Definition: vpMeSite.cpp:625
virtual void displayPoint(const vpImagePoint &ip, const vpColor &color)=0
vpMeSiteState
Definition: vpMeSite.h:87
static const vpColor blue
Definition: vpColor.h:173