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 * Simple mathematical function not available in the C math library (math.h).
33 *
34 * Authors:
35 * Eric Marchand
36 *
37 *****************************************************************************/
38
39 /*!
40 \file vpMath.h
41 \brief Provides simple Math computation that are not available in
42 the C mathematics library (math.h)
43 */
44
45 #ifndef vpMATH_HH
46 #define vpMATH_HH
47
48 #include <visp3/core/vpConfig.h>
49
50 #include <algorithm>
51 #include <climits>
52 #include <limits>
53 #if defined(_WIN32)
54 // Define _USE_MATH_DEFINES before including <math.h> to expose these macro
55 // definitions for common math constants. These are placed under an #ifdef
56 // since these commonly-defined names are not part of the C or C++ standards
57 # define _USE_MATH_DEFINES
58 #endif
59 #include <math.h>
60 #include <vector>
61
62 #if defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN) || defined(VISP_HAVE_FUNC_ISINF) || \
63 defined(VISP_HAVE_FUNC_STD_ISINF) || defined(VISP_HAVE_FUNC_STD_ROUND)
64 #include <cmath>
65 #endif
66
67 #if defined(_WIN32) // Not defined in Microsoft math.h
68
69 #ifndef M_PI
70 #define M_PI 3.14159265358979323846
71 #endif
72
73 #ifndef M_PI_2
74 #define M_PI_2 (M_PI / 2.0)
75 #endif
76
77 #ifndef M_PI_4
78 #define M_PI_4 (M_PI / 4.0)
79 #endif
80
81 #endif
82
83 #include <visp3/core/vpImagePoint.h>
84
85 /*!
86 \class vpMath
87 \ingroup group_core_math_tools
88 \brief Provides simple mathematics computation tools that are not
89 available in the C mathematics library (math.h)
90
91 \author Eric Marchand (Eric.Marchand@irisa.fr) Irisa / Inria Rennes
92 */
93
94 class VISP_EXPORT vpMath
95 {
96 public:
97 /*!
98 Convert an angle in radians into degrees.
99
100 \param rad : Angle in radians.
101 \return Angle converted in degrees.
102 */
deg(double rad)103 static inline double deg(double rad) { return (rad * 180.0) / M_PI; }
104
105 /*!
106 Convert an angle in degrees into radian.
107 \param deg : Angle in degrees.
108 \return Angle converted in radian.
109 */
rad(double deg)110 static inline double rad(double deg) { return (deg * M_PI) / 180.0; }
111
112 /*!
113 Compute x square value.
114 \return Square value \f$ x^2 \f$.
115 */
sqr(double x)116 static inline double sqr(double x) { return x * x; }
117
118 // factorial of x
119 static inline double fact(unsigned int x);
120
121 // combinaison
122 static inline long double comb(unsigned int n, unsigned int p);
123
124 // round x to the nearest integer
125 static inline int round(double x);
126
127 // return the sign of x (+-1)
128 static inline int(sign)(double x);
129
130 // test if a number equals 0 (with threshold value)
131 static inline bool nul(double x, double s = 0.001);
132
133 // test if two numbers are equals (with a user defined threshold)
134 static inline bool equal(double x, double y, double s = 0.001);
135
136 // test if a number is greater than another (with a user defined threshold)
137 static inline bool greater(double x, double y, double s = 0.001);
138
139 /*!
140 Find the maximum between two numbers (or other).
141 \param a : First number.
142 \param b : Second number.
143 \return The maximum of the two numbers.
144 */
maximum(const Type & a,const Type & b)145 template <class Type> static Type maximum(const Type &a, const Type &b) { return (a > b) ? a : b; }
146
147 /*!
148 Find the minimum between two numbers (or other).
149 \param a : First number.
150 \param b : Second number.
151 \return The minimum of the two numbers.
152 */
minimum(const Type & a,const Type & b)153 template <class Type> static Type minimum(const Type &a, const Type &b) { return (a < b) ? a : b; }
154
155 /*!
156 Find the absolute value of a number (or other).
157 \param x : The number.
158 \return The absolute value of x
159 */
abs(const Type & x)160 template <class Type> static Type abs(const Type &x) { return (x < 0) ? -x : x; }
161
162 // sinus cardinal
163 static double sinc(double x);
164 static double sinc(double sinx, double x);
165 static double mcosc(double cosx, double x);
166 static double msinc(double sinx, double x);
167
168 // sigmoid
169 static inline double sigmoid(double x, double x0 = 0., double x1 = 1., double n = 12.);
170
171 /*!
172 Exchange two numbers.
173
174 \param a First number to exchange.
175 \param b Second number to exchange
176 */
swap(Type & a,Type & b)177 template <class Type> static void swap(Type &a, Type &b)
178 {
179 Type tmp = b;
180 b = a;
181 a = tmp;
182 }
183
184 static bool isNaN(double value);
185 static bool isInf(double value);
186
187 static double lineFitting(const std::vector<vpImagePoint>& imPts, double& a, double& b, double& c);
188
saturate(unsigned char v)189 template <typename _Tp> static inline _Tp saturate(unsigned char v) { return _Tp(v); }
saturate(char v)190 template <typename _Tp> static inline _Tp saturate(char v) { return _Tp(v); }
saturate(unsigned short v)191 template <typename _Tp> static inline _Tp saturate(unsigned short v) { return _Tp(v); }
saturate(short v)192 template <typename _Tp> static inline _Tp saturate(short v) { return _Tp(v); }
saturate(unsigned v)193 template <typename _Tp> static inline _Tp saturate(unsigned v) { return _Tp(v); }
saturate(int v)194 template <typename _Tp> static inline _Tp saturate(int v) { return _Tp(v); }
saturate(float v)195 template <typename _Tp> static inline _Tp saturate(float v) { return _Tp(v); }
saturate(double v)196 template <typename _Tp> static inline _Tp saturate(double v) { return _Tp(v); }
197
198 static double getMean(const std::vector<double> &v);
199 static double getMedian(const std::vector<double> &v);
200 static double getStdev(const std::vector<double> &v, bool useBesselCorrection = false);
201
202 static int modulo(int a, int n);
203
204 private:
205 static const double ang_min_sinc;
206 static const double ang_min_mc;
207 };
208
209 // Begining of the inline functions definition
210
211 /*!
212 Computes and returns x!
213 \param x : parameter of factorial function.
214 */
fact(unsigned int x)215 double vpMath::fact(unsigned int x)
216 {
217 if ((x == 1) || (x == 0))
218 return 1;
219 return x * fact(x - 1);
220 }
221
222 /*!
223 Computes the number of combination of p elements inside n elements.
224
225 \param n : total number of elements.
226 \param p : requested number of elements.
227
228 \return Combination number \f$ n! / ((n-p)! p!) \f$
229 */
comb(unsigned int n,unsigned int p)230 long double vpMath::comb(unsigned int n, unsigned int p)
231 {
232 if (n == p)
233 return 1;
234 return fact(n) / (fact(n - p) * fact(p));
235 }
236
237 /*!
238 Round x to the nearest integer.
239
240 \param x : Value to round.
241
242 \return Nearest integer of x.
243
244 */
round(double x)245 int vpMath::round(double x)
246 {
247 #if defined(VISP_HAVE_FUNC_ROUND)
248 //:: to design the global namespace and avoid to call recursively
249 // vpMath::round
250 return (int)::round(x);
251 #elif defined(VISP_HAVE_FUNC_STD_ROUND)
252 return (int)std::round(x);
253 #else
254 return (x > 0.0) ? ((int)floor(x + 0.5)) : ((int)ceil(x - 0.5));
255 #endif
256 }
257
258 /*!
259 Return the sign of x.
260
261 \param x : Value to test.
262 \return -1 if x is negative, +1 if positive and 0 if zero.
263
264 */
265 int ( vpMath::sign ) (double x)
266 {
267 if (fabs(x) < std::numeric_limits<double>::epsilon())
268 return 0;
269 else {
270 if (x < 0)
271 return -1;
272 else
273 return 1;
274 }
275 }
276
277 /*!
278 Compares \f$ | x | \f$ to \f$ s \f$.
279 \param x : Value to test.
280 \param s : Tolerance threshold
281 \return true if \f$ | x | < s \f$.
282
283 */
nul(double x,double s)284 bool vpMath::nul(double x, double s) { return (fabs(x) < s); }
285
286 /*!
287 Compares \f$ | x - y | \f$ to \f$ s \f$.
288 \param x : x value.
289 \param y : y value.
290 \param s : Tolerance threshold.
291 \return true if \f$ | x - y | < s \f$.
292 */
equal(double x,double y,double s)293 bool vpMath::equal(double x, double y, double s) { return (nul(x - y, s)); }
294
295 /*!
296 Compares \f$ x \f$ to \f$ y - s \f$.
297 \param x : x value.
298 \param y : y value.
299 \param s : Tolerance threshold.
300 \return true if \f$ x > y - s \f$.
301 */
greater(double x,double y,double s)302 bool vpMath::greater(double x, double y, double s) { return (x > (y - s)); }
303
304 /*!
305
306 Sigmoid function between [x0,x1] with \f$ s(x)=0 if x\le x0\f$ and \f$ s(x)=1
307 if x \ge x1 \f$
308 \param x : Value of x.
309 \param x0 : Lower bound (default 0).
310 \param x1 : Upper bound (default 1).
311 \param n : Degree of the exponential (default 12).
312
313 \return Sigmoid value \f$1/(1+exp(-n*((x-x0)/(x1-x0)-0.5)))\f$
314 */
sigmoid(double x,double x0,double x1,double n)315 double vpMath::sigmoid(double x, double x0, double x1, double n)
316 {
317 if (x < x0)
318 return 0.;
319 else if (x > x1)
320 return 1.;
321 double l0 = 1. / (1. + exp(0.5 * n));
322 double l1 = 1. / (1. + exp(-0.5 * n));
323 return (1. / (1. + exp(-n * ((x - x0) / (x1 - x0) - 0.5))) - l0) / (l1 - l0);
324 }
325
326 // unsigned char
327 template <> inline unsigned char vpMath::saturate<unsigned char>(char v)
328 {
329 // On big endian arch like powerpc, char implementation is unsigned
330 // with CHAR_MIN=0, CHAR_MAX=255 and SCHAR_MIN=-128, SCHAR_MAX=127
331 // leading to (int)(char -127) = 129.
332 // On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to
333 // (int)(char -127) = -127.
334 if (std::numeric_limits<char>::is_signed)
335 return (unsigned char)(((std::max))((int)v, 0));
336 else
337 return (unsigned char)((unsigned int)v > SCHAR_MAX ? 0 : v);
338 }
339
340 template <> inline unsigned char vpMath::saturate<unsigned char>(unsigned short v)
341 {
342 return (unsigned char)((std::min))((unsigned int)v, (unsigned int)UCHAR_MAX);
343 }
344
345 template <> inline unsigned char vpMath::saturate<unsigned char>(int v)
346 {
347 return (unsigned char)((unsigned int)v <= UCHAR_MAX ? v : v > 0 ? UCHAR_MAX : 0);
348 }
349
350 template <> inline unsigned char vpMath::saturate<unsigned char>(short v) { return saturate<unsigned char>((int)v); }
351
352 template <> inline unsigned char vpMath::saturate<unsigned char>(unsigned int v)
353 {
354 return (unsigned char)((std::min))(v, (unsigned int)UCHAR_MAX);
355 }
356
357 template <> inline unsigned char vpMath::saturate<unsigned char>(float v)
358 {
359 int iv = vpMath::round(v);
360 return saturate<unsigned char>(iv);
361 }
362
363 template <> inline unsigned char vpMath::saturate<unsigned char>(double v)
364 {
365 int iv = vpMath::round(v);
366 return saturate<unsigned char>(iv);
367 }
368
369 // char
370 template <> inline char vpMath::saturate<char>(unsigned char v) { return (char)((std::min))((int)v, SCHAR_MAX); }
371
372 template <> inline char vpMath::saturate<char>(unsigned short v)
373 {
374 return (char)((std::min))((unsigned int)v, (unsigned int)SCHAR_MAX);
375 }
376
377 template <> inline char vpMath::saturate<char>(int v)
378 {
379 return (char)((unsigned int)(v - SCHAR_MIN) <= (unsigned int)UCHAR_MAX ? v : v > 0 ? SCHAR_MAX : SCHAR_MIN);
380 }
381
382 template <> inline char vpMath::saturate<char>(short v) { return saturate<char>((int)v); }
383
384 template <> inline char vpMath::saturate<char>(unsigned int v)
385 {
386 return (char)((std::min))(v, (unsigned int)SCHAR_MAX);
387 }
388
389 template <> inline char vpMath::saturate<char>(float v)
390 {
391 int iv = vpMath::round(v);
392 return saturate<char>(iv);
393 }
394
395 template <> inline char vpMath::saturate<char>(double v)
396 {
397 int iv = vpMath::round(v);
398 return saturate<char>(iv);
399 }
400
401 // unsigned short
402 template <> inline unsigned short vpMath::saturate<unsigned short>(char v)
403 {
404 // On big endian arch like powerpc, char implementation is unsigned
405 // with CHAR_MIN=0, CHAR_MAX=255 and SCHAR_MIN=-128, SCHAR_MAX=127
406 // leading to (int)(char -127) = 129.
407 // On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to
408 // (int)(char -127) = -127.
409 if (std::numeric_limits<char>::is_signed)
410 return (unsigned char)(((std::max))((int)v, 0));
411 else
412 return (unsigned char)((unsigned int)v > SCHAR_MAX ? 0 : v);
413 }
414
415 template <> inline unsigned short vpMath::saturate<unsigned short>(short v)
416 {
417 return (unsigned short)((std::max))((int)v, 0);
418 }
419
420 template <> inline unsigned short vpMath::saturate<unsigned short>(int v)
421 {
422 return (unsigned short)((unsigned int)v <= (unsigned int)USHRT_MAX ? v : v > 0 ? USHRT_MAX : 0);
423 }
424
425 template <> inline unsigned short vpMath::saturate<unsigned short>(unsigned int v)
426 {
427 return (unsigned short)((std::min))(v, (unsigned int)USHRT_MAX);
428 }
429
430 template <> inline unsigned short vpMath::saturate<unsigned short>(float v)
431 {
432 int iv = vpMath::round(v);
433 return vpMath::saturate<unsigned short>(iv);
434 }
435
436 template <> inline unsigned short vpMath::saturate<unsigned short>(double v)
437 {
438 int iv = vpMath::round(v);
439 return vpMath::saturate<unsigned short>(iv);
440 }
441
442 // short
443 template <> inline short vpMath::saturate<short>(unsigned short v) { return (short)((std::min))((int)v, SHRT_MAX); }
444 template <> inline short vpMath::saturate<short>(int v)
445 {
446 return (short)((unsigned int)(v - SHRT_MIN) <= (unsigned int)USHRT_MAX ? v : v > 0 ? SHRT_MAX : SHRT_MIN);
447 }
448 template <> inline short vpMath::saturate<short>(unsigned int v)
449 {
450 return (short)((std::min))(v, (unsigned int)SHRT_MAX);
451 }
452 template <> inline short vpMath::saturate<short>(float v)
453 {
454 int iv = vpMath::round(v);
455 return vpMath::saturate<short>(iv);
456 }
457 template <> inline short vpMath::saturate<short>(double v)
458 {
459 int iv = vpMath::round(v);
460 return vpMath::saturate<short>(iv);
461 }
462
463 // int
464 template <> inline int vpMath::saturate<int>(float v) { return vpMath::round(v); }
465
466 template <> inline int vpMath::saturate<int>(double v) { return vpMath::round(v); }
467
468 // unsigned int
469 // (Comment from OpenCV) we intentionally do not clip negative numbers, to
470 // make -1 become 0xffffffff etc.
471 template <> inline unsigned int vpMath::saturate<unsigned int>(float v) { return (unsigned int)vpMath::round(v); }
472
473 template <> inline unsigned int vpMath::saturate<unsigned int>(double v) { return (unsigned int)vpMath::round(v); }
474
475 #endif
476