From 48e087e083842dd3dcbe8fad1a003849e7c4cb47 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 14 Sep 2024 15:14:02 -0400
Subject: [PATCH] fix error using value for HybridNonlinearFactor

---
 gtsam/hybrid/HybridGaussianFactor.h  | 2 +-
 gtsam/hybrid/HybridNonlinearFactor.h | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index e40713f46..f52022348 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -119,7 +119,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   /// @name Standard API
   /// @{
 
-  /// Get the factor and scalar at a given discrete assignment.
+  /// Get factor at a given discrete assignment.
   sharedFactor operator()(const DiscreteValues &assignment) const;
 
   /**
diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h
index 1bd25a6b1..c3f127f4a 100644
--- a/gtsam/hybrid/HybridNonlinearFactor.h
+++ b/gtsam/hybrid/HybridNonlinearFactor.h
@@ -138,7 +138,7 @@ class HybridNonlinearFactor : public HybridFactor {
     auto errorFunc =
         [continuousValues](const std::pair<sharedFactor, double>& f) {
           auto [factor, val] = f;
-          return factor->error(continuousValues) + val;
+          return factor->error(continuousValues) + (0.5 * val * val);
         };
     DecisionTree<Key, double> result(factors_, errorFunc);
     return result;
@@ -157,7 +157,7 @@ class HybridNonlinearFactor : public HybridFactor {
     auto [factor, val] = factors_(discreteValues);
     // Compute the error for the selected factor
     const double factorError = factor->error(continuousValues);
-    return factorError + val;
+    return factorError + (0.5 * val * val);
   }
 
   /**