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