Visual Servoing Platform  version 3.5.1 under development (2023-03-14)
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  *****************************************************************************/
35 
42 #include <cmath>
43 #include <functional>
44 #include <numeric>
45 #include <stdint.h>
46 #include <cassert>
47 
48 #include <visp3/core/vpException.h>
49 #include <visp3/core/vpMath.h>
50 #include <visp3/core/vpMatrix.h>
51 
52 #if defined(VISP_HAVE_FUNC__FINITE)
53 #include <float.h>
54 #endif
55 
56 #if !(defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN)) || \
57  !(defined(VISP_HAVE_FUNC_ISINF) || defined(VISP_HAVE_FUNC_STD_ISINF)) || \
58  !(defined(VISP_HAVE_FUNC_ISFINITE) || defined(VISP_HAVE_FUNC_STD_ISFINITE) || defined(VISP_HAVE_FUNC__FINITE))
59 #if defined _MSC_VER || defined __BORLANDC__
60 typedef __int64 int64;
61 typedef unsigned __int64 uint64;
62 #else
63 typedef int64_t int64;
64 typedef uint64_t uint64;
65 #endif
66 
67 #ifndef DOXYGEN_SHOULD_SKIP_THIS
68 typedef union Vp64suf {
69  // int64 i; //Unused variable, should be harmless to comment it
70  uint64 u;
71  double f;
72 } Vp64suf;
73 
74 typedef union Vp32suf {
75  // int i; //Unused variable, should be harmless to comment it
76  unsigned u;
77  float f;
78 } Vp32suf;
79 #endif
80 #endif
81 
82 const double vpMath::ang_min_sinc = 1.0e-8;
83 const double vpMath::ang_min_mc = 2.5e-4;
84 
90 bool vpMath::isNaN(double value)
91 {
92 #if defined(VISP_HAVE_FUNC_ISNAN)
93  return isnan(value);
94 #elif defined(VISP_HAVE_FUNC_STD_ISNAN)
95  return std::isnan(value);
96 #elif defined(VISP_HAVE_FUNC__ISNAN)
97  return (_isnan(value) != 0);
98 #else
99  // Taken from OpenCV source code CvIsNan()
100  Vp64suf ieee754;
101  ieee754.f = value;
102  return (((unsigned)(ieee754.u >> 32) & 0x7fffffff) + ((unsigned)ieee754.u != 0) > 0x7ff00000) != 0;
103 #endif
104 }
105 
111 bool vpMath::isNaN(float value)
112 {
113 #if defined(VISP_HAVE_FUNC_ISNAN)
114  return isnan(value);
115 #elif defined(VISP_HAVE_FUNC_STD_ISNAN)
116  return std::isnan(value);
117 #elif defined(VISP_HAVE_FUNC__ISNAN)
118  return (_isnan(value) != 0);
119 #else
120  // Taken from OpenCV source code CvIsNan()
121  Vp32suf ieee754;
122  ieee754.f = value;
123  return ((unsigned)ieee754.u & 0x7fffffff) > 0x7f800000;
124 #endif
125 }
126 
134 bool vpMath::isInf(double value)
135 {
136 #if defined(VISP_HAVE_FUNC_ISINF)
137  return isinf(value);
138 #elif defined(VISP_HAVE_FUNC_STD_ISINF)
139  return std::isinf(value);
140 #else
141  // Taken from OpenCV source code CvIsInf()
142  Vp64suf ieee754;
143  ieee754.f = value;
144  return ((unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 && (unsigned)ieee754.u == 0;
145 #endif
146 }
147 
155 bool vpMath::isInf(float value)
156 {
157 #if defined(VISP_HAVE_FUNC_ISINF)
158  return isinf(value);
159 #elif defined(VISP_HAVE_FUNC_STD_ISINF)
160  return std::isinf(value);
161 #else
162  // Taken from OpenCV source code CvIsInf()
163  Vp32suf ieee754;
164  ieee754.f = value;
165  return ((unsigned)ieee754.u & 0x7fffffff) == 0x7f800000;
166 #endif
167 }
168 
175 bool vpMath::isFinite(double value)
176 {
177 #if defined(VISP_HAVE_FUNC_ISFINITE)
178  return isfinite(value);
179 #elif defined(VISP_HAVE_FUNC_STD_ISFINITE)
180  return std::isfinite(value);
181 #elif defined(VISP_HAVE_FUNC__FINITE)
182  return _finite(value);
183 #else
184  return !vpMath::isInf(value) && !vpMath::isNaN(value);
185 #endif
186 }
187 
194 bool vpMath::isFinite(float value)
195 {
196 #if defined(VISP_HAVE_FUNC_ISFINITE)
197  return isfinite(value);
198 #elif defined(VISP_HAVE_FUNC_STD_ISFINITE)
199  return std::isfinite(value);
200 #elif defined(VISP_HAVE_FUNC__FINITE)
201  return _finitef(value);
202 #else
203  return !vpMath::isInf(value) && !vpMath::isNaN(value);
204 #endif
205 }
206 
215 double vpMath::mcosc(double cosx, double x)
216 {
217  if (fabs(x) < ang_min_mc)
218  return 0.5;
219  else
220  return ((1.0 - cosx) / x / x);
221 }
222 
231 double vpMath::msinc(double sinx, double x)
232 {
233  if (fabs(x) < ang_min_mc)
234  return (1. / 6.0);
235  else
236  return ((1.0 - sinx / x) / x / x);
237 }
238 
246 double vpMath::sinc(double x)
247 {
248  if (fabs(x) < ang_min_sinc)
249  return 1.0;
250  else
251  return sin(x) / x;
252 }
261 double vpMath::sinc(double sinx, double x)
262 {
263  if (fabs(x) < ang_min_sinc)
264  return 1.0;
265  else
266  return (sinx / x);
267 }
268 
276 double vpMath::getMean(const std::vector<double> &v)
277 {
278  if (v.empty()) {
279  throw vpException(vpException::notInitialized, "Empty vector !");
280  }
281 
282  size_t size = v.size();
283 
284  double sum = std::accumulate(v.begin(), v.end(), 0.0);
285 
286  return sum / (double)size;
287 }
288 
296 double vpMath::getMedian(const std::vector<double> &v)
297 {
298  if (v.empty()) {
299  throw vpException(vpException::notInitialized, "Empty vector !");
300  }
301 
302  std::vector<double> v_copy = v;
303  size_t size = v_copy.size();
304 
305  size_t n = size / 2;
306  std::nth_element(v_copy.begin(), v_copy.begin() + n, v_copy.end());
307  double val_n = v_copy[n];
308 
309  if (size % 2 == 1) {
310  return val_n;
311  } else {
312  std::nth_element(v_copy.begin(), v_copy.begin() + n - 1, v_copy.end());
313  return 0.5 * (val_n + v_copy[n - 1]);
314  }
315 }
316 
326 double vpMath::getStdev(const std::vector<double> &v, bool useBesselCorrection)
327 {
328  if (v.empty()) {
329  throw vpException(vpException::notInitialized, "Empty vector !");
330  }
331 
332  double mean = getMean(v);
333 
334  std::vector<double> diff(v.size());
335 #if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98
336  std::transform(v.begin(), v.end(), diff.begin(), std::bind(std::minus<double>(), std::placeholders::_1, mean));
337 #else
338  std::transform(v.begin(), v.end(), diff.begin(), std::bind2nd(std::minus<double>(), mean));
339 #endif
340 
341  double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
342  double divisor = (double)v.size();
343  if (useBesselCorrection && v.size() > 1) {
344  divisor = divisor - 1;
345  }
346 
347  return std::sqrt(sq_sum / divisor);
348 }
349 
363 double vpMath::lineFitting(const std::vector<vpImagePoint> &imPts, double &a, double &b, double &c)
364 {
365  if (imPts.size() < 3) {
366  throw vpException(vpException::dimensionError, "Number of image points must be greater or equal to 3.");
367  }
368 
369  double x_mean = 0, y_mean = 0;
370  for (size_t i = 0; i < imPts.size(); i++) {
371  const vpImagePoint &imPt = imPts[i];
372  x_mean += imPt.get_u();
373  y_mean += imPt.get_v();
374  }
375  x_mean /= imPts.size();
376  y_mean /= imPts.size();
377 
378  vpMatrix AtA(2, 2, 0.0);
379  for (size_t i = 0; i < imPts.size(); i++) {
380  const vpImagePoint &imPt = imPts[i];
381  AtA[0][0] += (imPt.get_u() - x_mean) * (imPt.get_u() - x_mean);
382  AtA[0][1] += (imPt.get_u() - x_mean) * (imPt.get_v() - y_mean);
383  AtA[1][1] += (imPt.get_v() - y_mean) * (imPt.get_v() - y_mean);
384  }
385  AtA[1][0] = AtA[0][1];
386 
387  vpColVector eigenvalues;
388  vpMatrix eigenvectors;
389  AtA.eigenValues(eigenvalues, eigenvectors);
390 
391  a = eigenvectors[0][0];
392  b = eigenvectors[1][0];
393  c = a * x_mean + b * y_mean;
394 
395  double error = 0;
396  for (size_t i = 0; i < imPts.size(); i++) {
397  double x0 = imPts[i].get_u();
398  double y0 = imPts[i].get_v();
399 
400  error += std::fabs(a * x0 + b * y0 - c);
401  }
402 
403  return error / imPts.size();
404 }
405 
416 int vpMath::modulo(int a, int n) { return ((a % n) + n) % n; }
417 
456 vpHomogeneousMatrix vpMath::ned2ecef(double lonDeg, double latDeg, double radius)
457 {
458  double lon = vpMath::rad(lonDeg);
459  double lat = vpMath::rad(latDeg);
460 
461  vpHomogeneousMatrix ecef_M_ned;
462  ecef_M_ned[0][0] = -sin(lat)*cos(lon); ecef_M_ned[0][1] = -sin(lon); ecef_M_ned[0][2] = -cos(lat)*cos(lon); ecef_M_ned[0][3] = radius*cos(lon)*cos(lat);
463  ecef_M_ned[1][0] = -sin(lat)*sin(lon); ecef_M_ned[1][1] = cos(lon); ecef_M_ned[1][2] = -cos(lat)*sin(lon); ecef_M_ned[1][3] = radius*sin(lon)*cos(lat);
464  ecef_M_ned[2][0] = cos(lat); ecef_M_ned[2][1] = 0; ecef_M_ned[2][2] = -sin(lat); ecef_M_ned[2][3] = radius*sin(lat);
465 
466  return ecef_M_ned;
467 }
468 
508 vpHomogeneousMatrix vpMath::enu2ecef(double lonDeg, double latDeg, double radius)
509 {
510  double lon = vpMath::rad(lonDeg);
511  double lat = vpMath::rad(latDeg);
512 
513  vpHomogeneousMatrix ecef_M_enu;
514  ecef_M_enu[0][0] = -sin(lon); ecef_M_enu[0][1] = -sin(lat)*cos(lon); ecef_M_enu[0][2] = cos(lat)*cos(lon); ecef_M_enu[0][3] = radius*cos(lon)*cos(lat);
515  ecef_M_enu[1][0] = cos(lon); ecef_M_enu[1][1] = -sin(lat)*sin(lon); ecef_M_enu[1][2] = cos(lat)*sin(lon); ecef_M_enu[1][3] = radius*sin(lon)*cos(lat);
516  ecef_M_enu[2][0] = 0; ecef_M_enu[2][1] = cos(lat); ecef_M_enu[2][2] = sin(lat); ecef_M_enu[2][3] = radius*sin(lat);
517 
518  return ecef_M_enu;
519 }
520 
535 std::vector<std::pair<double, double> > vpMath::computeRegularPointsOnSphere(unsigned int maxPoints)
536 {
537  assert(maxPoints > 0);
538 
539  double a = 4.0 * M_PI / maxPoints;
540  double d = sqrt(a);
541  int m_theta = int(round(M_PI / d));
542  double d_theta = M_PI / m_theta;
543  double d_phi = a / d_theta;
544 
545  std::vector<std::pair<double, double> > lonlat_vec;
546 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
547  lonlat_vec.reserve(static_cast<unsigned int>(std::sqrt(maxPoints)));
548 #else
549  lonlat_vec.reserve(static_cast<unsigned int>(std::sqrt(static_cast<double>(maxPoints))));
550 #endif
551  for (int m = 0; m < m_theta; m++) {
552  double theta = M_PI * (m + 0.5) / m_theta;
553  int m_phi = static_cast<int>(round(2.0 * M_PI * sin(theta) / d_phi));
554 
555  for (int n = 0; n < m_phi; n++) {
556  double phi = 2.0 * M_PI * n / m_phi;
557  double lon = phi;
558  double lat = M_PI_2 - theta;
559  lonlat_vec.push_back(std::make_pair(deg(lon), deg(lat)));
560  }
561  }
562 
563  return lonlat_vec;
564 }
565 
585 std::vector<vpHomogeneousMatrix> vpMath::getLocalTangentPlaneTransformations(const std::vector<std::pair<double, double> > &lonlatVec, double radius,
586  vpHomogeneousMatrix (*toECEF)(double lonDeg_, double latDeg_, double radius_))
587 {
588  std::vector<vpHomogeneousMatrix> ecef_M_local_vec;
589  ecef_M_local_vec.reserve(lonlatVec.size());
590  for (size_t i = 0; i < lonlatVec.size(); i++) {
591  double lonDeg = lonlatVec[i].first;
592  double latDeg = lonlatVec[i].second;
593 
594  vpHomogeneousMatrix ecef_M_local = toECEF(lonDeg, latDeg, radius);
595  ecef_M_local_vec.push_back(ecef_M_local);
596  }
597  return ecef_M_local_vec;
598 }
599 
621 {
622  assert(from.size() == 3);
623  assert(to.size() == 3);
624  assert(tmp.size() == 3);
625  vpColVector forward = (from - to).normalize();
626  vpColVector right = vpColVector::crossProd(tmp.normalize(), forward).normalize();
627  vpColVector up = vpColVector::crossProd(forward, right).normalize();
628 
630  wMc[0][0] = right[0]; wMc[0][1] = up[0]; wMc[0][2] = forward[0]; wMc[0][3] = from[0];
631  wMc[1][0] = right[1]; wMc[1][1] = up[1]; wMc[1][2] = forward[1]; wMc[1][3] = from[1];
632  wMc[2][0] = right[2]; wMc[2][1] = up[2]; wMc[2][2] = forward[2]; wMc[2][3] = from[2];
633 
634  return wMc;
635 }
636 
644 {
645  if (r.size() == 4) {
646  throw(vpException(vpException::fatalError, "Cannot convert angles of a quaternion vector in degrees!"));
647  }
648  vpColVector r_deg(r.size());
649  for (unsigned int i = 0; i < r.size(); i++) {
650  r_deg[i] = vpMath::deg(r[i]);
651  }
652  return r_deg;
653 }
654 
662 {
663  vpColVector r_deg(r.size());
664  for (unsigned int i = 0; i < r.size(); i++) {
665  r_deg[i] = vpMath::deg(r[i]);
666  }
667  return r_deg;
668 }
669 
677 {
678  vpColVector ned(3);
679  ned[0] = enu[0];
680  ned[1] = -enu[1];
681  ned[2] = -enu[2] + M_PI_2;
682  return ned;
683 }
684 
692 {
693  vpColVector ned(3);
694  ned[0] = enu[1];
695  ned[1] = enu[0];
696  ned[2] = -enu[2];
697  return ned;
698 }
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:290
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpColVector & normalize()
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
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
@ fatalError
Fatal error.
Definition: vpException.h:96
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:89
double get_u() const
Definition: vpImagePoint.h:143
double get_v() const
Definition: vpImagePoint.h:154
static double msinc(double sinx, double x)
Definition: vpMath.cpp:231
static bool isNaN(double value)
Definition: vpMath.cpp:90
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:585
static double rad(double deg)
Definition: vpMath.h:121
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:296
static double sinc(double x)
Definition: vpMath.cpp:246
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:326
static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp)
Definition: vpMath.cpp:620
static int modulo(int a, int n)
Definition: vpMath.cpp:416
static vpHomogeneousMatrix enu2ecef(double lonDeg, double latDeg, double radius)
Definition: vpMath.cpp:508
static double lineFitting(const std::vector< vpImagePoint > &imPts, double &a, double &b, double &c)
Definition: vpMath.cpp:363
static int round(double x)
Definition: vpMath.h:326
static bool isFinite(double value)
Definition: vpMath.cpp:175
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:276
static bool isInf(double value)
Definition: vpMath.cpp:134
static vpColVector enu2ned(const vpRxyzVector &rxyz)
Definition: vpMath.cpp:676
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:215
static double deg(double rad)
Definition: vpMath.h:111
static std::vector< std::pair< double, double > > computeRegularPointsOnSphere(unsigned int maxPoints)
Definition: vpMath.cpp:535
static vpHomogeneousMatrix ned2ecef(double lonDeg, double latDeg, double radius)
Definition: vpMath.cpp:456
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpColVector eigenValues() const
Definition: vpMatrix.cpp:6022
Implementation of a generic rotation vector.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:184
Class that consider the case of a translation vector.