Visual Servoing Platform  version 3.5.1 under development (2023-03-14)
vpMath.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2022 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  * Fabien Spindler
37  * Julien Dufour
38  *
39  *****************************************************************************/
40 
47 #ifndef vpMATH_HH
48 #define vpMATH_HH
49 
50 #include <visp3/core/vpConfig.h>
51 
52 #include <algorithm>
53 #include <climits>
54 #include <limits>
55 #if defined(_WIN32)
56 // Define _USE_MATH_DEFINES before including <math.h> to expose these macro
57 // definitions for common math constants. These are placed under an #ifdef
58 // since these commonly-defined names are not part of the C or C++ standards
59 #define _USE_MATH_DEFINES
60 #endif
61 #include <math.h>
62 #include <vector>
63 
64 #if defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN) || defined(VISP_HAVE_FUNC_ISINF) || \
65  defined(VISP_HAVE_FUNC_STD_ISINF) || defined(VISP_HAVE_FUNC_STD_ROUND)
66 #include <cmath>
67 #endif
68 
69 #if defined(_WIN32) // Not defined in Microsoft math.h
70 
71 #ifndef M_PI
72 #define M_PI 3.14159265358979323846
73 #endif
74 
75 #ifndef M_PI_2
76 #define M_PI_2 (M_PI / 2.0)
77 #endif
78 
79 #ifndef M_PI_4
80 #define M_PI_4 (M_PI / 4.0)
81 #endif
82 
83 #endif
84 
85 #include <visp3/core/vpException.h>
86 #include <visp3/core/vpImagePoint.h>
87 
88 class vpPoint;
90 class vpColVector;
91 class vpRotationVector;
92 class vpRxyzVector;
94 
102 class VISP_EXPORT vpMath
103 {
104 public:
111  static inline double deg(double rad) { return (rad * 180.0) / M_PI; }
112 
113  static vpColVector deg(const vpRotationVector &r);
114  static vpColVector deg(const vpColVector &r);
115 
121  static inline double rad(double deg) { return (deg * M_PI) / 180.0; }
122 
127  static inline double sqr(double x) { return x * x; }
128 
129  // factorial of x
130  static inline double fact(unsigned int x);
131 
132  // combinaison
133  static inline long double comb(unsigned int n, unsigned int p);
134 
142  template <typename T> static inline T clamp(const T &v, const T &lower, const T &upper)
143  {
144 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
145  return std::clamp(v, lower, upper);
146 #else
147  if (upper < lower) {
148  throw vpException(vpException::badValue, "clamp: lower bound is greater than upper bound");
149  }
150  return (v < lower) ? lower : (upper < v) ? upper : v;
151 #endif
152  }
153 
154  // round x to the nearest integer
155  static inline int round(double x);
156 
157  // return the sign of x (+-1)
158  static inline int sign(double x);
159 
160  // test if a number equals 0 (with threshold value)
161  static inline bool nul(double x, double s = 0.001);
162 
163  // test if two numbers are equals (with a user defined threshold)
164  static inline bool equal(double x, double y, double s = 0.001);
165 
166  // test if a number is greater than another (with a user defined threshold)
167  static inline bool greater(double x, double y, double s = 0.001);
168 
175  template <class Type> static Type maximum(const Type &a, const Type &b) { return (a > b) ? a : b; }
176 
183  template <class Type> static Type minimum(const Type &a, const Type &b) { return (a < b) ? a : b; }
184 
190  template <class Type> static Type abs(const Type &x) { return (x < 0) ? -x : x; }
191 
192  // sinus cardinal
193  static double sinc(double x);
194  static double sinc(double sinx, double x);
195  static double mcosc(double cosx, double x);
196  static double msinc(double sinx, double x);
197 
198  // sigmoid
199  static inline double sigmoid(double x, double x0 = 0., double x1 = 1., double n = 12.);
200 
207  template <class Type> static void swap(Type &a, Type &b)
208  {
209  Type tmp = b;
210  b = a;
211  a = tmp;
212  }
213 
214  static bool isNaN(double value);
215  static bool isNaN(float value);
216  static bool isInf(double value);
217  static bool isInf(float value);
218  static bool isFinite(double value);
219  static bool isFinite(float value);
220 
221  static double lineFitting(const std::vector<vpImagePoint> &imPts, double &a, double &b, double &c);
222 
223  template <typename _Tp> static inline _Tp saturate(unsigned char v) { return _Tp(v); }
224  template <typename _Tp> static inline _Tp saturate(char v) { return _Tp(v); }
225  template <typename _Tp> static inline _Tp saturate(unsigned short v) { return _Tp(v); }
226  template <typename _Tp> static inline _Tp saturate(short v) { return _Tp(v); }
227  template <typename _Tp> static inline _Tp saturate(unsigned v) { return _Tp(v); }
228  template <typename _Tp> static inline _Tp saturate(int v) { return _Tp(v); }
229  template <typename _Tp> static inline _Tp saturate(float v) { return _Tp(v); }
230  template <typename _Tp> static inline _Tp saturate(double v) { return _Tp(v); }
231 
232  static double getMean(const std::vector<double> &v);
233  static double getMedian(const std::vector<double> &v);
234  static double getStdev(const std::vector<double> &v, bool useBesselCorrection = false);
235 
236  static int modulo(int a, int n);
237 
238  static vpHomogeneousMatrix ned2ecef(double lonDeg, double latDeg, double radius);
239  static vpHomogeneousMatrix enu2ecef(double lonDeg, double latDeg, double radius);
240  static vpColVector enu2ned(const vpRxyzVector &rxyz);
241  static vpColVector enu2ned(const vpTranslationVector &rxyz);
242 
253  template <typename T> static std::vector<double> linspace(T start_in, T end_in, unsigned int num_in)
254  {
255  std::vector<double> linspaced;
256 
257  double start = static_cast<double>(start_in);
258  double end = static_cast<double>(end_in);
259  double num = static_cast<double>(num_in);
260 
261  if (std::fabs(num) < std::numeric_limits<double>::epsilon()) {
262  return linspaced;
263  }
264  if (std::fabs(num - 1) < std::numeric_limits<double>::epsilon()) {
265  linspaced.push_back(start);
266  return linspaced;
267  }
268 
269  double delta = (end - start) / (num - 1);
270 
271  for (int i = 0; i < num - 1; i++) {
272  linspaced.push_back(start + delta * i);
273  }
274  linspaced.push_back(end); // I want to ensure that start and end
275  // are exactly the same as the input
276  return linspaced;
277  }
278 
279  static std::vector<std::pair<double, double> > computeRegularPointsOnSphere(unsigned int maxPoints);
280  static std::vector<vpHomogeneousMatrix>
281  getLocalTangentPlaneTransformations(const std::vector<std::pair<double, double> > &lonlatVec, double radius,
282  vpHomogeneousMatrix (*toECEF)(double lonDeg, double latDeg, double radius));
283 
284  static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp);
285 
286 private:
287  static const double ang_min_sinc;
288  static const double ang_min_mc;
289 };
290 
291 // Begining of the inline functions definition
292 
297 double vpMath::fact(unsigned int x)
298 {
299  if ((x == 1) || (x == 0))
300  return 1;
301  return x * fact(x - 1);
302 }
303 
312 long double vpMath::comb(unsigned int n, unsigned int p)
313 {
314  if (n == p)
315  return 1;
316  return fact(n) / (fact(n - p) * fact(p));
317 }
318 
326 int vpMath::round(double x)
327 {
328 #if defined(VISP_HAVE_FUNC_ROUND)
329  //:: to design the global namespace and avoid to call recursively
330  // vpMath::round
331  return (int)::round(x);
332 #elif defined(VISP_HAVE_FUNC_STD_ROUND)
333  return (int)std::round(x);
334 #else
335  return (x > 0.0) ? ((int)floor(x + 0.5)) : ((int)ceil(x - 0.5));
336 #endif
337 }
338 
345 int vpMath::sign(double x)
346 {
347  if (fabs(x) < std::numeric_limits<double>::epsilon())
348  return 0;
349  else {
350  if (x < 0)
351  return -1;
352  else
353  return 1;
354  }
355 }
356 
363 bool vpMath::nul(double x, double s) { return (fabs(x) < s); }
364 
372 bool vpMath::equal(double x, double y, double s) { return (nul(x - y, s)); }
373 
381 bool vpMath::greater(double x, double y, double s) { return (x > (y - s)); }
382 
394 double vpMath::sigmoid(double x, double x0, double x1, double n)
395 {
396  if (x < x0)
397  return 0.;
398  else if (x > x1)
399  return 1.;
400  double l0 = 1. / (1. + exp(0.5 * n));
401  double l1 = 1. / (1. + exp(-0.5 * n));
402  return (1. / (1. + exp(-n * ((x - x0) / (x1 - x0) - 0.5))) - l0) / (l1 - l0);
403 }
404 
405 // unsigned char
406 template <> inline unsigned char vpMath::saturate<unsigned char>(char v)
407 {
408  // On big endian arch like powerpc, char implementation is unsigned
409  // with CHAR_MIN=0, CHAR_MAX=255 and SCHAR_MIN=-128, SCHAR_MAX=127
410  // leading to (int)(char -127) = 129.
411  // On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to
412  // (int)(char -127) = -127.
413  if (std::numeric_limits<char>::is_signed)
414  return (unsigned char)(((std::max))((int)v, 0));
415  else
416  return (unsigned char)((unsigned int)v > SCHAR_MAX ? 0 : v);
417 }
418 
419 template <> inline unsigned char vpMath::saturate<unsigned char>(unsigned short v)
420 {
421  return (unsigned char)((std::min))((unsigned int)v, (unsigned int)UCHAR_MAX);
422 }
423 
424 template <> inline unsigned char vpMath::saturate<unsigned char>(int v)
425 {
426  return (unsigned char)((unsigned int)v <= UCHAR_MAX ? v : v > 0 ? UCHAR_MAX : 0);
427 }
428 
429 template <> inline unsigned char vpMath::saturate<unsigned char>(short v) { return saturate<unsigned char>((int)v); }
430 
431 template <> inline unsigned char vpMath::saturate<unsigned char>(unsigned int v)
432 {
433  return (unsigned char)((std::min))(v, (unsigned int)UCHAR_MAX);
434 }
435 
436 template <> inline unsigned char vpMath::saturate<unsigned char>(float v)
437 {
438  int iv = vpMath::round(v);
439  return saturate<unsigned char>(iv);
440 }
441 
442 template <> inline unsigned char vpMath::saturate<unsigned char>(double v)
443 {
444  int iv = vpMath::round(v);
445  return saturate<unsigned char>(iv);
446 }
447 
448 // char
449 template <> inline char vpMath::saturate<char>(unsigned char v) { return (char)((std::min))((int)v, SCHAR_MAX); }
450 
451 template <> inline char vpMath::saturate<char>(unsigned short v)
452 {
453  return (char)((std::min))((unsigned int)v, (unsigned int)SCHAR_MAX);
454 }
455 
456 template <> inline char vpMath::saturate<char>(int v)
457 {
458  return (char)((unsigned int)(v - SCHAR_MIN) <= (unsigned int)UCHAR_MAX ? v : v > 0 ? SCHAR_MAX : SCHAR_MIN);
459 }
460 
461 template <> inline char vpMath::saturate<char>(short v) { return saturate<char>((int)v); }
462 
463 template <> inline char vpMath::saturate<char>(unsigned int v)
464 {
465  return (char)((std::min))(v, (unsigned int)SCHAR_MAX);
466 }
467 
468 template <> inline char vpMath::saturate<char>(float v)
469 {
470  int iv = vpMath::round(v);
471  return saturate<char>(iv);
472 }
473 
474 template <> inline char vpMath::saturate<char>(double v)
475 {
476  int iv = vpMath::round(v);
477  return saturate<char>(iv);
478 }
479 
480 // unsigned short
481 template <> inline unsigned short vpMath::saturate<unsigned short>(char v)
482 {
483  // On big endian arch like powerpc, char implementation is unsigned
484  // with CHAR_MIN=0, CHAR_MAX=255 and SCHAR_MIN=-128, SCHAR_MAX=127
485  // leading to (int)(char -127) = 129.
486  // On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to
487  // (int)(char -127) = -127.
488  if (std::numeric_limits<char>::is_signed)
489  return (unsigned char)(((std::max))((int)v, 0));
490  else
491  return (unsigned char)((unsigned int)v > SCHAR_MAX ? 0 : v);
492 }
493 
494 template <> inline unsigned short vpMath::saturate<unsigned short>(short v)
495 {
496  return (unsigned short)((std::max))((int)v, 0);
497 }
498 
499 template <> inline unsigned short vpMath::saturate<unsigned short>(int v)
500 {
501  return (unsigned short)((unsigned int)v <= (unsigned int)USHRT_MAX ? v : v > 0 ? USHRT_MAX : 0);
502 }
503 
504 template <> inline unsigned short vpMath::saturate<unsigned short>(unsigned int v)
505 {
506  return (unsigned short)((std::min))(v, (unsigned int)USHRT_MAX);
507 }
508 
509 template <> inline unsigned short vpMath::saturate<unsigned short>(float v)
510 {
511  int iv = vpMath::round(v);
512  return vpMath::saturate<unsigned short>(iv);
513 }
514 
515 template <> inline unsigned short vpMath::saturate<unsigned short>(double v)
516 {
517  int iv = vpMath::round(v);
518  return vpMath::saturate<unsigned short>(iv);
519 }
520 
521 // short
522 template <> inline short vpMath::saturate<short>(unsigned short v) { return (short)((std::min))((int)v, SHRT_MAX); }
523 template <> inline short vpMath::saturate<short>(int v)
524 {
525  return (short)((unsigned int)(v - SHRT_MIN) <= (unsigned int)USHRT_MAX ? v : v > 0 ? SHRT_MAX : SHRT_MIN);
526 }
527 template <> inline short vpMath::saturate<short>(unsigned int v)
528 {
529  return (short)((std::min))(v, (unsigned int)SHRT_MAX);
530 }
531 template <> inline short vpMath::saturate<short>(float v)
532 {
533  int iv = vpMath::round(v);
534  return vpMath::saturate<short>(iv);
535 }
536 template <> inline short vpMath::saturate<short>(double v)
537 {
538  int iv = vpMath::round(v);
539  return vpMath::saturate<short>(iv);
540 }
541 
542 // int
543 template <> inline int vpMath::saturate<int>(float v) { return vpMath::round(v); }
544 
545 template <> inline int vpMath::saturate<int>(double v) { return vpMath::round(v); }
546 
547 // unsigned int
548 // (Comment from OpenCV) we intentionally do not clip negative numbers, to
549 // make -1 become 0xffffffff etc.
550 template <> inline unsigned int vpMath::saturate<unsigned int>(float v) { return (unsigned int)vpMath::round(v); }
551 
552 template <> inline unsigned int vpMath::saturate<unsigned int>(double v) { return (unsigned int)vpMath::round(v); }
553 
554 #endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
Implementation of an homogeneous matrix and operations on such kind of matrices.
Provides simple mathematics computation tools that are not available in the C mathematics library (ma...
Definition: vpMath.h:103
static _Tp saturate(char v)
Definition: vpMath.h:224
static void swap(Type &a, Type &b)
Definition: vpMath.h:207
static double fact(unsigned int x)
Definition: vpMath.h:297
static double rad(double deg)
Definition: vpMath.h:121
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:175
static _Tp saturate(short v)
Definition: vpMath.h:226
static _Tp saturate(double v)
Definition: vpMath.h:230
static double sqr(double x)
Definition: vpMath.h:127
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:372
static _Tp saturate(unsigned short v)
Definition: vpMath.h:225
static Type abs(const Type &x)
Definition: vpMath.h:190
static _Tp saturate(float v)
Definition: vpMath.h:229
static double sigmoid(double x, double x0=0., double x1=1., double n=12.)
Definition: vpMath.h:394
static bool nul(double x, double s=0.001)
Definition: vpMath.h:363
static _Tp saturate(unsigned v)
Definition: vpMath.h:227
static T clamp(const T &v, const T &lower, const T &upper)
Definition: vpMath.h:142
static int round(double x)
Definition: vpMath.h:326
static _Tp saturate(int v)
Definition: vpMath.h:228
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:183
static int sign(double x)
Definition: vpMath.h:345
static long double comb(unsigned int n, unsigned int p)
Definition: vpMath.h:312
static double deg(double rad)
Definition: vpMath.h:111
static _Tp saturate(unsigned char v)
Definition: vpMath.h:223
static bool greater(double x, double y, double s=0.001)
Definition: vpMath.h:381
static std::vector< double > linspace(T start_in, T end_in, unsigned int num_in)
Definition: vpMath.h:253
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
Implementation of a generic rotation vector.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:184
Class that consider the case of a translation vector.