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


Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019