Visual Servoing Platform  version 3.3.1 under development (2020-10-25)
vpMath.h
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 
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 
94 class VISP_EXPORT vpMath
95 {
96 public:
103  static inline double deg(double rad) { return (rad * 180.0) / M_PI; }
104 
110  static inline double rad(double deg) { return (deg * M_PI) / 180.0; }
111 
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 
145  template <class Type> static Type maximum(const Type &a, const Type &b) { return (a > b) ? a : b; }
146 
153  template <class Type> static Type minimum(const Type &a, const Type &b) { return (a < b) ? a : b; }
154 
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 
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 
189  template <typename _Tp> static inline _Tp saturate(unsigned char v) { return _Tp(v); }
190  template <typename _Tp> static inline _Tp saturate(char v) { return _Tp(v); }
191  template <typename _Tp> static inline _Tp saturate(unsigned short v) { return _Tp(v); }
192  template <typename _Tp> static inline _Tp saturate(short v) { return _Tp(v); }
193  template <typename _Tp> static inline _Tp saturate(unsigned v) { return _Tp(v); }
194  template <typename _Tp> static inline _Tp saturate(int v) { return _Tp(v); }
195  template <typename _Tp> static inline _Tp saturate(float v) { return _Tp(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 
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 
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 
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 
264 int
265 #ifndef DOXYGEN_SHOULD_SKIP_THIS
266  (
267 #endif
269 #ifndef DOXYGEN_SHOULD_SKIP_THIS
270  )
271 #endif
272  (double x)
273 {
274  if (fabs(x) < std::numeric_limits<double>::epsilon())
275  return 0;
276  else {
277  if (x < 0)
278  return -1;
279  else
280  return 1;
281  }
282 }
283 
291 bool vpMath::nul(double x, double s) { return (fabs(x) < s); }
292 
300 bool vpMath::equal(double x, double y, double s) { return (nul(x - y, s)); }
301 
309 bool vpMath::greater(double x, double y, double s) { return (x > (y - s)); }
310 
322 double vpMath::sigmoid(double x, double x0, double x1, double n)
323 {
324  if (x < x0)
325  return 0.;
326  else if (x > x1)
327  return 1.;
328  double l0 = 1. / (1. + exp(0.5 * n));
329  double l1 = 1. / (1. + exp(-0.5 * n));
330  return (1. / (1. + exp(-n * ((x - x0) / (x1 - x0) - 0.5))) - l0) / (l1 - l0);
331 }
332 
333 // unsigned char
334 template <> inline unsigned char vpMath::saturate<unsigned char>(char v)
335 {
336  // On big endian arch like powerpc, char implementation is unsigned
337  // with CHAR_MIN=0, CHAR_MAX=255 and SCHAR_MIN=-128, SCHAR_MAX=127
338  // leading to (int)(char -127) = 129.
339  // On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to
340  // (int)(char -127) = -127.
341  if (std::numeric_limits<char>::is_signed)
342  return (unsigned char)(((std::max))((int)v, 0));
343  else
344  return (unsigned char)((unsigned int)v > SCHAR_MAX ? 0 : v);
345 }
346 
347 template <> inline unsigned char vpMath::saturate<unsigned char>(unsigned short v)
348 {
349  return (unsigned char)((std::min))((unsigned int)v, (unsigned int)UCHAR_MAX);
350 }
351 
352 template <> inline unsigned char vpMath::saturate<unsigned char>(int v)
353 {
354  return (unsigned char)((unsigned int)v <= UCHAR_MAX ? v : v > 0 ? UCHAR_MAX : 0);
355 }
356 
357 template <> inline unsigned char vpMath::saturate<unsigned char>(short v) { return saturate<unsigned char>((int)v); }
358 
359 template <> inline unsigned char vpMath::saturate<unsigned char>(unsigned int v)
360 {
361  return (unsigned char)((std::min))(v, (unsigned int)UCHAR_MAX);
362 }
363 
364 template <> inline unsigned char vpMath::saturate<unsigned char>(float v)
365 {
366  int iv = vpMath::round(v);
367  return saturate<unsigned char>(iv);
368 }
369 
370 template <> inline unsigned char vpMath::saturate<unsigned char>(double v)
371 {
372  int iv = vpMath::round(v);
373  return saturate<unsigned char>(iv);
374 }
375 
376 // char
377 template <> inline char vpMath::saturate<char>(unsigned char v) { return (char)((std::min))((int)v, SCHAR_MAX); }
378 
379 template <> inline char vpMath::saturate<char>(unsigned short v)
380 {
381  return (char)((std::min))((unsigned int)v, (unsigned int)SCHAR_MAX);
382 }
383 
384 template <> inline char vpMath::saturate<char>(int v)
385 {
386  return (char)((unsigned int)(v - SCHAR_MIN) <= (unsigned int)UCHAR_MAX ? v : v > 0 ? SCHAR_MAX : SCHAR_MIN);
387 }
388 
389 template <> inline char vpMath::saturate<char>(short v) { return saturate<char>((int)v); }
390 
391 template <> inline char vpMath::saturate<char>(unsigned int v)
392 {
393  return (char)((std::min))(v, (unsigned int)SCHAR_MAX);
394 }
395 
396 template <> inline char vpMath::saturate<char>(float v)
397 {
398  int iv = vpMath::round(v);
399  return saturate<char>(iv);
400 }
401 
402 template <> inline char vpMath::saturate<char>(double v)
403 {
404  int iv = vpMath::round(v);
405  return saturate<char>(iv);
406 }
407 
408 // unsigned short
409 template <> inline unsigned short vpMath::saturate<unsigned short>(char v)
410 {
411  // On big endian arch like powerpc, char implementation is unsigned
412  // with CHAR_MIN=0, CHAR_MAX=255 and SCHAR_MIN=-128, SCHAR_MAX=127
413  // leading to (int)(char -127) = 129.
414  // On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to
415  // (int)(char -127) = -127.
416  if (std::numeric_limits<char>::is_signed)
417  return (unsigned char)(((std::max))((int)v, 0));
418  else
419  return (unsigned char)((unsigned int)v > SCHAR_MAX ? 0 : v);
420 }
421 
422 template <> inline unsigned short vpMath::saturate<unsigned short>(short v)
423 {
424  return (unsigned short)((std::max))((int)v, 0);
425 }
426 
427 template <> inline unsigned short vpMath::saturate<unsigned short>(int v)
428 {
429  return (unsigned short)((unsigned int)v <= (unsigned int)USHRT_MAX ? v : v > 0 ? USHRT_MAX : 0);
430 }
431 
432 template <> inline unsigned short vpMath::saturate<unsigned short>(unsigned int v)
433 {
434  return (unsigned short)((std::min))(v, (unsigned int)USHRT_MAX);
435 }
436 
437 template <> inline unsigned short vpMath::saturate<unsigned short>(float v)
438 {
439  int iv = vpMath::round(v);
440  return vpMath::saturate<unsigned short>(iv);
441 }
442 
443 template <> inline unsigned short vpMath::saturate<unsigned short>(double v)
444 {
445  int iv = vpMath::round(v);
446  return vpMath::saturate<unsigned short>(iv);
447 }
448 
449 // short
450 template <> inline short vpMath::saturate<short>(unsigned short v) { return (short)((std::min))((int)v, SHRT_MAX); }
451 template <> inline short vpMath::saturate<short>(int v)
452 {
453  return (short)((unsigned int)(v - SHRT_MIN) <= (unsigned int)USHRT_MAX ? v : v > 0 ? SHRT_MAX : SHRT_MIN);
454 }
455 template <> inline short vpMath::saturate<short>(unsigned int v)
456 {
457  return (short)((std::min))(v, (unsigned int)SHRT_MAX);
458 }
459 template <> inline short vpMath::saturate<short>(float v)
460 {
461  int iv = vpMath::round(v);
462  return vpMath::saturate<short>(iv);
463 }
464 template <> inline short vpMath::saturate<short>(double v)
465 {
466  int iv = vpMath::round(v);
467  return vpMath::saturate<short>(iv);
468 }
469 
470 // int
471 template <> inline int vpMath::saturate<int>(float v) { return vpMath::round(v); }
472 
473 template <> inline int vpMath::saturate<int>(double v) { return vpMath::round(v); }
474 
475 // unsigned int
476 // (Comment from OpenCV) we intentionally do not clip negative numbers, to
477 // make -1 become 0xffffffff etc.
478 template <> inline unsigned int vpMath::saturate<unsigned int>(float v) { return (unsigned int)vpMath::round(v); }
479 
480 template <> inline unsigned int vpMath::saturate<unsigned int>(double v) { return (unsigned int)vpMath::round(v); }
481 
482 #endif
static _Tp saturate(double v)
Definition: vpMath.h:196
static void swap(Type &a, Type &b)
Definition: vpMath.h:177
static _Tp saturate(char v)
Definition: vpMath.h:190
static double sigmoid(double x, double x0=0., double x1=1., double n=12.)
Definition: vpMath.h:322
static _Tp saturate(unsigned char v)
Definition: vpMath.h:189
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:300
Provides simple mathematics computation tools that are not available in the C mathematics library (ma...
Definition: vpMath.h:94
static _Tp saturate(unsigned short v)
Definition: vpMath.h:191
static double fact(unsigned int x)
Definition: vpMath.h:215
static Type abs(const Type &x)
Definition: vpMath.h:160
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
static _Tp saturate(short v)
Definition: vpMath.h:192
static bool nul(double x, double s=0.001)
Definition: vpMath.h:291
static double sqr(double x)
Definition: vpMath.h:116
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:153
static _Tp saturate(int v)
Definition: vpMath.h:194
static double rad(double deg)
Definition: vpMath.h:110
static _Tp saturate(unsigned v)
Definition: vpMath.h:193
static int round(double x)
Definition: vpMath.h:245
static _Tp saturate(float v)
Definition: vpMath.h:195
static long double comb(unsigned int n, unsigned int p)
Definition: vpMath.h:230
static double deg(double rad)
Definition: vpMath.h:103
static bool greater(double x, double y, double s=0.001)
Definition: vpMath.h:309
static int() sign(double x)
Definition: vpMath.h:272