From 4ae5596200b48146a4158f7f85087eace0404d70 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 8 Oct 2024 23:21:46 -0400
Subject: [PATCH] add back PotentiallyPrunedComponentError as an inline
 function

---
 gtsam/hybrid/HybridGaussianFactor.cpp | 13 +++++++++----
 1 file changed, 9 insertions(+), 4 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
index e24545293..b31fdca20 100644
--- a/gtsam/hybrid/HybridGaussianFactor.cpp
+++ b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -175,13 +175,19 @@ HybridGaussianProductFactor HybridGaussianFactor::asProductFactor() const {
            }}};
 }
 
+/* *******************************************************************************/
+inline static double PotentiallyPrunedComponentError(
+    const GaussianFactorValuePair& pair, const VectorValues& continuousValues) {
+  return pair.first ? pair.first->error(continuousValues) + pair.second
+                    : std::numeric_limits<double>::infinity();
+}
+
 /* *******************************************************************************/
 AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
     const VectorValues& continuousValues) const {
   // functor to convert from sharedFactor to double error value.
   auto errorFunc = [&continuousValues](const GaussianFactorValuePair& pair) {
-    return pair.first ? pair.first->error(continuousValues) + pair.second
-                      : std::numeric_limits<double>::infinity();
+    return PotentiallyPrunedComponentError(pair, continuousValues);
   };
   DecisionTree<Key, double> error_tree(factors_, errorFunc);
   return error_tree;
@@ -191,8 +197,7 @@ AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
 double HybridGaussianFactor::error(const HybridValues& values) const {
   // Directly index to get the component, no need to build the whole tree.
   const GaussianFactorValuePair pair = factors_(values.discrete());
-  return pair.first ? pair.first->error(values.continuous()) + pair.second
-                    : std::numeric_limits<double>::infinity();
+  return PotentiallyPrunedComponentError(pair, values.continuous());
 }
 
 }  // namespace gtsam