From 095f6ad7cc96d284fb663a4cfb62f0d5f48f9204 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sat, 12 Mar 2022 12:42:58 -0500
Subject: [PATCH] Add full elimination

---
 gtsam/hybrid/HybridFactorGraph.h             | 12 +++++++-----
 gtsam/hybrid/tests/testHybridConditional.cpp | 13 +++++++++++++
 2 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h
index 13bbd005d..9a57d36e6 100644
--- a/gtsam/hybrid/HybridFactorGraph.h
+++ b/gtsam/hybrid/HybridFactorGraph.h
@@ -70,17 +70,19 @@ class HybridFactorGraph : public FactorGraph<HybridFactor>, public Eliminateable
  public:
   HybridFactorGraph() = default;
 
-  /** Construct from container of factors (shared_ptr or plain objects) */
-  template <class CONTAINER>
-  explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {}
+//  /** Construct from container of factors (shared_ptr or plain objects) */
+//  template <class CONTAINER>
+//  explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {}
 
   /** Implicit copy/downcast constructor to override explicit template container
-   * constructor */
+   * constructor. In BayesTree this is used for:
+   * `cachedSeparatorMarginal_.reset(*separatorMarginal)`
+   * */
   template <class DERIVEDFACTOR>
   HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
 
   using FactorGraph::add;
-  
+
   /// Add a factor directly using a shared_ptr.
   void add(JacobianFactor &&factor);
 };
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index a56bcadf0..d5a21f34d 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -73,6 +73,19 @@ TEST(HybridFactorGraph, eliminateMultifrontal) {
   GTSAM_PRINT(*result.second);
 }
 
+TEST(HybridFactorGraph, eliminateFullMultifrontal) {
+  HybridFactorGraph hfg;
+
+  DiscreteKey x(X(1), 2);
+
+  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8})));
+
+  auto result = hfg.eliminateMultifrontal();
+
+  GTSAM_PRINT(*result);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;