Visual Servoing Platform version 3.5.0
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
51struct point {
52 double x;
53 double y;
54};
55
56struct droite {
57 double a;
58 double b;
59 double c;
60};
61
62template <class Type> inline void permute(Type &a, Type &b)
63{
64 Type t = a;
65 a = b;
66 b = t;
67}
68
69static 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
80static 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
92static 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
105static 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
118static 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
167else { 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
209static 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
266static 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
385vpMe::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
452void vpMe::setMaskNumber(const unsigned int &n)
453{
454 n_mask = n;
455 anglestep = 180 / n_mask;
456 initMask();
457}
458
459void 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:304
unsigned int getRows() const
Definition: vpArray2D.h:289
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:145
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:295
static int round(double x)
Definition: vpMath.h:247
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