Visual Servoing Platform version 3.5.0
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__
61typedef __int64 int64;
62typedef unsigned __int64 uint64;
63#else
64typedef int64_t int64;
65typedef uint64_t uint64;
66#endif
67
68#ifndef DOXYGEN_SHOULD_SKIP_THIS
69typedef 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
77const double vpMath::ang_min_sinc = 1.0e-8;
78const double vpMath::ang_min_mc = 2.5e-4;
79
85bool 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
106bool 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
129bool 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
152bool 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
177double 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
194double 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
210double vpMath::sinc(double x)
211{
212 if (fabs(x) < ang_min_sinc)
213 return 1.0;
214 else
215 return sin(x) / x;
216}
226double 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
241double 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
261double 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
291double 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
328double 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
381int vpMath::modulo(int a, int n) { return ((a % n) + n) % n; }
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
@ dimensionError
Bad dimension.
Definition: vpException.h:95
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
double get_u() const
Definition: vpImagePoint.h:262
double get_v() const
Definition: vpImagePoint.h:273
static double msinc(double sinx, double x)
Definition: vpMath.cpp:194
static bool isNaN(double value)
Definition: vpMath.cpp:85
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:261
static double sinc(double x)
Definition: vpMath.cpp:210
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:291
static int modulo(int a, int n)
Definition: vpMath.cpp:381
static double lineFitting(const std::vector< vpImagePoint > &imPts, double &a, double &b, double &c)
Definition: vpMath.cpp:328
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:241
static bool isInf(double value)
Definition: vpMath.cpp:129
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:177
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpColVector eigenValues() const
Definition: vpMatrix.cpp:6040