MRPT  1.9.9
data_utils.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10
11 #include <mrpt/math/wrap2pi.h>
13 #include <mrpt/math/ops_matrices.h>
14
15 namespace mrpt
16 {
17 /** This base provides a set of functions for maths stuff. \ingroup
18  * mrpt_math_grp
19  */
20 namespace math
21 {
23  * @{
24  */
25
26 /** @name Probability density distributions (pdf) distance metrics
27 @{ */
28
29 /** Computes the squared mahalanobis distance of a vector X given the mean MU
30  * and the covariance *inverse* COV_inv
31  * \f[ d^2 = (X-MU)^\top \Sigma^{-1} (X-MU) \f]
32  */
33 template <class VECTORLIKE1, class VECTORLIKE2, class MAT>
35  const VECTORLIKE1& X, const VECTORLIKE2& MU, const MAT& COV)
36 {
38 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
39  ASSERT_(!X.empty());
40  ASSERT_(X.size() == MU.size());
41  ASSERT_(X.size() == COV.rows() && COV.isSquare());
42 #endif
43  const size_t N = X.size();
44  Eigen::Matrix<typename MAT::Scalar, Eigen::Dynamic, 1> X_MU(N);
45  for (size_t i = 0; i < N; i++) X_MU[i] = X[i] - MU[i];
46  const Eigen::Matrix<typename MAT::Scalar, Eigen::Dynamic, 1> z =
47  COV.llt().solve(X_MU);
48  return z.dot(z);
49  MRPT_END
50 }
51
52 /** Computes the mahalanobis distance of a vector X given the mean MU and the
53  * covariance *inverse* COV_inv
54  * \f[ d = \sqrt{ (X-MU)^\top \Sigma^{-1} (X-MU) } \f]
55  */
56 template <class VECTORLIKE1, class VECTORLIKE2, class MAT>
58  const VECTORLIKE1& X, const VECTORLIKE2& MU, const MAT& COV)
59 {
60  return std::sqrt(mahalanobisDistance2(X, MU, COV));
61 }
62
63 /** Computes the squared mahalanobis distance between two *non-independent*
64  * Gaussians, given the two covariance matrices and the vector with the
65  * difference of their means.
66  * \f[ d^2 = \Delta_\mu^\top (\Sigma_1 + \Sigma_2 - 2 \Sigma_12 )^{-1}
67  * \Delta_\mu \f]
68  */
69 template <class VECTORLIKE, class MAT1, class MAT2, class MAT3>
71  const VECTORLIKE& mean_diffs, const MAT1& COV1, const MAT2& COV2,
72  const MAT3& CROSS_COV12)
73 {
75 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
76  ASSERT_(!mean_diffs.empty());
77  ASSERT_(mean_diffs.size() == COV1.rows());
78  ASSERT_(COV1.isSquare() && COV2.isSquare());
79  ASSERT_(COV1.rows() == COV2.rows());
80 #endif
81  MAT1 COV = COV1;
82  COV += COV2;
83  COV.substract_An(CROSS_COV12, 2);
84  MAT1 COV_inv;
85  COV.inv_fast(COV_inv);
86  return multiply_HCHt_scalar(mean_diffs, COV_inv);
87  MRPT_END
88 }
89
90 /** Computes the mahalanobis distance between two *non-independent* Gaussians
91  * (or independent if CROSS_COV12=nullptr), given the two covariance matrices
92  * and the vector with the difference of their means.
93  * \f[ d = \sqrt{ \Delta_\mu^\top (\Sigma_1 + \Sigma_2 - 2 \Sigma_12 )^{-1}
94  * \Delta_\mu } \f]
95  */
96 template <class VECTORLIKE, class MAT1, class MAT2, class MAT3>
98  const VECTORLIKE& mean_diffs, const MAT1& COV1, const MAT2& COV2,
99  const MAT3& CROSS_COV12)
100 {
101  return std::sqrt(mahalanobisDistance(mean_diffs, COV1, COV2, CROSS_COV12));
102 }
103
104 /** Computes the squared mahalanobis distance between a point and a Gaussian,
105  * given the covariance matrix and the vector with the difference between the
106  * mean and the point.
107  * \f[ d^2 = \Delta_\mu^\top \Sigma^{-1} \Delta_\mu \f]
108  */
109 template <class VECTORLIKE, class MATRIXLIKE>
111  const VECTORLIKE& delta_mu, const MATRIXLIKE& cov)
112 {
113  ASSERTDEB_(cov.isSquare());
114  ASSERTDEB_(size_t(cov.cols()) == size_t(delta_mu.size()));
115  return multiply_HCHt_scalar(delta_mu, cov.inverse());
116 }
117
118 /** Computes the mahalanobis distance between a point and a Gaussian, given the
119  * covariance matrix and the vector with the difference between the mean and the
120  * point.
121  * \f[ d^2 = \sqrt( \Delta_\mu^\top \Sigma^{-1} \Delta_\mu ) \f]
122  */
123 template <class VECTORLIKE, class MATRIXLIKE>
125  const VECTORLIKE& delta_mu, const MATRIXLIKE& cov)
126 {
127  return std::sqrt(mahalanobisDistance2(delta_mu, cov));
128 }
129
130 /** Computes the integral of the product of two Gaussians, with means separated
131  * by "mean_diffs" and covariances "COV1" and "COV2".
132  * \f[ D = \frac{1}{(2 \pi)^{0.5 N} \sqrt{} } \exp( \Delta_\mu^\top
133  * (\Sigma_1 + \Sigma_2 - 2 \Sigma_12)^{-1} \Delta_\mu) \f]
134  */
135 template <typename T>
137  const std::vector<T>& mean_diffs, const CMatrixTemplateNumeric<T>& COV1,
138  const CMatrixTemplateNumeric<T>& COV2)
139 {
140  const size_t vector_dim = mean_diffs.size();
141  ASSERT_(vector_dim >= 1);
142
143  CMatrixTemplateNumeric<T> C = COV1;
144  C += COV2; // Sum of covs:
145  const T cov_det = C.det();
147  C.inv_fast(C_inv);
148
149  return std::pow(M_2PI, -0.5 * vector_dim) * (1.0 / std::sqrt(cov_det)) *
150  exp(-0.5 * mean_diffs.multiply_HCHt_scalar(C_inv));
151 }
152
153 /** Computes the integral of the product of two Gaussians, with means separated
154  * by "mean_diffs" and covariances "COV1" and "COV2".
155  * \f[ D = \frac{1}{(2 \pi)^{0.5 N} \sqrt{} } \exp( \Delta_\mu^\top
156  * (\Sigma_1 + \Sigma_2)^{-1} \Delta_\mu) \f]
157  */
158 template <typename T, size_t DIM>
160  const std::vector<T>& mean_diffs,
163 {
164  ASSERT_(mean_diffs.size() == DIM);
165
167  C += COV2; // Sum of covs:
168  const T cov_det = C.det();
170  C.inv_fast(C_inv);
171
172  return std::pow(M_2PI, -0.5 * DIM) * (1.0 / std::sqrt(cov_det)) *
173  exp(-0.5 * mean_diffs.multiply_HCHt_scalar(C_inv));
174 }
175
176 /** Computes both, the integral of the product of two Gaussians and their square
177  * Mahalanobis distance.
178  * \sa productIntegralTwoGaussians, mahalanobisDistance2
179  */
180 template <typename T, class VECLIKE, class MATLIKE1, class MATLIKE2>
182  const VECLIKE& mean_diffs, const MATLIKE1& COV1, const MATLIKE2& COV2,
183  T& maha2_out, T& intprod_out, const MATLIKE1* CROSS_COV12 = nullptr)
184 {
185  const size_t vector_dim = mean_diffs.size();
186  ASSERT_(vector_dim >= 1);
187
188  MATLIKE1 C = COV1;
189  C += COV2; // Sum of covs:
190  if (CROSS_COV12)
191  {
192  C -= *CROSS_COV12;
193  C -= *CROSS_COV12;
194  }
195  const T cov_det = C.det();
196  MATLIKE1 C_inv;
197  C.inv_fast(C_inv);
198
199  maha2_out = mean_diffs.multiply_HCHt_scalar(C_inv);
200  intprod_out = std::pow(M_2PI, -0.5 * vector_dim) *
201  (1.0 / std::sqrt(cov_det)) * exp(-0.5 * maha2_out);
202 }
203
204 /** Computes both, the logarithm of the PDF and the square Mahalanobis distance
205  * between a point (given by its difference wrt the mean) and a Gaussian.
206  * \sa productIntegralTwoGaussians, mahalanobisDistance2, normalPDF,
207  * mahalanobisDistance2AndPDF
208  */
209 template <typename T, class VECLIKE, class MATRIXLIKE>
211  const VECLIKE& diff_mean, const MATRIXLIKE& cov, T& maha2_out,
212  T& log_pdf_out)
213 {
214  MRPT_START
215  ASSERTDEB_(cov.isSquare());
216  ASSERTDEB_(size_t(cov.cols()) == size_t(diff_mean.size()));
217  MATRIXLIKE C_inv;
218  cov.inv(C_inv);
219  maha2_out = multiply_HCHt_scalar(diff_mean, C_inv);
220  log_pdf_out = static_cast<typename MATRIXLIKE::Scalar>(-0.5) *
221  (maha2_out +
222  static_cast<typename MATRIXLIKE::Scalar>(cov.cols()) *
223  ::log(static_cast<typename MATRIXLIKE::Scalar>(M_2PI)) +
224  ::log(cov.det()));
225  MRPT_END
226 }
227
228 /** Computes both, the PDF and the square Mahalanobis distance between a point
229  * (given by its difference wrt the mean) and a Gaussian.
230  * \sa productIntegralTwoGaussians, mahalanobisDistance2, normalPDF
231  */
232 template <typename T, class VECLIKE, class MATRIXLIKE>
234  const VECLIKE& diff_mean, const MATRIXLIKE& cov, T& maha2_out, T& pdf_out)
235 {
236  mahalanobisDistance2AndLogPDF(diff_mean, cov, maha2_out, pdf_out);
237  pdf_out = std::exp(pdf_out); // log to linear
238 }
239
240 /** Computes covariances and mean of any vector of containers, given optional
241  * weights for the different samples.
242  * \param elements Any kind of vector of vectors/arrays, eg.
243  * std::vector<mrpt::math::CVectorDouble>, with all the input samples, each
244  * sample in a "row".
245  * \param covariances Output estimated covariance; it can be a fixed/dynamic
246  * matrix or a matrixview.
247  * \param means Output estimated mean; it can be CVectorDouble/CArrayDouble,
248  * etc...
249  * \param weights_mean If !=nullptr, it must point to a vector of
250  * size()==number of elements, with normalized weights to take into account for
251  * the mean.
252  * \param weights_cov If !=nullptr, it must point to a vector of size()==number
253  * of elements, with normalized weights to take into account for the covariance.
254  * \param elem_do_wrap2pi If !=nullptr; it must point to an array of "bool" of
255  * size()==dimension of each element, stating if it's needed to do a wrap to
256  * [-pi,pi] to each dimension.
257  * \sa This method is used in mrpt::math::unscented_transform_gaussian
258  * \ingroup stats_grp
259  */
260 template<class VECTOR_OF_VECTORS, class MATRIXLIKE,class VECTORLIKE,class VECTORLIKE2,class VECTORLIKE3>
261  inline void covariancesAndMeanWeighted( // Done inline to speed-up the special case expanded in covariancesAndMean() below.
262  const VECTOR_OF_VECTORS &elements,
263  MATRIXLIKE &covariances,
264  VECTORLIKE &means,
265  const VECTORLIKE2 *weights_mean,
266  const VECTORLIKE3 *weights_cov,
267  const bool *elem_do_wrap2pi = nullptr
268  )
269 {
270  ASSERTMSG_(
271  elements.size() != 0,
272  "No samples provided, so there is no way to deduce the output size.");
273  using T = typename MATRIXLIKE::Scalar;
274  const size_t DIM = elements[0].size();
275  means.resize(DIM);
276  covariances.setSize(DIM, DIM);
277  const size_t nElms = elements.size();
278  const T NORM = 1.0 / nElms;
279  if (weights_mean)
280  {
281  ASSERTDEB_(size_t(weights_mean->size()) == size_t(nElms));
282  }
283  // The mean goes first:
284  for (size_t i = 0; i < DIM; i++)
285  {
286  T accum = 0;
287  if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
288  { // i'th dimension is a "normal", real number:
289  if (weights_mean)
290  {
291  for (size_t j = 0; j < nElms; j++)
292  accum += (*weights_mean)[j] * elements[j][i];
293  }
294  else
295  {
296  for (size_t j = 0; j < nElms; j++) accum += elements[j][i];
297  accum *= NORM;
298  }
299  }
300  else
301  { // i'th dimension is a circle in [-pi,pi]: we need a little trick
302  // here:
303  double accum_L = 0, accum_R = 0;
304  double Waccum_L = 0, Waccum_R = 0;
305  for (size_t j = 0; j < nElms; j++)
306  {
307  double ang = elements[j][i];
308  const double w =
309  weights_mean != nullptr ? (*weights_mean)[j] : NORM;
310  if (fabs(ang) > 0.5 * M_PI)
311  { // LEFT HALF: 0,2pi
312  if (ang < 0) ang = (M_2PI + ang);
313  accum_L += ang * w;
314  Waccum_L += w;
315  }
316  else
317  { // RIGHT HALF: -pi,pi
318  accum_R += ang * w;
319  Waccum_R += w;
320  }
321  }
322  if (Waccum_L > 0) accum_L /= Waccum_L; // [0,2pi]
323  if (Waccum_R > 0) accum_R /= Waccum_R; // [-pi,pi]
324  if (accum_L > M_PI)
325  accum_L -= M_2PI; // Left side to [-pi,pi] again:
326  accum =
327  (accum_L * Waccum_L +
328  accum_R * Waccum_R); // The overall result:
329  }
330  means[i] = accum;
331  }
332  // Now the covariance:
333  for (size_t i = 0; i < DIM; i++)
334  for (size_t j = 0; j <= i; j++) // Only 1/2 of the matrix
335  {
336  typename MATRIXLIKE::Scalar elem = 0;
337  if (weights_cov)
338  {
339  ASSERTDEB_(size_t(weights_cov->size()) == size_t(nElms));
340  for (size_t k = 0; k < nElms; k++)
341  {
342  const T Ai = (elements[k][i] - means[i]);
343  const T Aj = (elements[k][j] - means[j]);
344  if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
345  elem += (*weights_cov)[k] * Ai * Aj;
346  else
347  elem += (*weights_cov)[k] * mrpt::math::wrapToPi(Ai) *
349  }
350  }
351  else
352  {
353  for (size_t k = 0; k < nElms; k++)
354  {
355  const T Ai = (elements[k][i] - means[i]);
356  const T Aj = (elements[k][j] - means[j]);
357  if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
358  elem += Ai * Aj;
359  else
360  elem +=
362  }
363  elem *= NORM;
364  }
365  covariances.get_unsafe(i, j) = elem;
366  if (i != j) covariances.get_unsafe(j, i) = elem;
367  }
368 }
369
370 /** Computes covariances and mean of any vector of containers.
371  * \param elements Any kind of vector of vectors/arrays, eg.
372  * std::vector<mrpt::math::CVectorDouble>, with all the input samples, each
373  * sample in a "row".
374  * \param covariances Output estimated covariance; it can be a fixed/dynamic
375  * matrix or a matrixview.
376  * \param means Output estimated mean; it can be CVectorDouble/CArrayDouble,
377  * etc...
378  * \param elem_do_wrap2pi If !=nullptr; it must point to an array of "bool" of
379  * size()==dimension of each element, stating if it's needed to do a wrap to
380  * [-pi,pi] to each dimension.
381  * \ingroup stats_grp
382  */
383 template <class VECTOR_OF_VECTORS, class MATRIXLIKE, class VECTORLIKE>
385  const VECTOR_OF_VECTORS& elements, MATRIXLIKE& covariances,
386  VECTORLIKE& means, const bool* elem_do_wrap2pi = nullptr)
387 { // The function below is inline-expanded here:
388  covariancesAndMeanWeighted<VECTOR_OF_VECTORS, MATRIXLIKE, VECTORLIKE,
390  elements, covariances, means, nullptr, nullptr, elem_do_wrap2pi);
391 }
392
393 /** Computes the weighted histogram for a vector of values and their
394  * corresponding weights.
395  * \param values [IN] The N values
396  * \param weights [IN] The weights for the corresponding N values (don't need
397  * to be normalized)
398  * \param binWidth [IN] The desired width of the bins
399  * \param out_binCenters [OUT] The centers of the M bins generated to cover
400  * from the minimum to the maximum value of "values" with the given "binWidth"
401  * \param out_binValues [OUT] The ratio of values at each given bin, such as
402  * the whole vector sums up the unity.
403  * \sa weightedHistogramLog
404  */
405 template <class VECTORLIKE1, class VECTORLIKE2>
407  const VECTORLIKE1& values, const VECTORLIKE1& weights, float binWidth,
408  VECTORLIKE2& out_binCenters, VECTORLIKE2& out_binValues)
409 {
410  MRPT_START
412
413  ASSERT_(values.size() == weights.size());
414  ASSERT_(binWidth > 0);
415  TNum minBin = minimum(values);
416  unsigned int nBins =
417  static_cast<unsigned>(ceil((maximum(values) - minBin) / binWidth));
418
419  // Generate bin center and border values:
420  out_binCenters.resize(nBins);
421  out_binValues.clear();
422  out_binValues.resize(nBins, 0);
423  TNum halfBin = TNum(0.5) * binWidth;
424  ;
425  VECTORLIKE2 binBorders(nBins + 1, minBin - halfBin);
426  for (unsigned int i = 0; i < nBins; i++)
427  {
428  binBorders[i + 1] = binBorders[i] + binWidth;
429  out_binCenters[i] = binBorders[i] + halfBin;
430  }
431
432  // Compute the histogram:
433  TNum totalSum = 0;
434  typename VECTORLIKE1::const_iterator itVal, itW;
435  for (itVal = values.begin(), itW = weights.begin(); itVal != values.end();
436  ++itVal, ++itW)
437  {
438  int idx = round(((*itVal) - minBin) / binWidth);
439  if (idx >= int(nBins)) idx = nBins - 1;
440  ASSERTDEB_(idx >= 0);
441  out_binValues[idx] += *itW;
442  totalSum += *itW;
443  }
444
445  if (totalSum) out_binValues /= totalSum;
446
447  MRPT_END
448 }
449
450 /** Computes the weighted histogram for a vector of values and their
451  * corresponding log-weights.
452  * \param values [IN] The N values
453  * \param weights [IN] The log-weights for the corresponding N values (don't
454  * need to be normalized)
455  * \param binWidth [IN] The desired width of the bins
456  * \param out_binCenters [OUT] The centers of the M bins generated to cover
457  * from the minimum to the maximum value of "values" with the given "binWidth"
458  * \param out_binValues [OUT] The ratio of values at each given bin, such as
459  * the whole vector sums up the unity.
460  * \sa weightedHistogram
461  */
462 template <class VECTORLIKE1, class VECTORLIKE2>
464  const VECTORLIKE1& values, const VECTORLIKE1& log_weights, float binWidth,
465  VECTORLIKE2& out_binCenters, VECTORLIKE2& out_binValues)
466 {
467  MRPT_START
469
470  ASSERT_(values.size() == log_weights.size());
471  ASSERT_(binWidth > 0);
472  TNum minBin = minimum(values);
473  unsigned int nBins =
474  static_cast<unsigned>(ceil((maximum(values) - minBin) / binWidth));
475
476  // Generate bin center and border values:
477  out_binCenters.resize(nBins);
478  out_binValues.clear();
479  out_binValues.resize(nBins, 0);
480  TNum halfBin = TNum(0.5) * binWidth;
481  ;
482  VECTORLIKE2 binBorders(nBins + 1, minBin - halfBin);
483  for (unsigned int i = 0; i < nBins; i++)
484  {
485  binBorders[i + 1] = binBorders[i] + binWidth;
486  out_binCenters[i] = binBorders[i] + halfBin;
487  }
488
489  // Compute the histogram:
490  const TNum max_log_weight = maximum(log_weights);
491  TNum totalSum = 0;
492  typename VECTORLIKE1::const_iterator itVal, itW;
493  for (itVal = values.begin(), itW = log_weights.begin();
494  itVal != values.end(); ++itVal, ++itW)
495  {
496  int idx = round(((*itVal) - minBin) / binWidth);
497  if (idx >= int(nBins)) idx = nBins - 1;
498  ASSERTDEB_(idx >= 0);
499  const TNum w = exp(*itW - max_log_weight);
500  out_binValues[idx] += w;
501  totalSum += w;
502  }
503
504  if (totalSum) out_binValues /= totalSum;
505
506  MRPT_END
507 }
508
509 /** A numerically-stable method to compute average likelihood values with
510  * strongly different ranges (unweighted likelihoods: compute the arithmetic
511  * mean).
512  * This method implements this equation:
513  *
514  * \f[ return = - \log N + \log \sum_{i=1}^N e^{ll_i-ll_{max}} + ll_{max} \f]
515  *
517  * href="http://www.mrpt.org/Averaging_Log-Likelihood_Values:Numerical_Stability">tutorial
518  * page</a>.
519  * \ingroup stats_grp
520  */
521 double averageLogLikelihood(const CVectorDouble& logLikelihoods);
522
523 /** Computes the average of a sequence of angles in radians taking into account
524  * the correct wrapping in the range \f$]-\pi,\pi [ \f$, for example, the mean
525  * of (2,-2) is \f$\pi \f$, not 0.
526  * \ingroup stats_grp
527  */
528 double averageWrap2Pi(const CVectorDouble& angles);
529
530 /** A numerically-stable method to average likelihood values with strongly
531  * different ranges (weighted likelihoods).
532  * This method implements this equation:
533  *
534  * \f[ return = \log \left( \frac{1}{\sum_i e^{lw_i}} \sum_i e^{lw_i}
535  * e^{ll_i} \right) \f]
536  *
538  * href="http://www.mrpt.org/Averaging_Log-Likelihood_Values:Numerical_Stability">tutorial
539  * page</a>.
540  * \ingroup stats_grp
541  */
542 double averageLogLikelihood(
543  const CVectorDouble& logWeights, const CVectorDouble& logLikelihoods);
544
545 /** @} */ // end of grouping container_ops_grp
546
547 } // End of MATH namespace
548 } // End of namespace
void mahalanobisDistance2AndPDF(const VECLIKE &diff_mean, const MATRIXLIKE &cov, T &maha2_out, T &pdf_out)
Computes both, the PDF and the square Mahalanobis distance between a point (given by its difference w...
Definition: data_utils.h:233
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:313
const GLbyte * weights
Definition: glext.h:4450
double Scalar
Definition: KmUtils.h:44
#define MRPT_START
Definition: exceptions.h:262
GLdouble GLdouble z
Definition: glext.h:3872
#define M_2PI
Definition: common.h:58
GLboolean GLenum GLenum GLvoid * values
Definition: glext.h:3582
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C)
Definition: ops_matrices.h:69
void productIntegralAndMahalanobisTwoGaussians(const VECLIKE &mean_diffs, const MATLIKE1 &COV1, const MATLIKE2 &COV2, T &maha2_out, T &intprod_out, const MATLIKE1 *CROSS_COV12=nullptr)
Computes both, the integral of the product of two Gaussians and their square Mahalanobis distance...
Definition: data_utils.h:181
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
CONTAINER::Scalar minimum(const CONTAINER &v)
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
A numeric matrix of compile-time fixed size.
void mahalanobisDistance2AndLogPDF(const VECLIKE &diff_mean, const MATRIXLIKE &cov, T &maha2_out, T &log_pdf_out)
Computes both, the logarithm of the PDF and the square Mahalanobis distance between a point (given by...
Definition: data_utils.h:210
void covariancesAndMean(const VECTOR_OF_VECTORS &elements, MATRIXLIKE &covariances, VECTORLIKE &means, const bool *elem_do_wrap2pi=nullptr)
Computes covariances and mean of any vector of containers.
Definition: data_utils.h:384
double averageLogLikelihood(const CVectorDouble &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
Definition: math.cpp:291
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
CONTAINER::Scalar maximum(const CONTAINER &v)
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
MAT::Scalar mahalanobisDistance2(const VECTORLIKE1 &X, const VECTORLIKE2 &MU, const MAT &COV)
Computes the squared mahalanobis distance of a vector X given the mean MU and the covariance inverse ...
Definition: data_utils.h:34
void weightedHistogram(const VECTORLIKE1 &values, const VECTORLIKE1 &weights, float binWidth, VECTORLIKE2 &out_binCenters, VECTORLIKE2 &out_binValues)
Computes the weighted histogram for a vector of values and their corresponding weights.
Definition: data_utils.h:406
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void covariancesAndMeanWeighted(const VECTOR_OF_VECTORS &elements, MATRIXLIKE &covariances, VECTORLIKE &means, const VECTORLIKE2 *weights_mean, const VECTORLIKE3 *weights_cov, const bool *elem_do_wrap2pi=nullptr)
Computes covariances and mean of any vector of containers, given optional weights for the different s...
Definition: data_utils.h:261
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:205
typename CONTAINER::value_type element_t
Definition: math_frwds.h:92
VECTORLIKE1::Scalar mahalanobisDistance(const VECTORLIKE1 &X, const VECTORLIKE2 &MU, const MAT &COV)
Computes the mahalanobis distance of a vector X given the mean MU and the covariance inverse COV_inv ...
Definition: data_utils.h:57
#define MRPT_END
Definition: exceptions.h:266
T productIntegralTwoGaussians(const std::vector< T > &mean_diffs, const CMatrixTemplateNumeric< T > &COV1, const CMatrixTemplateNumeric< T > &COV2)
Computes the integral of the product of two Gaussians, with means separated by "mean_diffs" and covar...
Definition: data_utils.h:136
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > 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:148
dynamic_vector< double > CVectorDouble
Column vector, like Eigen::MatrixXd, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:46
void weightedHistogramLog(const VECTORLIKE1 &values, const VECTORLIKE1 &log_weights, float binWidth, VECTORLIKE2 &out_binCenters, VECTORLIKE2 &out_binValues)
Computes the weighted histogram for a vector of values and their corresponding log-weights.
Definition: data_utils.h:463
const Scalar * const_iterator
Definition: eigen_plugins.h:27
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23

 Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020