Visual Servoing Platform  version 3.4.1 under development (2022-01-27)
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  // Taken from OpenCV source code CvIsNan()
95  Cv64suf ieee754;
96  ieee754.f = value;
97  return (((unsigned)(ieee754.u >> 32) & 0x7fffffff) + ((unsigned)ieee754.u != 0) > 0x7ff00000) != 0;
98 #endif
99 }
100 
106 bool vpMath::isNaN(float value)
107 {
108 #if defined(VISP_HAVE_FUNC_ISNAN)
109  return isnan(value);
110 #elif defined(VISP_HAVE_FUNC_STD_ISNAN)
111  return std::isnan(value);
112 #elif defined(VISP_HAVE_FUNC__ISNAN)
113  return (_isnan(value) != 0);
114 #else
115  // Taken from OpenCV source code CvIsNan()
116  Cv32suf ieee754;
117  ieee754.f = value;
118  return ((unsigned)ieee754.u & 0x7fffffff) > 0x7f800000;
119 #endif
120 }
121 
129 bool vpMath::isInf(double value)
130 {
131 #if defined(VISP_HAVE_FUNC_ISINF)
132  return isinf(value);
133 #elif defined(VISP_HAVE_FUNC_STD_ISINF)
134  return std::isinf(value);
135 #elif defined(VISP_HAVE_FUNC__FINITE)
136  return !_finite(value);
137 #else
138  // Taken from OpenCV source code CvIsInf()
139  Cv64suf ieee754;
140  ieee754.f = value;
141  return ((unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 && (unsigned)ieee754.u == 0;
142 #endif
143 }
144 
152 bool vpMath::isInf(float value)
153 {
154 #if defined(VISP_HAVE_FUNC_ISINF)
155  return isinf(value);
156 #elif defined(VISP_HAVE_FUNC_STD_ISINF)
157  return std::isinf(value);
158 #elif defined(VISP_HAVE_FUNC__FINITE)
159  return !_finite(value);
160 #else
161  // Taken from OpenCV source code CvIsInf()
162  Cv32suf ieee754;
163  ieee754.f = value;
164  return ((unsigned)ieee754.u & 0x7fffffff) == 0x7f800000;
165 #endif
166 }
167 
177 double vpMath::mcosc(double cosx, double x)
178 {
179  if (fabs(x) < ang_min_mc)
180  return 0.5;
181  else
182  return ((1.0 - cosx) / x / x);
183 }
184 
194 double vpMath::msinc(double sinx, double x)
195 {
196  if (fabs(x) < ang_min_mc)
197  return (1. / 6.0);
198  else
199  return ((1.0 - sinx / x) / x / x);
200 }
201 
210 double vpMath::sinc(double x)
211 {
212  if (fabs(x) < ang_min_sinc)
213  return 1.0;
214  else
215  return sin(x) / x;
216 }
226 double vpMath::sinc(double sinx, double x)
227 {
228  if (fabs(x) < ang_min_sinc)
229  return 1.0;
230  else
231  return (sinx / x);
232 }
233 
241 double vpMath::getMean(const std::vector<double> &v)
242 {
243  if (v.empty()) {
244  throw vpException(vpException::notInitialized, "Empty vector !");
245  }
246 
247  size_t size = v.size();
248 
249  double sum = std::accumulate(v.begin(), v.end(), 0.0);
250 
251  return sum / (double)size;
252 }
253 
261 double vpMath::getMedian(const std::vector<double> &v)
262 {
263  if (v.empty()) {
264  throw vpException(vpException::notInitialized, "Empty vector !");
265  }
266 
267  std::vector<double> v_copy = v;
268  size_t size = v_copy.size();
269 
270  size_t n = size / 2;
271  std::nth_element(v_copy.begin(), v_copy.begin() + n, v_copy.end());
272  double val_n = v_copy[n];
273 
274  if (size % 2 == 1) {
275  return val_n;
276  } else {
277  std::nth_element(v_copy.begin(), v_copy.begin() + n - 1, v_copy.end());
278  return 0.5 * (val_n + v_copy[n - 1]);
279  }
280 }
281 
291 double vpMath::getStdev(const std::vector<double> &v, bool useBesselCorrection)
292 {
293  if (v.empty()) {
294  throw vpException(vpException::notInitialized, "Empty vector !");
295  }
296 
297  double mean = getMean(v);
298 
299  std::vector<double> diff(v.size());
300 #if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98
301  std::transform(v.begin(), v.end(), diff.begin(), std::bind(std::minus<double>(), std::placeholders::_1, mean));
302 #else
303  std::transform(v.begin(), v.end(), diff.begin(), std::bind2nd(std::minus<double>(), mean));
304 #endif
305 
306  double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
307  double divisor = (double)v.size();
308  if (useBesselCorrection && v.size() > 1) {
309  divisor = divisor - 1;
310  }
311 
312  return std::sqrt(sq_sum / divisor);
313 }
314 
328 double vpMath::lineFitting(const std::vector<vpImagePoint>& imPts, double& a, double& b, double& c)
329 {
330  if (imPts.size() < 3) {
331  throw vpException(vpException::dimensionError, "Number of image points must be greater or equal to 3.");
332  }
333 
334  double x_mean = 0, y_mean = 0;
335  for (size_t i = 0; i < imPts.size(); i++) {
336  const vpImagePoint& imPt = imPts[i];
337  x_mean += imPt.get_u();
338  y_mean += imPt.get_v();
339  }
340  x_mean /= imPts.size();
341  y_mean /= imPts.size();
342 
343  vpMatrix AtA(2, 2, 0.0);
344  for (size_t i = 0; i < imPts.size(); i++) {
345  const vpImagePoint& imPt = imPts[i];
346  AtA[0][0] += (imPt.get_u() - x_mean)*(imPt.get_u() - x_mean);
347  AtA[0][1] += (imPt.get_u() - x_mean)*(imPt.get_v() - y_mean);
348  AtA[1][1] += (imPt.get_v() - y_mean)*(imPt.get_v() - y_mean);
349  }
350  AtA[1][0] = AtA[0][1];
351 
352  vpColVector eigenvalues;
353  vpMatrix eigenvectors;
354  AtA.eigenValues(eigenvalues, eigenvectors);
355 
356  a = eigenvectors[0][0];
357  b = eigenvectors[1][0];
358  c = a*x_mean + b*y_mean;
359 
360  double error = 0;
361  for (size_t i = 0; i < imPts.size(); i++) {
362  double x0 = imPts[i].get_u();
363  double y0 = imPts[i].get_v();
364 
365  error += std::fabs(a*x0 + b*y0 - c);
366  }
367 
368  return error / imPts.size();
369 }
370 
381 int vpMath::modulo(int a, int n) { return ((a % n) + n) % n; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
static double lineFitting(const std::vector< vpImagePoint > &imPts, double &a, double &b, double &c)
Definition: vpMath.cpp:328
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:261
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:291
error that can be emited by ViSP classes.
Definition: vpException.h:71
static double sinc(double x)
Definition: vpMath.cpp:210
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:241
double get_u() const
Definition: vpImagePoint.h:262
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:177
vpColVector eigenValues() const
Definition: vpMatrix.cpp:6040
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:129
static int modulo(int a, int n)
Definition: vpMath.cpp:381
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:194