Visual Servoing Platform  version 3.5.1 under development (2022-10-02)
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  } else {
192  n = 1; // c'est P[1] qu'on clippera
193  for (i = 0, bit_i = 1; !(code_P[1] & bit_i); i++, bit_i <<= 1) {
194  }
195  }
196 
197  P[n] = point_intersection(AB, D[i]); // clippe le point concerne
198 
199  // RECALE EXACTEMENT LE POINT (calcul flottant => arrondi)
200  // AFIN QUE LE CALCUL DES CODES NE BOUCLE PAS INDEFINIMENT
201  // =======================================================
202  recale(P[n], Xmin, Ymin, Xmax, Ymax);
203  }
204 }
205 
206 // calcule la surface relative des 2 portions definies
207 // par le segment PQ sur le carre Xmin,Ymin,Xmax,Ymax
208 // Rem : P,Q tries sur x, et donc seulement 6 cas
209 static double S_relative(point P, point Q, double Xmin, double Ymin, double Xmax, double Ymax)
210 {
211 
212  if (Q.x < P.x) // tri le couple de points
213  permute(P, Q); // selon leur abscisse x
214 
215  recale(P, Xmin, Ymin, Xmax, Ymax); // permet des calculs de S_relative
216  recale(Q, Xmin, Ymin, Xmax, Ymax); // moins approximatifs.
217 
218  // if(P.x==Xmin && Q.x==Xmax)
219  if ((std::fabs(P.x - Xmin) <=
220  vpMath::maximum(std::fabs(P.x), std::fabs(Xmin)) * std::numeric_limits<double>::epsilon()) &&
221  (std::fabs(Q.x - Xmax) <=
222  vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * std::numeric_limits<double>::epsilon()))
223  return (fabs(Ymax + Ymin - P.y - Q.y));
224 
225  // if( (P.y==Ymin && Q.y==Ymax) ||
226  // (Q.y==Ymin && P.y==Ymax))
227  if (((std::fabs(P.y - Ymin) <=
228  vpMath::maximum(std::fabs(P.y), std::fabs(Ymin)) * std::numeric_limits<double>::epsilon()) &&
229  (std::fabs(Q.y - Ymax) <=
230  vpMath::maximum(std::fabs(Q.y), std::fabs(Ymax)) * std::numeric_limits<double>::epsilon())) ||
231  ((std::fabs(Q.y - Ymin) <=
232  vpMath::maximum(std::fabs(Q.y), std::fabs(Ymin)) * std::numeric_limits<double>::epsilon()) &&
233  (std::fabs(P.y - Ymax) <=
234  vpMath::maximum(std::fabs(P.y), std::fabs(Ymax)) * std::numeric_limits<double>::epsilon())))
235  return (fabs(Xmax + Xmin - P.x - Q.x));
236 
237  // if( P.x==Xmin && Q.y==Ymax )
238  if (std::fabs(P.x - Xmin) <=
239  vpMath::maximum(std::fabs(P.x), std::fabs(Xmin)) * std::numeric_limits<double>::epsilon() &&
240  std::fabs(Q.y - Ymax) <=
241  vpMath::maximum(std::fabs(Q.y), std::fabs(Ymax)) * std::numeric_limits<double>::epsilon())
242  return (1 - (Ymax - P.y) * (Q.x - Xmin));
243  // if( P.x==Xmin && Q.y==Ymin )
244  if (std::fabs(P.x - Xmin) <=
245  vpMath::maximum(std::fabs(P.x), std::fabs(Xmin)) * std::numeric_limits<double>::epsilon() &&
246  std::fabs(Q.y - Ymin) <=
247  vpMath::maximum(std::fabs(Q.y), std::fabs(Ymin)) * std::numeric_limits<double>::epsilon())
248  return (1 - (P.y - Ymin) * (Q.x - Xmin));
249  // if( P.y==Ymin && Q.x==Xmax )
250  if (std::fabs(P.y - Ymin) <=
251  vpMath::maximum(std::fabs(P.y), std::fabs(Ymin)) * std::numeric_limits<double>::epsilon() &&
252  std::fabs(Q.x - Xmax) <=
253  vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * std::numeric_limits<double>::epsilon())
254  return (1 - (Xmax - P.x) * (Q.y - Ymin));
255  // if( P.y==Ymax && Q.x==Xmax )
256  if (std::fabs(P.y - Ymax) <=
257  vpMath::maximum(std::fabs(P.y), std::fabs(Ymax)) * std::numeric_limits<double>::epsilon() &&
258  std::fabs(Q.x - Xmax) <=
259  vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * std::numeric_limits<double>::epsilon())
260  return (1 - (Xmax - P.x) * (Ymax - Q.y));
261 
262  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);
263  exit(-1); // DEBUG Stoppe net l'execution
264 }
265 
266 static void calcul_masques(vpColVector &angle, // definitions des angles theta
267  unsigned int n, // taille masques (PAIRE ou IMPAIRE Ok)
268  vpMatrix *M) // resultat M[theta](n,n)
269 {
270  // Le coef |a| = |1/2n| n'est pas incorpore dans M(i,j) (=> que des int)
271 
272  unsigned int i_theta, // indice (boucle sur les masques)
273  i, j; // indices de boucle sur M(i,j)
274  double X, Y, // point correspondant/centre du masque
275  moitie = ((double)n) / 2.0; // moitie REELLE du masque
276  point P1, Q1, P, Q; // clippe Droite(theta) P1,Q1 -> P,Q
277  int sgn; // signe de M(i,j)
278  double v; // ponderation de M(i,j)
279 
280  unsigned int nb_theta = angle.getRows();
281 
282  for (i_theta = 0; i_theta < nb_theta; i_theta++) {
283  double theta = M_PI / 180 * angle[i_theta]; // indice i -> theta(i) en radians
284  // angle[] dans [0,180[
285  double cos_theta = cos(theta); // vecteur directeur de l'ECM
286  double sin_theta = sin(theta); // associe au masque
287 
288  // PRE-CALCULE 2 POINTS DE D(theta) BIEN EN DEHORS DU MASQUE
289  // =========================================================
290  // if( angle[i_theta]==90 ) // => tan(theta) infinie !
291  if (std::fabs(angle[i_theta] - 90) <= vpMath::maximum(std::fabs(angle[i_theta]), 90.) *
292  std::numeric_limits<double>::epsilon()) // => tan(theta) infinie !
293  {
294  P1.x = 0;
295  P1.y = -(int)n;
296  Q1.x = 0;
297  Q1.y = n;
298  } else {
299  double tan_theta = sin_theta / cos_theta; // pente de la droite D(theta)
300  P1.x = -(int)n;
301  P1.y = tan_theta * (-(int)n);
302  Q1.x = n;
303  Q1.y = tan_theta * n;
304  }
305 
306  // CALCULE MASQUE M(theta)
307  // ======================
308  M[i_theta].resize(n, n); // allocation (si necessaire)
309 
310  for (i = 0, Y = -moitie + 0.5; i < n; i++, Y++) {
311  for (j = 0, X = -moitie + 0.5; j < n; j++, X++) {
312  // produit vectoriel dir_droite*(X,Y)
313  sgn = vpMath::sign(cos_theta * Y - sin_theta * X);
314 
315  // Resultat = P,Q
316  if (clipping(P1, Q1, X - 0.5, Y - 0.5, X + 0.5, Y + 0.5, P, Q)) {
317  // v dans [0,1]
318  v = S_relative(P, Q, X - 0.5, Y - 0.5, X + 0.5, Y + 0.5);
319  } else
320  v = 1; // PQ ne coupe pas le pixel(i,j)
321 
322  M[i_theta][i][j] = vpMath::round(100 * sgn * v);
323 
324  // 2 chiffres significatifs
325  // M(i,j) sans incorporer le coef a
326  }
327  }
328  }
329 }
330 
331 #endif
332 
339 {
340 
341  if (mask != NULL)
342  delete[] mask;
343 
344  mask = new vpMatrix[n_mask];
345 
346  vpColVector angle(n_mask);
347 
348  unsigned int angle_pas;
349  angle_pas = 180 / n_mask;
350 
351  unsigned int k = 0;
352  for (unsigned int i = 0; /* i < 180, */ k < n_mask; i += angle_pas)
353  angle[k++] = i;
354 
355  calcul_masques(angle, mask_size, mask);
356 }
357 
359 {
360 
361  std::cout << std::endl;
362  std::cout << "Moving edges settings " << std::endl;
363  std::cout << std::endl;
364  std::cout << " Size of the convolution masks...." << mask_size << "x" << mask_size << " pixels" << std::endl;
365  std::cout << " Number of masks.................." << n_mask << " " << std::endl;
366  std::cout << " Query range +/- J................" << range << " pixels " << std::endl;
367  std::cout << " Likelihood test ratio............" << threshold << std::endl;
368  std::cout << " Contrast tolerance +/-..........." << mu1 * 100 << "% and " << mu2 * 100 << "% " << std::endl;
369  std::cout << " Sample step......................" << sample_step << " pixels" << std::endl;
370  std::cout << " Strip............................" << strip << " pixels " << std::endl;
371  std::cout << " Min_Samplestep..................." << min_samplestep << " pixels " << std::endl;
372 }
373 
375  : threshold(1500), mu1(0.5), mu2(0.5), min_samplestep(4), anglestep(1), mask_sign(0), range(4), sample_step(10),
376  ntotal_sample(0), points_to_track(500), mask_size(5), n_mask(180), strip(2), mask(NULL)
377 {
378  // ntotal_sample = 0; // not sure that it is used
379  // points_to_track = 500; // not sure that it is used
380  anglestep = (180 / n_mask);
381 
382  initMask();
383 }
384 
385 vpMe::vpMe(const vpMe &me)
386  : threshold(1500), mu1(0.5), mu2(0.5), min_samplestep(4), anglestep(1), mask_sign(0), range(4), sample_step(10),
387  ntotal_sample(0), points_to_track(500), mask_size(5), n_mask(180), strip(2), mask(NULL)
388 {
389  *this = me;
390 }
391 
394 {
395  if (mask != NULL) {
396  delete[] mask;
397  mask = NULL;
398  }
399  threshold = me.threshold;
400  mu1 = me.mu1;
401  mu2 = me.mu2;
403  anglestep = me.anglestep;
404  mask_size = me.mask_size;
405  n_mask = me.n_mask;
406  mask_sign = me.mask_sign;
407  range = me.range;
411  strip = me.strip;
412 
413  initMask();
414  return *this;
415 }
416 
417 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
420 {
421  if (mask != NULL) {
422  delete[] mask;
423  mask = NULL;
424  }
425  threshold = std::move(me.threshold);
426  mu1 = std::move(me.mu1);
427  mu2 = std::move(me.mu2);
428  min_samplestep = std::move(me.min_samplestep);
429  anglestep = std::move(me.anglestep);
430  mask_size = std::move(me.mask_size);
431  n_mask = std::move(me.n_mask);
432  mask_sign = std::move(me.mask_sign);
433  range = std::move(me.range);
434  sample_step = std::move(me.sample_step);
435  ntotal_sample = std::move(me.ntotal_sample);
436  points_to_track = std::move(me.points_to_track);
437  strip = std::move(me.strip);
438 
439  initMask();
440  return *this;
441 }
442 #endif
443 
445 {
446  if (mask != NULL) {
447  delete[] mask;
448  mask = NULL;
449  }
450 }
451 
452 void vpMe::setMaskNumber(const unsigned int &n)
453 {
454  n_mask = n;
455  anglestep = 180 / n_mask;
456  initMask();
457 }
458 
459 void vpMe::setMaskSize(const unsigned int &s)
460 {
461  mask_size = s;
462  initMask();
463 }
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:306
unsigned int getRows() const
Definition: vpArray2D.h:291
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static int() sign(double x)
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:171
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:364
static int round(double x)
Definition: vpMath.h:318
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Definition: vpMe.h:61
double threshold
Definition: vpMe.h:67
int ntotal_sample
Distance between sampled points (in pixels)
Definition: vpMe.h:75
double sample_step
Seek range - on both sides of the reference pixel.
Definition: vpMe.h:74
void print()
Definition: vpMe.cpp:358
double min_samplestep
Contrast continuity parameter (right boundary)
Definition: vpMe.h:70
void setMaskSize(const unsigned int &a)
Definition: vpMe.cpp:459
virtual ~vpMe()
Definition: vpMe.cpp:444
unsigned int n_mask
Definition: vpMe.h:84
unsigned int anglestep
Definition: vpMe.h:71
unsigned int mask_size
Definition: vpMe.h:80
int strip
Definition: vpMe.h:89
int points_to_track
Definition: vpMe.h:76
double mu1
Likelihood ratio threshold.
Definition: vpMe.h:68
vpMe()
Definition: vpMe.cpp:374
unsigned int range
Definition: vpMe.h:73
void initMask()
Definition: vpMe.cpp:338
double mu2
Contrast continuity parameter (left boundary)
Definition: vpMe.h:69
vpMe & operator=(const vpMe &me)
Copy operator.
Definition: vpMe.cpp:393
int mask_sign
Definition: vpMe.h:72
vpMatrix * mask
Definition: vpMe.h:91
void setMaskNumber(const unsigned int &a)
Definition: vpMe.cpp:452