ViSP  2.10.0
vpMe.cpp
1 /****************************************************************************
2  *
3  * $Id: vpMe.cpp 5060 2014-12-12 18:31:03Z 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  *
41  *****************************************************************************/
42 
50 #include <visp/vpMe.h>
51 #include <visp/vpColVector.h>
52 #include <visp/vpMath.h>
53 #include <stdlib.h>
54 #ifndef DOXYGEN_SHOULD_SKIP_THIS
55 
56 
57 struct point
58 {
59  double x ;
60  double y ;
61 } ;
62 
63 struct droite
64 {
65  double a ;
66  double b ;
67  double c ;
68 } ;
69 
70 
71 
72 
73 
74 
75 
76 template <class Type>
77 inline void
78 permute(Type & a, Type & b)
79 {
80  Type t = a;
81  a = b;
82  b = t;
83 }
84 
85 static droite
86 droite_cartesienne(point P, point Q)
87 {
88  droite PQ;
89 
90  PQ.a = P.y-Q.y;
91  PQ.b = Q.x-P.x;
92  PQ.c = Q.y*P.x - Q.x*P.y;
93 
94  return(PQ);
95 }
96 
97 
98 
99 static point
100 point_intersection(droite D1, droite D2)
101 {
102  point I;
103  double det; // determinant des 2 vect.normaux
104 
105  det = (D1.a*D2.b - D2.a*D1.b); // interdit D1,D2 paralleles
106  I.x = (D2.c*D1.b - D1.c*D2.b)/det;
107  I.y = (D1.c*D2.a - D2.c*D1.a)/det;
108 
109  return(I);
110 }
111 
112 static void
113 recale(point & P,
114  double Xmin, double Ymin, double Xmax, double Ymax)
115 {
116  if(vpMath::equal(P.x,Xmin))
117  P.x=Xmin; // a peu pres => exactement !
118  if(vpMath::equal(P.x,Xmax))
119  P.x=Xmax;
120 
121  if(vpMath::equal(P.y,Ymin))
122  P.y=Ymin;
123  if(vpMath::equal(P.y,Ymax))
124  P.y=Ymax;
125 }
126 
127 
128 static void
129 permute(point &A, point &B)
130 {
131  point C ;
132 
133  if (A.x>B.x) // fonction sans doute a tester...
134  {
135  C = A ;
136  A = B ;
137  B = C ;
138  }
139 }
140 
141 
142 // vrai si partie visible
143 static bool
144 clipping (point A, point B,
145  double Xmin, double Ymin, double Xmax, double Ymax,
146  point & Ac , point & Bc )// resultat: A,B clippes
147 {
148  droite AB, D[4];
149  D[0].a = 1; D[0].b = 0; D[0].c = -Xmin;
150  D[1].a = 1; D[1].b = 0; D[1].c = -Xmax;
151  D[2].a = 0; D[2].b = 1; D[2].c = -Ymin;
152  D[3].a = 0; D[3].b = 1; D[3].c = -Ymax;
153 
154  point P[2];
155  P[0]=A; P[1]=B;
156  int code_P[2], // codes de P[n]
157  i, bit_i, // i -> (0000100...)
158  n;
159 
160  AB = droite_cartesienne(A,B);
161 
162  for ( ; ; ) // 2 sorties directes internes
163  {
164  // CALCULE CODE DE VISIBILITE (Sutherland & Sproul)
165  // ================================================
166  for(n=0; n<2; n++)
167  {
168  code_P[n] = 0000;
169 
170  if( P[n].x < Xmin )
171  code_P[n] |= 1; // positionne bit0
172  if( P[n].x > Xmax )
173  code_P[n] |= 2; // .. bit1
174  if( P[n].y < Ymin )
175  code_P[n] |= 4; // .. bit2
176  if( P[n].y > Ymax )
177  code_P[n] |= 8; // .. bit3
178  }
179 
180 
181  // 2 CAS OU L'ON PEUT CONCLURE => sortie
182  // =====================================
183  if((code_P[0] | code_P[1])==0000) // Aucun bit a 1
184  /* NE TRIE PLUS LE RESULTAT ! S_relative() en tient compte
185  { if(P[0].x < P[1].x) // Rend le couple de points
186  { Ac=P[0]; Bc=P[1]; } // clippes (ordonnes selon
187  else { Ac=P[1]; Bc=P[0]; } // leur abscisse x)
188  */
189  {
190  Ac=P[0]; Bc=P[1];
191  if(vpMath::equal(Ac.x,Bc.x) && vpMath::equal(Ac.y,Bc.y))
192  return(false); // AB = 1 point = invisible
193  else
194  return(true); // Partie de AB clippee visible!
195  }
196 
197  if((code_P[0] & code_P[1])!=0000) // au moins 1 bit commun
198  {
199  return(false); // AB completement invisible!
200  }
201 
202 
203  // CAS GENERAL (on sait que code_P[0 ou 1] a au moins un bit a 1
204  // - clippe le point P[n] qui sort de la fenêtre (coupe Droite i)
205  // - reboucle avec le nouveau couple de points
206  // ================================================================
207  if(code_P[0] != 0000)
208  {
209  n=0; // c'est P[0] qu'on clippera
210  for(i=0,bit_i=1; !(code_P[0] & bit_i); i++,bit_i<<=1){;}
211  }
212  else
213  {
214  n=1; // c'est P[1] qu'on clippera
215  for(i=0,bit_i=1; !(code_P[1] & bit_i); i++,bit_i<<=1){;}
216  }
217 
218  P[n] = point_intersection(AB,D[i]); // clippe le point concerne
219 
220 
221  // RECALE EXACTEMENT LE POINT (calcul flottant => arrondi)
222  // AFIN QUE LE CALCUL DES CODES NE BOUCLE PAS INDEFINIMENT
223  // =======================================================
224  recale(P[n], Xmin,Ymin,Xmax,Ymax);
225 
226  }
227 
228 }
229 
230 
231 // calcule la surface relative des 2 portions definies
232 // par le segment PQ sur le carre Xmin,Ymin,Xmax,Ymax
233 // Rem : P,Q tries sur x, et donc seulement 6 cas
234 static double
235 S_relative(point P, point Q,
236  double Xmin, double Ymin, double Xmax, double Ymax)
237 {
238 
239  if(Q.x < P.x) // tri le couple de points
240  permute(P,Q); // selon leur abscisse x
241 
242 
243 
244  recale(P, Xmin,Ymin,Xmax,Ymax); // permet des calculs de S_relative
245  recale(Q, Xmin,Ymin,Xmax,Ymax); // moins approximatifs.
246 
247  //if(P.x==Xmin && Q.x==Xmax)
248  if((std::fabs(P.x-Xmin) <= vpMath::maximum(std::fabs(P.x),std::fabs(Xmin))*std::numeric_limits<double>::epsilon())
249  &&
250  (std::fabs(Q.x-Xmax) <= vpMath::maximum(std::fabs(Q.x),std::fabs(Xmax))*std::numeric_limits<double>::epsilon()))
251  return( fabs(Ymax+Ymin-P.y-Q.y) );
252 
253  //if( (P.y==Ymin && Q.y==Ymax) ||
254  // (Q.y==Ymin && P.y==Ymax))
255  if( ( (std::fabs(P.y-Ymin) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon())
256  &&
257  (std::fabs(Q.y-Ymax) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon()) )
258  ||
259  ( (std::fabs(Q.y-Ymin) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon())
260  &&
261  (std::fabs(P.y-Ymax) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon()) ) )
262  return( fabs(Xmax+Xmin-P.x-Q.x) );
263 
264  //if( P.x==Xmin && Q.y==Ymax )
265  if( std::fabs(P.x-Xmin) <= vpMath::maximum(std::fabs(P.x),std::fabs(Xmin))*std::numeric_limits<double>::epsilon()
266  &&
267  std::fabs(Q.y-Ymax) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon() )
268  return( 1-(Ymax-P.y)*(Q.x-Xmin) );
269  //if( P.x==Xmin && Q.y==Ymin )
270  if( std::fabs(P.x-Xmin) <= vpMath::maximum(std::fabs(P.x),std::fabs(Xmin))*std::numeric_limits<double>::epsilon()
271  &&
272  std::fabs(Q.y-Ymin) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon() )
273  return( 1-(P.y-Ymin)*(Q.x-Xmin) );
274  //if( P.y==Ymin && Q.x==Xmax )
275  if( std::fabs(P.y-Ymin) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon()
276  && std::fabs(Q.x-Xmax) <= vpMath::maximum(std::fabs(Q.x),std::fabs(Xmax))*std::numeric_limits<double>::epsilon() )
277  return( 1-(Xmax-P.x)*(Q.y-Ymin) );
278  //if( P.y==Ymax && Q.x==Xmax )
279  if( std::fabs(P.y-Ymax) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon()
280  && std::fabs(Q.x-Xmax) <= vpMath::maximum(std::fabs(Q.x),std::fabs(Xmax))*std::numeric_limits<double>::epsilon())
281  return( 1-(Xmax-P.x)*(Ymax-Q.y) );
282 
283 
284  printf("utils_ecm: ERREUR dans S_relative (%f,%f) (%f,%f) %f %f %f %f\n",
285  P.x,P.y,Q.x,Q.y,Xmin,Ymin,Xmax,Ymax);
286  exit(-1); // DEBUG Stoppe net l'execution
287 }
288 
289 
290 static void
291 calcul_masques(vpColVector &angle, // definitions des angles theta
292  unsigned int n, // taille masques (PAIRE ou IMPAIRE Ok)
293  vpMatrix *M) // resultat M[theta](n,n)
294 {
295  // Le coef |a| = |1/2n| n'est pas incorpore dans M(i,j) (=> que des int)
296 
297  unsigned int i_theta, // indice (boucle sur les masques)
298  i,j; // indices de boucle sur M(i,j)
299  double X,Y, // point correspondant/centre du masque
300  theta, cos_theta, sin_theta, tan_theta,
301  moitie = ((double)n)/2.0; // moitie REELLE du masque
302  point P1,Q1,P,Q; // clippe Droite(theta) P1,Q1 -> P,Q
303  int sgn; // signe de M(i,j)
304  double v; // ponderation de M(i,j)
305 
306  unsigned int nb_theta = angle.getRows() ;
307 
308  for(i_theta=0; i_theta<nb_theta; i_theta++)
309  {
310  theta = M_PI/180*angle[i_theta]; // indice i -> theta(i) en radians
311  // angle[] dans [0,180[
312  cos_theta = cos(theta); // vecteur directeur de l'ECM
313  sin_theta = sin(theta); // associe au masque
314 
315  // PRE-CALCULE 2 POINTS DE D(theta) BIEN EN DEHORS DU MASQUE
316  // =========================================================
317  //if( angle[i_theta]==90 ) // => tan(theta) infinie !
318  if( std::fabs(angle[i_theta]-90) <= vpMath::maximum(std::fabs(angle[i_theta]), 90.)*std::numeric_limits<double>::epsilon() ) // => tan(theta) infinie !
319  {
320  P1.x=0; P1.y=-(int)n;
321  Q1.x=0; Q1.y= n;
322  }
323  else
324  {
325  tan_theta = sin_theta/cos_theta; // pente de la droite D(theta)
326  P1.x=-(int)n; P1.y=tan_theta*(-(int)n);
327  Q1.x=n; Q1.y=tan_theta*n;
328  }
329 
330  // CALCULE MASQUE M(theta)
331  // ======================
332  M[i_theta].resize(n,n); // allocation (si necessaire)
333 
334  for(i=0,Y=-moitie+0.5 ; i<n ; i++,Y++)
335  {
336  for(j=0,X=-moitie+0.5 ; j<n ; j++,X++)
337  {
338  // produit vectoriel dir_droite*(X,Y)
339  sgn = vpMath::sign(cos_theta*Y - sin_theta*X);
340 
341  // Resultat = P,Q
342  if( clipping(P1,Q1, X-0.5,Y-0.5,X+0.5,Y+0.5, P,Q) )
343  {
344  // v dans [0,1]
345  v=S_relative(P,Q, X-0.5,Y-0.5,X+0.5,Y+0.5);
346  }
347  else
348  v=1; // PQ ne coupe pas le pixel(i,j)
349 
350  M[i_theta][i][j] = vpMath::round(100*sgn*v);
351 
352  // 2 chiffres significatifs
353  // M(i,j) sans incorporer le coef a
354  }
355  }
356  }
357 
358 }
359 
360 #endif
361 
367 void
369 {
370 
371  if (mask != NULL)
372  delete [] mask;
373 
374  mask = new vpMatrix[n_mask] ;
375 
376  vpColVector angle(n_mask) ;
377 
378  unsigned int angle_pas ;
379  angle_pas = 180 / n_mask ;
380 
381  unsigned int k =0 ;
382  for (unsigned int i = 0 ; /* i < 180, */ k < n_mask ; i += angle_pas)
383  angle[k++] = i ;
384 
385  calcul_masques(angle, mask_size, mask ) ;
386 
387 }
388 
389 
390 
391 void
393 {
394 
395  std::cout<< std::endl ;
396  std::cout<<"Moving edges settings " <<std::endl ;
397  std::cout<< std::endl ;
398  std::cout<<" Size of the convolution masks...."<<mask_size<<"x"<<mask_size<<" pixels"<<std::endl ;
399  std::cout<<" Number of masks.................."<<n_mask<<" "<<std::endl ;
400  std::cout<<" Query range +/- J................"<<range<<" pixels "<<std::endl ;
401  std::cout<<" Likelihood test ratio............"<<threshold<<std::endl ;
402  std::cout<<" Contrast tolerance +/-..........."<< mu1 * 100<<"% and "<<mu2 * 100<<"% "<<std::endl ;
403  std::cout<<" Sample step......................"<<sample_step<<" pixels"<<std::endl ;
404  std::cout<<" Strip............................"<<strip<<" pixels "<<std::endl ;
405  std::cout<<" Min_Samplestep..................."<<min_samplestep<<" pixels "<<std::endl ;
406 
407  #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
408  std::cout<<" Aberration......................."<<aberration<< std::endl ;
409  std::cout<<" init_aberration.................."<<init_aberration<<std::endl ;
410  #endif
411 }
412 
414  : threshold(1500), mu1(0.5), mu2(0.5), min_samplestep(4), anglestep(1), mask_sign(0),
415  range(4), sample_step(10), ntotal_sample(0), points_to_track(500), mask_size(5),
416  n_mask(180), strip(2), mask(NULL)
417 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
418  , aberration(2), init_aberration(5.)
419 #endif
420 {
421  //ntotal_sample = 0; // not sure that it is used
422  //points_to_track = 500; // not sure that it is used
423  anglestep = (180 / n_mask) ;
424 
425  initMask() ;
426 }
427 
428 vpMe::vpMe(const vpMe &me)
429  : threshold(1500), mu1(0.5), mu2(0.5), min_samplestep(4), anglestep(1), mask_sign(0),
430  range(4), sample_step(10), ntotal_sample(0), points_to_track(500), mask_size(5),
431  n_mask(180), strip(2), mask(NULL)
432 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
433  , aberration(2), init_aberration(5.)
434 #endif
435 {
436  *this = me;
437 }
438 
439 const
441 {
442  if (mask != NULL) {
443  delete [] mask;
444  mask = NULL ;
445  }
446  threshold = me.threshold ;
447  mu1 = me.mu1 ;
448  mu2 = me.mu2 ;
450  anglestep = me.anglestep ;
451  mask_size = me.mask_size ;
452  n_mask = me.n_mask ;
453  mask_sign = me.mask_sign ;
454  range = me.range ;
455  sample_step = me.sample_step ;
458  strip = me.strip ;
459 
460  #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
461  aberration = me.aberration ;
463  #endif
464 
465  initMask() ;
466  return *this;
467 }
468 
470 {
471  if (mask != NULL)
472  {
473  delete [] mask ;
474  mask = NULL;
475  }
476 }
477 
478 
479 void
480 vpMe::setMaskNumber(const unsigned int &n)
481 {
482  n_mask = n ;
483  anglestep = 180 / n_mask ;
484  initMask() ;
485 }
486 
487 void
488 vpMe::setMaskSize(const unsigned int &s)
489 {
490  mask_size = s ;
491  initMask() ;
492 }
493 
494 
495 
496 
double threshold
Definition: vpMe.h:77
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:199
double init_aberration
Definition: vpMe.h:314
const vpMe & operator=(const vpMe &me)
Definition: vpMe.cpp:440
vpMatrix * mask
Definition: vpMe.h:97
unsigned int range
Definition: vpMe.h:83
void setMaskNumber(const unsigned int &a)
Definition: vpMe.cpp:480
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:275
int ntotal_sample
Distance between sampled points (in pixels)
Definition: vpMe.h:85
Contains predetermined masks for sites and holds moving edges tracking parameters.
Definition: vpMe.h:70
int mask_sign
Definition: vpMe.h:82
static int round(const double x)
Definition: vpMath.h:228
void initMask()
Definition: vpMe.cpp:368
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
unsigned int mask_size
convolution masks' size in pixels (masks are square),
Definition: vpMe.h:88
void setMaskSize(const unsigned int &a)
Definition: vpMe.cpp:488
int points_to_track
Definition: vpMe.h:86
virtual ~vpMe()
Definition: vpMe.cpp:469
vpMe()
Array of matrices defining the different masks (one for every angle step).
Definition: vpMe.cpp:413
double min_samplestep
Contrast continuity parameter (right boundary)
Definition: vpMe.h:80
unsigned int n_mask
the number of convolution masks available for tracking ; defines resolution.
Definition: vpMe.h:90
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
int strip
Definition: vpMe.h:95
double mu1
Likelihood ratio threshold.
Definition: vpMe.h:78
double mu2
Contrast continuity parameter (left boundary)
Definition: vpMe.h:79
void print()
Definition: vpMe.cpp:392
unsigned int anglestep
Definition: vpMe.h:81
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
static int sign(double x)
Definition: vpMath.h:248
double sample_step
Seek range - on both sides of the reference pixel.
Definition: vpMe.h:84
double aberration
Definition: vpMe.h:313