diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 7cdff914f..ecfa02282 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -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); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index af1c5d80e..d25efb2e1 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -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