Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpMath.h
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  * Simple mathematical function not available in the C math library (math.h).
32  */
33 
40 #ifndef vpMATH_HH
41 #define vpMATH_HH
42 
43 #include <visp3/core/vpConfig.h>
44 
45 #include <algorithm>
46 #include <climits>
47 #include <limits>
48 #if defined(_WIN32)
49 // Define _USE_MATH_DEFINES before including <math.h> to expose these macro
50 // definitions for common math constants. These are placed under an #ifdef
51 // since these commonly-defined names are not part of the C or C++ standards
52 #ifndef _USE_MATH_DEFINES
53 #define _USE_MATH_DEFINES
54 #endif
55 #endif
56 #include <math.h>
57 #include <vector>
58 
59 #if defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN) || defined(VISP_HAVE_FUNC_ISINF) || \
60  defined(VISP_HAVE_FUNC_STD_ISINF) || defined(VISP_HAVE_FUNC_STD_ROUND)
61 #include <cmath>
62 #endif
63 
64 #if defined(_WIN32) // Not defined in Microsoft math.h
65 
66 #ifndef M_PI
67 #define M_PI 3.14159265358979323846
68 #endif
69 
70 #ifndef M_PI_2
71 #define M_PI_2 (M_PI / 2.0)
72 #endif
73 
74 #ifndef M_PI_4
75 #define M_PI_4 (M_PI / 4.0)
76 #endif
77 
78 #endif
79 
80 #ifndef M_PIf
81 #define M_PIf 3.14159265358979323846f
82 #endif
83 
84 #ifndef M_PI_2f
85 #define M_PI_2f (M_PIf / 2.0f)
86 #endif
87 
88 #ifndef M_PI_4f
89 #define M_PI_4f (M_PIf / 4.0f)
90 #endif
91 
92 #include <visp3/core/vpException.h>
93 #include <visp3/core/vpImagePoint.h>
94 
95 class vpPoint;
97 class vpColVector;
98 class vpRotationVector;
99 class vpRxyzVector;
100 class vpTranslationVector;
101 
108 class VISP_EXPORT vpMath
109 {
110 public:
117  static inline double deg(double rad) { return (rad * 180.0) / M_PI; }
118 
119  static vpColVector deg(const vpRotationVector &r);
120  static vpColVector deg(const vpColVector &r);
121 
127  static inline double rad(double deg) { return (deg * M_PI) / 180.0; }
128 
129  static vpColVector rad(const vpColVector &r);
130 
137  static float getAngleBetweenMinPiAndPi(const float &theta)
138  {
139  float theta1 = theta;
140  if (theta1 > M_PIf) {
141  theta1 -= 2.0f * M_PIf;
142  }
143  else if (theta1 <= -M_PIf) {
144  theta1 += 2.0f * M_PIf;
145  }
146  return theta1;
147  }
148 
155  static double getAngleBetweenMinPiAndPi(const double &theta)
156  {
157  double theta1 = theta;
158  if (theta1 > M_PI) {
159  theta1 -= 2.0 * M_PI;
160  }
161  else if (theta1 < -M_PI) {
162  theta1 += 2.0 * M_PI;
163  }
164  return theta1;
165  }
166 
175  static float modulo(const float &value, const float &modulo)
176  {
177  float quotient = std::floor(value / modulo);
178  float rest = value - quotient * modulo;
179  return rest;
180  }
181 
190  static double modulo(const double &value, const double &modulo)
191  {
192  double quotient = std::floor(value / modulo);
193  double rest = value - quotient * modulo;
194  return rest;
195  }
196 
201  static inline double sqr(double x) { return x * x; }
202 
203  // factorial of x
204  static inline double fact(unsigned int x);
205 
206  // combinaison
207  static inline long double comb(unsigned int n, unsigned int p);
208 
216  template <typename T> static inline T clamp(const T &v, const T &lower, const T &upper)
217  {
218  // Check if std:c++17 or higher.
219  // Here we cannot use (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) when ViSP
220  // is used as a 3rdparty. See issue #1274
221 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
222  return std::clamp(v, lower, upper);
223 #else
224  if (upper < lower) {
225  throw vpException(vpException::badValue, "clamp: lower bound is greater than upper bound");
226  }
227  return (v < lower) ? lower : (upper < v) ? upper : v;
228 #endif
229  }
230 
231  // round x to the nearest integer
232  static inline int round(double x);
233 
234  // return the sign of x (+-1)
235  static inline int sign(double x);
236 
237  // test if a number equals 0 (with threshold value)
238  static inline bool nul(double x, double threshold = 0.001);
239 
240  // test if two numbers are equals (with a user defined threshold)
241  static inline bool equal(double x, double y, double threshold = 0.001);
242 
243  // test if a number is greater than another (with a user defined threshold)
244  static inline bool greater(double x, double y, double threshold = 0.001);
245 
252  template <class Type> static Type maximum(const Type &a, const Type &b) { return (a > b) ? a : b; }
253 
260  template <class Type> static Type minimum(const Type &a, const Type &b) { return (a < b) ? a : b; }
261 
267  template <class Type> static Type abs(const Type &x) { return (x < 0) ? -x : x; }
268 
269  // sinus cardinal
270  static double sinc(double x);
271  static double sinc(double sinx, double x);
272  static double mcosc(double cosx, double x);
273  static double msinc(double sinx, double x);
274 
275  // sigmoid
276  static inline double sigmoid(double x, double x0 = 0., double x1 = 1., double n = 12.);
277 
284  template <class Type> static void swap(Type &a, Type &b)
285  {
286  Type tmp = b;
287  b = a;
288  a = tmp;
289  }
290 
291  static bool isNaN(double value);
292  static bool isNaN(float value);
293  static bool isInf(double value);
294  static bool isInf(float value);
295  static bool isFinite(double value);
296  static bool isFinite(float value);
297  static bool isNumber(const std::string &str);
298 
299  static double lineFitting(const std::vector<vpImagePoint> &imPts, double &a, double &b, double &c);
300 
301  template <typename Tp> static inline Tp saturate(unsigned char v) { return Tp(v); }
302  template <typename Tp> static inline Tp saturate(char v) { return Tp(v); }
303  template <typename Tp> static inline Tp saturate(unsigned short v) { return Tp(v); }
304  template <typename Tp> static inline Tp saturate(short v) { return Tp(v); }
305  template <typename Tp> static inline Tp saturate(unsigned v) { return Tp(v); }
306  template <typename Tp> static inline Tp saturate(int v) { return Tp(v); }
307  template <typename Tp> static inline Tp saturate(float v) { return Tp(v); }
308  template <typename Tp> static inline Tp saturate(double v) { return Tp(v); }
309 
310  static double getMean(const std::vector<double> &v);
311  static double getMedian(const std::vector<double> &v);
312  static double getStdev(const std::vector<double> &v, bool useBesselCorrection = false);
313 
314  static int modulo(int a, int n);
315 
316  static vpHomogeneousMatrix ned2ecef(double lonDeg, double latDeg, double radius);
317  static vpHomogeneousMatrix enu2ecef(double lonDeg, double latDeg, double radius);
318  static vpHomogeneousMatrix enu2ned(const vpHomogeneousMatrix &enu_M);
319 
330  template <typename T> static std::vector<double> linspace(T start_in, T end_in, unsigned int num_in)
331  {
332  std::vector<double> linspaced;
333 
334  double start = static_cast<double>(start_in);
335  double end = static_cast<double>(end_in);
336  double num = static_cast<double>(num_in);
337 
338  if (std::fabs(num) < std::numeric_limits<double>::epsilon()) {
339  return linspaced;
340  }
341  if (std::fabs(num - 1) < std::numeric_limits<double>::epsilon()) {
342  linspaced.push_back(start);
343  return linspaced;
344  }
345 
346  double delta = (end - start) / (num - 1);
347 
348  for (int i = 0; i < num - 1; i++) {
349  linspaced.push_back(start + delta * i);
350  }
351  linspaced.push_back(end); // I want to ensure that start and end
352  // are exactly the same as the input
353  return linspaced;
354  }
355 
356  static std::vector<std::pair<double, double> > computeRegularPointsOnSphere(unsigned int maxPoints);
357  static std::vector<vpHomogeneousMatrix>
358  getLocalTangentPlaneTransformations(const std::vector<std::pair<double, double> > &lonlatVec, double radius,
359  vpHomogeneousMatrix(*toECEF)(double lonDeg, double latDeg, double radius));
360 
361  static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp);
362 
363 private:
364  static const double ang_min_sinc;
365  static const double ang_min_mc;
366 };
367 
368 // Begining of the inline functions definition
369 
374 double vpMath::fact(unsigned int x)
375 {
376  if ((x == 1) || (x == 0))
377  return 1;
378  return x * fact(x - 1);
379 }
380 
389 long double vpMath::comb(unsigned int n, unsigned int p)
390 {
391  if (n == p)
392  return 1;
393  return fact(n) / (fact(n - p) * fact(p));
394 }
395 
403 int vpMath::round(double x)
404 {
405 #if defined(VISP_HAVE_FUNC_STD_ROUND)
406  return static_cast<int>(std::round(x));
407 #elif defined(VISP_HAVE_FUNC_ROUND)
408  //:: to design the global namespace and avoid to call recursively
409  // vpMath::round
410  return static_cast<int>(::round(x));
411 #else
412  return (x > 0.0) ? (static_cast<int>(floor(x + 0.5))) : (static_cast<int>(ceil(x - 0.5)));
413 #endif
414 }
415 
422 int vpMath::sign(double x)
423 {
424  if (fabs(x) < std::numeric_limits<double>::epsilon())
425  return 0;
426  else {
427  if (x < 0)
428  return -1;
429  else
430  return 1;
431  }
432 }
433 
440 bool vpMath::nul(double x, double threshold) { return (fabs(x) < threshold); }
441 
449 bool vpMath::equal(double x, double y, double threshold) { return (nul(x - y, threshold)); }
450 
458 bool vpMath::greater(double x, double y, double threshold) { return (x > (y - threshold)); }
459 
471 double vpMath::sigmoid(double x, double x0, double x1, double n)
472 {
473  if (x < x0)
474  return 0.;
475  else if (x > x1)
476  return 1.;
477  double l0 = 1. / (1. + exp(0.5 * n));
478  double l1 = 1. / (1. + exp(-0.5 * n));
479  return (1. / (1. + exp(-n * ((x - x0) / (x1 - x0) - 0.5))) - l0) / (l1 - l0);
480 }
481 
482 // unsigned char
483 template <> inline unsigned char vpMath::saturate<unsigned char>(char v)
484 {
485  // On big endian arch like powerpc, char implementation is unsigned
486  // with CHAR_MIN=0, CHAR_MAX=255 and SCHAR_MIN=-128, SCHAR_MAX=127
487  // leading to (int)(char -127) = 129.
488  // On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to
489  // (int)(char -127) = -127.
490  if (std::numeric_limits<char>::is_signed)
491  return static_cast<unsigned char>(std::max<int>(static_cast<int>(v), 0));
492  else
493  return static_cast<unsigned char>(static_cast<unsigned int>(v) > SCHAR_MAX ? 0 : v);
494 }
495 
496 template <> inline unsigned char vpMath::saturate<unsigned char>(unsigned short v)
497 {
498  return static_cast<unsigned char>(std::min<unsigned int>(static_cast<unsigned int>(v), static_cast<unsigned int>(UCHAR_MAX)));
499 }
500 
501 template <> inline unsigned char vpMath::saturate<unsigned char>(int v)
502 {
503  return static_cast<unsigned char>(static_cast<unsigned int>(v) <= UCHAR_MAX ? v : v > 0 ? UCHAR_MAX : 0);
504 }
505 
506 template <> inline unsigned char vpMath::saturate<unsigned char>(short v)
507 {
508  return saturate<unsigned char>(static_cast<int>(v));
509 }
510 
511 template <> inline unsigned char vpMath::saturate<unsigned char>(unsigned int v)
512 {
513  return static_cast<unsigned char>(std::min<unsigned int>(v, static_cast<unsigned int>(UCHAR_MAX)));
514 }
515 
516 template <> inline unsigned char vpMath::saturate<unsigned char>(float v)
517 {
518  int iv = vpMath::round(static_cast<double>(v));
519  return saturate<unsigned char>(iv);
520 }
521 
522 template <> inline unsigned char vpMath::saturate<unsigned char>(double v)
523 {
524  int iv = vpMath::round(v);
525  return saturate<unsigned char>(iv);
526 }
527 
528 // char
529 template <> inline char vpMath::saturate<char>(unsigned char v)
530 {
531  return static_cast<char>(std::min<int>(static_cast<int>(v), SCHAR_MAX));
532 }
533 
534 template <> inline char vpMath::saturate<char>(unsigned short v)
535 {
536  return static_cast<char>(std::min<unsigned int>(static_cast<unsigned int>(v), static_cast<unsigned int>(SCHAR_MAX)));
537 }
538 
539 template <> inline char vpMath::saturate<char>(int v)
540 {
541  return static_cast<char>(static_cast<unsigned int>(v - SCHAR_MIN) <= static_cast<unsigned int>(UCHAR_MAX) ? v : v > 0 ? SCHAR_MAX : SCHAR_MIN);
542 }
543 
544 template <> inline char vpMath::saturate<char>(short v)
545 {
546  return saturate<char>(static_cast<int>(v));
547 }
548 
549 template <> inline char vpMath::saturate<char>(unsigned int v)
550 {
551  return static_cast<char>(std::min<unsigned int>(v, static_cast<unsigned int>(SCHAR_MAX)));
552 }
553 
554 template <> inline char vpMath::saturate<char>(float v)
555 {
556  int iv = vpMath::round(v);
557  return saturate<char>(iv);
558 }
559 
560 template <> inline char vpMath::saturate<char>(double v)
561 {
562  int iv = vpMath::round(v);
563  return saturate<char>(iv);
564 }
565 
566 // unsigned short
567 template <> inline unsigned short vpMath::saturate<unsigned short>(char v)
568 {
569  // On big endian arch like powerpc, char implementation is unsigned
570  // with CHAR_MIN=0, CHAR_MAX=255 and SCHAR_MIN=-128, SCHAR_MAX=127
571  // leading to (int)(char -127) = 129.
572  // On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to
573  // (int)(char -127) = -127.
574  if (std::numeric_limits<char>::is_signed)
575  return static_cast<unsigned short>(std::max<int>(static_cast<int>(v), 0));
576  else
577  return static_cast<unsigned short>(static_cast<unsigned int>(v) > SCHAR_MAX ? 0 : v);
578 }
579 
580 template <> inline unsigned short vpMath::saturate<unsigned short>(short v)
581 {
582  return static_cast<unsigned short>(std::max<int>(static_cast<int>(v), 0));
583 }
584 
585 template <> inline unsigned short vpMath::saturate<unsigned short>(int v)
586 {
587  return static_cast<unsigned short>(static_cast<unsigned int>(v) <= static_cast<unsigned int>(USHRT_MAX) ? v : v > 0 ? USHRT_MAX : 0);
588 }
589 
590 template <> inline unsigned short vpMath::saturate<unsigned short>(unsigned int v)
591 {
592  return static_cast<unsigned short>(std::min<unsigned int>(v, static_cast<unsigned int>(USHRT_MAX)));
593 }
594 
595 template <> inline unsigned short vpMath::saturate<unsigned short>(float v)
596 {
597  int iv = vpMath::round(static_cast<double>(v));
598  return vpMath::saturate<unsigned short>(iv);
599 }
600 
601 template <> inline unsigned short vpMath::saturate<unsigned short>(double v)
602 {
603  int iv = vpMath::round(v);
604  return vpMath::saturate<unsigned short>(iv);
605 }
606 
607 // short
608 template <> inline short vpMath::saturate<short>(unsigned short v)
609 {
610  return static_cast<short>(std::min<int>(static_cast<int>(v), SHRT_MAX));
611 }
612 template <> inline short vpMath::saturate<short>(int v)
613 {
614  return static_cast<short>(static_cast<unsigned int>(v - SHRT_MIN) <= static_cast<unsigned int>(USHRT_MAX) ? v : v > 0 ? SHRT_MAX : SHRT_MIN);
615 }
616 template <> inline short vpMath::saturate<short>(unsigned int v)
617 {
618  return static_cast<short>(std::min<unsigned int>(v, static_cast<unsigned int>(SHRT_MAX)));
619 }
620 template <> inline short vpMath::saturate<short>(float v)
621 {
622  int iv = vpMath::round(static_cast<double>(v));
623  return vpMath::saturate<short>(iv);
624 }
625 template <> inline short vpMath::saturate<short>(double v)
626 {
627  int iv = vpMath::round(v);
628  return vpMath::saturate<short>(iv);
629 }
630 
631 // int
632 template <> inline int vpMath::saturate<int>(float v)
633 {
634  return vpMath::round(static_cast<double>(v));
635 }
636 
637 template <> inline int vpMath::saturate<int>(double v)
638 {
639  return vpMath::round(v);
640 }
641 
642 // unsigned int
643 // (Comment from OpenCV) we intentionally do not clip negative numbers, to
644 // make -1 become 0xffffffff etc.
645 template <> inline unsigned int vpMath::saturate<unsigned int>(float v)
646 {
647  return static_cast<unsigned int>(vpMath::round(static_cast<double>(v)));
648 }
649 
650 template <> inline unsigned int vpMath::saturate<unsigned int>(double v)
651 {
652  return static_cast<unsigned int>(vpMath::round(v));
653 }
654 
655 #endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:85
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:109
static Tp saturate(double v)
Definition: vpMath.h:308
static Tp saturate(char v)
Definition: vpMath.h:302
static void swap(Type &a, Type &b)
Definition: vpMath.h:284
static Tp saturate(unsigned char v)
Definition: vpMath.h:301
static float getAngleBetweenMinPiAndPi(const float &theta)
Definition: vpMath.h:137
static Tp saturate(unsigned v)
Definition: vpMath.h:305
static double fact(unsigned int x)
Definition: vpMath.h:374
static double rad(double deg)
Definition: vpMath.h:127
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:252
static double sqr(double x)
Definition: vpMath.h:201
static Tp saturate(int v)
Definition: vpMath.h:306
static bool greater(double x, double y, double threshold=0.001)
Definition: vpMath.h:458
static Type abs(const Type &x)
Definition: vpMath.h:267
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:449
static double sigmoid(double x, double x0=0., double x1=1., double n=12.)
Definition: vpMath.h:471
static T clamp(const T &v, const T &lower, const T &upper)
Definition: vpMath.h:216
static bool nul(double x, double threshold=0.001)
Definition: vpMath.h:440
static int round(double x)
Definition: vpMath.h:403
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:260
static Tp saturate(float v)
Definition: vpMath.h:307
static int sign(double x)
Definition: vpMath.h:422
static double modulo(const double &value, const double &modulo)
Gives the rest of value divided by modulo when the quotient can only be an integer.
Definition: vpMath.h:190
static long double comb(unsigned int n, unsigned int p)
Definition: vpMath.h:389
static double deg(double rad)
Definition: vpMath.h:117
static float modulo(const float &value, const float &modulo)
Gives the rest of value divided by modulo when the quotient can only be an integer.
Definition: vpMath.h:175
static Tp saturate(unsigned short v)
Definition: vpMath.h:303
static double getAngleBetweenMinPiAndPi(const double &theta)
Definition: vpMath.h:155
static Tp saturate(short v)
Definition: vpMath.h:304
static std::vector< double > linspace(T start_in, T end_in, unsigned int num_in)
Definition: vpMath.h:330
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
Implementation of a generic rotation vector.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:176
Class that consider the case of a translation vector.