diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index c31a9836f..b408eea9c 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -51,14 +51,14 @@ HybridGaussianConditional::HybridGaussianConditional( discreteParents, GetFactorValuePairs(conditionals)), BaseConditional(continuousFrontals.size()), conditionals_(conditionals) { - // Calculate logNormalizer_ as the minimum of the log normalizers of the + // Calculate logConstant_ as the minimum of the log normalizers of the // conditionals, by visiting the decision tree: - logNormalizer_ = std::numeric_limits::infinity(); + logConstant_ = std::numeric_limits::infinity(); conditionals_.visit( [this](const GaussianConditional::shared_ptr &conditional) { if (conditional) { - this->logNormalizer_ = std::min( - this->logNormalizer_, -conditional->logNormalizationConstant()); + this->logConstant_ = std::min( + this->logConstant_, -conditional->logNormalizationConstant()); } }); } @@ -85,7 +85,7 @@ GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree() // First check if conditional has not been pruned if (gc) { const double Cgm_Kgcm = - -gc->logNormalizationConstant() - this->logNormalizer_; + -this->logConstant_ - gc->logNormalizationConstant(); // 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) { @@ -216,7 +216,7 @@ std::shared_ptr HybridGaussianConditional::likelihood( -> GaussianFactorValuePair { const auto likelihood_m = conditional->likelihood(given); const double Cgm_Kgcm = - -conditional->logNormalizationConstant() - logNormalizer_; + -logConstant_ - conditional->logNormalizationConstant(); if (Cgm_Kgcm == 0.0) { return {likelihood_m, 0.0}; } else { @@ -330,7 +330,7 @@ double HybridGaussianConditional::conditionalError( // Check if valid pointer if (conditional) { return conditional->error(continuousValues) + // - -conditional->logNormalizationConstant() - logNormalizer_; + -logConstant_ - conditional->logNormalizationConstant(); } else { // If not valid, pointer, it means this conditional was pruned, // so we return maximum error. diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 5c3e500bd..72a999472 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -64,8 +64,9 @@ class GTSAM_EXPORT HybridGaussianConditional private: Conditionals conditionals_; ///< a decision tree of Gaussian conditionals. - double logNormalizer_; ///< log of the normalization constant - ///< (log(\sqrt(|2πΣ|))). + ///< Negative-log of the normalization constant (log(\sqrt(|2πΣ|))). + ///< Take advantage of the neg-log space so everything is a minimization + double logConstant_; /** * @brief Convert a HybridGaussianConditional of conditionals into @@ -151,7 +152,7 @@ class GTSAM_EXPORT HybridGaussianConditional /// The log normalization constant is max of the the individual /// log-normalization constants. - double logNormalizationConstant() const override { return -logNormalizer_; } + double logNormalizationConstant() const override { return -logConstant_; } /** * Create a likelihood factor for a hybrid Gaussian conditional,