Visual Servoing Platform  version 3.6.1 under development (2023-12-07)
vpMe.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Moving edges.
32  */
33 
39 #include <stdlib.h>
40 #include <visp3/core/vpColVector.h>
41 #include <visp3/core/vpMath.h>
42 #include <visp3/me/vpMe.h>
43 
44 #ifndef DOXYGEN_SHOULD_SKIP_THIS
45 namespace
46 {
47 struct vpPoint2D_t
48 {
49  double x;
50  double y;
51 };
52 
53 struct vpDroite2D_t
54 {
55  double a;
56  double b;
57  double c;
58 };
59 
60 template <class Type> inline void permute(Type &a, Type &b)
61 {
62  Type t = a;
63  a = b;
64  b = t;
65 }
66 
67 static vpDroite2D_t droite_cartesienne(vpPoint2D_t P, vpPoint2D_t Q)
68 {
69  vpDroite2D_t PQ;
70 
71  PQ.a = P.y - Q.y;
72  PQ.b = Q.x - P.x;
73  PQ.c = Q.y * P.x - Q.x * P.y;
74 
75  return (PQ);
76 }
77 
78 static vpPoint2D_t point_intersection(vpDroite2D_t D1, vpDroite2D_t D2)
79 {
80  vpPoint2D_t I;
81  double det; // determinant des 2 vect.normaux
82 
83  det = (D1.a * D2.b - D2.a * D1.b); // interdit D1,D2 paralleles
84  I.x = (D2.c * D1.b - D1.c * D2.b) / det;
85  I.y = (D1.c * D2.a - D2.c * D1.a) / det;
86 
87  return (I);
88 }
89 
90 static void recale(vpPoint2D_t &P, double Xmin, double Ymin, double Xmax, double Ymax)
91 {
92  if (vpMath::equal(P.x, Xmin))
93  P.x = Xmin; // a peu pres => exactement !
94  if (vpMath::equal(P.x, Xmax))
95  P.x = Xmax;
96 
97  if (vpMath::equal(P.y, Ymin))
98  P.y = Ymin;
99  if (vpMath::equal(P.y, Ymax))
100  P.y = Ymax;
101 }
102 
103 static void permute(vpPoint2D_t &A, vpPoint2D_t &B)
104 {
105  vpPoint2D_t C;
106 
107  if (A.x > B.x) // fonction sans doute a tester...
108  {
109  C = A;
110  A = B;
111  B = C;
112  }
113 }
114 
115 // vrai si partie visible
116 static bool clipping(vpPoint2D_t A, vpPoint2D_t B, double Xmin, double Ymin, double Xmax, double Ymax, vpPoint2D_t &Ac,
117  vpPoint2D_t &Bc) // resultat: A,B clippes
118 {
119  vpDroite2D_t AB, D[4];
120  D[0].a = 1;
121  D[0].b = 0;
122  D[0].c = -Xmin;
123  D[1].a = 1;
124  D[1].b = 0;
125  D[1].c = -Xmax;
126  D[2].a = 0;
127  D[2].b = 1;
128  D[2].c = -Ymin;
129  D[3].a = 0;
130  D[3].b = 1;
131  D[3].c = -Ymax;
132 
133  vpPoint2D_t P[2];
134  P[0] = A;
135  P[1] = B;
136  int code_P[2], // codes de P[n]
137  i, bit_i, // i -> (0000100...)
138  n;
139 
140  AB = droite_cartesienne(A, B);
141 
142  for (;;) // 2 sorties directes internes
143  {
144  // CALCULE CODE DE VISIBILITE (Sutherland & Sproul)
145  // ================================================
146  for (n = 0; n < 2; n++) {
147  code_P[n] = 0000;
148 
149  if (P[n].x < Xmin)
150  code_P[n] |= 1; // positionne bit0
151  if (P[n].x > Xmax)
152  code_P[n] |= 2; // .. bit1
153  if (P[n].y < Ymin)
154  code_P[n] |= 4; // .. bit2
155  if (P[n].y > Ymax)
156  code_P[n] |= 8; // .. bit3
157  }
158 
159  // 2 CAS OU L'ON PEUT CONCLURE => sortie
160  // =====================================
161  if ((code_P[0] | code_P[1]) == 0000) // Aucun bit a 1
162  /* NE TRIE PLUS LE RESULTAT ! S_relative() en tient compte
163  { if(P[0].x < P[1].x) // Rend le couple de points
164  { Ac=P[0]; Bc=P[1]; } // clippes (ordonnes selon
165  else { Ac=P[1]; Bc=P[0]; } // leur abscisse x)
166  */
167  {
168  Ac = P[0];
169  Bc = P[1];
170  if (vpMath::equal(Ac.x, Bc.x) && vpMath::equal(Ac.y, Bc.y))
171  return (false); // AB = 1 point = invisible
172  else
173  return (true); // Partie de AB clippee visible!
174  }
175 
176  if ((code_P[0] & code_P[1]) != 0000) // au moins 1 bit commun
177  {
178  return (false); // AB completement invisible!
179  }
180 
181  // CAS GENERAL (on sait que code_P[0 ou 1] a au moins un bit a 1
182  // - clippe le point P[n] qui sort de la fenetre (coupe Droite i)
183  // - reboucle avec le nouveau couple de points
184  // ================================================================
185  if (code_P[0] != 0000) {
186  n = 0; // c'est P[0] qu'on clippera
187  for (i = 0, bit_i = 1; !(code_P[0] & bit_i); i++, bit_i <<= 1) {
188  }
189  }
190  else {
191  n = 1; // c'est P[1] qu'on clippera
192  for (i = 0, bit_i = 1; !(code_P[1] & bit_i); i++, bit_i <<= 1) {
193  }
194  }
195 
196  P[n] = point_intersection(AB, D[i]); // clippe le point concerne
197 
198  // RECALE EXACTEMENT LE POINT (calcul flottant => arrondi)
199  // AFIN QUE LE CALCUL DES CODES NE BOUCLE PAS INDEFINIMENT
200  // =======================================================
201  recale(P[n], Xmin, Ymin, Xmax, Ymax);
202  }
203 }
204 
205 // calcule la surface relative des 2 portions definies
206 // par le segment PQ sur le carre Xmin,Ymin,Xmax,Ymax
207 // Rem : P,Q tries sur x, et donc seulement 6 cas
208 static double S_relative(vpPoint2D_t P, vpPoint2D_t Q, double Xmin, double Ymin, double Xmax, double Ymax)
209 {
210 
211  if (Q.x < P.x) // tri le couple de points
212  permute(P, Q); // selon leur abscisse x
213 
214  recale(P, Xmin, Ymin, Xmax, Ymax); // permet des calculs de S_relative
215  recale(Q, Xmin, Ymin, Xmax, Ymax); // moins approximatifs.
216 
217  // if(P.x==Xmin && Q.x==Xmax)
218  if ((std::fabs(P.x - Xmin) <=
219  vpMath::maximum(std::fabs(P.x), std::fabs(Xmin)) * std::numeric_limits<double>::epsilon()) &&
220  (std::fabs(Q.x - Xmax) <=
221  vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * std::numeric_limits<double>::epsilon()))
222  return (fabs(Ymax + Ymin - P.y - Q.y));
223 
224  // if( (P.y==Ymin && Q.y==Ymax) ||
225  // (Q.y==Ymin && P.y==Ymax))
226  if (((std::fabs(P.y - Ymin) <=
227  vpMath::maximum(std::fabs(P.y), std::fabs(Ymin)) * std::numeric_limits<double>::epsilon()) &&
228  (std::fabs(Q.y - Ymax) <=
229  vpMath::maximum(std::fabs(Q.y), std::fabs(Ymax)) * std::numeric_limits<double>::epsilon())) ||
230  ((std::fabs(Q.y - Ymin) <=
231  vpMath::maximum(std::fabs(Q.y), std::fabs(Ymin)) * std::numeric_limits<double>::epsilon()) &&
232  (std::fabs(P.y - Ymax) <=
233  vpMath::maximum(std::fabs(P.y), std::fabs(Ymax)) * std::numeric_limits<double>::epsilon())))
234  return (fabs(Xmax + Xmin - P.x - Q.x));
235 
236  // if( P.x==Xmin && Q.y==Ymax )
237  if (std::fabs(P.x - Xmin) <=
238  vpMath::maximum(std::fabs(P.x), std::fabs(Xmin)) * std::numeric_limits<double>::epsilon() &&
239  std::fabs(Q.y - Ymax) <=
240  vpMath::maximum(std::fabs(Q.y), std::fabs(Ymax)) * std::numeric_limits<double>::epsilon())
241  return (1 - (Ymax - P.y) * (Q.x - Xmin));
242  // if( P.x==Xmin && Q.y==Ymin )
243  if (std::fabs(P.x - Xmin) <=
244  vpMath::maximum(std::fabs(P.x), std::fabs(Xmin)) * std::numeric_limits<double>::epsilon() &&
245  std::fabs(Q.y - Ymin) <=
246  vpMath::maximum(std::fabs(Q.y), std::fabs(Ymin)) * std::numeric_limits<double>::epsilon())
247  return (1 - (P.y - Ymin) * (Q.x - Xmin));
248  // if( P.y==Ymin && Q.x==Xmax )
249  if (std::fabs(P.y - Ymin) <=
250  vpMath::maximum(std::fabs(P.y), std::fabs(Ymin)) * std::numeric_limits<double>::epsilon() &&
251  std::fabs(Q.x - Xmax) <=
252  vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * std::numeric_limits<double>::epsilon())
253  return (1 - (Xmax - P.x) * (Q.y - Ymin));
254  // if( P.y==Ymax && Q.x==Xmax )
255  if (std::fabs(P.y - Ymax) <=
256  vpMath::maximum(std::fabs(P.y), std::fabs(Ymax)) * std::numeric_limits<double>::epsilon() &&
257  std::fabs(Q.x - Xmax) <=
258  vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * std::numeric_limits<double>::epsilon())
259  return (1 - (Xmax - P.x) * (Ymax - Q.y));
260 
261  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));
262 }
263 
264 static void calcul_masques(vpColVector &angle, // definitions des angles theta
265  unsigned int n, // taille masques (PAIRE ou IMPAIRE Ok)
266  vpMatrix *M) // resultat M[theta](n,n)
267 {
268  unsigned int i, j;
269  double X, Y, moitie = ((double)n) / 2.0; // moitie REELLE du masque
270  vpPoint2D_t P1, Q1, P, Q; // clippe Droite(theta) P1,Q1 -> P,Q
271  double v; // ponderation de M(i,j)
272 
273  // For a mask of size nxn, normalization given by n*trunc(n/2.0)
274  // Typically, norm = 1/10 for a mask of size 5x5
275  double norm = 1.0 / (n * trunc(n / 2.0));
276 
277  unsigned int nb_theta = angle.getRows();
278 
279  for (unsigned int i_theta = 0; i_theta < nb_theta; i_theta++) {
280  double theta = M_PI / 180 * angle[i_theta]; // indice i -> theta(i) en radians
281  // angle[] dans [0,180[
282  double cos_theta = cos(theta); // vecteur directeur de l'ECM
283  double sin_theta = sin(theta); // associe au masque
284 
285  // PRE-CALCULE 2 POINTS DE D(theta) BIEN EN DEHORS DU MASQUE
286  // =========================================================
287  // if( angle[i_theta]==90 ) // => tan(theta) infinie !
288  if (std::fabs(angle[i_theta] - 90) <= vpMath::maximum(std::fabs(angle[i_theta]), 90.) *
289  std::numeric_limits<double>::epsilon()) // => tan(theta) infinie !
290  {
291  P1.x = 0;
292  P1.y = -(int)n;
293  Q1.x = 0;
294  Q1.y = n;
295  }
296  else {
297  double tan_theta = sin_theta / cos_theta; // pente de la droite D(theta)
298  P1.x = -(int)n;
299  P1.y = tan_theta * (-(int)n);
300  Q1.x = n;
301  Q1.y = tan_theta * n;
302  }
303 
304  // CALCULE MASQUE M(theta)
305  // ======================
306  M[i_theta].resize(n, n); // allocation (si necessaire)
307 
308  for (i = 0, Y = -moitie + 0.5; i < n; i++, Y++) {
309  for (j = 0, X = -moitie + 0.5; j < n; j++, X++) {
310  // produit vectoriel dir_droite*(X,Y)
311  int sgn = vpMath::sign(cos_theta * Y - sin_theta * X);
312 
313  // Resultat = P,Q
314  if (clipping(P1, Q1, X - 0.5, Y - 0.5, X + 0.5, Y + 0.5, P, Q)) {
315  // v dans [0,1]
316  v = S_relative(P, Q, X - 0.5, Y - 0.5, X + 0.5, Y + 0.5);
317  }
318  else
319  v = 1; // PQ ne coupe pas le pixel(i,j)
320 
321  M[i_theta][i][j] = sgn * v * norm;
322  }
323  }
324  }
325 }
326 }
327 #endif
328 
330 {
331  if (m_mask != nullptr)
332  delete[] m_mask;
333 
334  m_mask = new vpMatrix[m_mask_number];
335 
336  vpColVector angle(m_mask_number);
337 
338  unsigned int angle_pas;
339  angle_pas = 180 / m_mask_number;
340 
341  unsigned int k = 0;
342  for (unsigned int i = 0; k < m_mask_number; i += angle_pas)
343  angle[k++] = i;
344 
345  calcul_masques(angle, m_mask_size, m_mask);
346 }
347 
349 {
350  std::cout << std::endl;
351  std::cout << "Moving edges settings " << std::endl;
352  std::cout << std::endl;
353  std::cout << " Size of the convolution masks...." << m_mask_size << "x" << m_mask_size << " pixels" << std::endl;
354  std::cout << " Number of masks.................." << m_mask_number << std::endl;
355  std::cout << " Query range +/- J................" << m_range << " pixels" << std::endl;
356  std::cout << " Likelihood threshold type........" << (m_likelihood_threshold_type == NORMALIZED_THRESHOLD ? "normalized " : "old threshold (to be avoided)") << std::endl;
357  std::cout << " Likelihood threshold............." << m_threshold << std::endl;
358  std::cout << " Contrast tolerance +/-..........." << m_mu1 * 100 << "% and " << m_mu2 * 100 << "% " << std::endl;
359  std::cout << " Sample step......................" << m_sample_step << " pixels" << std::endl;
360  std::cout << " Strip............................" << m_strip << " pixels " << std::endl;
361  std::cout << " Min sample step.................." << m_min_samplestep << " pixels " << std::endl;
362 }
363 
365  : m_likelihood_threshold_type(OLD_THRESHOLD), m_threshold(10000),
366  m_mu1(0.5), m_mu2(0.5), m_min_samplestep(4), m_anglestep(1), m_mask_sign(0), m_range(4), m_sample_step(10),
367  m_ntotal_sample(0), m_points_to_track(500), m_mask_size(5), m_mask_number(180), m_strip(2), m_mask(nullptr)
368 {
369  m_anglestep = (180 / m_mask_number);
370 
371  initMask();
372 }
373 
374 vpMe::vpMe(const vpMe &me)
375  : m_likelihood_threshold_type(OLD_THRESHOLD), m_threshold(10000),
376  m_mu1(0.5), m_mu2(0.5), m_min_samplestep(4), m_anglestep(1), m_mask_sign(0), m_range(4), m_sample_step(10),
377  m_ntotal_sample(0), m_points_to_track(500), m_mask_size(5), m_mask_number(180), m_strip(2), m_mask(nullptr)
378 {
379  *this = me;
380 }
381 
383 {
384  if (m_mask != nullptr) {
385  delete[] m_mask;
386  m_mask = nullptr;
387  }
388 
389  m_likelihood_threshold_type = me.m_likelihood_threshold_type;
390  m_threshold = me.m_threshold;
391  m_mu1 = me.m_mu1;
392  m_mu2 = me.m_mu2;
393  m_min_samplestep = me.m_min_samplestep;
394  m_anglestep = me.m_anglestep;
395  m_mask_size = me.m_mask_size;
396  m_mask_number = me.m_mask_number;
397  m_mask_sign = me.m_mask_sign;
398  m_range = me.m_range;
399  m_sample_step = me.m_sample_step;
400  m_ntotal_sample = me.m_ntotal_sample;
401  m_points_to_track = me.m_points_to_track;
402  m_strip = me.m_strip;
403 
404  initMask();
405  return *this;
406 }
407 
409 {
410  if (m_mask != nullptr) {
411  delete[] m_mask;
412  m_mask = nullptr;
413  }
414  m_likelihood_threshold_type = std::move(me.m_likelihood_threshold_type);
415  m_threshold = std::move(me.m_threshold);
416  m_mu1 = std::move(me.m_mu1);
417  m_mu2 = std::move(me.m_mu2);
418  m_min_samplestep = std::move(me.m_min_samplestep);
419  m_anglestep = std::move(me.m_anglestep);
420  m_mask_size = std::move(me.m_mask_size);
421  m_mask_number = std::move(me.m_mask_number);
422  m_mask_sign = std::move(me.m_mask_sign);
423  m_range = std::move(me.m_range);
424  m_sample_step = std::move(me.m_sample_step);
425  m_ntotal_sample = std::move(me.m_ntotal_sample);
426  m_points_to_track = std::move(me.m_points_to_track);
427  m_strip = std::move(me.m_strip);
428 
429  initMask();
430  return *this;
431 }
432 
434 {
435  if (m_mask != nullptr) {
436  delete[] m_mask;
437  m_mask = nullptr;
438  }
439 }
440 
441 void vpMe::setMaskNumber(const unsigned int &mask_number)
442 {
443  m_mask_number = mask_number;
444  m_anglestep = 180 / m_mask_number;
445  initMask();
446 }
447 
448 void vpMe::setMaskSize(const unsigned int &mask_size)
449 {
450  m_mask_size = mask_size;
451  initMask();
452 }
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:282
unsigned int getRows() const
Definition: vpArray2D.h:267
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ fatalError
Fatal error.
Definition: vpException.h:84
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:252
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:449
static int sign(double x)
Definition: vpMath.h:422
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
Definition: vpMe.h:120
void print()
Definition: vpMe.cpp:348
void setMaskNumber(const unsigned int &mask_number)
Definition: vpMe.cpp:441
virtual ~vpMe()
Definition: vpMe.cpp:433
vpMe()
Definition: vpMe.cpp:364
void setMaskSize(const unsigned int &mask_size)
Definition: vpMe.cpp:448
void initMask()
Definition: vpMe.cpp:329
vpMe & operator=(const vpMe &me)
Definition: vpMe.cpp:382
@ NORMALIZED_THRESHOLD
Definition: vpMe.h:131