Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMath.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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*****************************************************************************/
35
42#include <cmath>
43#include <functional>
44#include <numeric>
45#include <stdint.h>
46#include <cassert>
47#include <ctype.h>
48
49#include <visp3/core/vpException.h>
50#include <visp3/core/vpMath.h>
51#include <visp3/core/vpMatrix.h>
52
53#if defined(VISP_HAVE_FUNC__FINITE)
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 !(defined(VISP_HAVE_FUNC_ISFINITE) || defined(VISP_HAVE_FUNC_STD_ISFINITE) || defined(VISP_HAVE_FUNC__FINITE))
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 Vp64suf
70{
71 // int64 i; //Unused variable, should be harmless to comment it
72 uint64 u;
73 double f;
74} Vp64suf;
75
76typedef union Vp32suf
77{
78 // int i; //Unused variable, should be harmless to comment it
79 unsigned u;
80 float f;
81} Vp32suf;
82#endif
83#endif
84
85const double vpMath::ang_min_sinc = 1.0e-8;
86const double vpMath::ang_min_mc = 2.5e-4;
87
93bool vpMath::isNaN(double value)
94{
95#if defined(VISP_HAVE_FUNC_STD_ISNAN)
96 return std::isnan(value);
97#elif defined(VISP_HAVE_FUNC_ISNAN)
98 return isnan(value);
99#elif defined(VISP_HAVE_FUNC__ISNAN)
100 return (_isnan(value) != 0);
101#else
102 // Taken from OpenCV source code CvIsNan()
104 ieee754.f = value;
105 return (((unsigned)(ieee754.u >> 32) & 0x7fffffff) + ((unsigned)ieee754.u != 0) > 0x7ff00000) != 0;
106#endif
107}
108
114bool vpMath::isNaN(float value)
115{
116#if defined(VISP_HAVE_FUNC_STD_ISNAN)
117 return std::isnan(value);
118#elif defined(VISP_HAVE_FUNC_ISNAN)
119 return isnan(value);
120#elif defined(VISP_HAVE_FUNC__ISNAN)
121 return (_isnan(value) != 0);
122#else
123 // Taken from OpenCV source code CvIsNan()
125 ieee754.f = value;
126 return ((unsigned)ieee754.u & 0x7fffffff) > 0x7f800000;
127#endif
128}
129
137bool vpMath::isInf(double value)
138{
139#if defined(VISP_HAVE_FUNC_STD_ISINF)
140 return std::isinf(value);
141#elif defined(VISP_HAVE_FUNC_ISINF)
142 return isinf(value);
143#else
144 // Taken from OpenCV source code CvIsInf()
146 ieee754.f = value;
147 return ((unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 && (unsigned)ieee754.u == 0;
148#endif
149}
150
158bool vpMath::isInf(float value)
159{
160#if defined(VISP_HAVE_FUNC_STD_ISINF)
161 return std::isinf(value);
162#elif defined(VISP_HAVE_FUNC_ISINF)
163 return isinf(value);
164#else
165 // Taken from OpenCV source code CvIsInf()
167 ieee754.f = value;
168 return ((unsigned)ieee754.u & 0x7fffffff) == 0x7f800000;
169#endif
170}
171
178bool vpMath::isFinite(double value)
179{
180#if defined(VISP_HAVE_FUNC_STD_ISFINITE)
181 return std::isfinite(value);
182#elif defined(VISP_HAVE_FUNC_ISFINITE)
183 return isfinite(value);
184#elif defined(VISP_HAVE_FUNC__FINITE)
185 return _finite(value);
186#else
187 return !vpMath::isInf(value) && !vpMath::isNaN(value);
188#endif
189}
190
197bool vpMath::isFinite(float value)
198{
199#if defined(VISP_HAVE_FUNC_STD_ISFINITE)
200 return std::isfinite(value);
201#elif defined(VISP_HAVE_FUNC_ISFINITE)
202 return isfinite(value);
203#elif defined(VISP_HAVE_FUNC__FINITE)
204 return _finitef(value);
205#else
206 return !vpMath::isInf(value) && !vpMath::isNaN(value);
207#endif
208}
209
215bool vpMath::isNumber(const std::string &str)
216{
217 for (size_t i = 0; i < str.size(); i++) {
218 if (isdigit(str[i]) == false) {
219 return false;
220 }
221 }
222 return true;
223}
224
233double vpMath::mcosc(double cosx, double x)
234{
235 if (fabs(x) < ang_min_mc)
236 return 0.5;
237 else
238 return ((1.0 - cosx) / x / x);
239}
240
249double vpMath::msinc(double sinx, double x)
250{
251 if (fabs(x) < ang_min_mc)
252 return (1. / 6.0);
253 else
254 return ((1.0 - sinx / x) / x / x);
255}
256
264double vpMath::sinc(double x)
265{
266 if (fabs(x) < ang_min_sinc)
267 return 1.0;
268 else
269 return sin(x) / x;
270}
279double vpMath::sinc(double sinx, double x)
280{
281 if (fabs(x) < ang_min_sinc)
282 return 1.0;
283 else
284 return (sinx / x);
285}
286
294double vpMath::getMean(const std::vector<double> &v)
295{
296 if (v.empty()) {
297 throw vpException(vpException::notInitialized, "Empty vector !");
298 }
299
300 size_t size = v.size();
301
302 double sum = std::accumulate(v.begin(), v.end(), 0.0);
303
304 return sum / (double)size;
305}
306
314double vpMath::getMedian(const std::vector<double> &v)
315{
316 if (v.empty()) {
317 throw vpException(vpException::notInitialized, "Empty vector !");
318 }
319
320 std::vector<double> v_copy = v;
321 size_t size = v_copy.size();
322
323 size_t n = size / 2;
324 std::nth_element(v_copy.begin(), v_copy.begin() + n, v_copy.end());
325 double val_n = v_copy[n];
326
327 if (size % 2 == 1) {
328 return val_n;
329 }
330 else {
331 std::nth_element(v_copy.begin(), v_copy.begin() + n - 1, v_copy.end());
332 return 0.5 * (val_n + v_copy[n - 1]);
333 }
334}
335
345double vpMath::getStdev(const std::vector<double> &v, bool useBesselCorrection)
346{
347 if (v.empty()) {
348 throw vpException(vpException::notInitialized, "Empty vector !");
349 }
350
351 double mean = getMean(v);
352
353 std::vector<double> diff(v.size());
354#if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98
355 std::transform(v.begin(), v.end(), diff.begin(), std::bind(std::minus<double>(), std::placeholders::_1, mean));
356#else
357 std::transform(v.begin(), v.end(), diff.begin(), std::bind2nd(std::minus<double>(), mean));
358#endif
359
360 double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
361 double divisor = (double)v.size();
362 if (useBesselCorrection && v.size() > 1) {
363 divisor = divisor - 1;
364 }
365
366 return std::sqrt(sq_sum / divisor);
367}
368
382double vpMath::lineFitting(const std::vector<vpImagePoint> &imPts, double &a, double &b, double &c)
383{
384 if (imPts.size() < 3) {
385 throw vpException(vpException::dimensionError, "Number of image points must be greater or equal to 3.");
386 }
387
388 double x_mean = 0, y_mean = 0;
389 for (size_t i = 0; i < imPts.size(); i++) {
390 const vpImagePoint &imPt = imPts[i];
391 x_mean += imPt.get_u();
392 y_mean += imPt.get_v();
393 }
394 x_mean /= imPts.size();
395 y_mean /= imPts.size();
396
397 vpMatrix AtA(2, 2, 0.0);
398 for (size_t i = 0; i < imPts.size(); i++) {
399 const vpImagePoint &imPt = imPts[i];
400 AtA[0][0] += (imPt.get_u() - x_mean) * (imPt.get_u() - x_mean);
401 AtA[0][1] += (imPt.get_u() - x_mean) * (imPt.get_v() - y_mean);
402 AtA[1][1] += (imPt.get_v() - y_mean) * (imPt.get_v() - y_mean);
403 }
404 AtA[1][0] = AtA[0][1];
405
409
410 a = eigenvectors[0][0];
411 b = eigenvectors[1][0];
412 c = a * x_mean + b * y_mean;
413
414 double error = 0;
415 for (size_t i = 0; i < imPts.size(); i++) {
416 double x0 = imPts[i].get_u();
417 double y0 = imPts[i].get_v();
418
419 error += std::fabs(a * x0 + b * y0 - c);
420 }
421
422 return error / imPts.size();
423}
424
435int vpMath::modulo(int a, int n) { return ((a % n) + n) % n; }
436
475vpHomogeneousMatrix vpMath::ned2ecef(double lonDeg, double latDeg, double radius)
476{
477 double lon = vpMath::rad(lonDeg);
478 double lat = vpMath::rad(latDeg);
479
481 ecef_M_ned[0][0] = -sin(lat) * cos(lon);
482 ecef_M_ned[0][1] = -sin(lon);
483 ecef_M_ned[0][2] = -cos(lat) * cos(lon);
484 ecef_M_ned[0][3] = radius * cos(lon) * cos(lat);
485 ecef_M_ned[1][0] = -sin(lat) * sin(lon);
486 ecef_M_ned[1][1] = cos(lon);
487 ecef_M_ned[1][2] = -cos(lat) * sin(lon);
488 ecef_M_ned[1][3] = radius * sin(lon) * cos(lat);
489 ecef_M_ned[2][0] = cos(lat);
490 ecef_M_ned[2][1] = 0;
491 ecef_M_ned[2][2] = -sin(lat);
492 ecef_M_ned[2][3] = radius * sin(lat);
493
494 return ecef_M_ned;
495}
496
536vpHomogeneousMatrix vpMath::enu2ecef(double lonDeg, double latDeg, double radius)
537{
538 double lon = vpMath::rad(lonDeg);
539 double lat = vpMath::rad(latDeg);
540
542 ecef_M_enu[0][0] = -sin(lon);
543 ecef_M_enu[0][1] = -sin(lat) * cos(lon);
544 ecef_M_enu[0][2] = cos(lat) * cos(lon);
545 ecef_M_enu[0][3] = radius * cos(lon) * cos(lat);
546 ecef_M_enu[1][0] = cos(lon);
547 ecef_M_enu[1][1] = -sin(lat) * sin(lon);
548 ecef_M_enu[1][2] = cos(lat) * sin(lon);
549 ecef_M_enu[1][3] = radius * sin(lon) * cos(lat);
550 ecef_M_enu[2][0] = 0;
551 ecef_M_enu[2][1] = cos(lat);
552 ecef_M_enu[2][2] = sin(lat);
553 ecef_M_enu[2][3] = radius * sin(lat);
554
555 return ecef_M_enu;
556}
557
572std::vector<std::pair<double, double> > vpMath::computeRegularPointsOnSphere(unsigned int maxPoints)
573{
574 assert(maxPoints > 0);
575
576 double a = 4.0 * M_PI / maxPoints;
577 double d = sqrt(a);
578 int m_theta = int(round(M_PI / d));
579 double d_theta = M_PI / m_theta;
580 double d_phi = a / d_theta;
581
582 std::vector<std::pair<double, double> > lonlat_vec;
583#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
584 lonlat_vec.reserve(static_cast<unsigned int>(std::sqrt(maxPoints)));
585#else
586 lonlat_vec.reserve(static_cast<unsigned int>(std::sqrt(static_cast<double>(maxPoints))));
587#endif
588 for (int m = 0; m < m_theta; m++) {
589 double theta = M_PI * (m + 0.5) / m_theta;
590 int m_phi = static_cast<int>(round(2.0 * M_PI * sin(theta) / d_phi));
591
592 for (int n = 0; n < m_phi; n++) {
593 double phi = 2.0 * M_PI * n / m_phi;
594 double lon = phi;
595 double lat = M_PI_2 - theta;
596 lonlat_vec.push_back(std::make_pair(deg(lon), deg(lat)));
597 }
598 }
599
600 return lonlat_vec;
601}
602
622std::vector<vpHomogeneousMatrix> vpMath::getLocalTangentPlaneTransformations(const std::vector<std::pair<double, double> > &lonlatVec, double radius,
623 vpHomogeneousMatrix(*toECEF)(double lonDeg_, double latDeg_, double radius_))
624{
625 std::vector<vpHomogeneousMatrix> ecef_M_local_vec;
626 ecef_M_local_vec.reserve(lonlatVec.size());
627 for (size_t i = 0; i < lonlatVec.size(); i++) {
628 double lonDeg = lonlatVec[i].first;
629 double latDeg = lonlatVec[i].second;
630
633 }
634 return ecef_M_local_vec;
635}
636
658{
659 assert(from.size() == 3);
660 assert(to.size() == 3);
661 assert(tmp.size() == 3);
662 vpColVector forward = (from - to).normalize();
665
667 wMc[0][0] = right[0];
668 wMc[0][1] = up[0];
669 wMc[0][2] = forward[0];
670 wMc[0][3] = from[0];
671 wMc[1][0] = right[1];
672 wMc[1][1] = up[1];
673 wMc[1][2] = forward[1];
674 wMc[1][3] = from[1];
675 wMc[2][0] = right[2];
676 wMc[2][1] = up[2];
677 wMc[2][2] = forward[2];
678 wMc[2][3] = from[2];
679
680 return wMc;
681}
682
690{
691 if (r.size() == 4) {
692 throw(vpException(vpException::fatalError, "Cannot convert angles of a quaternion vector in degrees!"));
693 }
694 vpColVector r_deg(r.size());
695 for (unsigned int i = 0; i < r.size(); i++) {
696 r_deg[i] = vpMath::deg(r[i]);
697 }
698 return r_deg;
699}
700
708{
709 vpColVector r_deg(r.size());
710 for (unsigned int i = 0; i < r.size(); i++) {
711 r_deg[i] = vpMath::deg(r[i]);
712 }
713 return r_deg;
714}
715
723{
724 vpColVector r_rad(r.size());
725 for (unsigned int i = 0; i < r.size(); i++) {
726 r_rad[i] = vpMath::rad(r[i]);
727 }
728 return r_rad;
729}
730
737{
739 ned_M_enu[0][0] = 0;
740 ned_M_enu[0][1] = 1;
741 ned_M_enu[1][0] = 1;
742 ned_M_enu[1][1] = 0;
743 ned_M_enu[2][2] = -1;
744
746 return ned_M;
747}
Implementation of column vector and the associated operations.
vpColVector & normalize()
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition vpException.h:86
@ dimensionError
Bad dimension.
Definition vpException.h:83
@ fatalError
Fatal error.
Definition vpException.h:84
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
double get_u() const
static double msinc(double sinx, double x)
Definition vpMath.cpp:249
static bool isNaN(double value)
Definition vpMath.cpp:93
static std::vector< vpHomogeneousMatrix > getLocalTangentPlaneTransformations(const std::vector< std::pair< double, double > > &lonlatVec, double radius, vpHomogeneousMatrix(*toECEF)(double lonDeg, double latDeg, double radius))
Definition vpMath.cpp:622
static double rad(double deg)
Definition vpMath.h:116
static double getMedian(const std::vector< double > &v)
Definition vpMath.cpp:314
static double sinc(double x)
Definition vpMath.cpp:264
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition vpMath.cpp:345
static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp)
Definition vpMath.cpp:657
static int modulo(int a, int n)
Definition vpMath.cpp:435
static vpHomogeneousMatrix enu2ecef(double lonDeg, double latDeg, double radius)
Definition vpMath.cpp:536
static double lineFitting(const std::vector< vpImagePoint > &imPts, double &a, double &b, double &c)
Definition vpMath.cpp:382
static int round(double x)
Definition vpMath.h:323
static bool isFinite(double value)
Definition vpMath.cpp:178
static double getMean(const std::vector< double > &v)
Definition vpMath.cpp:294
static bool isInf(double value)
Definition vpMath.cpp:137
static vpHomogeneousMatrix enu2ned(const vpHomogeneousMatrix &enu_M)
Definition vpMath.cpp:736
static double mcosc(double cosx, double x)
Definition vpMath.cpp:233
static double deg(double rad)
Definition vpMath.h:106
static bool isNumber(const std::string &str)
Definition vpMath.cpp:215
static _Tp saturate(unsigned char v)
Definition vpMath.h:221
static std::vector< std::pair< double, double > > computeRegularPointsOnSphere(unsigned int maxPoints)
Definition vpMath.cpp:572
static vpHomogeneousMatrix ned2ecef(double lonDeg, double latDeg, double radius)
Definition vpMath.cpp:475
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
vpColVector eigenValues() const
Implementation of a generic rotation vector.