43 #include <visp3/core/vpConfig.h>
52 #ifndef _USE_MATH_DEFINES
53 #define _USE_MATH_DEFINES
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)
67 #define M_PI 3.14159265358979323846
71 #define M_PI_2 (M_PI / 2.0)
75 #define M_PI_4 (M_PI / 4.0)
81 #define M_PIf 3.14159265358979323846f
85 #define M_PI_2f (M_PIf / 2.0f)
89 #define M_PI_4f (M_PIf / 4.0f)
92 #include <visp3/core/vpException.h>
93 #include <visp3/core/vpImagePoint.h>
117 static inline double deg(
double rad) {
return (rad * 180.0) / M_PI; }
127 static inline double rad(
double deg) {
return (deg * M_PI) / 180.0; }
139 float theta1 = theta;
140 if (theta1 > M_PIf) {
141 theta1 -= 2.0f * M_PIf;
143 else if (theta1 <= -M_PIf) {
144 theta1 += 2.0f * M_PIf;
157 double theta1 = theta;
159 theta1 -= 2.0 * M_PI;
161 else if (theta1 < -M_PI) {
162 theta1 += 2.0 * M_PI;
175 static float modulo(
const float &value,
const float &modulo)
177 float quotient = std::floor(value / modulo);
178 float rest = value - quotient * modulo;
190 static double modulo(
const double &value,
const double &modulo)
192 double quotient = std::floor(value / modulo);
193 double rest = value - quotient * modulo;
201 static inline double sqr(
double x) {
return x * x; }
204 static inline double fact(
unsigned int x);
207 static inline long double comb(
unsigned int n,
unsigned int p);
216 template <
typename T>
static inline T
clamp(
const T &v,
const T &lower,
const T &upper)
221 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
222 return std::clamp(v, lower, upper);
227 return (v < lower) ? lower : (upper < v) ? upper : v;
232 static inline int round(
double x);
235 static inline int sign(
double x);
238 static inline bool nul(
double x,
double threshold = 0.001);
241 static inline bool equal(
double x,
double y,
double threshold = 0.001);
244 static inline bool greater(
double x,
double y,
double threshold = 0.001);
252 template <
class Type>
static Type
maximum(
const Type &a,
const Type &b) {
return (a > b) ? a : b; }
260 template <
class Type>
static Type
minimum(
const Type &a,
const Type &b) {
return (a < b) ? a : b; }
267 template <
class Type>
static Type
abs(
const Type &x) {
return (x < 0) ? -x : x; }
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);
276 static inline double sigmoid(
double x,
double x0 = 0.,
double x1 = 1.,
double n = 12.);
284 template <
class Type>
static void swap(Type &a, Type &b)
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);
299 static double lineFitting(
const std::vector<vpImagePoint> &imPts,
double &a,
double &b,
double &c);
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); }
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);
314 static int modulo(
int a,
int n);
330 template <
typename T>
static std::vector<double>
linspace(T start_in, T end_in,
unsigned int num_in)
332 std::vector<double> linspaced;
334 double start =
static_cast<double>(start_in);
335 double end =
static_cast<double>(end_in);
336 double num =
static_cast<double>(num_in);
338 if (std::fabs(num) < std::numeric_limits<double>::epsilon()) {
341 if (std::fabs(num - 1) < std::numeric_limits<double>::epsilon()) {
342 linspaced.push_back(start);
346 double delta = (end - start) / (num - 1);
348 for (
int i = 0; i < num - 1; i++) {
349 linspaced.push_back(start + delta * i);
351 linspaced.push_back(end);
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,
364 static const double ang_min_sinc;
365 static const double ang_min_mc;
376 if ((x == 1) || (x == 0))
378 return x *
fact(x - 1);
405 #if defined(VISP_HAVE_FUNC_STD_ROUND)
406 return static_cast<int>(std::round(x));
407 #elif defined(VISP_HAVE_FUNC_ROUND)
410 return static_cast<int>(
::round(x));
412 return (x > 0.0) ? (
static_cast<int>(floor(x + 0.5))) : (
static_cast<int>(ceil(x - 0.5)));
424 if (fabs(x) < std::numeric_limits<double>::epsilon())
440 bool vpMath::nul(
double x,
double threshold) {
return (fabs(x) < threshold); }
449 bool vpMath::equal(
double x,
double y,
double threshold) {
return (
nul(x - y, threshold)); }
458 bool vpMath::greater(
double x,
double y,
double threshold) {
return (x > (y - threshold)); }
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);
483 template <>
inline unsigned char vpMath::saturate<unsigned char>(
char v)
490 if (std::numeric_limits<char>::is_signed)
491 return static_cast<unsigned char>(std::max<int>(
static_cast<int>(v), 0));
493 return static_cast<unsigned char>(
static_cast<unsigned int>(v) > SCHAR_MAX ? 0 : v);
496 template <>
inline unsigned char vpMath::saturate<unsigned char>(
unsigned short v)
498 return static_cast<unsigned char>(std::min<unsigned int>(
static_cast<unsigned int>(v),
static_cast<unsigned int>(UCHAR_MAX)));
501 template <>
inline unsigned char vpMath::saturate<unsigned char>(
int v)
503 return static_cast<unsigned char>(
static_cast<unsigned int>(v) <= UCHAR_MAX ? v : v > 0 ? UCHAR_MAX : 0);
506 template <>
inline unsigned char vpMath::saturate<unsigned char>(
short v)
508 return saturate<unsigned char>(
static_cast<int>(v));
511 template <>
inline unsigned char vpMath::saturate<unsigned char>(
unsigned int v)
513 return static_cast<unsigned char>(std::min<unsigned int>(v,
static_cast<unsigned int>(UCHAR_MAX)));
516 template <>
inline unsigned char vpMath::saturate<unsigned char>(
float v)
519 return saturate<unsigned char>(iv);
522 template <>
inline unsigned char vpMath::saturate<unsigned char>(
double v)
525 return saturate<unsigned char>(iv);
529 template <>
inline char vpMath::saturate<char>(
unsigned char v)
531 return static_cast<char>(std::min<int>(
static_cast<int>(v), SCHAR_MAX));
534 template <>
inline char vpMath::saturate<char>(
unsigned short v)
536 return static_cast<char>(std::min<unsigned int>(
static_cast<unsigned int>(v),
static_cast<unsigned int>(SCHAR_MAX)));
539 template <>
inline char vpMath::saturate<char>(
int v)
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);
544 template <>
inline char vpMath::saturate<char>(
short v)
546 return saturate<char>(
static_cast<int>(v));
549 template <>
inline char vpMath::saturate<char>(
unsigned int v)
551 return static_cast<char>(std::min<unsigned int>(v,
static_cast<unsigned int>(SCHAR_MAX)));
554 template <>
inline char vpMath::saturate<char>(
float v)
557 return saturate<char>(iv);
560 template <>
inline char vpMath::saturate<char>(
double v)
563 return saturate<char>(iv);
567 template <>
inline unsigned short vpMath::saturate<unsigned short>(
char v)
574 if (std::numeric_limits<char>::is_signed)
575 return static_cast<unsigned short>(std::max<int>(
static_cast<int>(v), 0));
577 return static_cast<unsigned short>(
static_cast<unsigned int>(v) > SCHAR_MAX ? 0 : v);
580 template <>
inline unsigned short vpMath::saturate<unsigned short>(
short v)
582 return static_cast<unsigned short>(std::max<int>(
static_cast<int>(v), 0));
585 template <>
inline unsigned short vpMath::saturate<unsigned short>(
int v)
587 return static_cast<unsigned short>(
static_cast<unsigned int>(v) <=
static_cast<unsigned int>(USHRT_MAX) ? v : v > 0 ? USHRT_MAX : 0);
590 template <>
inline unsigned short vpMath::saturate<unsigned short>(
unsigned int v)
592 return static_cast<unsigned short>(std::min<unsigned int>(v,
static_cast<unsigned int>(USHRT_MAX)));
595 template <>
inline unsigned short vpMath::saturate<unsigned short>(
float v)
598 return vpMath::saturate<unsigned short>(iv);
601 template <>
inline unsigned short vpMath::saturate<unsigned short>(
double v)
604 return vpMath::saturate<unsigned short>(iv);
608 template <>
inline short vpMath::saturate<short>(
unsigned short v)
610 return static_cast<short>(std::min<int>(
static_cast<int>(v), SHRT_MAX));
612 template <>
inline short vpMath::saturate<short>(
int v)
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);
616 template <>
inline short vpMath::saturate<short>(
unsigned int v)
618 return static_cast<short>(std::min<unsigned int>(v,
static_cast<unsigned int>(SHRT_MAX)));
620 template <>
inline short vpMath::saturate<short>(
float v)
623 return vpMath::saturate<short>(iv);
625 template <>
inline short vpMath::saturate<short>(
double v)
628 return vpMath::saturate<short>(iv);
632 template <>
inline int vpMath::saturate<int>(
float v)
637 template <>
inline int vpMath::saturate<int>(
double v)
645 template <>
inline unsigned int vpMath::saturate<unsigned int>(
float v)
647 return static_cast<unsigned int>(
vpMath::round(
static_cast<double>(v)));
650 template <>
inline unsigned int vpMath::saturate<unsigned int>(
double v)
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
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...
static Tp saturate(double v)
static Tp saturate(char v)
static void swap(Type &a, Type &b)
static Tp saturate(unsigned char v)
static float getAngleBetweenMinPiAndPi(const float &theta)
static Tp saturate(unsigned v)
static double fact(unsigned int x)
static double rad(double deg)
static Type maximum(const Type &a, const Type &b)
static double sqr(double x)
static Tp saturate(int v)
static bool greater(double x, double y, double threshold=0.001)
static Type abs(const Type &x)
static bool equal(double x, double y, double threshold=0.001)
static double sigmoid(double x, double x0=0., double x1=1., double n=12.)
static T clamp(const T &v, const T &lower, const T &upper)
static bool nul(double x, double threshold=0.001)
static int round(double x)
static Type minimum(const Type &a, const Type &b)
static Tp saturate(float v)
static int sign(double x)
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.
static long double comb(unsigned int n, unsigned int p)
static double deg(double rad)
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.
static Tp saturate(unsigned short v)
static double getAngleBetweenMinPiAndPi(const double &theta)
static Tp saturate(short v)
static std::vector< double > linspace(T start_in, T end_in, unsigned int num_in)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
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.