MRPT  2.0.1
data_utils.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, 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 #pragma once
10 
11 #include <mrpt/math/CMatrixFixed.h>
13 #include <mrpt/math/ops_matrices.h>
14 #include <mrpt/math/wrap2pi.h>
15 
16 namespace mrpt
17 {
18 /** This base provides a set of functions for maths stuff. \ingroup
19  * mrpt_math_grp
20  */
21 namespace math
22 {
23 /** \addtogroup stats_grp
24  * @{
25  */
26 
27 /** @name Probability density distributions (pdf) distance metrics
28 @{ */
29 
30 /** Computes the squared mahalanobis distance of a vector X given the mean MU
31  * and the covariance *inverse* COV_inv
32  * \f[ d^2 = (X-MU)^\top \Sigma^{-1} (X-MU) \f]
33  */
34 template <class VECTORLIKE1, class VECTORLIKE2, class MAT>
36  const VECTORLIKE1& X, const VECTORLIKE2& MU, const MAT& COV)
37 {
39 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
40  ASSERT_(!X.empty());
41  ASSERT_(X.size() == MU.size());
42  ASSERT_(X.size() == COV.rows() && COV.isSquare());
43 #endif
44  const size_t N = X.size();
46  for (size_t i = 0; i < N; i++) X_MU[i] = X[i] - MU[i];
47  auto z = 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.inverse_LLt(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 {
114  ASSERTDEB_(size_t(cov.cols()) == size_t(delta_mu.size()));
115  return multiply_HtCH_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 CMatrixDynamic<T>& COV1,
138  const CMatrixDynamic<T>& COV2)
139 {
140  const size_t vector_dim = mean_diffs.size();
141  ASSERT_(vector_dim >= 1);
142 
143  CMatrixDynamic<T> C = COV1;
144  C += COV2; // Sum of covs:
145  const T cov_det = C.det();
146  CMatrixDynamic<T> C_inv;
147  C.inverse_LLt(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, const CMatrixFixed<T, DIM, DIM>& COV1,
161  const CMatrixFixed<T, DIM, DIM>& COV2)
162 {
163  ASSERT_(mean_diffs.size() == DIM);
164 
165  CMatrixFixed<T, DIM, DIM> C = COV1;
166  C += COV2; // Sum of covs:
167  const T cov_det = C.det();
169  C.inverse_LLt(C_inv);
170 
171  return std::pow(M_2PI, -0.5 * DIM) * (1.0 / std::sqrt(cov_det)) *
172  exp(-0.5 * mean_diffs.multiply_HCHt_scalar(C_inv));
173 }
174 
175 /** Computes both, the integral of the product of two Gaussians and their square
176  * Mahalanobis distance.
177  * \sa productIntegralTwoGaussians, mahalanobisDistance2
178  */
179 template <typename T, class VECLIKE, class MATLIKE1, class MATLIKE2>
181  const VECLIKE& mean_diffs, const MATLIKE1& COV1, const MATLIKE2& COV2,
182  T& maha2_out, T& intprod_out, const MATLIKE1* CROSS_COV12 = nullptr)
183 {
184  const size_t vector_dim = mean_diffs.size();
185  ASSERT_(vector_dim >= 1);
186 
187  MATLIKE1 C = COV1;
188  C += COV2; // Sum of covs:
189  if (CROSS_COV12)
190  {
191  C -= *CROSS_COV12;
192  C -= *CROSS_COV12;
193  }
194  const T cov_det = C.det();
195  MATLIKE1 C_inv;
196  C.inverse_LLt(C_inv);
197 
198  maha2_out = mrpt::math::multiply_HCHt_scalar(mean_diffs, C_inv);
199  intprod_out = std::pow(M_2PI, -0.5 * vector_dim) *
200  (1.0 / std::sqrt(cov_det)) * exp(-0.5 * maha2_out);
201 }
202 
203 /** Computes both, the logarithm of the PDF and the square Mahalanobis distance
204  * between a point (given by its difference wrt the mean) and a Gaussian.
205  * \sa productIntegralTwoGaussians, mahalanobisDistance2, normalPDF,
206  * mahalanobisDistance2AndPDF
207  */
208 template <typename T, class VECLIKE, class MATRIXLIKE>
210  const VECLIKE& diff_mean, const MATRIXLIKE& cov, T& maha2_out,
211  T& log_pdf_out)
212 {
213  MRPT_START
215  ASSERTDEB_(size_t(cov.cols()) == size_t(diff_mean.size()));
216  const MATRIXLIKE C_inv = cov.inverse_LLt();
217  maha2_out = multiply_HtCH_scalar(diff_mean, C_inv);
218  log_pdf_out = static_cast<typename MATRIXLIKE::Scalar>(-0.5) *
219  (maha2_out +
220  static_cast<typename MATRIXLIKE::Scalar>(cov.cols()) *
221  ::log(static_cast<typename MATRIXLIKE::Scalar>(M_2PI)) +
222  ::log(cov.det()));
223  MRPT_END
224 }
225 
226 /** Computes both, the PDF and the square Mahalanobis distance between a point
227  * (given by its difference wrt the mean) and a Gaussian.
228  * \sa productIntegralTwoGaussians, mahalanobisDistance2, normalPDF
229  */
230 template <typename T, class VECLIKE, class MATRIXLIKE>
232  const VECLIKE& diff_mean, const MATRIXLIKE& cov, T& maha2_out, T& pdf_out)
233 {
234  mahalanobisDistance2AndLogPDF(diff_mean, cov, maha2_out, pdf_out);
235  pdf_out = std::exp(pdf_out); // log to linear
236 }
237 
238 /** Computes covariances and mean of any vector of containers, given optional
239  * weights for the different samples.
240  * \param elements Any kind of vector of vectors/arrays, eg.
241  * std::vector<mrpt::math::CVectorDouble>, with all the input samples, each
242  * sample in a "row".
243  * \param covariances Output estimated covariance; it can be a fixed/dynamic
244  * matrix or a matrixview.
245  * \param means Output estimated mean; it can be
246  * CVectorDouble/CVectorFixedDouble, etc... \param weights_mean If !=nullptr, it
247  * must point to a vector of size()==number of elements, with normalized weights
248  * to take into account for the mean. \param weights_cov If !=nullptr, it must
249  * point to a vector of size()==number of elements, with normalized weights to
250  * take into account for the covariance. \param elem_do_wrap2pi If !=nullptr; it
251  * must point to an array of "bool" of size()==dimension of each element,
252  * stating if it's needed to do a wrap to
253  * [-pi,pi] to each dimension.
254  * \sa This method is used in mrpt::math::unscented_transform_gaussian
255  * \ingroup stats_grp
256  */
257 template <
258  class VECTOR_OF_VECTORS, class MATRIXLIKE, class VECTORLIKE,
259  class VECTORLIKE2, class VECTORLIKE3>
260 inline void
261  covariancesAndMeanWeighted( // Done inline to speed-up the special case
262  // expanded in covariancesAndMean() below.
263  const VECTOR_OF_VECTORS& elements, MATRIXLIKE& covariances,
264  VECTORLIKE& means, const VECTORLIKE2* weights_mean,
265  const VECTORLIKE3* weights_cov, const bool* elem_do_wrap2pi = nullptr)
266 {
267  ASSERTMSG_(
268  elements.size() != 0,
269  "No samples provided, so there is no way to deduce the output size.");
270  using T = typename MATRIXLIKE::Scalar;
271  const size_t DIM = elements[0].size();
272  means.resize(DIM);
273  covariances.resize(DIM, DIM);
274  const size_t nElms = elements.size();
275  const T NORM = 1.0 / nElms;
276  if (weights_mean)
277  {
278  ASSERTDEB_(size_t(weights_mean->size()) == size_t(nElms));
279  }
280  // The mean goes first:
281  for (size_t i = 0; i < DIM; i++)
282  {
283  T accum = 0;
284  if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
285  { // i'th dimension is a "normal", real number:
286  if (weights_mean)
287  {
288  for (size_t j = 0; j < nElms; j++)
289  accum += (*weights_mean)[j] * elements[j][i];
290  }
291  else
292  {
293  for (size_t j = 0; j < nElms; j++) accum += elements[j][i];
294  accum *= NORM;
295  }
296  }
297  else
298  { // i'th dimension is a circle in [-pi,pi]: we need a little trick
299  // here:
300  double accum_L = 0, accum_R = 0;
301  double Waccum_L = 0, Waccum_R = 0;
302  for (size_t j = 0; j < nElms; j++)
303  {
304  double ang = elements[j][i];
305  const double w =
306  weights_mean != nullptr ? (*weights_mean)[j] : NORM;
307  if (fabs(ang) > 0.5 * M_PI)
308  { // LEFT HALF: 0,2pi
309  if (ang < 0) ang = (M_2PI + ang);
310  accum_L += ang * w;
311  Waccum_L += w;
312  }
313  else
314  { // RIGHT HALF: -pi,pi
315  accum_R += ang * w;
316  Waccum_R += w;
317  }
318  }
319  if (Waccum_L > 0) accum_L /= Waccum_L; // [0,2pi]
320  if (Waccum_R > 0) accum_R /= Waccum_R; // [-pi,pi]
321  if (accum_L > M_PI)
322  accum_L -= M_2PI; // Left side to [-pi,pi] again:
323  accum =
324  (accum_L * Waccum_L +
325  accum_R * Waccum_R); // The overall result:
326  }
327  means[i] = accum;
328  }
329  // Now the covariance:
330  for (size_t i = 0; i < DIM; i++)
331  for (size_t j = 0; j <= i; j++) // Only 1/2 of the matrix
332  {
333  typename MATRIXLIKE::Scalar elem = 0;
334  if (weights_cov)
335  {
336  ASSERTDEB_(size_t(weights_cov->size()) == size_t(nElms));
337  for (size_t k = 0; k < nElms; k++)
338  {
339  const T Ai = (elements[k][i] - means[i]);
340  const T Aj = (elements[k][j] - means[j]);
341  if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
342  elem += (*weights_cov)[k] * Ai * Aj;
343  else
344  elem += (*weights_cov)[k] * mrpt::math::wrapToPi(Ai) *
346  }
347  }
348  else
349  {
350  for (size_t k = 0; k < nElms; k++)
351  {
352  const T Ai = (elements[k][i] - means[i]);
353  const T Aj = (elements[k][j] - means[j]);
354  if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
355  elem += Ai * Aj;
356  else
357  elem +=
359  }
360  elem *= NORM;
361  }
362  covariances(i, j) = elem;
363  if (i != j) covariances(j, i) = elem;
364  }
365 }
366 
367 /** Computes covariances and mean of any vector of containers.
368  * \param elements Any kind of vector of vectors/arrays, eg.
369  * std::vector<mrpt::math::CVectorDouble>, with all the input samples, each
370  * sample in a "row".
371  * \param covariances Output estimated covariance; it can be a fixed/dynamic
372  * matrix or a matrixview.
373  * \param means Output estimated mean; it can be
374  * CVectorDouble/CVectorFixedDouble, etc... \param elem_do_wrap2pi If !=nullptr;
375  * it must point to an array of "bool" of size()==dimension of each element,
376  * stating if it's needed to do a wrap to
377  * [-pi,pi] to each dimension.
378  * \ingroup stats_grp
379  */
380 template <class VECTOR_OF_VECTORS, class MATRIXLIKE, class VECTORLIKE>
382  const VECTOR_OF_VECTORS& elements, MATRIXLIKE& covariances,
383  VECTORLIKE& means, const bool* elem_do_wrap2pi = nullptr)
384 { // The function below is inline-expanded here:
386  VECTOR_OF_VECTORS, MATRIXLIKE, VECTORLIKE, CVectorDouble,
387  CVectorDouble>(
388  elements, covariances, means, nullptr, nullptr, elem_do_wrap2pi);
389 }
390 
391 /** Computes the weighted histogram for a vector of values and their
392  * corresponding weights.
393  * \param values [IN] The N values
394  * \param weights [IN] The weights for the corresponding N values (don't need
395  * to be normalized)
396  * \param binWidth [IN] The desired width of the bins
397  * \param out_binCenters [OUT] The centers of the M bins generated to cover
398  * from the minimum to the maximum value of "values" with the given "binWidth"
399  * \param out_binValues [OUT] The ratio of values at each given bin, such as
400  * the whole vector sums up the unity.
401  * \sa weightedHistogramLog
402  */
403 template <class VECTORLIKE1, class VECTORLIKE2>
405  const VECTORLIKE1& values, const VECTORLIKE1& weights, float binWidth,
406  VECTORLIKE2& out_binCenters, VECTORLIKE2& out_binValues)
407 {
408  MRPT_START
410 
411  ASSERT_(values.size() == weights.size());
412  ASSERT_(binWidth > 0);
413  TNum minBin = minimum(values);
414  auto nBins =
415  static_cast<unsigned>(ceil((maximum(values) - minBin) / binWidth));
416 
417  // Generate bin center and border values:
418  out_binCenters.resize(nBins);
419  out_binValues.clear();
420  out_binValues.resize(nBins, 0);
421  TNum halfBin = TNum(0.5) * binWidth;
422  ;
423  VECTORLIKE2 binBorders(nBins + 1, minBin - halfBin);
424  for (unsigned int i = 0; i < nBins; i++)
425  {
426  binBorders[i + 1] = binBorders[i] + binWidth;
427  out_binCenters[i] = binBorders[i] + halfBin;
428  }
429 
430  // Compute the histogram:
431  TNum totalSum = 0;
432  typename VECTORLIKE1::const_iterator itVal, itW;
433  for (itVal = values.begin(), itW = weights.begin(); itVal != values.end();
434  ++itVal, ++itW)
435  {
436  int idx = round(((*itVal) - minBin) / binWidth);
437  if (idx >= int(nBins)) idx = nBins - 1;
438  ASSERTDEB_(idx >= 0);
439  out_binValues[idx] += *itW;
440  totalSum += *itW;
441  }
442 
443  if (totalSum) out_binValues /= totalSum;
444 
445  MRPT_END
446 }
447 
448 /** Computes the weighted histogram for a vector of values and their
449  * corresponding log-weights.
450  * \param values [IN] The N values
451  * \param weights [IN] The log-weights for the corresponding N values (don't
452  * need to be normalized)
453  * \param binWidth [IN] The desired width of the bins
454  * \param out_binCenters [OUT] The centers of the M bins generated to cover
455  * from the minimum to the maximum value of "values" with the given "binWidth"
456  * \param out_binValues [OUT] The ratio of values at each given bin, such as
457  * the whole vector sums up the unity.
458  * \sa weightedHistogram
459  */
460 template <class VECTORLIKE1, class VECTORLIKE2>
462  const VECTORLIKE1& values, const VECTORLIKE1& log_weights, float binWidth,
463  VECTORLIKE2& out_binCenters, VECTORLIKE2& out_binValues)
464 {
465  MRPT_START
467 
468  ASSERT_(values.size() == log_weights.size());
469  ASSERT_(binWidth > 0);
470  TNum minBin = minimum(values);
471  auto nBins =
472  static_cast<unsigned>(ceil((maximum(values) - minBin) / binWidth));
473 
474  // Generate bin center and border values:
475  out_binCenters.resize(nBins);
476  out_binValues.clear();
477  out_binValues.resize(nBins, 0);
478  TNum halfBin = TNum(0.5) * binWidth;
479  ;
480  VECTORLIKE2 binBorders(nBins + 1, minBin - halfBin);
481  for (unsigned int i = 0; i < nBins; i++)
482  {
483  binBorders[i + 1] = binBorders[i] + binWidth;
484  out_binCenters[i] = binBorders[i] + halfBin;
485  }
486 
487  // Compute the histogram:
488  const TNum max_log_weight = maximum(log_weights);
489  TNum totalSum = 0;
490  typename VECTORLIKE1::const_iterator itVal, itW;
491  for (itVal = values.begin(), itW = log_weights.begin();
492  itVal != values.end(); ++itVal, ++itW)
493  {
494  int idx = round(((*itVal) - minBin) / binWidth);
495  if (idx >= int(nBins)) idx = nBins - 1;
496  ASSERTDEB_(idx >= 0);
497  const TNum w = exp(*itW - max_log_weight);
498  out_binValues[idx] += w;
499  totalSum += w;
500  }
501 
502  if (totalSum) out_binValues /= totalSum;
503 
504  MRPT_END
505 }
506 
507 /** A numerically-stable method to compute average likelihood values with
508  * strongly different ranges (unweighted likelihoods: compute the arithmetic
509  * mean).
510  * This method implements this equation:
511  *
512  * \f[ return = - \log N + \log \sum_{i=1}^N e^{ll_i-ll_{max}} + ll_{max} \f]
513  *
514  * See also the <a
515  * href="http://www.mrpt.org/Averaging_Log-Likelihood_Values:Numerical_Stability">tutorial
516  * page</a>.
517  * \ingroup stats_grp
518  */
519 double averageLogLikelihood(const CVectorDouble& logLikelihoods);
520 
521 /** Computes the average of a sequence of angles in radians taking into account
522  * the correct wrapping in the range \f$ ]-\pi,\pi [ \f$, for example, the mean
523  * of (2,-2) is \f$ \pi \f$, not 0.
524  * \ingroup stats_grp
525  */
526 double averageWrap2Pi(const CVectorDouble& angles);
527 
528 /** A numerically-stable method to average likelihood values with strongly
529  * different ranges (weighted likelihoods).
530  * This method implements this equation:
531  *
532  * \f[ return = \log \left( \frac{1}{\sum_i e^{lw_i}} \sum_i e^{lw_i}
533  * e^{ll_i} \right) \f]
534  *
535  * See also the <a
536  * href="http://www.mrpt.org/Averaging_Log-Likelihood_Values:Numerical_Stability">tutorial
537  * page</a>.
538  * \ingroup stats_grp
539  */
540 double averageLogLikelihood(
541  const CVectorDouble& logWeights, const CVectorDouble& logLikelihoods);
542 
543 /** @} */ // end of grouping container_ops_grp
544 
545 } // namespace math
546 } // namespace mrpt
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:231
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
MAT_C::Scalar multiply_HtCH_scalar(const VECTOR_H &H, const MAT_C &C)
r (scalar) = H^t*C*H (H: column vector, C: symmetric matrix)
Definition: ops_matrices.h:54
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
double Scalar
Definition: KmUtils.h:43
#define MRPT_START
Definition: exceptions.h:241
#define M_2PI
Definition: common.h:58
Template for column vectors of dynamic size, compatible with Eigen.
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 (scalar) = H*C*H^t (H: row vector, C: symmetric matrix)
Definition: ops_matrices.h:63
bool isSquare() const
returns true if matrix is NxN
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:180
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
T productIntegralTwoGaussians(const std::vector< T > &mean_diffs, const CMatrixDynamic< T > &COV1, const CMatrixDynamic< T > &COV2)
Computes the integral of the product of two Gaussians, with means separated by "mean_diffs" and covar...
Definition: data_utils.h:136
Derived inverse() const
Returns the inverse of a general matrix using LU.
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:209
CVectorDynamic< double > CVectorDouble
T dot(const CVectorDynamic< T > &v) const
dot product of this \cdot v
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:381
double averageLogLikelihood(const CVectorDouble &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
Definition: math.cpp:293
CONTAINER::Scalar maximum(const CONTAINER &v)
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
CMatrixDynamic< T > inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
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 cols() const
Number of columns in the matrix.
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:35
ContainerType<T>::element_t exposes the value of any STL or Eigen container.
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:404
CONTAINER::Scalar minimum(const CONTAINER &v)
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:190
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:245
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:461
This template class provides the basic functionality for a general 2D any-size, resizable container o...
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020