Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
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__
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 Vp64suf
70 {
71  // int64 i; //Unused variable, should be harmless to comment it
72  uint64 u;
73  double f;
74 } Vp64suf;
75 
76 typedef 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 
85 const double vpMath::ang_min_sinc = 1.0e-8;
86 const double vpMath::ang_min_mc = 2.5e-4;
87 
93 bool 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()
103  Vp64suf ieee754;
104  ieee754.f = value;
105  return (((unsigned)(ieee754.u >> 32) & 0x7fffffff) + ((unsigned)ieee754.u != 0) > 0x7ff00000) != 0;
106 #endif
107 }
108 
114 bool 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()
124  Vp32suf ieee754;
125  ieee754.f = value;
126  return ((unsigned)ieee754.u & 0x7fffffff) > 0x7f800000;
127 #endif
128 }
129 
137 bool 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()
145  Vp64suf ieee754;
146  ieee754.f = value;
147  return ((unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 && (unsigned)ieee754.u == 0;
148 #endif
149 }
150 
158 bool 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()
166  Vp32suf ieee754;
167  ieee754.f = value;
168  return ((unsigned)ieee754.u & 0x7fffffff) == 0x7f800000;
169 #endif
170 }
171 
178 bool 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 
197 bool 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 
215 bool 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 
233 double 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 
249 double 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 
264 double vpMath::sinc(double x)
265 {
266  if (fabs(x) < ang_min_sinc)
267  return 1.0;
268  else
269  return sin(x) / x;
270 }
279 double 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 
294 double 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 
314 double 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 
345 double 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 
382 double 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 
406  vpColVector eigenvalues;
407  vpMatrix eigenvectors;
408  AtA.eigenValues(eigenvalues, eigenvectors);
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 
435 int vpMath::modulo(int a, int n) { return ((a % n) + n) % n; }
436 
475 vpHomogeneousMatrix vpMath::ned2ecef(double lonDeg, double latDeg, double radius)
476 {
477  double lon = vpMath::rad(lonDeg);
478  double lat = vpMath::rad(latDeg);
479 
480  vpHomogeneousMatrix ecef_M_ned;
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 
536 vpHomogeneousMatrix vpMath::enu2ecef(double lonDeg, double latDeg, double radius)
537 {
538  double lon = vpMath::rad(lonDeg);
539  double lat = vpMath::rad(latDeg);
540 
541  vpHomogeneousMatrix ecef_M_enu;
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 
572 std::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 
589  for (int m = 0; m < m_theta; m++) {
590  double theta = M_PI * (m + 0.5) / m_theta;
591  int m_phi = static_cast<int>(round(2.0 * M_PI * sin(theta) / d_phi));
592 
593  for (int n = 0; n < m_phi; n++) {
594  double phi = 2.0 * M_PI * n / m_phi;
595  double lon = phi;
596  double lat = M_PI_2 - theta;
597  lonlat_vec.push_back(std::make_pair(deg(lon), deg(lat)));
598  }
599  }
600 
601  return lonlat_vec;
602 }
603 
623 std::vector<vpHomogeneousMatrix> vpMath::getLocalTangentPlaneTransformations(const std::vector<std::pair<double, double> > &lonlatVec, double radius,
624  vpHomogeneousMatrix(*toECEF)(double lonDeg_, double latDeg_, double radius_))
625 {
626  std::vector<vpHomogeneousMatrix> ecef_M_local_vec;
627  ecef_M_local_vec.reserve(lonlatVec.size());
628  for (size_t i = 0; i < lonlatVec.size(); i++) {
629  double lonDeg = lonlatVec[i].first;
630  double latDeg = lonlatVec[i].second;
631 
632  vpHomogeneousMatrix ecef_M_local = toECEF(lonDeg, latDeg, radius);
633  ecef_M_local_vec.push_back(ecef_M_local);
634  }
635  return ecef_M_local_vec;
636 }
637 
659 {
660  assert(from.size() == 3);
661  assert(to.size() == 3);
662  assert(tmp.size() == 3);
663  vpColVector forward = (from - to).normalize();
664  vpColVector right = vpColVector::crossProd(tmp.normalize(), forward).normalize();
665  vpColVector up = vpColVector::crossProd(forward, right).normalize();
666 
668  wMc[0][0] = right[0];
669  wMc[0][1] = up[0];
670  wMc[0][2] = forward[0];
671  wMc[0][3] = from[0];
672  wMc[1][0] = right[1];
673  wMc[1][1] = up[1];
674  wMc[1][2] = forward[1];
675  wMc[1][3] = from[1];
676  wMc[2][0] = right[2];
677  wMc[2][1] = up[2];
678  wMc[2][2] = forward[2];
679  wMc[2][3] = from[2];
680 
681  return wMc;
682 }
683 
691 {
692  if (r.size() == 4) {
693  throw(vpException(vpException::fatalError, "Cannot convert angles of a quaternion vector in degrees!"));
694  }
695  vpColVector r_deg(r.size());
696  for (unsigned int i = 0; i < r.size(); i++) {
697  r_deg[i] = vpMath::deg(r[i]);
698  }
699  return r_deg;
700 }
701 
709 {
710  vpColVector r_deg(r.size());
711  for (unsigned int i = 0; i < r.size(); i++) {
712  r_deg[i] = vpMath::deg(r[i]);
713  }
714  return r_deg;
715 }
716 
724 {
725  vpColVector r_rad(r.size());
726  for (unsigned int i = 0; i < r.size(); i++) {
727  r_rad[i] = vpMath::rad(r[i]);
728  }
729  return r_rad;
730 }
731 
738 {
739  vpHomogeneousMatrix ned_M_enu;
740  ned_M_enu[0][0] = 0;
741  ned_M_enu[0][1] = 1;
742  ned_M_enu[1][0] = 1;
743  ned_M_enu[1][1] = 0;
744  ned_M_enu[2][2] = -1;
745 
746  vpHomogeneousMatrix ned_M = ned_M_enu * enu_M;
747  return ned_M;
748 }
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:286
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
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 ...
Definition: vpImagePoint.h:82
double get_u() const
Definition: vpImagePoint.h:136
double get_v() const
Definition: vpImagePoint.h:147
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:623
static double rad(double deg)
Definition: vpMath.h:127
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:658
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:403
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:737
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:233
static double deg(double rad)
Definition: vpMath.h:117
static bool isNumber(const std::string &str)
Definition: vpMath.cpp:215
static float modulo(const float &value, const float &modulo)
Gives the rest of value divided by modulo when the quotient can only be an integer.
Definition: vpMath.h:175
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:146
vpColVector eigenValues() const
Definition: vpMatrix.cpp:6031
Implementation of a generic rotation vector.