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;
}
double cMathUtil::EvalGaussian(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample)
{
assert(mean.size() == covar.size());
assert(sample.size() == covar.size());
Eigen::VectorXd diff = sample - mean;
double exp_val = diff.dot(diff.cwiseQuotient(covar));
double likelihood = std::exp(-0.5 * exp_val);
double partition = CalcGaussianPartition(covar);
likelihood /= partition;
return likelihood;
}