diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index e07b300fa..e60368717 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -125,7 +125,8 @@ AlgebraicDecisionTree GaussianMixtureFactor::error( /* *******************************************************************************/ double GaussianMixtureFactor::error(const HybridValues &values) const { const FactorAndConstant factor_z = factors_(values.discrete()); - return factor_z.factor->error(values.continuous()) + factor_z.constant; + return factor_z.error(values.continuous()); } +/* *******************************************************************************/ } // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index aca4f365b..a96f253ce 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -58,9 +58,11 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { sharedFactor factor; double constant; - // Return error with constant added. + // Return error with constant correction. double error(const VectorValues &values) const { - return factor->error(values) + constant; + // Note minus sign: constant is log of normalization constant for probabilities. + // Errors is the negative log-likelihood, hence we subtract the constant here. + return factor->error(values) - constant; } // Check pointer equality.