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;
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);
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;