normalization constant

release/4.3a0
Frank Dellaert 2022-12-30 13:58:54 -05:00
parent f22ada6c0a
commit ff51f4221e
2 changed files with 30 additions and 13 deletions

View File

@ -168,26 +168,30 @@ namespace gtsam {
/* ************************************************************************* */
double GaussianConditional::logDeterminant() const {
double logDet;
if (this->get_model()) {
Vector diag = this->R().diagonal();
this->get_model()->whitenInPlace(diag);
logDet = diag.unaryExpr([](double x) { return log(x); }).sum();
if (get_model()) {
Vector diag = R().diagonal();
get_model()->whitenInPlace(diag);
return diag.unaryExpr([](double x) { return log(x); }).sum();
} else {
logDet =
this->R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
return R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
}
return logDet;
}
/* ************************************************************************* */
// density = exp(-error(x)) / sqrt((2*pi)^n*det(Sigma))
// log = -error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
double GaussianConditional::logDensity(const VectorValues& x) const {
// normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
// log = - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
double GaussianConditional::logNormalizationConstant() const {
constexpr double log2pi = 1.8378770664093454835606594728112;
size_t n = d().size();
// log det(Sigma)) = - 2.0 * logDeterminant()
return - error(x) - 0.5 * n * log2pi + logDeterminant();
return - 0.5 * n * log2pi + logDeterminant();
}
/* ************************************************************************* */
// density = k exp(-error(x))
// log = log(k) -error(x) - 0.5 * n*log(2*pi)
double GaussianConditional::logDensity(const VectorValues& x) const {
return logNormalizationConstant() - error(x);
}
/* ************************************************************************* */

View File

@ -169,7 +169,7 @@ namespace gtsam {
*
* @return double
*/
double determinant() const { return exp(this->logDeterminant()); }
inline double determinant() const { return exp(logDeterminant()); }
/**
* @brief Compute the log determinant of the R matrix.
@ -184,6 +184,19 @@ namespace gtsam {
*/
double logDeterminant() const;
/**
* normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
* log = - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
*/
double logNormalizationConstant() const;
/**
* normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
*/
inline double normalizationConstant() const {
return exp(logNormalizationConstant());
}
/**
* Solves a conditional Gaussian and writes the solution into the entries of
* \c x for each frontal variable of the conditional. The parents are