33 template <
class VECTORLIKE1,
class VECTORLIKE2,
class MAT>
35 const VECTORLIKE1& X,
const VECTORLIKE2& MU,
const MAT& COV)
38 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
41 ASSERT_(X.size() == COV.rows() && COV.isSquare());
43 const size_t N = X.size();
44 Eigen::Matrix<typename MAT::Scalar, Eigen::Dynamic, 1> X_MU(N);
45 for (
size_t i = 0; i < N; i++) X_MU[i] = X[i] - MU[i];
46 const Eigen::Matrix<typename MAT::Scalar, Eigen::Dynamic, 1>
z =
47 COV.llt().solve(X_MU);
56 template <
class VECTORLIKE1,
class VECTORLIKE2,
class MAT>
58 const VECTORLIKE1& X,
const VECTORLIKE2& MU,
const MAT& COV)
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)
75 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
77 ASSERT_(mean_diffs.size() == COV1.rows());
78 ASSERT_(COV1.isSquare() && COV2.isSquare());
79 ASSERT_(COV1.rows() == COV2.rows());
83 COV.substract_An(CROSS_COV12, 2);
85 COV.inv_fast(COV_inv);
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)
109 template <
class VECTORLIKE,
class MATRIXLIKE>
111 const VECTORLIKE& delta_mu,
const MATRIXLIKE&
cov)
123 template <
class VECTORLIKE,
class MATRIXLIKE>
125 const VECTORLIKE& delta_mu,
const MATRIXLIKE&
cov)
135 template <
typename T>
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));
158 template <
typename T,
size_t DIM>
160 const std::vector<T>& mean_diffs,
164 ASSERT_(mean_diffs.size() == DIM);
168 const T cov_det = C.det();
172 return std::pow(
M_2PI, -0.5 * DIM) * (1.0 / std::sqrt(cov_det)) *
173 exp(-0.5 * mean_diffs.multiply_HCHt_scalar(C_inv));
180 template <
typename T,
class VECLIKE,
class MATLIKE1,
class MATLIKE2>
182 const VECLIKE& mean_diffs,
const MATLIKE1& COV1,
const MATLIKE2& COV2,
183 T& maha2_out, T& intprod_out,
const MATLIKE1* CROSS_COV12 =
nullptr)
185 const size_t vector_dim = mean_diffs.size();
195 const T cov_det = C.det();
199 maha2_out = mean_diffs.multiply_HCHt_scalar(C_inv);
200 intprod_out = std::pow(
M_2PI, -0.5 * vector_dim) *
201 (1.0 / std::sqrt(cov_det)) * exp(-0.5 * maha2_out);
209 template <
typename T,
class VECLIKE,
class MATRIXLIKE>
211 const VECLIKE& diff_mean,
const MATRIXLIKE&
cov, T& maha2_out,
232 template <
typename T,
class VECLIKE,
class MATRIXLIKE>
234 const VECLIKE& diff_mean,
const MATRIXLIKE&
cov, T& maha2_out, T& pdf_out)
237 pdf_out = std::exp(pdf_out);
260 template<
class VECTOR_OF_VECTORS,
class MATRIXLIKE,
class VECTORLIKE,
class VECTORLIKE2,
class VECTORLIKE3>
262 const VECTOR_OF_VECTORS &elements,
263 MATRIXLIKE &covariances,
265 const VECTORLIKE2 *weights_mean,
266 const VECTORLIKE3 *weights_cov,
267 const bool *elem_do_wrap2pi =
nullptr
271 elements.size() != 0,
272 "No samples provided, so there is no way to deduce the output size.");
274 const size_t DIM = elements[0].size();
276 covariances.setSize(DIM, DIM);
277 const size_t nElms = elements.size();
278 const T NORM = 1.0 / nElms;
281 ASSERTDEB_(
size_t(weights_mean->size()) ==
size_t(nElms));
284 for (
size_t i = 0; i < DIM; i++)
287 if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
291 for (
size_t j = 0; j < nElms; j++)
292 accum += (*weights_mean)[j] * elements[j][i];
296 for (
size_t j = 0; j < nElms; j++) accum += elements[j][i];
303 double accum_L = 0, accum_R = 0;
304 double Waccum_L = 0, Waccum_R = 0;
305 for (
size_t j = 0; j < nElms; j++)
307 double ang = elements[j][i];
309 weights_mean !=
nullptr ? (*weights_mean)[j] : NORM;
310 if (fabs(ang) > 0.5 *
M_PI)
312 if (ang < 0) ang = (
M_2PI + ang);
322 if (Waccum_L > 0) accum_L /= Waccum_L;
323 if (Waccum_R > 0) accum_R /= Waccum_R;
327 (accum_L * Waccum_L +
333 for (
size_t i = 0; i < DIM; i++)
334 for (
size_t j = 0; j <= i; j++)
339 ASSERTDEB_(
size_t(weights_cov->size()) ==
size_t(nElms));
340 for (
size_t k = 0; k < nElms; k++)
342 const T Ai = (elements[k][i] - means[i]);
343 const T Aj = (elements[k][j] - means[j]);
344 if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
345 elem += (*weights_cov)[k] * Ai * Aj;
353 for (
size_t k = 0; k < nElms; k++)
355 const T Ai = (elements[k][i] - means[i]);
356 const T Aj = (elements[k][j] - means[j]);
357 if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
365 covariances.get_unsafe(i, j) = elem;
366 if (i != j) covariances.get_unsafe(j, i) = elem;
383 template <
class VECTOR_OF_VECTORS,
class MATRIXLIKE,
class VECTORLIKE>
385 const VECTOR_OF_VECTORS& elements, MATRIXLIKE& covariances,
386 VECTORLIKE& means,
const bool* elem_do_wrap2pi =
nullptr)
390 elements, covariances, means,
nullptr,
nullptr, elem_do_wrap2pi);
405 template <
class VECTORLIKE1,
class VECTORLIKE2>
407 const VECTORLIKE1&
values,
const VECTORLIKE1&
weights,
float binWidth,
408 VECTORLIKE2& out_binCenters, VECTORLIKE2& out_binValues)
417 static_cast<unsigned>(ceil((
maximum(
values) - minBin) / binWidth));
420 out_binCenters.resize(nBins);
421 out_binValues.clear();
422 out_binValues.resize(nBins, 0);
423 TNum halfBin = TNum(0.5) * binWidth;
425 VECTORLIKE2 binBorders(nBins + 1, minBin - halfBin);
426 for (
unsigned int i = 0; i < nBins; i++)
428 binBorders[i + 1] = binBorders[i] + binWidth;
429 out_binCenters[i] = binBorders[i] + halfBin;
438 int idx =
round(((*itVal) - minBin) / binWidth);
439 if (idx >=
int(nBins)) idx = nBins - 1;
441 out_binValues[idx] += *itW;
445 if (totalSum) out_binValues /= totalSum;
462 template <
class VECTORLIKE1,
class VECTORLIKE2>
464 const VECTORLIKE1&
values,
const VECTORLIKE1& log_weights,
float binWidth,
465 VECTORLIKE2& out_binCenters, VECTORLIKE2& out_binValues)
474 static_cast<unsigned>(ceil((
maximum(
values) - minBin) / binWidth));
477 out_binCenters.resize(nBins);
478 out_binValues.clear();
479 out_binValues.resize(nBins, 0);
480 TNum halfBin = TNum(0.5) * binWidth;
482 VECTORLIKE2 binBorders(nBins + 1, minBin - halfBin);
483 for (
unsigned int i = 0; i < nBins; i++)
485 binBorders[i + 1] = binBorders[i] + binWidth;
486 out_binCenters[i] = binBorders[i] + halfBin;
490 const TNum max_log_weight =
maximum(log_weights);
493 for (itVal =
values.begin(), itW = log_weights.begin();
494 itVal !=
values.end(); ++itVal, ++itW)
496 int idx =
round(((*itVal) - minBin) / binWidth);
497 if (idx >=
int(nBins)) idx = nBins - 1;
499 const TNum
w = exp(*itW - max_log_weight);
500 out_binValues[idx] +=
w;
504 if (totalSum) out_binValues /= totalSum;