From 3a8220c26471daee80af1292e87529794564dd0e Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 30 Dec 2022 13:59:20 -0500
Subject: [PATCH] Fixed error calculation

---
 gtsam/hybrid/GaussianMixtureFactor.cpp | 3 ++-
 gtsam/hybrid/GaussianMixtureFactor.h   | 6 ++++--
 2 files changed, 6 insertions(+), 3 deletions(-)

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<Key> 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.