9#ifndef MRPT_DATA_UTILS_MATH_H
10#define MRPT_DATA_UTILS_MATH_H
33 template<
class VECTORLIKE1,
class VECTORLIKE2,
class MAT>
36 const VECTORLIKE2 &MU,
40 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
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);
57 template<
class VECTORLIKE1,
class VECTORLIKE2,
class MAT>
60 const VECTORLIKE2 &MU,
70 template<
class VECTORLIKE,
class MAT1,
class MAT2,
class MAT3>
73 const VECTORLIKE &mean_diffs,
76 const MAT3 &CROSS_COV12 )
79 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
82 ASSERT_( COV1.isSquare() && COV2.isSquare() );
85 const size_t N =
size(COV1,1);
88 COV.substract_An(CROSS_COV12,2);
90 COV.inv_fast(COV_inv);
98 template<
class VECTORLIKE,
class MAT1,
class MAT2,
class MAT3>
inline typename VECTORLIKE::Scalar
100 const VECTORLIKE &mean_diffs,
103 const MAT3 &CROSS_COV12 )
111 template<
class VECTORLIKE,
class MATRIXLIKE>
112 inline typename MATRIXLIKE::Scalar
123 template<
class VECTORLIKE,
class MATRIXLIKE>
124 inline typename MATRIXLIKE::Scalar
133 template <
typename T>
135 const std::vector<T> &mean_diffs,
140 const size_t vector_dim = mean_diffs.size();
145 const T cov_det = C.det();
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) );
156 template <
typename T,
size_t DIM>
158 const std::vector<T> &mean_diffs,
163 ASSERT_(mean_diffs.size()==DIM);
167 const T cov_det = C.det();
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) );
178 template <
typename T,
class VECLIKE,
class MATLIKE1,
class MATLIKE2>
180 const VECLIKE &mean_diffs,
181 const MATLIKE1 &COV1,
182 const MATLIKE2 &COV2,
185 const MATLIKE1 *CROSS_COV12=NULL
188 const size_t vector_dim = mean_diffs.size();
193 if (CROSS_COV12) { C-=*CROSS_COV12; C-=*CROSS_COV12; }
194 const T cov_det = C.det();
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);
205 template <
typename T,
class VECLIKE,
class MATRIXLIKE>
207 const VECLIKE &diff_mean,
208 const MATRIXLIKE &
cov,
214 ASSERTDEB_(
size_t(
cov.getColCount())==
size_t(diff_mean.size()))
218 log_pdf_out =
static_cast<typename MATRIXLIKE::Scalar
>(-0.5)* (
220 static_cast<typename MATRIXLIKE::Scalar
>(
cov.getColCount())*::log(
static_cast<typename MATRIXLIKE::Scalar
>(
M_2PI))+
229 template <
typename T,
class VECLIKE,
class MATRIXLIKE>
231 const VECLIKE &diff_mean,
232 const MATRIXLIKE &
cov,
237 pdf_out = std::exp(pdf_out);
251 template<
class VECTOR_OF_VECTORS,
class MATRIXLIKE,
class VECTORLIKE,
class VECTORLIKE2,
class VECTORLIKE3>
253 const VECTOR_OF_VECTORS &elements,
254 MATRIXLIKE &covariances,
256 const VECTORLIKE2 *weights_mean,
257 const VECTORLIKE3 *weights_cov,
258 const bool *elem_do_wrap2pi = NULL
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();
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)) }
270 for (
size_t i=0;i<DIM;i++)
273 if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
277 for (
size_t j=0;j<nElms;j++)
278 accum+= (*weights_mean)[j] * elements[j][i];
282 for (
size_t j=0;j<nElms;j++) accum+=elements[j][i];
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++)
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)
296 if (ang<0) ang = (
M_2PI + ang);
306 if (Waccum_L>0) accum_L /= Waccum_L;
307 if (Waccum_R>0) accum_R /= Waccum_R;
309 accum = (accum_L* Waccum_L + accum_R * Waccum_R );
314 for (
size_t i=0;i<DIM;i++)
315 for (
size_t j=0;j<=i;j++)
317 typename MATRIXLIKE::Scalar elem=0;
320 ASSERTDEB_(
size_t(weights_cov->size())==
size_t(nElms))
321 for (
size_t k=0;k<nElms;k++)
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;
332 for (
size_t k=0;k<nElms;k++)
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])
342 covariances.get_unsafe(i,j) = elem;
343 if (i!=j) covariances.get_unsafe(j,i)=elem;
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)
357 covariancesAndMeanWeighted<VECTOR_OF_VECTORS,MATRIXLIKE,VECTORLIKE,CVectorDouble,CVectorDouble>(elements,covariances,means,NULL,NULL,elem_do_wrap2pi);
369 template<
class VECTORLIKE1,
class VECTORLIKE2>
371 const VECTORLIKE1 &values,
372 const VECTORLIKE1 &weights,
374 VECTORLIKE2 &out_binCenters,
375 VECTORLIKE2 &out_binValues )
380 ASSERT_( values.size() == weights.size() );
382 TNum minBin =
minimum( values );
383 unsigned int nBins =
static_cast<unsigned>(ceil((
maximum( values )-minBin) / binWidth));
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++)
392 binBorders[i+1] = binBorders[i]+binWidth;
393 out_binCenters[i] = binBorders[i]+halfBin;
398 typename VECTORLIKE1::const_iterator itVal, itW;
399 for (itVal = values.begin(), itW = weights.begin(); itVal!=values.end(); ++itVal, ++itW )
401 int idx = round(((*itVal)-minBin)/binWidth);
402 if (idx>=
int(nBins)) idx=nBins-1;
404 out_binValues[idx] += *itW;
409 out_binValues /= totalSum;
423 template<
class VECTORLIKE1,
class VECTORLIKE2>
425 const VECTORLIKE1 &values,
426 const VECTORLIKE1 &log_weights,
428 VECTORLIKE2 &out_binCenters,
429 VECTORLIKE2 &out_binValues )
434 ASSERT_( values.size() == log_weights.size() );
436 TNum minBin =
minimum( values );
437 unsigned int nBins =
static_cast<unsigned>(ceil((
maximum( values )-minBin) / binWidth));
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++)
446 binBorders[i+1] = binBorders[i]+binWidth;
447 out_binCenters[i] = binBorders[i]+halfBin;
451 const TNum max_log_weight =
maximum(log_weights);
453 typename VECTORLIKE1::const_iterator itVal, itW;
454 for (itVal = values.begin(), itW = log_weights.begin(); itVal!=values.end(); ++itVal, ++itW )
456 int idx = round(((*itVal)-minBin)/binWidth);
457 if (idx>=
int(nBins)) idx=nBins-1;
459 const TNum w = exp(*itW-max_log_weight);
460 out_binValues[idx] += w;
465 out_binValues /= totalSum;
A numeric matrix of compile-time fixed size.
A matrix of dynamic size.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
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.
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.
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.
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...
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...
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 ...
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.
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.
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.
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...
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...
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
#define ASSERTMSG_(f, __ERROR_MSG)
size_t size(const MATRIXLIKE &m, int dim)
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,...
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)
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