Visual Servoing Platform  version 3.3.1 under development (2020-10-25)
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 #include <visp3/core/vpMatrix.h>
53 
54 #if defined(VISP_HAVE_FUNC__ISNAN)
55 #include <float.h>
56 #endif
57 
58 #if !(defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN)) || \
59  !(defined(VISP_HAVE_FUNC_ISINF) || defined(VISP_HAVE_FUNC_STD_ISINF))
60 #if defined _MSC_VER || defined __BORLANDC__
61 typedef __int64 int64;
62 typedef unsigned __int64 uint64;
63 #else
64 typedef int64_t int64;
65 typedef uint64_t uint64;
66 #endif
67 
68 #ifndef DOXYGEN_SHOULD_SKIP_THIS
69 typedef union Cv64suf {
70  // int64 i; //Unused variable, should be harmless to comment it
71  uint64 u;
72  double f;
73 } Cv64suf;
74 #endif
75 #endif
76 
77 const double vpMath::ang_min_sinc = 1.0e-8;
78 const double vpMath::ang_min_mc = 2.5e-4;
79 
85 bool vpMath::isNaN(double value)
86 {
87 #if defined(VISP_HAVE_FUNC_ISNAN)
88  return isnan(value);
89 #elif defined(VISP_HAVE_FUNC_STD_ISNAN)
90  return std::isnan(value);
91 #elif defined(VISP_HAVE_FUNC__ISNAN)
92  return (_isnan(value) != 0);
93 #else
94 #if 0
95  //This trick should work for any compiler which claims to use IEEE floating point.
96  //Do not work with g++ and -ffast-math option.
97  return (value != value);
98 #else
99  // Taken from OpenCV source code CvIsNan()
100  Cv64suf ieee754;
101  ieee754.f = value;
102  return (((unsigned)(ieee754.u >> 32) & 0x7fffffff) + ((unsigned)ieee754.u != 0) > 0x7ff00000) != 0;
103 #endif
104 #endif
105 }
111 bool vpMath::isInf(double value)
112 {
113 #if defined(VISP_HAVE_FUNC_ISINF)
114  return isinf(value);
115 #elif defined(VISP_HAVE_FUNC_STD_ISINF)
116  return std::isinf(value);
117 #elif defined(VISP_HAVE_FUNC__FINITE)
118  return !_finite(value);
119 #else
120  // Taken from OpenCV source code CvIsInf()
121  Cv64suf ieee754;
122  ieee754.f = value;
123  return ((unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 && (unsigned)ieee754.u == 0;
124 #endif
125 }
126 
136 double vpMath::mcosc(double cosx, double x)
137 {
138  if (fabs(x) < ang_min_mc)
139  return 0.5;
140  else
141  return ((1.0 - cosx) / x / x);
142 }
143 
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 
169 double vpMath::sinc(double x)
170 {
171  if (fabs(x) < ang_min_sinc)
172  return 1.0;
173  else
174  return sin(x) / x;
175 }
185 double vpMath::sinc(double sinx, double x)
186 {
187  if (fabs(x) < ang_min_sinc)
188  return 1.0;
189  else
190  return (sinx / x);
191 }
192 
200 double vpMath::getMean(const std::vector<double> &v)
201 {
202  if (v.empty()) {
203  throw vpException(vpException::notInitialized, "Empty vector !");
204  }
205 
206  size_t size = v.size();
207 
208  double sum = std::accumulate(v.begin(), v.end(), 0.0);
209 
210  return sum / (double)size;
211 }
212 
220 double vpMath::getMedian(const std::vector<double> &v)
221 {
222  if (v.empty()) {
223  throw vpException(vpException::notInitialized, "Empty vector !");
224  }
225 
226  std::vector<double> v_copy = v;
227  size_t size = v_copy.size();
228 
229  size_t n = size / 2;
230  std::nth_element(v_copy.begin(), v_copy.begin() + n, v_copy.end());
231  double val_n = v_copy[n];
232 
233  if (size % 2 == 1) {
234  return val_n;
235  } else {
236  std::nth_element(v_copy.begin(), v_copy.begin() + n - 1, v_copy.end());
237  return 0.5 * (val_n + v_copy[n - 1]);
238  }
239 }
240 
250 double vpMath::getStdev(const std::vector<double> &v, bool useBesselCorrection)
251 {
252  if (v.empty()) {
253  throw vpException(vpException::notInitialized, "Empty vector !");
254  }
255 
256  double mean = getMean(v);
257 
258  std::vector<double> diff(v.size());
259 #if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98
260  std::transform(v.begin(), v.end(), diff.begin(), std::bind(std::minus<double>(), std::placeholders::_1, mean));
261 #else
262  std::transform(v.begin(), v.end(), diff.begin(), std::bind2nd(std::minus<double>(), mean));
263 #endif
264 
265  double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
266  double divisor = (double)v.size();
267  if (useBesselCorrection && v.size() > 1) {
268  divisor = divisor - 1;
269  }
270 
271  return std::sqrt(sq_sum / divisor);
272 }
273 
287 double vpMath::lineFitting(const std::vector<vpImagePoint>& imPts, double& a, double& b, double& c)
288 {
289  if (imPts.size() < 3) {
290  throw vpException(vpException::dimensionError, "Number of image points must be greater or equal to 3.");
291  }
292 
293  double x_mean = 0, y_mean = 0;
294  for (size_t i = 0; i < imPts.size(); i++) {
295  const vpImagePoint& imPt = imPts[i];
296  x_mean += imPt.get_u();
297  y_mean += imPt.get_v();
298  }
299  x_mean /= imPts.size();
300  y_mean /= imPts.size();
301 
302  vpMatrix AtA(2, 2, 0.0);
303  for (size_t i = 0; i < imPts.size(); i++) {
304  const vpImagePoint& imPt = imPts[i];
305  AtA[0][0] += (imPt.get_u() - x_mean)*(imPt.get_u() - x_mean);
306  AtA[0][1] += (imPt.get_u() - x_mean)*(imPt.get_v() - y_mean);
307  AtA[1][1] += (imPt.get_v() - y_mean)*(imPt.get_v() - y_mean);
308  }
309  AtA[1][0] = AtA[0][1];
310 
311  vpColVector eigenvalues;
312  vpMatrix eigenvectors;
313  AtA.eigenValues(eigenvalues, eigenvectors);
314 
315  a = eigenvectors[0][0];
316  b = eigenvectors[1][0];
317  c = a*x_mean + b*y_mean;
318 
319  double error = 0;
320  for (size_t i = 0; i < imPts.size(); i++) {
321  double x0 = imPts[i].get_u();
322  double y0 = imPts[i].get_v();
323 
324  error += std::fabs(a*x0 + b*y0 - c);
325  }
326 
327  return error / imPts.size();
328 }
329 
340 int vpMath::modulo(int a, int n) { return ((a % n) + n) % n; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
static double lineFitting(const std::vector< vpImagePoint > &imPts, double &a, double &b, double &c)
Definition: vpMath.cpp:287
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:220
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:250
error that can be emited by ViSP classes.
Definition: vpException.h:71
static double sinc(double x)
Definition: vpMath.cpp:169
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:200
double get_u() const
Definition: vpImagePoint.h:262
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:136
vpColVector eigenValues() const
Definition: vpMatrix.cpp:6260
static bool isNaN(double value)
Definition: vpMath.cpp:85
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
static bool isInf(double value)
Definition: vpMath.cpp:111
static int modulo(int a, int n)
Definition: vpMath.cpp:340
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
double get_v() const
Definition: vpImagePoint.h:273
static double msinc(double sinx, double x)
Definition: vpMath.cpp:153