Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
13#include <mrpt/math/wrap2pi.h>
16
17namespace 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 {
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);
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
73 const VECTORLIKE &mean_diffs,
74 const MAT1 &COV1,
75 const MAT2 &COV2,
76 const MAT3 &CROSS_COV12 )
77 {
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 const size_t N = size(COV1,1);
86 MAT1 COV = COV1;
87 COV+=COV2;
88 COV.substract_An(CROSS_COV12,2);
89 MAT1 COV_inv;
90 COV.inv_fast(COV_inv);
91 return multiply_HCHt_scalar(mean_diffs,COV_inv);
93 }
94
95 /** 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.
96 * \f[ d = \sqrt{ \Delta_\mu^\top (\Sigma_1 + \Sigma_2 - 2 \Sigma_12 )^{-1} \Delta_\mu } \f]
97 */
98 template<class VECTORLIKE,class MAT1,class MAT2,class MAT3> inline typename VECTORLIKE::Scalar
100 const VECTORLIKE &mean_diffs,
101 const MAT1 &COV1,
102 const MAT2 &COV2,
103 const MAT3 &CROSS_COV12 )
104 {
105 return std::sqrt( mahalanobisDistance( mean_diffs, COV1,COV2,CROSS_COV12 ));
106 }
107
108 /** 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.
109 * \f[ d^2 = \Delta_\mu^\top \Sigma^{-1} \Delta_\mu \f]
110 */
111 template<class VECTORLIKE,class MATRIXLIKE>
112 inline typename MATRIXLIKE::Scalar
113 mahalanobisDistance2(const VECTORLIKE &delta_mu,const MATRIXLIKE &cov)
114 {
115 ASSERTDEB_(cov.isSquare())
116 ASSERTDEB_(cov.getColCount()==delta_mu.size())
117 return multiply_HCHt_scalar(delta_mu,cov.inverse());
118 }
119
120 /** 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.
121 * \f[ d^2 = \sqrt( \Delta_\mu^\top \Sigma^{-1} \Delta_\mu ) \f]
122 */
123 template<class VECTORLIKE,class MATRIXLIKE>
124 inline typename MATRIXLIKE::Scalar
125 mahalanobisDistance(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 by "mean_diffs" and covariances "COV1" and "COV2".
131 * \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]
132 */
133 template <typename T>
135 const std::vector<T> &mean_diffs,
136 const CMatrixTemplateNumeric<T> &COV1,
137 const CMatrixTemplateNumeric<T> &COV2
138 )
139 {
140 const size_t vector_dim = mean_diffs.size();
141 ASSERT_(vector_dim>=1)
142
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 by "mean_diffs" and covariances "COV1" and "COV2".
154 * \f[ D = \frac{1}{(2 \pi)^{0.5 N} \sqrt{} } \exp( \Delta_\mu^\top (\Sigma_1 + \Sigma_2)^{-1} \Delta_\mu) \f]
155 */
156 template <typename T, size_t DIM>
158 const std::vector<T> &mean_diffs,
161 )
162 {
163 ASSERT_(mean_diffs.size()==DIM);
164
166 C+= COV2; // Sum of covs:
167 const T cov_det = C.det();
169 C.inv_fast(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 Mahalanobis distance.
176 * \sa productIntegralTwoGaussians, mahalanobisDistance2
177 */
178 template <typename T, class VECLIKE,class MATLIKE1, class MATLIKE2>
180 const VECLIKE &mean_diffs,
181 const MATLIKE1 &COV1,
182 const MATLIKE2 &COV2,
183 T &maha2_out,
184 T &intprod_out,
185 const MATLIKE1 *CROSS_COV12=NULL
186 )
187 {
188 const size_t vector_dim = mean_diffs.size();
189 ASSERT_(vector_dim>=1)
190
191 MATLIKE1 C = COV1;
192 C+= COV2; // Sum of covs:
193 if (CROSS_COV12) { C-=*CROSS_COV12; C-=*CROSS_COV12; }
194 const T cov_det = C.det();
195 MATLIKE1 C_inv;
196 C.inv_fast(C_inv);
197
198 maha2_out = mean_diffs.multiply_HCHt_scalar(C_inv);
199 intprod_out = std::pow( M_2PI, -0.5*vector_dim ) * (1.0/std::sqrt( cov_det ))*exp(-0.5*maha2_out);
200 }
201
202 /** 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.
203 * \sa productIntegralTwoGaussians, mahalanobisDistance2, normalPDF, mahalanobisDistance2AndPDF
204 */
205 template <typename T, class VECLIKE,class MATRIXLIKE>
207 const VECLIKE &diff_mean,
208 const MATRIXLIKE &cov,
209 T &maha2_out,
210 T &log_pdf_out)
211 {
213 ASSERTDEB_(cov.isSquare())
214 ASSERTDEB_(size_t(cov.getColCount())==size_t(diff_mean.size()))
215 MATRIXLIKE C_inv;
216 cov.inv(C_inv);
217 maha2_out = multiply_HCHt_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.getColCount())*::log(static_cast<typename MATRIXLIKE::Scalar>(M_2PI))+
221 ::log(cov.det())
222 );
224 }
225
226 /** Computes both, the PDF and the square Mahalanobis distance between a point (given by its difference wrt the mean) and a Gaussian.
227 * \sa productIntegralTwoGaussians, mahalanobisDistance2, normalPDF
228 */
229 template <typename T, class VECLIKE,class MATRIXLIKE>
231 const VECLIKE &diff_mean,
232 const MATRIXLIKE &cov,
233 T &maha2_out,
234 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
241 /** Computes covariances and mean of any vector of containers, given optional weights for the different samples.
242 * \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".
243 * \param covariances Output estimated covariance; it can be a fixed/dynamic matrix or a matrixview.
244 * \param means Output estimated mean; it can be CVectorDouble/CArrayDouble, etc...
245 * \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.
246 * \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.
247 * \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.
248 * \sa This method is used in mrpt::math::unscented_transform_gaussian
249 * \ingroup stats_grp
250 */
251 template<class VECTOR_OF_VECTORS, class MATRIXLIKE,class VECTORLIKE,class VECTORLIKE2,class VECTORLIKE3>
252 inline void covariancesAndMeanWeighted( // Done inline to speed-up the special case expanded in covariancesAndMean() below.
253 const VECTOR_OF_VECTORS &elements,
254 MATRIXLIKE &covariances,
255 VECTORLIKE &means,
256 const VECTORLIKE2 *weights_mean,
257 const VECTORLIKE3 *weights_cov,
258 const bool *elem_do_wrap2pi = NULL
259 )
260 {
261 ASSERTMSG_(elements.size()!=0,"No samples provided, so there is no way to deduce the output size.")
262 typedef typename MATRIXLIKE::Scalar T;
263 const size_t DIM = elements[0].size();
264 means.resize(DIM);
265 covariances.setSize(DIM,DIM);
266 const size_t nElms=elements.size();
267 const T NORM=1.0/nElms;
268 if (weights_mean) { ASSERTDEB_(size_t(weights_mean->size())==size_t(nElms)) }
269 // The mean goes first:
270 for (size_t i=0;i<DIM;i++)
271 {
272 T accum = 0;
273 if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
274 { // i'th dimension is a "normal", real number:
275 if (weights_mean)
276 {
277 for (size_t j=0;j<nElms;j++)
278 accum+= (*weights_mean)[j] * elements[j][i];
279 }
280 else
281 {
282 for (size_t j=0;j<nElms;j++) accum+=elements[j][i];
283 accum*=NORM;
284 }
285 }
286 else
287 { // i'th dimension is a circle in [-pi,pi]: we need a little trick here:
288 double accum_L=0,accum_R=0;
289 double Waccum_L=0,Waccum_R=0;
290 for (size_t j=0;j<nElms;j++)
291 {
292 double ang = elements[j][i];
293 const double w = weights_mean!=NULL ? (*weights_mean)[j] : NORM;
294 if (fabs( ang )>0.5*M_PI)
295 { // LEFT HALF: 0,2pi
296 if (ang<0) ang = (M_2PI + ang);
297 accum_L += ang * w;
298 Waccum_L += w;
299 }
300 else
301 { // RIGHT HALF: -pi,pi
302 accum_R += ang * w;
303 Waccum_R += w;
304 }
305 }
306 if (Waccum_L>0) accum_L /= Waccum_L; // [0,2pi]
307 if (Waccum_R>0) accum_R /= Waccum_R; // [-pi,pi]
308 if (accum_L>M_PI) accum_L -= M_2PI; // Left side to [-pi,pi] again:
309 accum = (accum_L* Waccum_L + accum_R * Waccum_R ); // The overall result:
310 }
311 means[i]=accum;
312 }
313 // Now the covariance:
314 for (size_t i=0;i<DIM;i++)
315 for (size_t j=0;j<=i;j++) // Only 1/2 of the matrix
316 {
317 typename MATRIXLIKE::Scalar elem=0;
318 if (weights_cov)
319 {
320 ASSERTDEB_(size_t(weights_cov->size())==size_t(nElms))
321 for (size_t k=0;k<nElms;k++)
322 {
323 const T Ai = (elements[k][i]-means[i]);
324 const T Aj = (elements[k][j]-means[j]);
325 if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
326 elem+= (*weights_cov)[k] * Ai * Aj;
327 else elem+= (*weights_cov)[k] * mrpt::math::wrapToPi(Ai) * mrpt::math::wrapToPi(Aj);
328 }
329 }
330 else
331 {
332 for (size_t k=0;k<nElms;k++)
333 {
334 const T Ai = (elements[k][i]-means[i]);
335 const T Aj = (elements[k][j]-means[j]);
336 if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
337 elem+= Ai * Aj;
338 else elem+= mrpt::math::wrapToPi(Ai) * mrpt::math::wrapToPi(Aj);
339 }
340 elem*=NORM;
341 }
342 covariances.get_unsafe(i,j) = elem;
343 if (i!=j) covariances.get_unsafe(j,i)=elem;
344 }
345 }
346
347 /** Computes covariances and mean of any vector of containers.
348 * \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".
349 * \param covariances Output estimated covariance; it can be a fixed/dynamic matrix or a matrixview.
350 * \param means Output estimated mean; it can be CVectorDouble/CArrayDouble, etc...
351 * \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.
352 * \ingroup stats_grp
353 */
354 template<class VECTOR_OF_VECTORS, class MATRIXLIKE,class VECTORLIKE>
355 void covariancesAndMean(const VECTOR_OF_VECTORS &elements,MATRIXLIKE &covariances,VECTORLIKE &means, const bool *elem_do_wrap2pi = NULL)
356 { // The function below is inline-expanded here:
357 covariancesAndMeanWeighted<VECTOR_OF_VECTORS,MATRIXLIKE,VECTORLIKE,CVectorDouble,CVectorDouble>(elements,covariances,means,NULL,NULL,elem_do_wrap2pi);
358 }
359
360
361 /** Computes the weighted histogram for a vector of values and their corresponding weights.
362 * \param values [IN] The N values
363 * \param weights [IN] The weights for the corresponding N values (don't need to be normalized)
364 * \param binWidth [IN] The desired width of the bins
365 * \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"
366 * \param out_binValues [OUT] The ratio of values at each given bin, such as the whole vector sums up the unity.
367 * \sa weightedHistogramLog
368 */
369 template<class VECTORLIKE1,class VECTORLIKE2>
371 const VECTORLIKE1 &values,
372 const VECTORLIKE1 &weights,
373 float binWidth,
374 VECTORLIKE2 &out_binCenters,
375 VECTORLIKE2 &out_binValues )
376 {
379
380 ASSERT_( values.size() == weights.size() );
381 ASSERT_( binWidth > 0 );
382 TNum minBin = minimum( values );
383 unsigned int nBins = static_cast<unsigned>(ceil((maximum( values )-minBin) / binWidth));
384
385 // Generate bin center and border values:
386 out_binCenters.resize(nBins);
387 out_binValues.clear(); out_binValues.resize(nBins,0);
388 TNum halfBin = TNum(0.5)*binWidth;;
389 VECTORLIKE2 binBorders(nBins+1,minBin-halfBin);
390 for (unsigned int i=0;i<nBins;i++)
391 {
392 binBorders[i+1] = binBorders[i]+binWidth;
393 out_binCenters[i] = binBorders[i]+halfBin;
394 }
395
396 // Compute the histogram:
397 TNum totalSum = 0;
398 typename VECTORLIKE1::const_iterator itVal, itW;
399 for (itVal = values.begin(), itW = weights.begin(); itVal!=values.end(); ++itVal, ++itW )
400 {
401 int idx = round(((*itVal)-minBin)/binWidth);
402 if (idx>=int(nBins)) idx=nBins-1;
403 ASSERTDEB_(idx>=0);
404 out_binValues[idx] += *itW;
405 totalSum+= *itW;
406 }
407
408 if (totalSum)
409 out_binValues /= totalSum;
410
411
413 }
414
415 /** Computes the weighted histogram for a vector of values and their corresponding log-weights.
416 * \param values [IN] The N values
417 * \param weights [IN] The log-weights for the corresponding N values (don't need to be normalized)
418 * \param binWidth [IN] The desired width of the bins
419 * \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"
420 * \param out_binValues [OUT] The ratio of values at each given bin, such as the whole vector sums up the unity.
421 * \sa weightedHistogram
422 */
423 template<class VECTORLIKE1,class VECTORLIKE2>
425 const VECTORLIKE1 &values,
426 const VECTORLIKE1 &log_weights,
427 float binWidth,
428 VECTORLIKE2 &out_binCenters,
429 VECTORLIKE2 &out_binValues )
430 {
433
434 ASSERT_( values.size() == log_weights.size() );
435 ASSERT_( binWidth > 0 );
436 TNum minBin = minimum( values );
437 unsigned int nBins = static_cast<unsigned>(ceil((maximum( values )-minBin) / binWidth));
438
439 // Generate bin center and border values:
440 out_binCenters.resize(nBins);
441 out_binValues.clear(); out_binValues.resize(nBins,0);
442 TNum halfBin = TNum(0.5)*binWidth;;
443 VECTORLIKE2 binBorders(nBins+1,minBin-halfBin);
444 for (unsigned int i=0;i<nBins;i++)
445 {
446 binBorders[i+1] = binBorders[i]+binWidth;
447 out_binCenters[i] = binBorders[i]+halfBin;
448 }
449
450 // Compute the histogram:
451 const TNum max_log_weight = maximum(log_weights);
452 TNum totalSum = 0;
453 typename VECTORLIKE1::const_iterator itVal, itW;
454 for (itVal = values.begin(), itW = log_weights.begin(); itVal!=values.end(); ++itVal, ++itW )
455 {
456 int idx = round(((*itVal)-minBin)/binWidth);
457 if (idx>=int(nBins)) idx=nBins-1;
458 ASSERTDEB_(idx>=0);
459 const TNum w = exp(*itW-max_log_weight);
460 out_binValues[idx] += w;
461 totalSum+= w;
462 }
463
464 if (totalSum)
465 out_binValues /= totalSum;
466
468 }
469
470 /** A numerically-stable method to compute average likelihood values with strongly different ranges (unweighted likelihoods: compute the arithmetic mean).
471 * This method implements this equation:
472 *
473 * \f[ return = - \log N + \log \sum_{i=1}^N e^{ll_i-ll_{max}} + ll_{max} \f]
474 *
475 * See also the <a href="http://www.mrpt.org/Averaging_Log-Likelihood_Values:Numerical_Stability">tutorial page</a>.
476 * \ingroup stats_grp
477 */
478 double BASE_IMPEXP averageLogLikelihood( const CVectorDouble &logLikelihoods );
479
480 /** 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.
481 * \ingroup stats_grp
482 */
484
485 /** A numerically-stable method to average likelihood values with strongly different ranges (weighted likelihoods).
486 * This method implements this equation:
487 *
488 * \f[ return = \log \left( \frac{1}{\sum_i e^{lw_i}} \sum_i e^{lw_i} e^{ll_i} \right) \f]
489 *
490 * See also the <a href="http://www.mrpt.org/Averaging_Log-Likelihood_Values:Numerical_Stability">tutorial page</a>.
491 * \ingroup stats_grp
492 */
494 const CVectorDouble &logWeights,
495 const CVectorDouble &logLikelihoods );
496
497 /** @} */ // end of grouping container_ops_grp
498
499 } // End of MATH namespace
500} // End of namespace
501
502#endif
#define M_PI
Definition: bits.h:65
A numeric matrix of compile-time fixed size.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: types_math.h:65
EIGEN_STRONG_INLINE Scalar maximum() const
[VECTORS OR MATRICES] Finds the maximum value
EIGEN_STRONG_INLINE Scalar minimum() const
[VECTORS OR MATRICES] Finds the minimum value
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
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:179
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:370
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:134
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:206
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
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
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...
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:355
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:424
double BASE_IMPEXP averageLogLikelihood(const CVectorDouble &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
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:252
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:230
#define MRPT_START
Definition: mrpt_macros.h:349
#define ASSERT_(f)
Definition: mrpt_macros.h:261
#define M_2PI
Definition: mrpt_macros.h:363
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: mrpt_macros.h:283
#define MRPT_END
Definition: mrpt_macros.h:353
#define ASSERTMSG_(f, __ERROR_MSG)
Definition: mrpt_macros.h:260
@ UNINITIALIZED_MATRIX
Definition: math_frwds.h:75
size_t size(const MATRIXLIKE &m, int dim)
Definition: bits.h:38
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
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
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
CONTAINER::value_type element_t
Definition: math_frwds.h:85



Page generated by Doxygen 1.9.5 for MRPT 1.4.0 SVN: at Sun Nov 27 02:56:59 UTC 2022