Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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#include <visp3/core/vpException.h>
81#include <visp3/core/vpImagePoint.h>
82
83class vpPoint;
85class vpColVector;
87class vpRxyzVector;
89
97class VISP_EXPORT vpMath
98{
99public:
106 static inline double deg(double rad) { return (rad * 180.0) / M_PI; }
107
108 static vpColVector deg(const vpRotationVector &r);
109 static vpColVector deg(const vpColVector &r);
110
116 static inline double rad(double deg) { return (deg * M_PI) / 180.0; }
117
118 static vpColVector rad(const vpColVector &r);
119
124 static inline double sqr(double x) { return x * x; }
125
126 // factorial of x
127 static inline double fact(unsigned int x);
128
129 // combinaison
130 static inline long double comb(unsigned int n, unsigned int p);
131
139 template <typename T> static inline T clamp(const T &v, const T &lower, const T &upper)
140 {
141#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
142 return std::clamp(v, lower, upper);
143#else
144 if (upper < lower) {
145 throw vpException(vpException::badValue, "clamp: lower bound is greater than upper bound");
146 }
147 return (v < lower) ? lower : (upper < v) ? upper : v;
148#endif
149 }
150
151 // round x to the nearest integer
152 static inline int round(double x);
153
154 // return the sign of x (+-1)
155 static inline int sign(double x);
156
157 // test if a number equals 0 (with threshold value)
158 static inline bool nul(double x, double threshold = 0.001);
159
160 // test if two numbers are equals (with a user defined threshold)
161 static inline bool equal(double x, double y, double threshold = 0.001);
162
163 // test if a number is greater than another (with a user defined threshold)
164 static inline bool greater(double x, double y, double threshold = 0.001);
165
172 template <class Type> static Type maximum(const Type &a, const Type &b) { return (a > b) ? a : b; }
173
180 template <class Type> static Type minimum(const Type &a, const Type &b) { return (a < b) ? a : b; }
181
187 template <class Type> static Type abs(const Type &x) { return (x < 0) ? -x : x; }
188
189 // sinus cardinal
190 static double sinc(double x);
191 static double sinc(double sinx, double x);
192 static double mcosc(double cosx, double x);
193 static double msinc(double sinx, double x);
194
195 // sigmoid
196 static inline double sigmoid(double x, double x0 = 0., double x1 = 1., double n = 12.);
197
204 template <class Type> static void swap(Type &a, Type &b)
205 {
206 Type tmp = b;
207 b = a;
208 a = tmp;
209 }
210
211 static bool isNaN(double value);
212 static bool isNaN(float value);
213 static bool isInf(double value);
214 static bool isInf(float value);
215 static bool isFinite(double value);
216 static bool isFinite(float value);
217 static bool isNumber(const std::string &str);
218
219 static double lineFitting(const std::vector<vpImagePoint> &imPts, double &a, double &b, double &c);
220
221 template <typename _Tp> static inline _Tp saturate(unsigned char v) { return _Tp(v); }
222 template <typename _Tp> static inline _Tp saturate(char v) { return _Tp(v); }
223 template <typename _Tp> static inline _Tp saturate(unsigned short v) { return _Tp(v); }
224 template <typename _Tp> static inline _Tp saturate(short v) { return _Tp(v); }
225 template <typename _Tp> static inline _Tp saturate(unsigned v) { return _Tp(v); }
226 template <typename _Tp> static inline _Tp saturate(int v) { return _Tp(v); }
227 template <typename _Tp> static inline _Tp saturate(float v) { return _Tp(v); }
228 template <typename _Tp> static inline _Tp saturate(double v) { return _Tp(v); }
229
230 static double getMean(const std::vector<double> &v);
231 static double getMedian(const std::vector<double> &v);
232 static double getStdev(const std::vector<double> &v, bool useBesselCorrection = false);
233
234 static int modulo(int a, int n);
235
236 static vpHomogeneousMatrix ned2ecef(double lonDeg, double latDeg, double radius);
237 static vpHomogeneousMatrix enu2ecef(double lonDeg, double latDeg, double radius);
238 static vpHomogeneousMatrix enu2ned(const vpHomogeneousMatrix &enu_M);
239
250 template <typename T> static std::vector<double> linspace(T start_in, T end_in, unsigned int num_in)
251 {
252 std::vector<double> linspaced;
253
254 double start = static_cast<double>(start_in);
255 double end = static_cast<double>(end_in);
256 double num = static_cast<double>(num_in);
257
258 if (std::fabs(num) < std::numeric_limits<double>::epsilon()) {
259 return linspaced;
260 }
261 if (std::fabs(num - 1) < std::numeric_limits<double>::epsilon()) {
262 linspaced.push_back(start);
263 return linspaced;
264 }
265
266 double delta = (end - start) / (num - 1);
267
268 for (int i = 0; i < num - 1; i++) {
269 linspaced.push_back(start + delta * i);
270 }
271 linspaced.push_back(end); // I want to ensure that start and end
272 // are exactly the same as the input
273 return linspaced;
274 }
275
276 static std::vector<std::pair<double, double> > computeRegularPointsOnSphere(unsigned int maxPoints);
277 static std::vector<vpHomogeneousMatrix>
278 getLocalTangentPlaneTransformations(const std::vector<std::pair<double, double> > &lonlatVec, double radius,
279 vpHomogeneousMatrix(*toECEF)(double lonDeg, double latDeg, double radius));
280
281 static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp);
282
283private:
284 static const double ang_min_sinc;
285 static const double ang_min_mc;
286};
287
288// Begining of the inline functions definition
289
294double vpMath::fact(unsigned int x)
295{
296 if ((x == 1) || (x == 0))
297 return 1;
298 return x * fact(x - 1);
299}
300
309long double vpMath::comb(unsigned int n, unsigned int p)
310{
311 if (n == p)
312 return 1;
313 return fact(n) / (fact(n - p) * fact(p));
314}
315
323int vpMath::round(double x)
324{
325#if defined(VISP_HAVE_FUNC_STD_ROUND)
326 return (int)std::round(x);
327#elif defined(VISP_HAVE_FUNC_ROUND)
328 //:: to design the global namespace and avoid to call recursively
329 // vpMath::round
330 return (int)::round(x);
331#else
332 return (x > 0.0) ? ((int)floor(x + 0.5)) : ((int)ceil(x - 0.5));
333#endif
334}
335
342int vpMath::sign(double x)
343{
344 if (fabs(x) < std::numeric_limits<double>::epsilon())
345 return 0;
346 else {
347 if (x < 0)
348 return -1;
349 else
350 return 1;
351 }
352}
353
360bool vpMath::nul(double x, double threshold) { return (fabs(x) < threshold); }
361
369bool vpMath::equal(double x, double y, double threshold) { return (nul(x - y, threshold)); }
370
378bool vpMath::greater(double x, double y, double threshold) { return (x > (y - threshold)); }
379
391double vpMath::sigmoid(double x, double x0, double x1, double n)
392{
393 if (x < x0)
394 return 0.;
395 else if (x > x1)
396 return 1.;
397 double l0 = 1. / (1. + exp(0.5 * n));
398 double l1 = 1. / (1. + exp(-0.5 * n));
399 return (1. / (1. + exp(-n * ((x - x0) / (x1 - x0) - 0.5))) - l0) / (l1 - l0);
400}
401
402// unsigned char
403template <> inline unsigned char vpMath::saturate<unsigned char>(char v)
404{
405 // On big endian arch like powerpc, char implementation is unsigned
406 // with CHAR_MIN=0, CHAR_MAX=255 and SCHAR_MIN=-128, SCHAR_MAX=127
407 // leading to (int)(char -127) = 129.
408 // On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to
409 // (int)(char -127) = -127.
410 if (std::numeric_limits<char>::is_signed)
411 return (unsigned char)(((std::max))((int)v, 0));
412 else
413 return (unsigned char)((unsigned int)v > SCHAR_MAX ? 0 : v);
414}
415
416template <> inline unsigned char vpMath::saturate<unsigned char>(unsigned short v)
417{
418 return (unsigned char)((std::min))((unsigned int)v, (unsigned int)UCHAR_MAX);
419}
420
421template <> inline unsigned char vpMath::saturate<unsigned char>(int v)
422{
423 return (unsigned char)((unsigned int)v <= UCHAR_MAX ? v : v > 0 ? UCHAR_MAX : 0);
424}
425
426template <> inline unsigned char vpMath::saturate<unsigned char>(short v) { return saturate<unsigned char>((int)v); }
427
428template <> inline unsigned char vpMath::saturate<unsigned char>(unsigned int v)
429{
430 return (unsigned char)((std::min))(v, (unsigned int)UCHAR_MAX);
431}
432
433template <> inline unsigned char vpMath::saturate<unsigned char>(float v)
434{
435 int iv = vpMath::round(v);
436 return saturate<unsigned char>(iv);
437}
438
439template <> inline unsigned char vpMath::saturate<unsigned char>(double v)
440{
441 int iv = vpMath::round(v);
442 return saturate<unsigned char>(iv);
443}
444
445// char
446template <> inline char vpMath::saturate<char>(unsigned char v) { return (char)((std::min))((int)v, SCHAR_MAX); }
447
448template <> inline char vpMath::saturate<char>(unsigned short v)
449{
450 return (char)((std::min))((unsigned int)v, (unsigned int)SCHAR_MAX);
451}
452
453template <> inline char vpMath::saturate<char>(int v)
454{
455 return (char)((unsigned int)(v - SCHAR_MIN) <= (unsigned int)UCHAR_MAX ? v : v > 0 ? SCHAR_MAX : SCHAR_MIN);
456}
457
458template <> inline char vpMath::saturate<char>(short v) { return saturate<char>((int)v); }
459
460template <> inline char vpMath::saturate<char>(unsigned int v)
461{
462 return (char)((std::min))(v, (unsigned int)SCHAR_MAX);
463}
464
465template <> inline char vpMath::saturate<char>(float v)
466{
467 int iv = vpMath::round(v);
468 return saturate<char>(iv);
469}
470
471template <> inline char vpMath::saturate<char>(double v)
472{
473 int iv = vpMath::round(v);
474 return saturate<char>(iv);
475}
476
477// unsigned short
478template <> inline unsigned short vpMath::saturate<unsigned short>(char v)
479{
480 // On big endian arch like powerpc, char implementation is unsigned
481 // with CHAR_MIN=0, CHAR_MAX=255 and SCHAR_MIN=-128, SCHAR_MAX=127
482 // leading to (int)(char -127) = 129.
483 // On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to
484 // (int)(char -127) = -127.
485 if (std::numeric_limits<char>::is_signed)
486 return (unsigned char)(((std::max))((int)v, 0));
487 else
488 return (unsigned char)((unsigned int)v > SCHAR_MAX ? 0 : v);
489}
490
491template <> inline unsigned short vpMath::saturate<unsigned short>(short v)
492{
493 return (unsigned short)((std::max))((int)v, 0);
494}
495
496template <> inline unsigned short vpMath::saturate<unsigned short>(int v)
497{
498 return (unsigned short)((unsigned int)v <= (unsigned int)USHRT_MAX ? v : v > 0 ? USHRT_MAX : 0);
499}
500
501template <> inline unsigned short vpMath::saturate<unsigned short>(unsigned int v)
502{
503 return (unsigned short)((std::min))(v, (unsigned int)USHRT_MAX);
504}
505
506template <> inline unsigned short vpMath::saturate<unsigned short>(float v)
507{
508 int iv = vpMath::round(v);
510}
511
512template <> inline unsigned short vpMath::saturate<unsigned short>(double v)
513{
514 int iv = vpMath::round(v);
516}
517
518// short
519template <> inline short vpMath::saturate<short>(unsigned short v) { return (short)((std::min))((int)v, SHRT_MAX); }
520template <> inline short vpMath::saturate<short>(int v)
521{
522 return (short)((unsigned int)(v - SHRT_MIN) <= (unsigned int)USHRT_MAX ? v : v > 0 ? SHRT_MAX : SHRT_MIN);
523}
524template <> inline short vpMath::saturate<short>(unsigned int v)
525{
526 return (short)((std::min))(v, (unsigned int)SHRT_MAX);
527}
528template <> inline short vpMath::saturate<short>(float v)
529{
530 int iv = vpMath::round(v);
531 return vpMath::saturate<short>(iv);
532}
533template <> inline short vpMath::saturate<short>(double v)
534{
535 int iv = vpMath::round(v);
536 return vpMath::saturate<short>(iv);
537}
538
539// int
540template <> inline int vpMath::saturate<int>(float v) { return vpMath::round(v); }
541
542template <> inline int vpMath::saturate<int>(double v) { return vpMath::round(v); }
543
544// unsigned int
545// (Comment from OpenCV) we intentionally do not clip negative numbers, to
546// make -1 become 0xffffffff etc.
547template <> inline unsigned int vpMath::saturate<unsigned int>(float v) { return (unsigned int)vpMath::round(v); }
548
549template <> inline unsigned int vpMath::saturate<unsigned int>(double v) { return (unsigned int)vpMath::round(v); }
550
551#endif
Implementation of column vector and the associated operations.
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:98
static _Tp saturate(char v)
Definition vpMath.h:222
static void swap(Type &a, Type &b)
Definition vpMath.h:204
static double fact(unsigned int x)
Definition vpMath.h:294
static double rad(double deg)
Definition vpMath.h:116
static Type maximum(const Type &a, const Type &b)
Definition vpMath.h:172
static std::vector< double > linspace(T start_in, T end_in, unsigned int num_in)
Definition vpMath.h:250
static _Tp saturate(short v)
Definition vpMath.h:224
static _Tp saturate(double v)
Definition vpMath.h:228
static double sqr(double x)
Definition vpMath.h:124
static _Tp saturate(unsigned short v)
Definition vpMath.h:223
static bool greater(double x, double y, double threshold=0.001)
Definition vpMath.h:378
static Type abs(const Type &x)
Definition vpMath.h:187
static _Tp saturate(float v)
Definition vpMath.h:227
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:369
static double sigmoid(double x, double x0=0., double x1=1., double n=12.)
Definition vpMath.h:391
static _Tp saturate(unsigned v)
Definition vpMath.h:225
static T clamp(const T &v, const T &lower, const T &upper)
Definition vpMath.h:139
static bool nul(double x, double threshold=0.001)
Definition vpMath.h:360
static int round(double x)
Definition vpMath.h:323
static _Tp saturate(int v)
Definition vpMath.h:226
static Type minimum(const Type &a, const Type &b)
Definition vpMath.h:180
static int sign(double x)
Definition vpMath.h:342
static long double comb(unsigned int n, unsigned int p)
Definition vpMath.h:309
static double deg(double rad)
Definition vpMath.h:106
static _Tp saturate(unsigned char v)
Definition vpMath.h:221
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.
Class that consider the case of a translation vector.