Visual Servoing Platform  version 3.5.1 under development (2022-07-07)
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 #include <cassert>
50 
51 #include <visp3/core/vpException.h>
52 #include <visp3/core/vpMath.h>
53 #include <visp3/core/vpMatrix.h>
54 
55 #if defined(VISP_HAVE_FUNC__ISNAN)
56 #include <float.h>
57 #endif
58 
59 #if !(defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN)) || \
60  !(defined(VISP_HAVE_FUNC_ISINF) || defined(VISP_HAVE_FUNC_STD_ISINF))
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 Cv64suf {
71  // int64 i; //Unused variable, should be harmless to comment it
72  uint64 u;
73  double f;
74 } Cv64suf;
75 #endif
76 #endif
77 
78 const double vpMath::ang_min_sinc = 1.0e-8;
79 const double vpMath::ang_min_mc = 2.5e-4;
80 
86 bool vpMath::isNaN(double value)
87 {
88 #if defined(VISP_HAVE_FUNC_ISNAN)
89  return isnan(value);
90 #elif defined(VISP_HAVE_FUNC_STD_ISNAN)
91  return std::isnan(value);
92 #elif defined(VISP_HAVE_FUNC__ISNAN)
93  return (_isnan(value) != 0);
94 #else
95  // Taken from OpenCV source code CvIsNan()
96  Cv64suf ieee754;
97  ieee754.f = value;
98  return (((unsigned)(ieee754.u >> 32) & 0x7fffffff) + ((unsigned)ieee754.u != 0) > 0x7ff00000) != 0;
99 #endif
100 }
101 
107 bool vpMath::isNaN(float value)
108 {
109 #if defined(VISP_HAVE_FUNC_ISNAN)
110  return isnan(value);
111 #elif defined(VISP_HAVE_FUNC_STD_ISNAN)
112  return std::isnan(value);
113 #elif defined(VISP_HAVE_FUNC__ISNAN)
114  return (_isnan(value) != 0);
115 #else
116  // Taken from OpenCV source code CvIsNan()
117  Cv32suf ieee754;
118  ieee754.f = value;
119  return ((unsigned)ieee754.u & 0x7fffffff) > 0x7f800000;
120 #endif
121 }
122 
130 bool vpMath::isInf(double value)
131 {
132 #if defined(VISP_HAVE_FUNC_ISINF)
133  return isinf(value);
134 #elif defined(VISP_HAVE_FUNC_STD_ISINF)
135  return std::isinf(value);
136 #elif defined(VISP_HAVE_FUNC__FINITE)
137  return !_finite(value);
138 #else
139  // Taken from OpenCV source code CvIsInf()
140  Cv64suf ieee754;
141  ieee754.f = value;
142  return ((unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 && (unsigned)ieee754.u == 0;
143 #endif
144 }
145 
153 bool vpMath::isInf(float value)
154 {
155 #if defined(VISP_HAVE_FUNC_ISINF)
156  return isinf(value);
157 #elif defined(VISP_HAVE_FUNC_STD_ISINF)
158  return std::isinf(value);
159 #elif defined(VISP_HAVE_FUNC__FINITE)
160  return !_finite(value);
161 #else
162  // Taken from OpenCV source code CvIsInf()
163  Cv32suf ieee754;
164  ieee754.f = value;
165  return ((unsigned)ieee754.u & 0x7fffffff) == 0x7f800000;
166 #endif
167 }
168 
178 double vpMath::mcosc(double cosx, double x)
179 {
180  if (fabs(x) < ang_min_mc)
181  return 0.5;
182  else
183  return ((1.0 - cosx) / x / x);
184 }
185 
195 double vpMath::msinc(double sinx, double x)
196 {
197  if (fabs(x) < ang_min_mc)
198  return (1. / 6.0);
199  else
200  return ((1.0 - sinx / x) / x / x);
201 }
202 
211 double vpMath::sinc(double x)
212 {
213  if (fabs(x) < ang_min_sinc)
214  return 1.0;
215  else
216  return sin(x) / x;
217 }
227 double vpMath::sinc(double sinx, double x)
228 {
229  if (fabs(x) < ang_min_sinc)
230  return 1.0;
231  else
232  return (sinx / x);
233 }
234 
242 double vpMath::getMean(const std::vector<double> &v)
243 {
244  if (v.empty()) {
245  throw vpException(vpException::notInitialized, "Empty vector !");
246  }
247 
248  size_t size = v.size();
249 
250  double sum = std::accumulate(v.begin(), v.end(), 0.0);
251 
252  return sum / (double)size;
253 }
254 
262 double vpMath::getMedian(const std::vector<double> &v)
263 {
264  if (v.empty()) {
265  throw vpException(vpException::notInitialized, "Empty vector !");
266  }
267 
268  std::vector<double> v_copy = v;
269  size_t size = v_copy.size();
270 
271  size_t n = size / 2;
272  std::nth_element(v_copy.begin(), v_copy.begin() + n, v_copy.end());
273  double val_n = v_copy[n];
274 
275  if (size % 2 == 1) {
276  return val_n;
277  } else {
278  std::nth_element(v_copy.begin(), v_copy.begin() + n - 1, v_copy.end());
279  return 0.5 * (val_n + v_copy[n - 1]);
280  }
281 }
282 
292 double vpMath::getStdev(const std::vector<double> &v, bool useBesselCorrection)
293 {
294  if (v.empty()) {
295  throw vpException(vpException::notInitialized, "Empty vector !");
296  }
297 
298  double mean = getMean(v);
299 
300  std::vector<double> diff(v.size());
301 #if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98
302  std::transform(v.begin(), v.end(), diff.begin(), std::bind(std::minus<double>(), std::placeholders::_1, mean));
303 #else
304  std::transform(v.begin(), v.end(), diff.begin(), std::bind2nd(std::minus<double>(), mean));
305 #endif
306 
307  double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
308  double divisor = (double)v.size();
309  if (useBesselCorrection && v.size() > 1) {
310  divisor = divisor - 1;
311  }
312 
313  return std::sqrt(sq_sum / divisor);
314 }
315 
329 double vpMath::lineFitting(const std::vector<vpImagePoint> &imPts, double &a, double &b, double &c)
330 {
331  if (imPts.size() < 3) {
332  throw vpException(vpException::dimensionError, "Number of image points must be greater or equal to 3.");
333  }
334 
335  double x_mean = 0, y_mean = 0;
336  for (size_t i = 0; i < imPts.size(); i++) {
337  const vpImagePoint &imPt = imPts[i];
338  x_mean += imPt.get_u();
339  y_mean += imPt.get_v();
340  }
341  x_mean /= imPts.size();
342  y_mean /= imPts.size();
343 
344  vpMatrix AtA(2, 2, 0.0);
345  for (size_t i = 0; i < imPts.size(); i++) {
346  const vpImagePoint &imPt = imPts[i];
347  AtA[0][0] += (imPt.get_u() - x_mean) * (imPt.get_u() - x_mean);
348  AtA[0][1] += (imPt.get_u() - x_mean) * (imPt.get_v() - y_mean);
349  AtA[1][1] += (imPt.get_v() - y_mean) * (imPt.get_v() - y_mean);
350  }
351  AtA[1][0] = AtA[0][1];
352 
353  vpColVector eigenvalues;
354  vpMatrix eigenvectors;
355  AtA.eigenValues(eigenvalues, eigenvectors);
356 
357  a = eigenvectors[0][0];
358  b = eigenvectors[1][0];
359  c = a * x_mean + b * y_mean;
360 
361  double error = 0;
362  for (size_t i = 0; i < imPts.size(); i++) {
363  double x0 = imPts[i].get_u();
364  double y0 = imPts[i].get_v();
365 
366  error += std::fabs(a * x0 + b * y0 - c);
367  }
368 
369  return error / imPts.size();
370 }
371 
382 int vpMath::modulo(int a, int n) { return ((a % n) + n) % n; }
383 
422 vpHomogeneousMatrix vpMath::ned2ecef(double lonDeg, double latDeg, double radius)
423 {
424  double lon = vpMath::rad(lonDeg);
425  double lat = vpMath::rad(latDeg);
426 
427  vpHomogeneousMatrix ecef_M_ned;
428  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);
429  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);
430  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);
431 
432  return ecef_M_ned;
433 }
434 
474 vpHomogeneousMatrix vpMath::enu2ecef(double lonDeg, double latDeg, double radius)
475 {
476  double lon = vpMath::rad(lonDeg);
477  double lat = vpMath::rad(latDeg);
478 
479  vpHomogeneousMatrix ecef_M_enu;
480  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);
481  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);
482  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);
483 
484  return ecef_M_enu;
485 }
486 
501 std::vector<std::pair<double, double> > vpMath::computeRegularPointsOnSphere(unsigned int maxPoints)
502 {
503  assert(maxPoints > 0);
504 
505  double a = 4.0 * M_PI / maxPoints;
506  double d = sqrt(a);
507  int m_theta = int(round(M_PI / d));
508  double d_theta = M_PI / m_theta;
509  double d_phi = a / d_theta;
510 
511  std::vector<std::pair<double, double> > lonlat_vec;
512  lonlat_vec.reserve(std::sqrt(maxPoints));
513  for (int m = 0; m < m_theta; m++) {
514  double theta = M_PI * (m + 0.5) / m_theta;
515  int m_phi = static_cast<int>(round(2.0 * M_PI * sin(theta) / d_phi));
516 
517  for (int n = 0; n < m_phi; n++) {
518  double phi = 2.0 * M_PI * n / m_phi;
519  double lon = phi;
520  double lat = M_PI_2 - theta;
521  lonlat_vec.push_back(std::make_pair(deg(lon), deg(lat)));
522  }
523  }
524 
525  return lonlat_vec;
526 }
527 
547 std::vector<vpHomogeneousMatrix> vpMath::getLocalTangentPlaneTransformations(const std::vector<std::pair<double, double> > &lonlatVec, double radius,
548  vpHomogeneousMatrix (*toECEF)(double lonDeg_, double latDeg_, double radius_))
549 {
550  std::vector<vpHomogeneousMatrix> ecef_M_local_vec;
551  ecef_M_local_vec.reserve(lonlatVec.size());
552  for (size_t i = 0; i < lonlatVec.size(); i++) {
553  double lonDeg = lonlatVec[i].first;
554  double latDeg = lonlatVec[i].second;
555 
556  vpHomogeneousMatrix ecef_M_local = toECEF(lonDeg, latDeg, radius);
557  ecef_M_local_vec.push_back(ecef_M_local);
558  }
559  return ecef_M_local_vec;
560 }
561 
583 {
584  assert(from.size() == 3);
585  assert(to.size() == 3);
586  assert(tmp.size() == 3);
587  vpColVector forward = (from - to).normalize();
588  vpColVector right = vpColVector::crossProd(tmp.normalize(), forward).normalize();
589  vpColVector up = vpColVector::crossProd(forward, right).normalize();
590 
592  wMc[0][0] = right[0]; wMc[0][1] = up[0]; wMc[0][2] = forward[0]; wMc[0][3] = from[0];
593  wMc[1][0] = right[1]; wMc[1][1] = up[1]; wMc[1][2] = forward[1]; wMc[1][3] = from[1];
594  wMc[2][0] = right[2]; wMc[2][1] = up[2]; wMc[2][2] = forward[2]; wMc[2][3] = from[2];
595 
596  return wMc;
597 }
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:293
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
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:195
static bool isNaN(double value)
Definition: vpMath.cpp:86
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:547
static double rad(double deg)
Definition: vpMath.h:117
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:262
static double sinc(double x)
Definition: vpMath.cpp:211
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:292
static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp)
Definition: vpMath.cpp:582
static int modulo(int a, int n)
Definition: vpMath.cpp:382
static vpHomogeneousMatrix enu2ecef(double lonDeg, double latDeg, double radius)
Definition: vpMath.cpp:474
static double lineFitting(const std::vector< vpImagePoint > &imPts, double &a, double &b, double &c)
Definition: vpMath.cpp:329
static int round(double x)
Definition: vpMath.h:318
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:242
static bool isInf(double value)
Definition: vpMath.cpp:130
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:178
static double deg(double rad)
Definition: vpMath.h:110
static std::vector< std::pair< double, double > > computeRegularPointsOnSphere(unsigned int maxPoints)
Definition: vpMath.cpp:501
static vpHomogeneousMatrix ned2ecef(double lonDeg, double latDeg, double radius)
Definition: vpMath.cpp:422
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpColVector eigenValues() const
Definition: vpMatrix.cpp:6032