Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
vpMath.cpp
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 #include <cmath>
46 #include <functional>
47 #include <numeric>
48 #include <stdint.h>
49 
50 #include <visp3/core/vpException.h>
51 #include <visp3/core/vpMath.h>
52 
53 #if defined(VISP_HAVE_FUNC__ISNAN)
54 #include <float.h>
55 #endif
56 
57 #if !(defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN)) || \
58  !(defined(VISP_HAVE_FUNC_ISINF) || defined(VISP_HAVE_FUNC_STD_ISINF))
59 #if defined _MSC_VER || defined __BORLANDC__
60 typedef __int64 int64;
61 typedef unsigned __int64 uint64;
62 #else
63 typedef int64_t int64;
64 typedef uint64_t uint64;
65 #endif
66 
67 #ifndef DOXYGEN_SHOULD_SKIP_THIS
68 typedef union Cv64suf {
69  // int64 i; //Unused variable, should be harmless to comment it
70  uint64 u;
71  double f;
72 } Cv64suf;
73 #endif
74 #endif
75 
76 const double vpMath::ang_min_sinc = 1.0e-8;
77 const double vpMath::ang_min_mc = 2.5e-4;
78 
84 bool vpMath::isNaN(double value)
85 {
86 #if defined(VISP_HAVE_FUNC_ISNAN)
87  return isnan(value);
88 #elif defined(VISP_HAVE_FUNC_STD_ISNAN)
89  return std::isnan(value);
90 #elif defined(VISP_HAVE_FUNC__ISNAN)
91  return (_isnan(value) != 0);
92 #else
93 #if 0
94  //This trick should work for any compiler which claims to use IEEE floating point.
95  //Do not work with g++ and -ffast-math option.
96  return (value != value);
97 #else
98  // Taken from OpenCV source code CvIsNan()
99  Cv64suf ieee754;
100  ieee754.f = value;
101  return (((unsigned)(ieee754.u >> 32) & 0x7fffffff) + ((unsigned)ieee754.u != 0) > 0x7ff00000) != 0;
102 #endif
103 #endif
104 }
110 bool vpMath::isInf(double value)
111 {
112 #if defined(VISP_HAVE_FUNC_ISINF)
113  return isinf(value);
114 #elif defined(VISP_HAVE_FUNC_STD_ISINF)
115  return std::isinf(value);
116 #elif defined(VISP_HAVE_FUNC__FINITE)
117  return !_finite(value);
118 #else
119  // Taken from OpenCV source code CvIsInf()
120  Cv64suf ieee754;
121  ieee754.f = value;
122  return ((unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 && (unsigned)ieee754.u == 0;
123 #endif
124 }
125 
135 double vpMath::mcosc(double cosx, double x)
136 {
137  if (fabs(x) < ang_min_mc)
138  return 0.5;
139  else
140  return ((1.0 - cosx) / x / x);
141 }
142 
153 double vpMath::msinc(double sinx, double x)
154 {
155  if (fabs(x) < ang_min_mc)
156  return (1. / 6.0);
157  else
158  return ((1.0 - sinx / x) / x / x);
159 }
160 
170 double vpMath::sinc(double x)
171 {
172  if (fabs(x) < ang_min_sinc)
173  return 1.0;
174  else
175  return sin(x) / x;
176 }
187 double vpMath::sinc(double sinx, double x)
188 {
189  if (fabs(x) < ang_min_sinc)
190  return 1.0;
191  else
192  return (sinx / x);
193 }
194 
202 double vpMath::getMean(const std::vector<double> &v)
203 {
204  if (v.empty()) {
205  throw vpException(vpException::notInitialized, "Empty vector !");
206  }
207 
208  size_t size = v.size();
209 
210  double sum = std::accumulate(v.begin(), v.end(), 0.0);
211 
212  return sum / (double)size;
213 }
214 
222 double vpMath::getMedian(const std::vector<double> &v)
223 {
224  if (v.empty()) {
225  throw vpException(vpException::notInitialized, "Empty vector !");
226  }
227 
228  std::vector<double> v_copy = v;
229  size_t size = v_copy.size();
230 
231  size_t n = size / 2;
232  std::nth_element(v_copy.begin(), v_copy.begin() + n, v_copy.end());
233  double val_n = v_copy[n];
234 
235  if (size % 2 == 1) {
236  return val_n;
237  } else {
238  std::nth_element(v_copy.begin(), v_copy.begin() + n - 1, v_copy.end());
239  return 0.5 * (val_n + v_copy[n - 1]);
240  }
241 }
242 
252 double vpMath::getStdev(const std::vector<double> &v, bool useBesselCorrection)
253 {
254  if (v.empty()) {
255  throw vpException(vpException::notInitialized, "Empty vector !");
256  }
257 
258  double mean = getMean(v);
259 
260  std::vector<double> diff(v.size());
261 #if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98
262  std::transform(v.begin(), v.end(), diff.begin(), std::bind(std::minus<double>(), std::placeholders::_1, mean));
263 #else
264  std::transform(v.begin(), v.end(), diff.begin(), std::bind2nd(std::minus<double>(), mean));
265 #endif
266 
267  double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
268  double divisor = (double)v.size();
269  if (useBesselCorrection && v.size() > 1) {
270  divisor = divisor - 1;
271  }
272 
273  return std::sqrt(sq_sum / divisor);
274 }
275 
286 int vpMath::modulo(int a, int n) { return ((a % n) + n) % n; }
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:222
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:252
error that can be emited by ViSP classes.
Definition: vpException.h:71
static double sinc(double x)
Definition: vpMath.cpp:170
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:202
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:135
static bool isNaN(double value)
Definition: vpMath.cpp:84
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
static bool isInf(double value)
Definition: vpMath.cpp:110
static int modulo(int a, int n)
Definition: vpMath.cpp:286
static double msinc(double sinx, double x)
Definition: vpMath.cpp:153