diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 9cf19638c..3e74cebb4 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -465,6 +465,12 @@ string DiscreteConditional::html(const KeyFormatter& keyFormatter, double DiscreteConditional::evaluate(const HybridValues& x) const { return this->evaluate(x.discrete()); } + +/* ************************************************************************* */ +double DiscreteConditional::errorConstant() const { + return 0.0; +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 4c7af1489..53331a0be 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -264,11 +264,12 @@ class GTSAM_EXPORT DiscreteConditional } /** - * logNormalizationConstant K is just zero, such that + * errorConstant is just zero, such that * logProbability(x) = log(evaluate(x)) = - error(x) * and hence error(x) = - log(evaluate(x)) > 0 for all x. + * Thus -log(K) for the normalization constant k is 0. */ - double logNormalizationConstant() const override { return 0.0; } + double errorConstant() const override; /// @} diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 3cb3bba65..57d8bba39 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -161,18 +161,18 @@ double HybridConditional::logProbability(const HybridValues &values) const { } /* ************************************************************************ */ -double HybridConditional::logNormalizationConstant() const { +double HybridConditional::errorConstant() const { if (auto gc = asGaussian()) { - return gc->logNormalizationConstant(); + return gc->errorConstant(); } if (auto gm = asHybrid()) { - return gm->logNormalizationConstant(); // 0.0! + return gm->errorConstant(); // 0.0! } if (auto dc = asDiscrete()) { - return dc->logNormalizationConstant(); // 0.0! + return dc->errorConstant(); // 0.0! } throw std::runtime_error( - "HybridConditional::logProbability: conditional type not handled"); + "HybridConditional::errorConstant: conditional type not handled"); } /* ************************************************************************ */ diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 51eeeb5bb..92118385c 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -193,11 +193,12 @@ class GTSAM_EXPORT HybridConditional double logProbability(const HybridValues& values) const override; /** - * Return the log normalization constant. + * Return the negative log of the normalization constant. + * This shows up in the error as -(error(x) + errorConstant) * Note this is 0.0 for discrete and hybrid conditionals, but depends * on the continuous parameters for Gaussian conditionals. */ - double logNormalizationConstant() const override; + double errorConstant() const override; /// Return the probability (or density) of the underlying conditional. double evaluate(const HybridValues& values) const override; diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 4ee5241a3..f6cd3e5f7 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -36,7 +36,7 @@ HybridGaussianFactor::FactorValuePairs GetFactorValuePairs( // Check if conditional is pruned if (conditional) { // Assign log(\sqrt(|2πΣ|)) = -log(1 / sqrt(|2πΣ|)) - value = conditional->logNormalizationConstant(); + value = conditional->errorConstant(); } return {std::dynamic_pointer_cast(conditional), value}; }; @@ -57,8 +57,8 @@ HybridGaussianConditional::HybridGaussianConditional( conditionals_.visit( [this](const GaussianConditional::shared_ptr &conditional) { if (conditional) { - this->logConstant_ = std::min( - this->logConstant_, conditional->logNormalizationConstant()); + this->logConstant_ = + std::min(this->logConstant_, conditional->errorConstant()); } }); } @@ -84,8 +84,7 @@ GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree() auto wrap = [this](const GaussianConditional::shared_ptr &gc) { // First check if conditional has not been pruned if (gc) { - const double Cgm_Kgcm = - gc->logNormalizationConstant() - this->logConstant_; + const double Cgm_Kgcm = gc->errorConstant() - this->logConstant_; // If there is a difference in the covariances, we need to account for // that since the error is dependent on the mode. if (Cgm_Kgcm > 0.0) { @@ -215,8 +214,7 @@ std::shared_ptr HybridGaussianConditional::likelihood( [&](const GaussianConditional::shared_ptr &conditional) -> GaussianFactorValuePair { const auto likelihood_m = conditional->likelihood(given); - const double Cgm_Kgcm = - conditional->logNormalizationConstant() - logConstant_; + const double Cgm_Kgcm = conditional->errorConstant() - logConstant_; if (Cgm_Kgcm == 0.0) { return {likelihood_m, 0.0}; } else { diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 676c45f61..8851c7741 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -150,9 +150,15 @@ class GTSAM_EXPORT HybridGaussianConditional /// Returns the continuous keys among the parents. KeyVector continuousParents() const; - /// The log normalization constant is max of the the individual - /// log-normalization constants. - double logNormalizationConstant() const override { return logConstant_; } + /** + * @brief Return log normalization constant in negative log space. + * + * The log normalization constant is the max of the individual + * log-normalization constants. + * + * @return double + */ + inline double errorConstant() const override { return logConstant_; } /** * Create a likelihood factor for a hybrid Gaussian conditional, diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index ff9ca7a55..a6c15d1fd 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -329,9 +329,9 @@ static std::shared_ptr createDiscreteFactor( // Logspace version of: // exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); - // logNormalizationConstant gives `-log(k)` + // errorConstant gives `-log(k)` // which is `-log(k) = log(1/k) = log(\sqrt{|2πΣ|})`. - return -factor->error(kEmpty) + conditional->logNormalizationConstant(); + return -factor->error(kEmpty) + conditional->errorConstant(); }; AlgebraicDecisionTree logProbabilities( @@ -357,7 +357,7 @@ static std::shared_ptr createHybridGaussianFactor( // Add 2.0 term since the constant term will be premultiplied by 0.5 // as per the Hessian definition, // and negative since we want log(k) - hf->constantTerm() += -2.0 * conditional->logNormalizationConstant(); + hf->constantTerm() += -2.0 * conditional->errorConstant(); } return {factor, 0.0}; }; diff --git a/gtsam/inference/Conditional-inst.h b/gtsam/inference/Conditional-inst.h index b61a5ac22..ba42a9a24 100644 --- a/gtsam/inference/Conditional-inst.h +++ b/gtsam/inference/Conditional-inst.h @@ -59,10 +59,17 @@ double Conditional::evaluate( /* ************************************************************************* */ template -double Conditional::logNormalizationConstant() +double Conditional::errorConstant() const { throw std::runtime_error( - "Conditional::logNormalizationConstant is not implemented"); + "Conditional::errorConstant is not implemented"); +} + +/* ************************************************************************* */ +template +double Conditional::logNormalizationConstant() + const { + return -errorConstant(); } /* ************************************************************************* */ @@ -89,7 +96,7 @@ bool Conditional::CheckInvariants( // normalization constant const double error = conditional.error(values); if (error < 0.0) return false; // prob_or_density is negative. - const double expected = -(conditional.logNormalizationConstant() + error); + const double expected = -(conditional.errorConstant() + error); if (std::abs(logProb - expected) > 1e-9) return false; // logProb is not consistent with error return true; diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 936a078a8..d09341489 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -164,9 +164,16 @@ namespace gtsam { } /** - * All conditional types need to implement a - * (negative) log normalization constant - * to make it such that error>=0. + * @brief All conditional types need to implement this as the negative log + * of the normalization constant. + * + * @return double + */ + virtual double errorConstant() const; + + /** + * All conditional types need to implement a log normalization constant to + * make it such that error>=0. */ virtual double logNormalizationConstant() const; diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index b7f296ea5..978475bcb 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -247,14 +247,15 @@ namespace gtsam { /* normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma)) logConstant = -log(normalizationConstant) - = 0.5 * n*log(2*pi) + 0.5 * log(det(Sigma)) - - log(det(Sigma)) = -2.0 * logDeterminant() - thus, logConstant = 0.5*n*log(2*pi) - logDeterminant() + = -0.5 * n*log(2*pi) - 0.5 * log(det(Sigma)) - BayesNet logConstant = sum(0.5*n_i*log(2*pi) - logDeterminant_i()) - = sum(0.5*n_i*log(2*pi)) - sum(logDeterminant_i()) - = sum(0.5*n_i*log(2*pi)) - bn->logDeterminant() + log(det(Sigma)) = -2.0 * logDeterminant() + thus, logConstant = -0.5*n*log(2*pi) + logDeterminant() + + BayesNet logConstant = sum(-0.5*n_i*log(2*pi) + logDeterminant_i()) + = sum(-0.5*n_i*log(2*pi)) + sum(logDeterminant_i()) + = sum(-0.5*n_i*log(2*pi)) + bn->logDeterminant() + = sum(logNormalizationConstant_i) */ double logNormConst = 0.0; for (const sharedConditional& cg : *this) { diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index fc633d04d..ca4a1ed33 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -182,7 +182,7 @@ namespace gtsam { /* ************************************************************************* */ // normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma)) // neg-log = 0.5 * n*log(2*pi) + 0.5 * log det(Sigma) - double GaussianConditional::logNormalizationConstant() const { + double GaussianConditional::errorConstant() const { constexpr double log2pi = 1.8378770664093454835606594728112; size_t n = d().size(); // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1} @@ -195,10 +195,10 @@ namespace gtsam { } /* ************************************************************************* */ - // density = 1/k exp(-error(x)) - // log = -log(k) - error(x) + // density = k exp(-error(x)) + // log = log(k) - error(x) double GaussianConditional::logProbability(const VectorValues& x) const { - return -logNormalizationConstant() - error(x); + return logNormalizationConstant() - error(x); } double GaussianConditional::logProbability(const HybridValues& x) const { diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 757687ed8..31f3797d5 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -133,12 +133,14 @@ namespace gtsam { /// @{ /** - * Return normalization constant in negative log space. + * @brief Return the negative log of the normalization constant. * * normalization constant k = 1.0 / sqrt((2*pi)^n*det(Sigma)) * -log(k) = 0.5 * n*log(2*pi) + 0.5 * log det(Sigma) + * + * @return double */ - double logNormalizationConstant() const override; + double errorConstant() const override; /** * Calculate log-probability log(evaluate(x)) for given values `x`: diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index ea9ec1aec..39901584b 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -255,7 +255,7 @@ double Gaussian::logDeterminant() const { } /* *******************************************************************************/ -double Gaussian::logNormalizationConstant() const { +double Gaussian::errorConstant() const { // log(det(Sigma)) = -2.0 * logDetR // which gives neg-log = 0.5*n*log(2*pi) + 0.5*(-2.0 * logDetR()) // = 0.5*n*log(2*pi) - (0.5*2.0 * logDetR()) @@ -266,6 +266,10 @@ double Gaussian::logNormalizationConstant() const { return 0.5 * n * log2pi - logDetR(); } +/* *******************************************************************************/ +double Gaussian::logNormalizationConstant() const { + return -errorConstant(); +} /* ************************************************************************* */ // Diagonal diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 31ffd1107..851dc3c1f 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -271,11 +271,17 @@ namespace gtsam { /// Compute the log of |Σ| double logDeterminant() const; + /** + * @brief Compute the negative log of the normalization constant + * for a Gaussian noise model k = \sqrt(1/|2πΣ|). + * + * @return double + */ + double errorConstant() const; + /** * @brief Method to compute the normalization constant * for a Gaussian noise model k = \sqrt(1/|2πΣ|). - * We compute this in the negative log-space for numerical accuracy, - * thus returning -log(k). * * @return double */ diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 9c448de50..71eb94dd5 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -548,6 +548,7 @@ virtual class GaussianConditional : gtsam::JacobianFactor { bool equals(const gtsam::GaussianConditional& cg, double tol) const; // Standard Interface + double errorConstant() const; double logNormalizationConstant() const; double logProbability(const gtsam::VectorValues& x) const; double evaluate(const gtsam::VectorValues& x) const;