Visual Servoing Platform  version 3.6.1 under development (2024-06-12)
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 
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  !(defined(VISP_HAVE_FUNC_ISFINITE) || defined(VISP_HAVE_FUNC_STD_ISFINITE) || defined(VISP_HAVE_FUNC__FINITE))
61 #if defined _MSC_VER || defined __BORLANDC__
62 typedef __int64 int64;
63 typedef unsigned __int64 uint64;
64 #else
65 typedef int64_t int64;
66 typedef uint64_t uint64;
67 #endif
68 
69 #ifndef DOXYGEN_SHOULD_SKIP_THIS
70 typedef union Vp64suf
71 {
72  // int64 i; //Unused variable, should be harmless to comment it
73  uint64 u;
74  double f;
75 } Vp64suf;
76 
77 typedef union Vp32suf
78 {
79  // int i; //Unused variable, should be harmless to comment it
80  unsigned u;
81  float f;
82 } Vp32suf;
83 #endif
84 #endif
85 
86 const double vpMath::ang_min_sinc = 1.0e-8;
87 const double vpMath::ang_min_mc = 2.5e-4;
88 
94 bool vpMath::isNaN(double value)
95 {
96 #if defined(VISP_HAVE_FUNC_STD_ISNAN)
97  return std::isnan(value);
98 #elif defined(VISP_HAVE_FUNC_ISNAN)
99  return isnan(value);
100 #elif defined(VISP_HAVE_FUNC__ISNAN)
101  return (_isnan(value) != 0);
102 #else
103  // Taken from OpenCV source code CvIsNan()
104  Vp64suf ieee754;
105  ieee754.f = value;
106  return (((unsigned)(ieee754.u >> 32) & 0x7fffffff) + ((unsigned)ieee754.u != 0) > 0x7ff00000) != 0;
107 #endif
108 }
109 
115 bool vpMath::isNaN(float value)
116 {
117 #if defined(VISP_HAVE_FUNC_STD_ISNAN)
118  return std::isnan(value);
119 #elif defined(VISP_HAVE_FUNC_ISNAN)
120  return isnan(value);
121 #elif defined(VISP_HAVE_FUNC__ISNAN)
122  return (_isnan(value) != 0);
123 #else
124  // Taken from OpenCV source code CvIsNan()
125  Vp32suf ieee754;
126  ieee754.f = value;
127  return ((unsigned)ieee754.u & 0x7fffffff) > 0x7f800000;
128 #endif
129 }
130 
138 bool vpMath::isInf(double value)
139 {
140 #if defined(VISP_HAVE_FUNC_STD_ISINF)
141  return std::isinf(value);
142 #elif defined(VISP_HAVE_FUNC_ISINF)
143  return isinf(value);
144 #else
145  // Taken from OpenCV source code CvIsInf()
146  Vp64suf ieee754;
147  ieee754.f = value;
148  return ((unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 && (unsigned)ieee754.u == 0;
149 #endif
150 }
151 
159 bool vpMath::isInf(float value)
160 {
161 #if defined(VISP_HAVE_FUNC_STD_ISINF)
162  return std::isinf(value);
163 #elif defined(VISP_HAVE_FUNC_ISINF)
164  return isinf(value);
165 #else
166  // Taken from OpenCV source code CvIsInf()
167  Vp32suf ieee754;
168  ieee754.f = value;
169  return ((unsigned)ieee754.u & 0x7fffffff) == 0x7f800000;
170 #endif
171 }
172 
179 bool vpMath::isFinite(double value)
180 {
181 #if defined(VISP_HAVE_FUNC_STD_ISFINITE)
182  return std::isfinite(value);
183 #elif defined(VISP_HAVE_FUNC_ISFINITE)
184  return isfinite(value);
185 #elif defined(VISP_HAVE_FUNC__FINITE)
186  return _finite(value);
187 #else
188  return !vpMath::isInf(value) && !vpMath::isNaN(value);
189 #endif
190 }
191 
198 bool vpMath::isFinite(float value)
199 {
200 #if defined(VISP_HAVE_FUNC_STD_ISFINITE)
201  return std::isfinite(value);
202 #elif defined(VISP_HAVE_FUNC_ISFINITE)
203  return isfinite(value);
204 #elif defined(VISP_HAVE_FUNC__FINITE)
205  return _finitef(value);
206 #else
207  return !vpMath::isInf(value) && !vpMath::isNaN(value);
208 #endif
209 }
210 
216 bool vpMath::isNumber(const std::string &str)
217 {
218  size_t str_size = str.size();
219  for (size_t i = 0; i < str_size; ++i) {
220  if (isdigit(str[i]) == false) {
221  return false;
222  }
223  }
224  return true;
225 }
226 
235 double vpMath::mcosc(double cosx, double x)
236 {
237  if (fabs(x) < ang_min_mc) {
238  return 0.5;
239  }
240  else {
241  return ((1.0 - cosx) / x / x);
242  }
243 }
244 
253 double vpMath::msinc(double sinx, double x)
254 {
255  if (fabs(x) < ang_min_mc) {
256  return (1. / 6.0);
257  }
258  else {
259  return ((1.0 - (sinx / x)) / x / x);
260  }
261 }
262 
270 double vpMath::sinc(double x)
271 {
272  if (fabs(x) < ang_min_sinc) {
273  return 1.0;
274  }
275  else {
276  return sin(x) / x;
277  }
278 }
287 double vpMath::sinc(double sinx, double x)
288 {
289  if (fabs(x) < ang_min_sinc) {
290  return 1.0;
291  }
292  else {
293  return (sinx / x);
294  }
295 }
296 
304 double vpMath::getMean(const std::vector<double> &v)
305 {
306  if (v.empty()) {
307  throw vpException(vpException::notInitialized, "Empty vector !");
308  }
309 
310  size_t size = v.size();
311 
312  double sum = std::accumulate(v.begin(), v.end(), 0.0);
313 
314  return sum / (static_cast<double>(size));
315 }
316 
324 double vpMath::getMedian(const std::vector<double> &v)
325 {
326  if (v.empty()) {
327  throw vpException(vpException::notInitialized, "Empty vector !");
328  }
329 
330  std::vector<double> v_copy = v;
331  size_t size = v_copy.size();
332 
333  size_t n = size / 2;
334  std::nth_element(v_copy.begin(), v_copy.begin() + n, v_copy.end());
335  double val_n = v_copy[n];
336 
337  if ((size % 2) == 1) {
338  return val_n;
339  }
340  else {
341  std::nth_element(v_copy.begin(), v_copy.begin() + (n - 1), v_copy.end());
342  return 0.5 * (val_n + v_copy[n - 1]);
343  }
344 }
345 
355 double vpMath::getStdev(const std::vector<double> &v, bool useBesselCorrection)
356 {
357  if (v.empty()) {
358  throw vpException(vpException::notInitialized, "Empty vector !");
359  }
360 
361  double mean = getMean(v);
362 
363  std::vector<double> diff(v.size());
364 #if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98
365  std::transform(v.begin(), v.end(), diff.begin(), std::bind(std::minus<double>(), std::placeholders::_1, mean));
366 #else
367  std::transform(v.begin(), v.end(), diff.begin(), std::bind2nd(std::minus<double>(), mean));
368 #endif
369 
370  double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
371  double divisor = static_cast<double> (v.size());
372  if (useBesselCorrection && (v.size() > 1)) {
373  divisor = divisor - 1;
374  }
375 
376  return std::sqrt(sq_sum / divisor);
377 }
378 
392 double vpMath::lineFitting(const std::vector<vpImagePoint> &imPts, double &a, double &b, double &c)
393 {
394  if (imPts.size() < 3) {
395  throw vpException(vpException::dimensionError, "Number of image points must be greater or equal to 3.");
396  }
397 
398  double x_mean = 0, y_mean = 0;
399  size_t imPts_size = imPts.size();
400  for (size_t i = 0; i < imPts_size; ++i) {
401  const vpImagePoint &imPt = imPts[i];
402  x_mean += imPt.get_u();
403  y_mean += imPt.get_v();
404  }
405  x_mean /= imPts.size();
406  y_mean /= imPts.size();
407 
408  vpMatrix AtA(2, 2, 0.0);
409  imPts_size = imPts.size();
410  for (size_t i = 0; i < imPts_size; ++i) {
411  const vpImagePoint &imPt = imPts[i];
412  AtA[0][0] += (imPt.get_u() - x_mean) * (imPt.get_u() - x_mean);
413  AtA[0][1] += (imPt.get_u() - x_mean) * (imPt.get_v() - y_mean);
414  AtA[1][1] += (imPt.get_v() - y_mean) * (imPt.get_v() - y_mean);
415  }
416  AtA[1][0] = AtA[0][1];
417 
418  vpColVector eigenvalues;
419  vpMatrix eigenvectors;
420  AtA.eigenValues(eigenvalues, eigenvectors);
421 
422  a = eigenvectors[0][0];
423  b = eigenvectors[1][0];
424  c = (a * x_mean) + (b * y_mean);
425 
426  double error = 0;
427  imPts_size = imPts.size();
428  for (size_t i = 0; i < imPts_size; ++i) {
429  double x0 = imPts[i].get_u();
430  double y0 = imPts[i].get_v();
431 
432  error += std::fabs((a * x0) + ((b * y0) - c));
433  }
434 
435  return error / imPts.size();
436 }
437 
448 int vpMath::modulo(int a, int n) { return ((a % n) + n) % n; }
449 
488 vpHomogeneousMatrix vpMath::ned2ecef(double lonDeg, double latDeg, double radius)
489 {
490  double lon = vpMath::rad(lonDeg);
491  double lat = vpMath::rad(latDeg);
492 
493  vpHomogeneousMatrix ecef_M_ned;
494  ecef_M_ned[0][0] = -sin(lat) * cos(lon);
495  ecef_M_ned[0][1] = -sin(lon);
496  ecef_M_ned[0][2] = -cos(lat) * cos(lon);
497  ecef_M_ned[0][3] = radius * cos(lon) * cos(lat);
498  ecef_M_ned[1][0] = -sin(lat) * sin(lon);
499  ecef_M_ned[1][1] = cos(lon);
500  ecef_M_ned[1][2] = -cos(lat) * sin(lon);
501  ecef_M_ned[1][3] = radius * sin(lon) * cos(lat);
502  ecef_M_ned[2][0] = cos(lat);
503  ecef_M_ned[2][1] = 0;
504  ecef_M_ned[2][2] = -sin(lat);
505  ecef_M_ned[2][3] = radius * sin(lat);
506 
507  return ecef_M_ned;
508 }
509 
549 vpHomogeneousMatrix vpMath::enu2ecef(double lonDeg, double latDeg, double radius)
550 {
551  double lon = vpMath::rad(lonDeg);
552  double lat = vpMath::rad(latDeg);
553 
554  vpHomogeneousMatrix ecef_M_enu;
555  ecef_M_enu[0][0] = -sin(lon);
556  ecef_M_enu[0][1] = -sin(lat) * cos(lon);
557  ecef_M_enu[0][2] = cos(lat) * cos(lon);
558  ecef_M_enu[0][3] = radius * cos(lon) * cos(lat);
559  ecef_M_enu[1][0] = cos(lon);
560  ecef_M_enu[1][1] = -sin(lat) * sin(lon);
561  ecef_M_enu[1][2] = cos(lat) * sin(lon);
562  ecef_M_enu[1][3] = radius * sin(lon) * cos(lat);
563  ecef_M_enu[2][0] = 0;
564  ecef_M_enu[2][1] = cos(lat);
565  ecef_M_enu[2][2] = sin(lat);
566  ecef_M_enu[2][3] = radius * sin(lat);
567 
568  return ecef_M_enu;
569 }
570 
585 std::vector<std::pair<double, double> > vpMath::computeRegularPointsOnSphere(unsigned int maxPoints)
586 {
587  assert(maxPoints > 0);
588 
589  double a = (4.0 * M_PI) / maxPoints;
590  double d = sqrt(a);
591  int m_theta = static_cast<int>(round(M_PI / d));
592  double d_theta = M_PI / m_theta;
593  double d_phi = a / d_theta;
594 
595  std::vector<std::pair<double, double> > lonlat_vec;
596 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
597  lonlat_vec.reserve(static_cast<unsigned int>(std::sqrt(maxPoints)));
598 #else
599  lonlat_vec.reserve(static_cast<unsigned int>(std::sqrt(static_cast<double>(maxPoints))));
600 #endif
601 
602  for (int m = 0; m < m_theta; ++m) {
603  double theta = (M_PI * (m + 0.5)) / m_theta;
604  int m_phi = static_cast<int>(round((2.0 * M_PI * sin(theta)) / d_phi));
605 
606  for (int n = 0; n < m_phi; ++n) {
607  double phi = (2.0 * M_PI * n) / m_phi;
608  double lon = phi;
609  double lat = M_PI_2 - theta;
610  lonlat_vec.push_back(std::make_pair(deg(lon), deg(lat)));
611  }
612  }
613 
614  return lonlat_vec;
615 }
616 
636 std::vector<vpHomogeneousMatrix> vpMath::getLocalTangentPlaneTransformations(const std::vector<std::pair<double, double> > &lonlatVec, double radius,
637  vpHomogeneousMatrix(*toECEF)(double lonDeg_, double latDeg_, double radius_))
638 {
639  std::vector<vpHomogeneousMatrix> ecef_M_local_vec;
640  ecef_M_local_vec.reserve(lonlatVec.size());
641  size_t lonlatVec_size = lonlatVec.size();
642  for (size_t i = 0; i < lonlatVec_size; ++i) {
643  double lonDeg = lonlatVec[i].first;
644  double latDeg = lonlatVec[i].second;
645 
646  vpHomogeneousMatrix ecef_M_local = toECEF(lonDeg, latDeg, radius);
647  ecef_M_local_vec.push_back(ecef_M_local);
648  }
649  return ecef_M_local_vec;
650 }
651 
673 {
674  assert(from.size() == 3);
675  assert(to.size() == 3);
676  assert(tmp.size() == 3);
677  vpColVector forward = (from - to).normalize();
678  vpColVector right = vpColVector::crossProd(tmp.normalize(), forward).normalize();
679  vpColVector up = vpColVector::crossProd(forward, right).normalize();
680 
682  wMc[0][0] = right[0];
683  wMc[0][1] = up[0];
684  wMc[0][2] = forward[0];
685  wMc[0][3] = from[0];
686  wMc[1][0] = right[1];
687  wMc[1][1] = up[1];
688  wMc[1][2] = forward[1];
689  wMc[1][3] = from[1];
690  wMc[2][0] = right[2];
691  wMc[2][1] = up[2];
692  wMc[2][2] = forward[2];
693  wMc[2][3] = from[2];
694 
695  return wMc;
696 }
697 
705 {
706  if (r.size() == 4) {
707  throw(vpException(vpException::fatalError, "Cannot convert angles of a quaternion vector in degrees!"));
708  }
709  vpColVector r_deg(r.size());
710  unsigned int r_size = 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_deg(r.size());
726  unsigned int r_size = r.size();
727  for (unsigned int i = 0; i < r_size; ++i) {
728  r_deg[i] = vpMath::deg(r[i]);
729  }
730  return r_deg;
731 }
732 
740 {
741  vpColVector r_rad(r.size());
742  unsigned int r_size = r.size();
743  for (unsigned int i = 0; i < r_size; ++i) {
744  r_rad[i] = vpMath::rad(r[i]);
745  }
746  return r_rad;
747 }
748 
755 {
756  vpHomogeneousMatrix ned_M_enu;
757  ned_M_enu[0][0] = 0;
758  ned_M_enu[0][1] = 1;
759  ned_M_enu[1][0] = 1;
760  ned_M_enu[1][1] = 0;
761  ned_M_enu[2][2] = -1;
762 
763  vpHomogeneousMatrix ned_M = ned_M_enu * enu_M;
764  return ned_M;
765 }
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:342
Implementation of column vector and the associated operations.
Definition: vpColVector.h:171
vpColVector & normalize()
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:74
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ fatalError
Fatal error.
Definition: vpException.h:72
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:253
static bool isNaN(double value)
Definition: vpMath.cpp:94
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:636
static double rad(double deg)
Definition: vpMath.h:129
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:324
static double sinc(double x)
Definition: vpMath.cpp:270
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:355
static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp)
Definition: vpMath.cpp:672
static vpHomogeneousMatrix enu2ecef(double lonDeg, double latDeg, double radius)
Definition: vpMath.cpp:549
static double lineFitting(const std::vector< vpImagePoint > &imPts, double &a, double &b, double &c)
Definition: vpMath.cpp:392
static int round(double x)
Definition: vpMath.h:407
static bool isFinite(double value)
Definition: vpMath.cpp:179
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:304
static bool isInf(double value)
Definition: vpMath.cpp:138
static vpHomogeneousMatrix enu2ned(const vpHomogeneousMatrix &enu_M)
Definition: vpMath.cpp:754
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:235
static double deg(double rad)
Definition: vpMath.h:119
static bool isNumber(const std::string &str)
Definition: vpMath.cpp:216
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:177
static std::vector< std::pair< double, double > > computeRegularPointsOnSphere(unsigned int maxPoints)
Definition: vpMath.cpp:585
static vpHomogeneousMatrix ned2ecef(double lonDeg, double latDeg, double radius)
Definition: vpMath.cpp:488
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:151
vpColVector eigenValues() const
Definition: vpMatrix.cpp:5819
Implementation of a generic rotation vector.