Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
45namespace
46{
47struct vpPoint2D_t
48{
49 double x;
50 double y;
51};
52
53struct vpDroite2D_t
54{
55 double a;
56 double b;
57 double c;
58};
59
60template <class Type> inline void permute(Type &a, Type &b)
61{
62 Type t = a;
63 a = b;
64 b = t;
65}
66
67static 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
78static 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
90static 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
103static 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
116static 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
208static 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
264static 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 (mask != NULL)
332 delete[] mask;
333
334 mask = new vpMatrix[n_mask];
335
336 vpColVector angle(n_mask);
337
338 unsigned int angle_pas;
339 angle_pas = 180 / n_mask;
340
341 unsigned int k = 0;
342 for (unsigned int i = 0; k < n_mask; i += angle_pas)
343 angle[k++] = i;
344
345 calcul_masques(angle, mask_size, 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...." << mask_size << "x" << mask_size << " pixels" << std::endl;
354 std::cout << " Number of masks.................." << n_mask << std::endl;
355 std::cout << " Query range +/- J................" << 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............." << threshold << std::endl;
358 std::cout << " Contrast tolerance +/-..........." << mu1 * 100 << "% and " << mu2 * 100 << "% " << std::endl;
359 std::cout << " Sample step......................" << sample_step << " pixels" << std::endl;
360 std::cout << " Strip............................" << strip << " pixels " << std::endl;
361 std::cout << " Min sample step.................." << min_samplestep << " pixels " << std::endl;
362}
363
365 : m_likelihood_threshold_type(OLD_THRESHOLD), threshold(10000),
366 mu1(0.5), mu2(0.5), min_samplestep(4), anglestep(1), mask_sign(0), range(4), sample_step(10),
367 ntotal_sample(0), points_to_track(500), mask_size(5), n_mask(180), strip(2), mask(NULL)
368{
369 // ntotal_sample = 0; // not sure that it is used -> Fabien
370 // points_to_track = 500; // not sure that it is used -> Fabien
371 anglestep = (180 / n_mask);
372
373 initMask();
374}
375
376vpMe::vpMe(const vpMe &me)
377 : m_likelihood_threshold_type(OLD_THRESHOLD), threshold(10000),
378 mu1(0.5), mu2(0.5), min_samplestep(4), anglestep(1), mask_sign(0), range(4), sample_step(10),
379 ntotal_sample(0), points_to_track(500), mask_size(5), n_mask(180), strip(2), mask(NULL)
380{
381 *this = me;
382}
383
385{
386 if (mask != NULL) {
387 delete[] mask;
388 mask = NULL;
389 }
390
391 m_likelihood_threshold_type = me.m_likelihood_threshold_type;
392 threshold = me.threshold;
393 mu1 = me.mu1;
394 mu2 = me.mu2;
396 anglestep = me.anglestep;
397 mask_size = me.mask_size;
398 n_mask = me.n_mask;
399 mask_sign = me.mask_sign;
400 range = me.range;
404 strip = me.strip;
405
406 initMask();
407 return *this;
408}
409
410#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
412{
413 if (mask != NULL) {
414 delete[] mask;
415 mask = NULL;
416 }
417 m_likelihood_threshold_type = std::move(me.m_likelihood_threshold_type);
418 threshold = std::move(me.threshold);
419 mu1 = std::move(me.mu1);
420 mu2 = std::move(me.mu2);
421 min_samplestep = std::move(me.min_samplestep);
422 anglestep = std::move(me.anglestep);
423 mask_size = std::move(me.mask_size);
424 n_mask = std::move(me.n_mask);
425 mask_sign = std::move(me.mask_sign);
426 range = std::move(me.range);
427 sample_step = std::move(me.sample_step);
428 ntotal_sample = std::move(me.ntotal_sample);
429 points_to_track = std::move(me.points_to_track);
430 strip = std::move(me.strip);
431
432 initMask();
433 return *this;
434}
435#endif
436
438{
439 if (mask != NULL) {
440 delete[] mask;
441 mask = NULL;
442 }
443}
444
445void vpMe::setMaskNumber(const unsigned int &n)
446{
447 n_mask = n;
448 anglestep = 180 / n_mask;
449 initMask();
450}
451
452void vpMe::setMaskSize(const unsigned int &s)
453{
454 mask_size = s;
455 initMask();
456}
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of column vector and the associated operations.
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:172
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:369
static int sign(double x)
Definition vpMath.h:342
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
Definition vpMe.h:122
double threshold
Definition vpMe.h:143
int ntotal_sample
Distance between sampled points in pixels.
Definition vpMe.h:151
double sample_step
Seek range - on both sides of the reference pixel.
Definition vpMe.h:150
void print()
Definition vpMe.cpp:348
double min_samplestep
Contrast continuity parameter (right boundary)
Definition vpMe.h:146
void setMaskSize(const unsigned int &a)
Definition vpMe.cpp:452
virtual ~vpMe()
Definition vpMe.cpp:437
unsigned int n_mask
Definition vpMe.h:160
unsigned int anglestep
Definition vpMe.h:147
unsigned int mask_size
Definition vpMe.h:156
int strip
Definition vpMe.h:165
int points_to_track
Definition vpMe.h:152
double mu1
Old likelihood ratio threshold (to be avoided) or easy-to-use normalized threshold: minimal contrast.
Definition vpMe.h:144
vpMe()
Array of matrices defining the different masks (one for every angle step).
Definition vpMe.cpp:364
unsigned int range
Definition vpMe.h:149
void initMask()
Definition vpMe.cpp:329
double mu2
Contrast continuity parameter (left boundary)
Definition vpMe.h:145
vpMe & operator=(const vpMe &me)
Definition vpMe.cpp:384
int mask_sign
Definition vpMe.h:148
@ NORMALIZED_THRESHOLD
Easy-to-use normalized likelihood threshold corresponding to the minimal luminance contrast to consid...
Definition vpMe.h:132
vpMatrix * mask
Definition vpMe.h:166
void setMaskNumber(const unsigned int &a)
Definition vpMe.cpp:445