double cMathUtil::CalcGaussianPartition(const Eigen::VectorXd& covar)
{
int data_size = static_cast<int>(covar.size());
double det = covar.prod();
double partition = std::sqrt(std::pow(2 * M_PI, data_size) * det);
return partition;
}
double cMathUtil::EvalGaussianLogp(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample)
{
int data_size = static_cast<int>(covar.size());
Eigen::VectorXd diff = sample - mean;
double logp = -0.5 * diff.dot(diff.cwiseQuotient(covar));
double det = covar.prod();
logp += -0.5 * (data_size * std::log(2 * M_PI) + std::log(det));
return logp;
}