MRPT  1.9.9
math.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "math-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/CMatrixD.h>
13 #include <mrpt/math/data_utils.h>
15 #include <mrpt/math/ops_matrices.h>
16 #include <mrpt/math/utils.h>
17 #include <mrpt/math/wrap2pi.h>
19 #include <Eigen/Dense>
20 #include <algorithm>
21 #include <cmath> // erf(), ...
22 #include <iostream>
23 #include <mrpt/math/interp_fit.hpp>
24 
25 using namespace mrpt;
26 using namespace mrpt::math;
27 using namespace std;
29 
30 /*---------------------------------------------------------------
31  normalPDF
32  ---------------------------------------------------------------*/
33 double math::normalPDF(double x, double mu, double std)
34 {
35  return ::exp(-0.5 * square((x - mu) / std)) /
36  (std * 2.506628274631000502415765284811);
37 }
38 
39 /*---------------------------------------------------------------
40  chi2inv
41  ---------------------------------------------------------------*/
42 double math::chi2inv(double P, unsigned int dim)
43 {
44  ASSERT_(P >= 0 && P < 1);
45  if (P == 0)
46  return 0;
47  else
48  return dim * pow(1.0 - 2.0 / (9 * dim) +
49  sqrt(2.0 / (9 * dim)) * normalQuantile(P),
50  3);
51 }
52 
53 /*---------------------------------------------------------------
54  factorial64
55  ---------------------------------------------------------------*/
56 uint64_t math::factorial64(unsigned int n)
57 {
58  uint64_t ret = 1;
59 
60  for (unsigned int i = 2; i <= n; i++) ret *= i;
61 
62  return ret;
63 }
64 
65 /*---------------------------------------------------------------
66  factorial
67  ---------------------------------------------------------------*/
68 double math::factorial(unsigned int n)
69 {
70  double retLog = 0;
71 
72  for (unsigned int i = 2; i <= n; i++) retLog += ::log((double)n);
73 
74  return ::exp(retLog);
75 }
76 
77 /*---------------------------------------------------------------
78  normalQuantile
79  ---------------------------------------------------------------*/
80 double math::normalQuantile(double p)
81 {
82  double q, t, u;
83 
84  static const double a[6] = {-3.969683028665376e+01, 2.209460984245205e+02,
85  -2.759285104469687e+02, 1.383577518672690e+02,
86  -3.066479806614716e+01, 2.506628277459239e+00};
87  static const double b[5] = {-5.447609879822406e+01, 1.615858368580409e+02,
88  -1.556989798598866e+02, 6.680131188771972e+01,
89  -1.328068155288572e+01};
90  static const double c[6] = {-7.784894002430293e-03, -3.223964580411365e-01,
91  -2.400758277161838e+00, -2.549732539343734e+00,
92  4.374664141464968e+00, 2.938163982698783e+00};
93  static const double d[4] = {7.784695709041462e-03, 3.224671290700398e-01,
94  2.445134137142996e+00, 3.754408661907416e+00};
95 
96  ASSERT_(!std::isnan(p));
97  ASSERT_(p < 1.0 && p > 0.0);
98 
99  q = min(p, 1 - p);
100 
101  if (q > 0.02425)
102  {
103  /* Rational approximation for central region. */
104  u = q - 0.5;
105  t = u * u;
106  u = u *
107  (((((a[0] * t + a[1]) * t + a[2]) * t + a[3]) * t + a[4]) * t +
108  a[5]) /
109  (((((b[0] * t + b[1]) * t + b[2]) * t + b[3]) * t + b[4]) * t + 1);
110  }
111  else
112  {
113  /* Rational approximation for tail region. */
114  t = sqrt(-2 * ::log(q));
115  u = (((((c[0] * t + c[1]) * t + c[2]) * t + c[3]) * t + c[4]) * t +
116  c[5]) /
117  ((((d[0] * t + d[1]) * t + d[2]) * t + d[3]) * t + 1);
118  }
119 
120  /* The relative error of the approximation has absolute value less
121  than 1.15e-9. One iteration of Halley's rational method (third
122  order) gives full machine precision... */
123  t = normalCDF(u) - q; /* error */
124  t = t * 2.506628274631000502415765284811 *
125  ::exp(u * u / 2); /* f(u)/df(u) */
126  u = u - t / (1 + u * t / 2); /* Halley's method */
127 
128  return (p > 0.5 ? -u : u);
129 }
130 
131 /*---------------------------------------------------------------
132  normalCDF
133  ---------------------------------------------------------------*/
134 double math::normalCDF(double u)
135 {
136  static const double a[5] = {1.161110663653770e-002, 3.951404679838207e-001,
137  2.846603853776254e+001, 1.887426188426510e+002,
138  3.209377589138469e+003};
139  static const double b[5] = {1.767766952966369e-001, 8.344316438579620e+000,
140  1.725514762600375e+002, 1.813893686502485e+003,
141  8.044716608901563e+003};
142  static const double c[9] = {
143  2.15311535474403846e-8, 5.64188496988670089e-1, 8.88314979438837594e00,
144  6.61191906371416295e01, 2.98635138197400131e02, 8.81952221241769090e02,
145  1.71204761263407058e03, 2.05107837782607147e03, 1.23033935479799725E03};
146  static const double d[9] = {
147  1.00000000000000000e00, 1.57449261107098347e01, 1.17693950891312499e02,
148  5.37181101862009858e02, 1.62138957456669019e03, 3.29079923573345963e03,
149  4.36261909014324716e03, 3.43936767414372164e03, 1.23033935480374942e03};
150  static const double p[6] = {1.63153871373020978e-2, 3.05326634961232344e-1,
151  3.60344899949804439e-1, 1.25781726111229246e-1,
152  1.60837851487422766e-2, 6.58749161529837803e-4};
153  static const double q[6] = {1.00000000000000000e00, 2.56852019228982242e00,
154  1.87295284992346047e00, 5.27905102951428412e-1,
155  6.05183413124413191e-2, 2.33520497626869185e-3};
156  double y, z;
157 
158  ASSERT_(!std::isnan(u));
159  ASSERT_(std::isfinite(u));
160 
161  y = fabs(u);
162  if (y <= 0.46875 * 1.4142135623730950488016887242097)
163  {
164  /* evaluate erf() for |u| <= sqrt(2)*0.46875 */
165  z = y * y;
166  y = u * ((((a[0] * z + a[1]) * z + a[2]) * z + a[3]) * z + a[4]) /
167  ((((b[0] * z + b[1]) * z + b[2]) * z + b[3]) * z + b[4]);
168  return 0.5 + y;
169  }
170 
171  z = ::exp(-y * y / 2) / 2;
172  if (y <= 4.0)
173  {
174  /* evaluate erfc() for sqrt(2)*0.46875 <= |u| <= sqrt(2)*4.0 */
175  y = y / 1.4142135623730950488016887242097;
176  y = ((((((((c[0] * y + c[1]) * y + c[2]) * y + c[3]) * y + c[4]) * y +
177  c[5]) *
178  y +
179  c[6]) *
180  y +
181  c[7]) *
182  y +
183  c[8])
184 
185  / ((((((((d[0] * y + d[1]) * y + d[2]) * y + d[3]) * y + d[4]) * y +
186  d[5]) *
187  y +
188  d[6]) *
189  y +
190  d[7]) *
191  y +
192  d[8]);
193 
194  y = z * y;
195  }
196  else
197  {
198  /* evaluate erfc() for |u| > sqrt(2)*4.0 */
199  z = z * 1.4142135623730950488016887242097 / y;
200  y = 2 / (y * y);
201  y = y *
202  (((((p[0] * y + p[1]) * y + p[2]) * y + p[3]) * y + p[4]) * y +
203  p[5]) /
204  (((((q[0] * y + q[1]) * y + q[2]) * y + q[3]) * y + q[4]) * y +
205  q[5]);
206  y = z * (0.564189583547756286948 - y);
207  }
208  return (u < 0.0 ? y : 1 - y);
209 }
210 
211 // Loads a vector from a text file:
212 bool math::loadVector(std::istream& f, ::std::vector<int>& d)
213 {
214  MRPT_START
215 
216  std::string str;
217  if (!std::getline(f, str)) return false;
218 
219  const char* s = str.c_str();
220 
221  char *nextTok, *context;
222  const char* delim = " \t";
223 
224  d.clear();
225  nextTok = mrpt::system::strtok((char*)s, delim, &context);
226  while (nextTok != nullptr)
227  {
228  d.push_back(atoi(nextTok));
229  nextTok = mrpt::system::strtok(nullptr, delim, &context);
230  };
231 
232  return true;
233  MRPT_END
234 }
235 
236 bool math::loadVector(std::istream& f, ::std::vector<double>& d)
237 {
238  MRPT_START
239 
240  std::string str;
241  if (!std::getline(f, str)) return false;
242 
243  const char* s = str.c_str();
244 
245  char *nextTok, *context;
246  const char* delim = " \t";
247 
248  d.clear();
249  nextTok = mrpt::system::strtok((char*)s, delim, &context);
250  while (nextTok != nullptr)
251  {
252  d.push_back(atof(nextTok));
253  nextTok = mrpt::system::strtok(nullptr, delim, &context);
254  };
255 
256  return true;
257  MRPT_END
258 }
259 
260 // See declaration for the documentation
262  const CVectorDouble& logWeights, const CVectorDouble& logLikelihoods)
263 {
264  MRPT_START
265 
266  // Explained in:
267  // https://www.mrpt.org/Averaging_Log-Likelihood_Values:Numerical_Stability
268  ASSERT_(logWeights.size() == logLikelihoods.size());
269 
270  if (!logWeights.size())
271  THROW_EXCEPTION("ERROR: logWeights vector is empty!");
272 
274  double lw_max = math::maximum(logWeights);
275  double ll_max = math::maximum(logLikelihoods);
276  double SUM1 = 0, SUM2 = 0, tmpVal;
277 
278  for (itLW = logWeights.begin(), itLL = logLikelihoods.begin();
279  itLW != logWeights.end(); itLW++, itLL++)
280  {
281  tmpVal = *itLW - lw_max;
282  SUM1 += std::exp(tmpVal);
283  SUM2 += std::exp(tmpVal + *itLL - ll_max);
284  }
285 
286  double res = -std::log(SUM1) + std::log(SUM2) + ll_max;
288  return res;
289  MRPT_END
290 }
291 
292 // Unweighted version:
293 double math::averageLogLikelihood(const CVectorDouble& logLikelihoods)
294 {
295  MRPT_START
296 
297  // Explained in:
298  // https://www.mrpt.org/Averaging_Log-Likelihood_Values:Numerical_Stability
299  size_t N = logLikelihoods.size();
300  if (!N) THROW_EXCEPTION("ERROR: logLikelihoods vector is empty!");
301 
302  double ll_max = math::maximum(logLikelihoods);
303  double SUM1 = 0;
304 
305  for (size_t i = 0; i < N; i++) SUM1 += exp(logLikelihoods[i] - ll_max);
306 
307  double res = log(SUM1) - log(static_cast<double>(N)) + ll_max;
308 
310  return res;
311  MRPT_END
312 }
313 
314 // Wrapped angles average:
315 double math::averageWrap2Pi(const CVectorDouble& angles)
316 {
317  if (angles.empty()) return 0;
318 
319  int W_phi_R = 0, W_phi_L = 0;
320  double phi_R = 0, phi_L = 0;
321 
322  // First: XY
323  // -----------------------------------
324  for (CVectorDouble::Index i = 0; i < angles.size(); i++)
325  {
326  double phi = angles[i];
327  if (abs(phi) > 1.5707963267948966192313216916398)
328  {
329  // LEFT HALF: 0,2pi
330  if (phi < 0) phi = (M_2PI + phi);
331 
332  phi_L += phi;
333  W_phi_L++;
334  }
335  else
336  {
337  // RIGHT HALF: -pi,pi
338  phi_R += phi;
339  W_phi_R++;
340  }
341  }
342 
343  // Next: PHI
344  // -----------------------------------
345  // The mean value from each side:
346  if (W_phi_L) phi_L /= static_cast<double>(W_phi_L); // [0,2pi]
347  if (W_phi_R) phi_R /= static_cast<double>(W_phi_R); // [-pi,pi]
348 
349  // Left side to [-pi,pi] again:
350  if (phi_L > M_PI) phi_L -= M_2PI;
351 
352  // The total mean:
353  return ((phi_L * W_phi_L + phi_R * W_phi_R) / (W_phi_L + W_phi_R));
354 }
355 
357  const CMatrixFloat& cov, const CVectorFloat& mean, float stdCount,
358  const string& style, size_t nEllipsePoints)
359 {
360  MRPT_START
361  const auto cov2 = CMatrixDouble(cov);
362  auto mean2 = CVectorDouble(2);
363  mean2[0] = mean[0];
364  mean2[1] = mean[1];
365 
367  cov2, mean2, stdCount, style, nEllipsePoints);
368 
369  MRPT_END
370 }
371 
373  const CMatrixDouble& cov, const CVectorDouble& mean, float stdCount,
374  const string& style, size_t nEllipsePoints)
375 {
376  MRPT_START
377 
378  ASSERT_(cov.cols() == cov.rows() && cov.cols() == 2);
379  ASSERT_(cov(0, 1) == cov(1, 0));
380  ASSERT_(!((cov(0, 0) == 0) ^ (cov(1, 1) == 0))); // Both or none 0
381  ASSERT_(mean.size() == 2);
382 
383  const auto cov2x2 = mrpt::math::CMatrixFixed<double, 2, 2>(cov.asEigen());
384  const auto N = nEllipsePoints;
385  double ang = 0;
386  const double dAng = (M_2PI / (nEllipsePoints - 1));
387  std::vector<double> X(N), Y(N), Cos(N), Sin(N);
388 
389  // Fill the angles:
390  for (std::size_t idx = 0; idx < N; idx++, ang += dAng)
391  {
392  Cos[idx] = std::cos(ang);
393  Sin[idx] = std::sin(ang);
394  }
395 
396  std::vector<double> eVals;
398  cov2x2.eig(eigVec, eVals);
399  eigVals.setDiagonal(eVals);
400 
401  eigVals.asEigen() = eigVals.array().sqrt().matrix();
402 
404  M.asEigen() = eigVals.asEigen() * eigVec.transpose();
405 
406  // Compute the points of the ellipsoid:
407  // ----------------------------------------------
408  for (std::size_t idx = 0; idx < N; idx++)
409  {
410  X[idx] = mean[0] + stdCount * (Cos[idx] * M(0, 0) + Sin[idx] * M(1, 0));
411  X[idx] = mean[1] + stdCount * (Cos[idx] * M(0, 1) + Sin[idx] * M(1, 1));
412  }
413 
414  // Save the code to plot the ellipsoid:
415  // ----------------------------------------------
416  std::string str;
417  str += string("plot([ ");
418  for (size_t i = 0; i < X.size(); i++)
419  {
420  str += format("%.4f", X[i]);
421  if (i != (X.size() - 1)) str += ",";
422  }
423  str += string("],[ ");
424  for (size_t i = 0; i < X.size(); i++)
425  {
426  str += format("%.4f", Y[i]);
427  if (i != (Y.size() - 1)) str += ",";
428  }
429 
430  str += format("],'%s');\n", style.c_str());
431 
432  return str;
433  MRPT_END_WITH_CLEAN_UP(std::cerr << "The matrix that led to error was: "
434  << std::endl
435  << cov.asEigen() << std::endl;)
436 }
437 
439  const double x, const double x0, const double y0, const double x1,
440  const double y1, bool wrap2pi)
441 {
442  MRPT_START
443  if (x0 == x1)
444  THROW_EXCEPTION_FMT("ERROR: Both x0 and x1 are equal (=%f)", x0);
445 
446  const double Ax = x1 - x0;
447  const double Ay = y1 - y0;
448 
449  double r = y0 + Ay * (x - x0) / Ax;
450  if (!wrap2pi)
451  return r;
452  else
453  return mrpt::math::wrapToPi(r);
454 
455  MRPT_END
456 }
457 
458 /*---------------------------------------------------------------
459  median filter of a vector
460  ---------------------------------------------------------------*/
461 // template<typename VECTOR>
463  const std::vector<double>& inV, std::vector<double>& outV, int _winSize,
464  int numberOfSigmas)
465 {
466  MRPT_UNUSED_PARAM(numberOfSigmas);
467  ASSERT_((int)inV.size() >= _winSize);
468  ASSERT_(_winSize >= 2); // The minimum window size is 3 elements
469  size_t winSize = _winSize;
470 
471  if (!(winSize % 2)) // We use an odd number of elements for the window size
472  winSize++;
473 
474  size_t sz = inV.size();
475  outV.resize(sz);
476 
477  std::vector<double> aux(winSize);
478  size_t mpoint = winSize / 2;
479  for (size_t k = 0; k < sz; ++k)
480  {
481  aux.clear();
482 
483  size_t idx_to_start =
484  std::max(size_t(0), k - mpoint); // Dealing with the boundaries
485  size_t n_elements =
486  std::min(std::min(winSize, sz + mpoint - k), k + mpoint + 1);
487 
488  aux.resize(n_elements);
489  for (size_t m = idx_to_start, n = 0; m < idx_to_start + n_elements;
490  ++m, ++n)
491  aux[n] = inV[m];
492 
493  std::sort(aux.begin(), aux.end());
494 
495  size_t auxSz = aux.size();
496  size_t auxMPoint = auxSz / 2;
497  outV[k] = (auxSz % 2) ? (aux[auxMPoint])
498  : (0.5 * (aux[auxMPoint - 1] +
499  aux[auxMPoint])); // If the window is
500  // even, take the
501  // mean value of the
502  // middle points
503  } // end-for
504 } // end medianFilter
505 
506 double mrpt::math::chi2CDF(unsigned int degreesOfFreedom, double arg)
507 {
508  return noncentralChi2CDF(degreesOfFreedom, 0.0, arg);
509 }
510 
511 template <class T>
513  T arg, T& lans, T& dans, T& pans, unsigned int& j)
514 {
515  double tol = -50.0;
516  if (lans < tol)
517  {
518  lans = lans + std::log(arg / j);
519  dans = std::exp(lans);
520  }
521  else
522  {
523  dans = dans * arg / j;
524  }
525  pans = pans - dans;
526  j += 2;
527 }
528 
529 std::pair<double, double> mrpt::math::noncentralChi2PDF_CDF(
530  unsigned int degreesOfFreedom, double noncentrality, double arg, double eps)
531 {
532  ASSERTMSG_(
533  noncentrality >= 0.0 && arg >= 0.0 && eps > 0.0,
534  "noncentralChi2PDF_CDF(): parameters must be positive.");
535 
536  if (arg == 0.0 && degreesOfFreedom > 0) return std::make_pair(0.0, 0.0);
537 
538  // Determine initial values
539  double b1 = 0.5 * noncentrality, ao = std::exp(-b1), eps2 = eps / ao,
540  lnrtpi2 = 0.22579135264473, probability, density, lans, dans, pans,
541  sum, am, hold;
542  unsigned int maxit = 500, i, m;
543  if (degreesOfFreedom % 2)
544  {
545  i = 1;
546  lans = -0.5 * (arg + std::log(arg)) - lnrtpi2;
547  dans = std::exp(lans);
548  pans = std::erf(std::sqrt(arg / 2.0));
549  }
550  else
551  {
552  i = 2;
553  lans = -0.5 * arg;
554  dans = std::exp(lans);
555  pans = 1.0 - dans;
556  }
557 
558  // Evaluate first term
559  if (degreesOfFreedom == 0)
560  {
561  m = 1;
562  degreesOfFreedom = 2;
563  am = b1;
564  sum = 1.0 / ao - 1.0 - am;
565  density = am * dans;
566  probability = 1.0 + am * pans;
567  }
568  else
569  {
570  m = 0;
571  degreesOfFreedom = degreesOfFreedom - 1;
572  am = 1.0;
573  sum = 1.0 / ao - 1.0;
574  while (i < degreesOfFreedom)
575  noncentralChi2OneIteration(arg, lans, dans, pans, i);
576  degreesOfFreedom = degreesOfFreedom + 1;
577  density = dans;
578  probability = pans;
579  }
580  // Evaluate successive terms of the expansion
581  for (++m; m < maxit; ++m)
582  {
583  am = b1 * am / m;
584  noncentralChi2OneIteration(arg, lans, dans, pans, degreesOfFreedom);
585  sum = sum - am;
586  density = density + am * dans;
587  hold = am * pans;
588  probability = probability + hold;
589  if ((pans * sum < eps2) && (hold < eps2)) break; // converged
590  }
591  if (m == maxit) THROW_EXCEPTION("noncentralChi2PDF_CDF(): no convergence.");
592  return std::make_pair(
593  0.5 * ao * density, std::min(1.0, std::max(0.0, ao * probability)));
594 }
595 
597  unsigned int degreesOfFreedom, double arg, double accuracy)
598 {
600  degreesOfFreedom, 0.0, arg, accuracy)
601  .first;
602 }
603 
605  unsigned int degreesOfFreedom, double noncentrality, double arg)
606 {
607  const double a = degreesOfFreedom + noncentrality;
608  const double b = (a + noncentrality) / square(a);
609  const double t =
610  (std::pow((double)arg / a, 1.0 / 3.0) - (1.0 - 2.0 / 9.0 * b)) /
611  std::sqrt(2.0 / 9.0 * b);
612  return 0.5 * (1.0 + std::erf(t / std::sqrt(2.0)));
613 }
614 
616 {
617  v.resize(s.ReadAs<uint32_t>());
618  if (v.size() > 0) s.ReadBufferFixEndianness(&v[0], v.size());
619  return s;
620 }
622 {
623  v.resize(s.ReadAs<uint32_t>());
624  if (v.size() > 0) s.ReadBufferFixEndianness(&v[0], v.size());
625  return s;
626 }
628 {
629  s.WriteAs<uint32_t>(v.size());
630  if (v.size() > 0) s.WriteBufferFixEndianness(&v[0], v.size());
631  return s;
632 }
634 {
635  s.WriteAs<uint32_t>(v.size());
636  if (v.size() > 0) s.WriteBufferFixEndianness(&v[0], v.size());
637  return s;
638 }
bool eig(Derived &eVecs, std::vector< Scalar > &eVals, bool sorted=true) const
Computes the eigenvectors and eigenvalues for a square, general matrix.
double averageWrap2Pi(const CVectorDouble &angles)
Computes the average of a sequence of angles in radians taking into account the correct wrapping in t...
Definition: math.cpp:315
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define MRPT_START
Definition: exceptions.h:241
#define M_2PI
Definition: common.h:58
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
double normalCDF(double p)
Evaluates the Gaussian cumulative density function.
Definition: math.cpp:134
void WriteBufferFixEndianness(const T *ptr, size_t ElementCount)
Writes a sequence of elemental datatypes, taking care of reordering their bytes from the running arch...
Definition: CArchive.h:133
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
STL namespace.
void WriteAs(const TYPE_FROM_ACTUAL &value)
Definition: CArchive.h:162
#define MRPT_END_WITH_CLEAN_UP(stuff)
Definition: exceptions.h:247
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CMatrixD::Ptr &pObj)
double factorial(unsigned int n)
Computes the factorial of an integer number and returns it as a double value (internally it uses loga...
Definition: math.cpp:68
double normalQuantile(double p)
Evaluates the Gaussian distribution quantile for the probability value p=[0,1].
Definition: math.cpp:80
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
std::pair< double, double > noncentralChi2PDF_CDF(unsigned int degreesOfFreedom, double noncentrality, double arg, double eps=1e-7)
Returns the &#39;exact&#39; PDF (first) and CDF (second) of a Non-central chi-squared probability distributio...
Definition: math.cpp:529
CVectorDynamic< double > CVectorDouble
#define MRPT_CHECK_NORMAL_NUMBER(v)
Throws an exception if the number is NaN, IND, or +/-INF, or return the same number otherwise...
Definition: exceptions.h:125
T square(const T x)
Inline function for the square of a number.
double averageLogLikelihood(const CVectorDouble &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
Definition: math.cpp:293
STORED_TYPE ReadAs()
De-serialize a variable and returns it by value.
Definition: CArchive.h:155
bool loadVector(std::istream &f, std::vector< int > &d)
Loads one row of a text file as a numerical std::vector.
double chi2PDF(unsigned int degreesOfFreedom, double arg, double accuracy=1e-7)
Definition: math.cpp:596
char * strtok(char *str, const char *strDelimit, char **context) noexcept
An OS-independent method for tokenizing a string.
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
uint64_t factorial64(unsigned int n)
Computes the factorial of an integer number and returns it as a 64-bit integer number.
Definition: math.cpp:56
CONTAINER::Scalar maximum(const CONTAINER &v)
const double eps
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
typename vec_t::const_iterator const_iterator
CMatrixDouble cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:149
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
double chi2CDF(unsigned int degreesOfFreedom, double arg)
Definition: math.cpp:506
void noncentralChi2OneIteration(T arg, T &lans, T &dans, T &pans, unsigned int &j)
Definition: math.cpp:512
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
double chi2inv(double P, unsigned int dim=1)
The "quantile" of the Chi-Square distribution, for dimension "dim" and probability 0<P<1 (the inverse...
Definition: math.cpp:42
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &s, const CVectorFloat &a)
Definition: math.cpp:627
#define MRPT_END
Definition: exceptions.h:245
void medianFilter(const std::vector< double > &inV, std::vector< double > &outV, int winSize, int numberOfSigmas=2)
Definition: math.cpp:462
double mean(const CONTAINER &v)
Computes the mean value of a vector.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
double noncentralChi2CDF(unsigned int degreesOfFreedom, double noncentrality, double arg)
Definition: math.cpp:604
std::string MATLAB_plotCovariance2D(const CMatrixFloat &cov22, const CVectorFloat &mean, float stdCount, const std::string &style=std::string("b"), size_t nEllipsePoints=30)
Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2...
Definition: math.cpp:356
double interpolate2points(const double x, const double x0, const double y0, const double x1, const double y1, bool wrap2pi=false)
Linear interpolation/extrapolation: evaluates at "x" the line (x0,y0)-(x1,y1).
Definition: math.cpp:438
void resize(std::size_t N, bool zeroNewElements=false)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
size_t ReadBufferFixEndianness(T *ptr, size_t ElementCount)
Reads a sequence of elemental datatypes, taking care of reordering their bytes from the MRPT stream s...
Definition: CArchive.h:94
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
Definition: math.cpp:33
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
This template class provides the basic functionality for a general 2D any-size, resizable container o...
void setDiagonal(const std::size_t N, const T value)
Resize to NxN, set all entries to zero, except the main diagonal which is set to value ...
Definition: MatrixBase.h:34
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7e629e01a Sat Dec 14 00:05:55 2019 +0100 at sáb dic 14 00:15:10 CET 2019