From 2a5833bf6a78d5493a6ac273cbcfafa3691fe0b1 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jan 2025 14:02:15 -0500
Subject: [PATCH] custom ProductAndNormalize for TableFactor

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 13 +++----------
 gtsam/hybrid/HybridGaussianFactorGraph.h   | 10 ++++++++++
 2 files changed, 13 insertions(+), 10 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index b9f327021..502a52742 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -24,7 +24,6 @@
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteJunctionTree.h>
 #include <gtsam/discrete/DiscreteKey.h>
-#include <gtsam/discrete/DiscreteTableConditional.h>
 #include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/discrete/TableFactor.h>
 #include <gtsam/hybrid/HybridConditional.h>
@@ -256,14 +255,8 @@ static TableFactor::shared_ptr DiscreteFactorFromErrors(
   return std::make_shared<TableFactor>(discreteKeys, potentials);
 }
 
-/**
- * @brief Multiply all the `factors` and normalize the
- * product to prevent underflow.
- *
- * @param factors The factors to multiply as a DiscreteFactorGraph.
- * @return TableFactor
- */
-static TableFactor ProductAndNormalize(const DiscreteFactorGraph &factors) {
+/* ************************************************************************ */
+TableFactor TableProductAndNormalize(const DiscreteFactorGraph &factors) {
   // PRODUCT: multiply all factors
 #if GTSAM_HYBRID_TIMING
   gttic_(DiscreteProduct);
@@ -352,7 +345,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
 #endif
   /**** NOTE: This does sum-product. ****/
   // Get product factor
-  TableFactor product = ProductAndNormalize(dfg);
+  TableFactor product = TableProductAndNormalize(dfg);
 
 #if GTSAM_HYBRID_TIMING
   gttic_(EliminateDiscreteSum);
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h
index e3c1e2d55..9803975cf 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.h
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.h
@@ -20,6 +20,7 @@
 
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/discrete/DiscreteTableConditional.h>
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
@@ -270,4 +271,13 @@ template <>
 struct traits<HybridGaussianFactorGraph>
     : public Testable<HybridGaussianFactorGraph> {};
 
+/**
+ * @brief Multiply all the `factors` and normalize the
+ * product to prevent underflow.
+ *
+ * @param factors The factors to multiply as a DiscreteFactorGraph.
+ * @return TableFactor
+ */
+TableFactor TableProductAndNormalize(const DiscreteFactorGraph& factors);
+
 }  // namespace gtsam