Visual Servoing Platform  version 3.6.1 under development (2024-12-04)
vpMe.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 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 BEGIN_VISP_NAMESPACE
45 
46 #ifndef DOXYGEN_SHOULD_SKIP_THIS
47 namespace
48 {
49 struct vpPoint2Dt
50 {
51  double x;
52  double y;
53 };
54 
55 struct vpDroite2Dt
56 {
57  double a;
58  double b;
59  double c;
60 };
61 
62 struct vpBBoxt
63 {
64  double Xmin;
65  double Ymin;
66  double Xmax;
67  double Ymax;
68 };
69 
70 template <class Type> inline void permute(Type &a, Type &b)
71 {
72  Type t = a;
73  a = b;
74  b = t;
75 }
76 
77 static vpDroite2Dt droiteCartesienne(vpPoint2Dt P, vpPoint2Dt Q)
78 {
79  vpDroite2Dt PQ;
80 
81  PQ.a = P.y - Q.y;
82  PQ.b = Q.x - P.x;
83  PQ.c = (Q.y * P.x) - (Q.x * P.y);
84 
85  return PQ;
86 }
87 
88 static vpPoint2Dt pointIntersection(vpDroite2Dt D1, vpDroite2Dt D2)
89 {
90  vpPoint2Dt I;
91  double det; // determinant des 2 vect.normaux
92 
93  det = ((D1.a * D2.b) - (D2.a * D1.b)); // interdit D1,D2 paralleles
94  I.x = ((D2.c * D1.b) - (D1.c * D2.b)) / det;
95  I.y = ((D1.c * D2.a) - (D2.c * D1.a)) / det;
96 
97  return I;
98 }
99 
100 static void recale(vpPoint2Dt &P, const vpBBoxt &bbox)
101 {
102  if (vpMath::equal(P.x, bbox.Xmin)) {
103  P.x = bbox.Xmin; // a peu pres => exactement !
104  }
105 
106  if (vpMath::equal(P.x, bbox.Xmax)) {
107  P.x = bbox.Xmax;
108  }
109 
110  if (vpMath::equal(P.y, bbox.Ymin)) {
111  P.y = bbox.Ymin;
112  }
113 
114  if (vpMath::equal(P.y, bbox.Ymax)) {
115  P.y = bbox.Ymax;
116  }
117 }
118 
119 static void permute(vpPoint2Dt &A, vpPoint2Dt &B)
120 {
121  vpPoint2Dt C;
122 
123  if (A.x > B.x) // fonction sans doute a tester...
124  {
125  C = A;
126  A = B;
127  B = C;
128  }
129 }
130 
131 // vrai si partie visible
132 static bool clipping(vpPoint2Dt A, vpPoint2Dt B, const vpBBoxt &bbox, vpPoint2Dt &Ac,
133  vpPoint2Dt &Bc) // resultat: A,B clippes
134 {
135  vpDroite2Dt AB, D[4];
136  const unsigned int index_0 = 0;
137  const unsigned int index_1 = 1;
138  const unsigned int index_2 = 2;
139  const unsigned int index_3 = 3;
140  const unsigned int val_1 = 1;
141  const unsigned int val_2 = 2;
142  const unsigned int val_4 = 4;
143  const unsigned int val_8 = 8;
144 
145  D[index_0].a = 1;
146  D[index_0].b = 0;
147  D[index_0].c = -bbox.Xmin;
148  D[index_1].a = 1;
149  D[index_1].b = 0;
150  D[index_1].c = -bbox.Xmax;
151  D[index_2].a = 0;
152  D[index_2].b = 1;
153  D[index_2].c = -bbox.Ymin;
154  D[index_3].a = 0;
155  D[index_3].b = 1;
156  D[index_3].c = -bbox.Ymax;
157 
158  const unsigned int nbP = val_2;
159  vpPoint2Dt P[nbP];
160  P[index_0] = A;
161  P[index_1] = B;
162  unsigned int code_P[nbP], // codes de P[n]
163  i, bit_i, // i -> (0000100...)
164  n;
165 
166  AB = droiteCartesienne(A, B);
167 
168  for (;;) // 2 sorties directes internes
169  {
170  // CALCULE CODE DE VISIBILITE (Sutherland & Sproul)
171  // ================================================
172  for (n = 0; n < nbP; ++n) {
173  code_P[n] = 0;
174 
175  if (P[n].x < bbox.Xmin) {
176  code_P[n] |= val_1; // positionne bit0
177  }
178 
179  if (P[n].x > bbox.Xmax) {
180  code_P[n] |= val_2; // .. bit1
181  }
182 
183  if (P[n].y < bbox.Ymin) {
184  code_P[n] |= val_4; // .. bit2
185  }
186 
187  if (P[n].y > bbox.Ymax) {
188  code_P[n] |= val_8; // .. bit3
189  }
190  }
191 
192  // 2 CAS OU L'ON PEUT CONCLURE => sortie
193  // =====================================
194  if ((code_P[0] | code_P[1]) == 0) {
195  Ac = P[0];
196  Bc = P[1];
197  if (vpMath::equal(Ac.x, Bc.x) && vpMath::equal(Ac.y, Bc.y)) {
198  return false; // AB = 1 point = invisible
199  }
200  else {
201  return true; // Partie de AB clippee visible!
202  }
203  }
204 
205  if ((code_P[0] & code_P[1]) != 0) // au moins 1 bit commun
206  {
207  return false; // AB completement invisible!
208  }
209 
210  // CAS GENERAL (on sait que code_P[0 ou 1] a au moins un bit a 1
211  // - clippe le point P[n] qui sort de la fenetre (coupe Droite i)
212  // - reboucle avec le nouveau couple de points
213  // ================================================================
214  if (code_P[0] != 0) {
215  n = 0; // c'est P[0] qu'on clippera
216  bit_i = 1;
217  i = 0;
218  while (!(code_P[0] & bit_i)) {
219  bit_i <<= 1;
220  ++i;
221  }
222  }
223  else {
224  n = 1; // c'est P[1] qu'on clippera
225  bit_i = 1;
226  i = 0;
227  while (!(code_P[1] & bit_i)) {
228  bit_i <<= 1;
229  ++i;
230  }
231  }
232 
233  P[n] = pointIntersection(AB, D[i]); // clippe le point concerne
234 
235  // RECALE EXACTEMENT LE POINT (calcul flottant => arrondi)
236  // AFIN QUE LE CALCUL DES CODES NE BOUCLE PAS INDEFINIMENT
237  // =======================================================
238  recale(P[n], bbox);
239  }
240 }
241 
242 // calcule la surface relative des 2 portions definies
243 // par le segment PQ sur le carre Xmin,Ymin,Xmax,Ymax
244 // Rem : P,Q tries sur x, et donc seulement 6 cas
245 static double surfaceRelative(vpPoint2Dt P, vpPoint2Dt Q, const vpBBoxt &bbox)
246 {
247 
248  if (Q.x < P.x) { // tri le couple de points
249  permute(P, Q); // selon leur abscisse x
250  }
251 
252  recale(P, bbox); // permet des calculs de S_relative
253  recale(Q, bbox); // moins approximatifs.
254 
255  // Case P.x=Xmin and Q.x=Xmax
256  if ((std::fabs(P.x - bbox.Xmin) <=
257  (vpMath::maximum(std::fabs(P.x), std::fabs(bbox.Xmin)) * std::numeric_limits<double>::epsilon())) &&
258  (std::fabs(Q.x - bbox.Xmax) <=
259  (vpMath::maximum(std::fabs(Q.x), std::fabs(bbox.Xmax)) * std::numeric_limits<double>::epsilon()))) {
260  return fabs((bbox.Ymax + bbox.Ymin) - (P.y - Q.y));
261  }
262 
263  // Case (P.y=Ymin and Q.y==Ymax) or (Q.y=Ymin and P.y==Ymax)
264  if (((std::fabs(P.y - bbox.Ymin) <=
265  (vpMath::maximum(std::fabs(P.y), std::fabs(bbox.Ymin)) * std::numeric_limits<double>::epsilon())) &&
266  (std::fabs(Q.y - bbox.Ymax) <=
267  (vpMath::maximum(std::fabs(Q.y), std::fabs(bbox.Ymax)) * std::numeric_limits<double>::epsilon()))) ||
268  ((std::fabs(Q.y - bbox.Ymin) <=
269  (vpMath::maximum(std::fabs(Q.y), std::fabs(bbox.Ymin)) * std::numeric_limits<double>::epsilon())) &&
270  (std::fabs(P.y - bbox.Ymax) <=
271  (vpMath::maximum(std::fabs(P.y), std::fabs(bbox.Ymax)) * std::numeric_limits<double>::epsilon())))) {
272  return fabs((bbox.Xmax + bbox.Xmin) - (P.x - Q.x));
273  }
274 
275  // Case P.x=Xmin and Q.y=Ymax
276  if ((std::fabs(P.x - bbox.Xmin) <=
277  (vpMath::maximum(std::fabs(P.x), std::fabs(bbox.Xmin)) * std::numeric_limits<double>::epsilon())) &&
278  (std::fabs(Q.y - bbox.Ymax) <=
279  (vpMath::maximum(std::fabs(Q.y), std::fabs(bbox.Ymax)) * std::numeric_limits<double>::epsilon()))) {
280  return (1 - ((bbox.Ymax - P.y) * (Q.x - bbox.Xmin)));
281  }
282 
283  // Case P.x=Xmin and Q.y=Ymin
284  if ((std::fabs(P.x - bbox.Xmin) <=
285  (vpMath::maximum(std::fabs(P.x), std::fabs(bbox.Xmin)) * std::numeric_limits<double>::epsilon())) &&
286  (std::fabs(Q.y - bbox.Ymin) <=
287  (vpMath::maximum(std::fabs(Q.y), std::fabs(bbox.Ymin)) * std::numeric_limits<double>::epsilon()))) {
288  return (1 - ((P.y - bbox.Ymin) * (Q.x - bbox.Xmin)));
289  }
290 
291  // Case P.y=Ymin and Q.x=Xmax
292  if ((std::fabs(P.y - bbox.Ymin) <=
293  (vpMath::maximum(std::fabs(P.y), std::fabs(bbox.Ymin)) * std::numeric_limits<double>::epsilon())) &&
294  (std::fabs(Q.x - bbox.Xmax) <=
295  (vpMath::maximum(std::fabs(Q.x), std::fabs(bbox.Xmax)) * std::numeric_limits<double>::epsilon()))) {
296  return (1 - ((bbox.Xmax - P.x) * (Q.y - bbox.Ymin)));
297  }
298 
299  // Case P.y=Ymax and Q.x=Xmax
300  if ((std::fabs(P.y - bbox.Ymax) <=
301  (vpMath::maximum(std::fabs(P.y), std::fabs(bbox.Ymax)) * std::numeric_limits<double>::epsilon())) &&
302  (std::fabs(Q.x - bbox.Xmax) <=
303  (vpMath::maximum(std::fabs(Q.x), std::fabs(bbox.Xmax)) * std::numeric_limits<double>::epsilon()))) {
304  return (1 - ((bbox.Xmax - P.x) * (bbox.Ymax - Q.y)));
305  }
306 
307  throw(vpException(vpException::fatalError, "utils_ecm: error in surfaceRelative (%f,%f) (%f,%f) %f %f %f %f",
308  P.x, P.y, Q.x, Q.y, bbox.Xmin, bbox.Ymin, bbox.Xmax, bbox.Ymax));
309 }
310 
311 static void calculMasques(vpColVector &angle, // definitions des angles theta
312  unsigned int n, // taille masques (PAIRE ou IMPAIRE Ok)
313  vpMatrix *M) // resultat M[theta](n,n)
314 {
315  unsigned int i, j;
316  double X, Y, moitie = (static_cast<double>(n)) / 2.0; // moitie REELLE du masque
317  vpPoint2Dt P1, Q1, P, Q; // clippe Droite(theta) P1,Q1 -> P,Q
318  double v; // ponderation de M(i,j)
319 
320  // For a mask of size nxn, normalization given by n*trunc(n/2.0)
321  // Typically, norm = 1/10 for a mask of size 5x5
322  double norm = 1.0 / (n * trunc(n / 2.0));
323 
324  unsigned int nb_theta = angle.getRows();
325 
326  for (unsigned int i_theta = 0; i_theta < nb_theta; ++i_theta) {
327  double theta = (M_PI / 180) * angle[i_theta]; // indice i -> theta(i) en radians
328  // angle[] dans [0,180[
329  double cos_theta = cos(theta); // vecteur directeur de l'ECM
330  double sin_theta = sin(theta); // associe au masque
331 
332  // PRE-CALCULE 2 POINTS DE D(theta) BIEN EN DEHORS DU MASQUE
333  // =========================================================
334  // if( angle[i_theta]==90 ) // => tan(theta) infinie !
335  const double thetaWhoseTanInfinite = 90.;
336  if (std::fabs(angle[i_theta] - thetaWhoseTanInfinite) <= (vpMath::maximum(std::fabs(angle[i_theta]), thetaWhoseTanInfinite) *
337  std::numeric_limits<double>::epsilon())) // => tan(theta) infinie !
338  {
339  P1.x = 0;
340  P1.y = -static_cast<int>(n);
341  Q1.x = 0;
342  Q1.y = n;
343  }
344  else {
345  double tan_theta = sin_theta / cos_theta; // pente de la droite D(theta)
346  P1.x = -static_cast<int>(n);
347  P1.y = tan_theta * (-static_cast<int>(n));
348  Q1.x = n;
349  Q1.y = tan_theta * n;
350  }
351 
352  // CALCULE MASQUE M(theta)
353  // ======================
354  M[i_theta].resize(n, n); // allocation (si necessaire)
355 
356  for (i = 0, Y = (-moitie + 0.5); i < n; ++i, ++Y) {
357  for (j = 0, X = (-moitie + 0.5); j < n; ++j, ++X) {
358  // produit vectoriel dir_droite*(X,Y)
359  int sgn = vpMath::sign((cos_theta * Y) - (sin_theta * X));
360 
361  // Resultat = P,Q
362  vpBBoxt bbox;
363  bbox.Xmin = X - 0.5;
364  bbox.Ymin = Y - 0.5;
365  bbox.Xmax = X + 0.5;
366  bbox.Ymax = Y + 0.5;
367  if (clipping(P1, Q1, bbox, P, Q)) {
368  // v dans [0,1]
369  v = surfaceRelative(P, Q, bbox);
370  }
371  else {
372  v = 1; // PQ ne coupe pas le pixel(i,j)
373  }
374 
375  M[i_theta][i][j] = sgn * v * norm;
376  }
377  }
378  }
379 }
380 }
381 #endif
382 
384 {
385  if (m_mask != nullptr) {
386  delete[] m_mask;
387  }
388 
389  m_mask = new vpMatrix[m_mask_number];
390 
391  vpColVector angle(m_mask_number);
392 
393  unsigned int angle_pas = 180 / m_mask_number;
394 
395  unsigned int i = 0;
396  for (unsigned int k = 0; k < m_mask_number; ++k) {
397  angle[k] = i;
398  i += angle_pas;
399  }
400 
401  calculMasques(angle, m_mask_size, m_mask);
402 }
403 
405 {
406  std::cout << std::endl;
407  std::cout << "Moving edges settings " << std::endl;
408  std::cout << std::endl;
409  std::cout << " Size of the convolution masks...." << m_mask_size << "x" << m_mask_size << " pixels" << std::endl;
410  std::cout << " Number of masks.................." << m_mask_number << std::endl;
411  std::cout << " Query range +/- J................" << m_range << " pixels" << std::endl;
412  std::cout << " Likelihood threshold type........" << (m_likelihood_threshold_type == NORMALIZED_THRESHOLD ? "normalized " : "old threshold (to be avoided)") << std::endl;
413 
414  if (m_useAutomaticThreshold) {
415  std::cout << " Likelihood threshold............." << "unused" << std::endl;
416  std::cout << " Likelihood margin ratio.........." << m_thresholdMarginRatio << std::endl;
417  std::cout << " Minimum likelihood threshold....." << m_minThreshold << std::endl;
418  }
419  else {
420  std::cout << " Likelihood threshold............." << m_threshold << std::endl;
421  std::cout << " Likelihood margin ratio.........." << "unused" << std::endl;
422  std::cout << " Minimum likelihood threshold....." << "unused" << std::endl;
423  }
424 
425  const unsigned int val_100 = 100;
426  std::cout << " Contrast tolerance +/-..........." << (m_mu1 * val_100) << "% and " << (m_mu2 * val_100) << "% " << std::endl;
427  std::cout << " Sample step......................" << m_sample_step << " pixels" << std::endl;
428  std::cout << " Strip............................" << m_strip << " pixels " << std::endl;
429  std::cout << " Min sample step.................." << m_min_samplestep << " pixels " << std::endl;
430 }
431 
433  : m_likelihood_threshold_type(OLD_THRESHOLD), m_thresholdMarginRatio(-1), m_minThreshold(-1), m_useAutomaticThreshold(false),
434  m_mu1(0.5), m_mu2(0.5), m_anglestep(1), m_mask_sign(0), m_ntotal_sample(0), m_mask(nullptr)
435 {
436  const double threshold_default = 1000;
437  const int points_to_track_default = 500;
438  const unsigned int mask_number_default = 180;
439  const unsigned int mask_size_default = 5;
440  const int strip_default = 2;
441  const double min_samplestep_default = 4;
442  const unsigned int flatAngle = 180;
443  const unsigned int range_default = 4;
444  const double sample_step_default = 10;
445 
446  m_threshold = threshold_default;
447  m_points_to_track = points_to_track_default;
448  m_mask_number = mask_number_default;
449  m_mask_size = mask_size_default;
450  m_strip = strip_default;
451  m_min_samplestep = min_samplestep_default;
452  m_range = range_default;
453  m_sample_step = sample_step_default;
454 
455  m_anglestep = (flatAngle / m_mask_number);
456 
457  initMask();
458 }
459 
460 vpMe::vpMe(const vpMe &me)
461  : m_likelihood_threshold_type(OLD_THRESHOLD), m_thresholdMarginRatio(-1), m_minThreshold(-1), m_useAutomaticThreshold(false),
462  m_mu1(0.5), m_mu2(0.5), m_anglestep(1), m_mask_sign(0), m_ntotal_sample(0), m_mask(nullptr)
463 {
464  const double threshold_default = 1000;
465  const int points_to_track_default = 500;
466  const unsigned int mask_number_default = 180;
467  const unsigned int mask_size_default = 5;
468  const int strip_default = 2;
469  const double min_samplestep_default = 4;
470  const unsigned int range_default = 4;
471  const double sample_step_default = 10;
472 
473  m_threshold = threshold_default;
474  m_points_to_track = points_to_track_default;
475  m_mask_number = mask_number_default;
476  m_mask_size = mask_size_default;
477  m_strip = strip_default;
478  m_min_samplestep = min_samplestep_default;
479  m_range = range_default;
480  m_sample_step = sample_step_default;
481 
482  *this = me;
483 }
484 
486 {
487  if (m_mask != nullptr) {
488  delete[] m_mask;
489  m_mask = nullptr;
490  }
491 
492  m_likelihood_threshold_type = me.m_likelihood_threshold_type;
493  m_threshold = me.m_threshold;
494  m_thresholdMarginRatio = me.m_thresholdMarginRatio;
495  m_minThreshold = me.m_minThreshold;
496  m_useAutomaticThreshold = me.m_useAutomaticThreshold;
497  m_mu1 = me.m_mu1;
498  m_mu2 = me.m_mu2;
499  m_min_samplestep = me.m_min_samplestep;
500  m_anglestep = me.m_anglestep;
501  m_mask_size = me.m_mask_size;
502  m_mask_number = me.m_mask_number;
503  m_mask_sign = me.m_mask_sign;
504  m_range = me.m_range;
505  m_sample_step = me.m_sample_step;
506  m_ntotal_sample = me.m_ntotal_sample;
507  m_points_to_track = me.m_points_to_track;
508  m_strip = me.m_strip;
509 
510  initMask();
511  return *this;
512 }
513 
514 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
516 {
517  if (m_mask != nullptr) {
518  delete[] m_mask;
519  m_mask = nullptr;
520  }
521  m_likelihood_threshold_type = std::move(me.m_likelihood_threshold_type);
522  m_threshold = std::move(me.m_threshold);
523  m_thresholdMarginRatio = std::move(me.m_thresholdMarginRatio);
524  m_minThreshold = std::move(me.m_minThreshold);
525  m_useAutomaticThreshold = std::move(me.m_useAutomaticThreshold);
526  m_mu1 = std::move(me.m_mu1);
527  m_mu2 = std::move(me.m_mu2);
528  m_min_samplestep = std::move(me.m_min_samplestep);
529  m_anglestep = std::move(me.m_anglestep);
530  m_mask_size = std::move(me.m_mask_size);
531  m_mask_number = std::move(me.m_mask_number);
532  m_mask_sign = std::move(me.m_mask_sign);
533  m_range = std::move(me.m_range);
534  m_sample_step = std::move(me.m_sample_step);
535  m_ntotal_sample = std::move(me.m_ntotal_sample);
536  m_points_to_track = std::move(me.m_points_to_track);
537  m_strip = std::move(me.m_strip);
538 
539  initMask();
540  return *this;
541 }
542 #endif
543 
545 {
546  if (m_mask != nullptr) {
547  delete[] m_mask;
548  m_mask = nullptr;
549  }
550 }
551 
552 void vpMe::setMaskNumber(const unsigned int &mask_number)
553 {
554  const unsigned int flatAngle = 180;
555  m_mask_number = mask_number;
556  m_anglestep = flatAngle / m_mask_number;
557  initMask();
558 }
559 
560 void vpMe::setMaskSize(const unsigned int &mask_size)
561 {
562  m_mask_size = mask_size;
563  initMask();
564 }
565 
566 END_VISP_NAMESPACE
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
unsigned int getRows() const
Definition: vpArray2D.h:347
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ fatalError
Fatal error.
Definition: vpException.h:72
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:254
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:459
static int sign(double x)
Definition: vpMath.h:429
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
Definition: vpMe.h:134
void initMask()
Definition: vpMe.cpp:383
void print()
Definition: vpMe.cpp:404
void setMaskNumber(const unsigned int &mask_number)
Definition: vpMe.cpp:552
virtual ~vpMe()
Definition: vpMe.cpp:544
vpMe()
Definition: vpMe.cpp:432
void setMaskSize(const unsigned int &mask_size)
Definition: vpMe.cpp:560
vpMe & operator=(const vpMe &me)
Definition: vpMe.cpp:485
@ NORMALIZED_THRESHOLD
Definition: vpMe.h:145