From b4633865140a3a6a2a473da400c42797a234c663 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 31 Dec 2022 18:27:24 -0500
Subject: [PATCH] Made SumFrontals a method to test

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp |  7 ++---
 gtsam/hybrid/HybridGaussianFactorGraph.h   | 32 +++++++++++++++++++---
 2 files changed, 31 insertions(+), 8 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 05b617fce..e03e58f7a 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -81,15 +81,14 @@ static GaussianMixtureFactor::Sum &addGaussian(
 }
 
 /* ************************************************************************ */
-static GaussianMixtureFactor::Sum sumFrontals(
-    const HybridGaussianFactorGraph &factors) {
+GaussianMixtureFactor::Sum HybridGaussianFactorGraph::SumFrontals() const {
   // sum out frontals, this is the factor on the separator
   gttic(sum);
 
   GaussianMixtureFactor::Sum sum;
   std::vector<GaussianFactor::shared_ptr> deferredFactors;
 
-  for (auto &f : factors) {
+  for (auto &f : factors_) {
     if (f->isHybrid()) {
       // TODO(dellaert): just use a virtual method defined in HybridFactor.
       if (auto gm = boost::dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
@@ -194,7 +193,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
 
   // Collect all the frontal factors to create Gaussian factor graphs
   // indexed on the discrete keys.
-  GaussianMixtureFactor::Sum sum = sumFrontals(factors);
+  GaussianMixtureFactor::Sum sum = factors.SumFrontals();
 
   // If a tree leaf contains nullptr,
   // convert that leaf to an empty GaussianFactorGraph.
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h
index c851adfe5..db0687642 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.h
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.h
@@ -21,6 +21,7 @@
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
+#include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/inference/EliminateableFactorGraph.h>
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/Ordering.h>
@@ -118,14 +119,12 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
       : Base(graph) {}
 
   /// @}
+  /// @name Adding factors.
+  /// @{
 
-  using Base::empty;
   using Base::reserve;
-  using Base::size;
-  using Base::operator[];
   using Base::add;
   using Base::push_back;
-  using Base::resize;
 
   /// Add a Jacobian factor to the factor graph.
   void add(JacobianFactor&& factor);
@@ -172,6 +171,25 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
     }
   }
 
+  /// @}
+  /// @name Testable
+  /// @{
+
+  // TODO(dellaert):  customize print and equals.
+  // void print(const std::string& s = "HybridGaussianFactorGraph",
+  //            const KeyFormatter& keyFormatter = DefaultKeyFormatter) const
+  //     override;
+  // bool equals(const This& fg, double tol = 1e-9) const override;
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  using Base::empty;
+  using Base::size;
+  using Base::operator[];
+  using Base::resize;
+
   /**
    * @brief Compute error for each discrete assignment,
    * and return as a tree.
@@ -217,6 +235,12 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
    * @return const Ordering
    */
   const Ordering getHybridOrdering() const;
+
+  /// Compute a DecisionTree<Key, GaussianFactorGraph> with the marginal for
+  /// each discrete assignment.
+  GaussianMixtureFactor::Sum SumFrontals() const;
+
+  /// @}
 };
 
 }  // namespace gtsam