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