From 9e737dbbd8efe75b1a183fc34983b8fab75e6250 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jun 2022 12:47:21 -0400
Subject: [PATCH 01/17] initial pruning method

---
 gtsam/hybrid/HybridBayesNet.cpp | 95 +++++++++++++++++++++++++++++++++
 1 file changed, 95 insertions(+)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 1292711d8..7d1834723 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -14,3 +14,98 @@
  */
 
 #include <gtsam/hybrid/HybridBayesNet.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+/// Return the DiscreteKey vector as a set.
+static std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &dkeys) {
+  std::set<DiscreteKey> s;
+  s.insert(dkeys.begin(), dkeys.end());
+  return s;
+}
+
+/* ************************************************************************* */
+HybridBayesNet HybridBayesNet::prune(
+    const DecisionTreeFactor::shared_ptr &discreteFactor) const {
+  /* To Prune, we visitWith every leaf in the GaussianMixture.
+   * For each leaf, using the assignment we can check the discrete decision tree
+   * for 0.0 probability, then just set the leaf to a nullptr.
+   *
+   * We can later check the GaussianMixture for just nullptrs.
+   */
+
+  HybridBayesNet prunedBayesNetFragment;
+
+  // Functional which loops over all assignments and create a set of
+  // GaussianConditionals
+  auto pruner =
+      [&](const Assignment<Key> &choices,
+          const GaussianFactor::shared_ptr &gf) -> GaussianFactor::shared_ptr {
+    // typecast so we can use this to get probability value
+    DiscreteValues values(choices);
+
+    if ((*discreteFactor)(values) == 0.0) {
+      // empty aka null pointer
+      boost::shared_ptr<GaussianFactor> null;
+      return null;
+    } else {
+      return gf;
+    }
+  };
+
+  // Go through all the conditionals in the
+  // Bayes Net and prune them as per discreteFactor.
+  for (size_t i = 0; i < this->size(); i++) {
+    HybridConditional::shared_ptr conditional = this->at(i);
+
+    GaussianMixture::shared_ptr gaussianMixture =
+        boost::dynamic_pointer_cast<GaussianMixture>(conditional->inner());
+
+    if (gaussianMixture) {
+      // We may have mixtures with less discrete keys than discreteFactor so we
+      // skip those since the label assignment does not exist.
+      auto gmKeySet = DiscreteKeysAsSet(gaussianMixture->discreteKeys());
+      auto dfKeySet = DiscreteKeysAsSet(discreteFactor->discreteKeys());
+      if (gmKeySet != dfKeySet) {
+        // Add the gaussianMixture which doesn't have to be pruned.
+        prunedBayesNetFragment.push_back(
+            boost::make_shared<HybridConditional>(gaussianMixture));
+        continue;
+      }
+
+      // The GaussianMixture stores all the conditionals and uneliminated
+      // factors in the factors tree.
+      auto gaussianMixtureTree = gaussianMixture->factors();
+
+      // Run the pruning to get a new, pruned tree
+      auto prunedFactors = gaussianMixtureTree.apply(pruner);
+
+      DiscreteKeys discreteKeys = gaussianMixture->discreteKeys();
+      // reverse keys to get a natural ordering
+      std::reverse(discreteKeys.begin(), discreteKeys.end());
+
+      // Convert to GaussianConditionals
+      auto prunedTree = GaussianMixture::Conditionals(
+          prunedFactors, [](const GaussianFactor::shared_ptr &factor) {
+            return boost::dynamic_pointer_cast<GaussianConditional>(factor);
+          });
+
+      // Create the new gaussian mixture and add it to the bayes net.
+      auto prunedGaussianMixture = boost::make_shared<GaussianMixture>(
+          gaussianMixture->frontals(), gaussianMixture->parents(), discreteKeys,
+          prunedTree);
+
+      prunedBayesNetFragment.push_back(
+          boost::make_shared<HybridConditional>(prunedGaussianMixture));
+
+    } else {
+      // Add the non-GaussianMixture conditional
+      prunedBayesNetFragment.push_back(conditional);
+    }
+  }
+
+  return prunedBayesNetFragment;
+}
+
+}  // namespace gtsam

From 89768cf692144a8ccc24d66f993bcd249059f32d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jun 2022 13:07:46 -0400
Subject: [PATCH 02/17] record continuous keys separately

---
 gtsam/hybrid/HybridFactor.cpp |  9 +++++----
 gtsam/hybrid/HybridFactor.h   | 12 +++++++++---
 2 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp
index 127c9761c..cdae5faa6 100644
--- a/gtsam/hybrid/HybridFactor.cpp
+++ b/gtsam/hybrid/HybridFactor.cpp
@@ -50,7 +50,7 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
 
 /* ************************************************************************ */
 HybridFactor::HybridFactor(const KeyVector &keys)
-    : Base(keys), isContinuous_(true), nrContinuous_(keys.size()) {}
+    : Base(keys), isContinuous_(true), continuousKeys_(keys) {}
 
 /* ************************************************************************ */
 HybridFactor::HybridFactor(const KeyVector &continuousKeys,
@@ -59,8 +59,8 @@ HybridFactor::HybridFactor(const KeyVector &continuousKeys,
       isDiscrete_((continuousKeys.size() == 0) && (discreteKeys.size() != 0)),
       isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)),
       isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)),
-      nrContinuous_(continuousKeys.size()),
-      discreteKeys_(discreteKeys) {}
+      discreteKeys_(discreteKeys),
+      continuousKeys_(continuousKeys) {}
 
 /* ************************************************************************ */
 HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
@@ -73,7 +73,8 @@ bool HybridFactor::equals(const HybridFactor &lf, double tol) const {
   const This *e = dynamic_cast<const This *>(&lf);
   return e != nullptr && Base::equals(*e, tol) &&
          isDiscrete_ == e->isDiscrete_ && isContinuous_ == e->isContinuous_ &&
-         isHybrid_ == e->isHybrid_ && nrContinuous_ == e->nrContinuous_;
+         isHybrid_ == e->isHybrid_ && continuousKeys_ == e->continuousKeys_ &&
+         discreteKeys_ == e->discreteKeys_;
 }
 
 /* ************************************************************************ */
diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index 244fba4cc..ffa28d3f7 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -50,7 +50,10 @@ class GTSAM_EXPORT HybridFactor : public Factor {
   size_t nrContinuous_ = 0;
 
  protected:
+  // Set of DiscreteKeys for this factor.
   DiscreteKeys discreteKeys_;
+  // For bookkeeping
+  KeyVector continuousKeys_;
 
  public:
   // typedefs needed to play nice with gtsam
@@ -117,10 +120,13 @@ class GTSAM_EXPORT HybridFactor : public Factor {
   bool isHybrid() const { return isHybrid_; }
 
   /// Return the number of continuous variables in this factor.
-  size_t nrContinuous() const { return nrContinuous_; }
+  size_t nrContinuous() const { return continuousKeys_.size(); }
 
-  /// Return vector of discrete keys.
-  DiscreteKeys discreteKeys() const { return discreteKeys_; }
+  /// Return the discrete keys for this factor.
+  const DiscreteKeys &discreteKeys() const { return discreteKeys_; }
+
+  /// Return only the continuous keys for this factor.
+  const KeyVector &continuousKeys() const { return continuousKeys_; }
 
   /// @}
 };

From ad77a45a0d9ded056d01a6e9b7af4331e9ac3425 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jun 2022 13:08:32 -0400
Subject: [PATCH 03/17] formatting and docs update

---
 gtsam/hybrid/GaussianMixture.cpp | 15 ++++++---------
 gtsam/hybrid/GaussianMixture.h   | 11 +++++++----
 2 files changed, 13 insertions(+), 13 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index 000057518..8970a3993 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -36,8 +36,7 @@ GaussianMixture::GaussianMixture(
       conditionals_(conditionals) {}
 
 /* *******************************************************************************/
-const GaussianMixture::Conditionals &
-GaussianMixture::conditionals() {
+const GaussianMixture::Conditionals &GaussianMixture::conditionals() {
   return conditionals_;
 }
 
@@ -48,8 +47,8 @@ GaussianMixture GaussianMixture::FromConditionals(
     const std::vector<GaussianConditional::shared_ptr> &conditionalsList) {
   Conditionals dt(discreteParents, conditionalsList);
 
-  return GaussianMixture(continuousFrontals, continuousParents,
-                                    discreteParents, dt);
+  return GaussianMixture(continuousFrontals, continuousParents, discreteParents,
+                         dt);
 }
 
 /* *******************************************************************************/
@@ -66,8 +65,7 @@ GaussianMixture::Sum GaussianMixture::add(
 }
 
 /* *******************************************************************************/
-GaussianMixture::Sum
-GaussianMixture::asGaussianFactorGraphTree() const {
+GaussianMixture::Sum GaussianMixture::asGaussianFactorGraphTree() const {
   auto lambda = [](const GaussianFactor::shared_ptr &factor) {
     GaussianFactorGraph result;
     result.push_back(factor);
@@ -77,15 +75,14 @@ GaussianMixture::asGaussianFactorGraphTree() const {
 }
 
 /* *******************************************************************************/
-bool GaussianMixture::equals(const HybridFactor &lf,
-                                        double tol) const {
+bool GaussianMixture::equals(const HybridFactor &lf, double tol) const {
   const This *e = dynamic_cast<const This *>(&lf);
   return e != nullptr && BaseFactor::equals(*e, tol);
 }
 
 /* *******************************************************************************/
 void GaussianMixture::print(const std::string &s,
-                                       const KeyFormatter &formatter) const {
+                            const KeyFormatter &formatter) const {
   std::cout << s;
   if (isContinuous()) std::cout << "Continuous ";
   if (isDiscrete()) std::cout << "Discrete ";
diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h
index e85506715..6100b49d1 100644
--- a/gtsam/hybrid/GaussianMixture.h
+++ b/gtsam/hybrid/GaussianMixture.h
@@ -28,11 +28,14 @@ namespace gtsam {
 
 /**
  * @brief A conditional of gaussian mixtures indexed by discrete variables, as
- * part of a Bayes Network.
+ * part of a Bayes Network. This is the result of the elimination of a
+ * continuous variable in a hybrid scheme, such that the remaining variables are
+ * discrete+continuous.
  *
- * Represents the conditional density P(X | M, Z) where X is a continuous random
- * variable, M is the selection of discrete variables corresponding to a subset
- * of the Gaussian variables and Z is parent of this node
+ * Represents the conditional density P(X | M, Z) where X is the set of
+ * continuous random variables, M is the selection of discrete variables
+ * corresponding to a subset of the Gaussian variables and Z is parent of this
+ * node .
  *
  * The probability P(x|y,z,...) is proportional to
  * \f$ \sum_i k_i \exp - \frac{1}{2} |R_i x - (d_i - S_i y - T_i z - ...)|^2 \f$

From c2e5061b711ecdf4f7308a01f0e3f6d5e7419391 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jun 2022 13:25:19 -0400
Subject: [PATCH 04/17] add pruning to HybridBayesNet

---
 gtsam/hybrid/HybridBayesNet.cpp | 34 ++++++++++++++++-----------------
 gtsam/hybrid/HybridBayesNet.h   |  6 +++++-
 2 files changed, 22 insertions(+), 18 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 7d1834723..d3c77d83e 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -39,18 +39,18 @@ HybridBayesNet HybridBayesNet::prune(
 
   // Functional which loops over all assignments and create a set of
   // GaussianConditionals
-  auto pruner =
-      [&](const Assignment<Key> &choices,
-          const GaussianFactor::shared_ptr &gf) -> GaussianFactor::shared_ptr {
+  auto pruner = [&](const Assignment<Key> &choices,
+                    const GaussianConditional::shared_ptr &conditional)
+      -> GaussianConditional::shared_ptr {
     // typecast so we can use this to get probability value
     DiscreteValues values(choices);
 
     if ((*discreteFactor)(values) == 0.0) {
       // empty aka null pointer
-      boost::shared_ptr<GaussianFactor> null;
+      boost::shared_ptr<GaussianConditional> null;
       return null;
     } else {
-      return gf;
+      return conditional;
     }
   };
 
@@ -74,28 +74,28 @@ HybridBayesNet HybridBayesNet::prune(
         continue;
       }
 
-      // The GaussianMixture stores all the conditionals and uneliminated
-      // factors in the factors tree.
-      auto gaussianMixtureTree = gaussianMixture->factors();
-
       // Run the pruning to get a new, pruned tree
-      auto prunedFactors = gaussianMixtureTree.apply(pruner);
+      GaussianMixture::Conditionals prunedTree =
+          gaussianMixture->conditionals().apply(pruner);
 
       DiscreteKeys discreteKeys = gaussianMixture->discreteKeys();
       // reverse keys to get a natural ordering
       std::reverse(discreteKeys.begin(), discreteKeys.end());
 
-      // Convert to GaussianConditionals
-      auto prunedTree = GaussianMixture::Conditionals(
-          prunedFactors, [](const GaussianFactor::shared_ptr &factor) {
-            return boost::dynamic_pointer_cast<GaussianConditional>(factor);
-          });
+      // Convert from boost::iterator_range to std::vector.
+      std::vector<Key> frontals, parents;
+      for (Key key : gaussianMixture->frontals()) {
+        frontals.push_back(key);
+      }
+      for (Key key : gaussianMixture->parents()) {
+        parents.push_back(key);
+      }
 
       // Create the new gaussian mixture and add it to the bayes net.
       auto prunedGaussianMixture = boost::make_shared<GaussianMixture>(
-          gaussianMixture->frontals(), gaussianMixture->parents(), discreteKeys,
-          prunedTree);
+          frontals, parents, discreteKeys, prunedTree);
 
+      // Type-erase and add to the pruned Bayes Net fragment.
       prunedBayesNetFragment.push_back(
           boost::make_shared<HybridConditional>(prunedGaussianMixture));
 
diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h
index 43eead280..834c4c3ef 100644
--- a/gtsam/hybrid/HybridBayesNet.h
+++ b/gtsam/hybrid/HybridBayesNet.h
@@ -17,6 +17,7 @@
 
 #pragma once
 
+#include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/inference/BayesNet.h>
 
@@ -35,7 +36,10 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
   using sharedConditional = boost::shared_ptr<ConditionalType>;
 
   /** Construct empty bayes net */
-  HybridBayesNet() = default;
+  HybridBayesNet() : Base() {}
+
+  HybridBayesNet prune(
+      const DecisionTreeFactor::shared_ptr &discreteFactor) const;
 };
 
 }  // namespace gtsam

From 28db8b20ff15e3b66e5e61c72e385e46212af702 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 4 Jun 2022 11:45:42 -0400
Subject: [PATCH 05/17] use KeyVector and iterator constructor

---
 gtsam/hybrid/HybridBayesNet.cpp | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index d3c77d83e..0403ca0d3 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -82,14 +82,12 @@ HybridBayesNet HybridBayesNet::prune(
       // reverse keys to get a natural ordering
       std::reverse(discreteKeys.begin(), discreteKeys.end());
 
-      // Convert from boost::iterator_range to std::vector.
-      std::vector<Key> frontals, parents;
-      for (Key key : gaussianMixture->frontals()) {
-        frontals.push_back(key);
-      }
-      for (Key key : gaussianMixture->parents()) {
-        parents.push_back(key);
-      }
+      // Convert from boost::iterator_range to KeyVector
+      // so we can pass it to constructor.
+      KeyVector frontals(gaussianMixture->frontals().begin(),
+                         gaussianMixture->frontals().end()),
+          parents(gaussianMixture->parents().begin(),
+                  gaussianMixture->parents().end());
 
       // Create the new gaussian mixture and add it to the bayes net.
       auto prunedGaussianMixture = boost::make_shared<GaussianMixture>(

From 5c5c05370a14141a9ec60628bdb1327cab98f1de Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jun 2022 18:38:21 -0400
Subject: [PATCH 06/17] Add HybridFactorGraph base class and more methods for
 adding gaussian factors

---
 gtsam/hybrid/HybridFactorGraph.h         | 148 +++++++++++++++++++++++
 gtsam/hybrid/HybridGaussianFactorGraph.h |  61 ++++++++--
 2 files changed, 202 insertions(+), 7 deletions(-)
 create mode 100644 gtsam/hybrid/HybridFactorGraph.h

diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h
new file mode 100644
index 000000000..ce0241d25
--- /dev/null
+++ b/gtsam/hybrid/HybridFactorGraph.h
@@ -0,0 +1,148 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   HybridFactorGraph.h
+ * @brief  Hybrid factor graph base class that uses type erasure
+ * @author Varun Agrawal
+ * @date   May 28, 2022
+ */
+
+#pragma once
+
+#include <gtsam/discrete/DiscreteFactor.h>
+#include <gtsam/hybrid/HybridDiscreteFactor.h>
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/inference/FactorGraph.h>
+#include <gtsam/inference/Ordering.h>
+
+#include <boost/format.hpp>
+namespace gtsam {
+
+using SharedFactor = boost::shared_ptr<Factor>;
+
+/**
+ * Hybrid Factor Graph
+ * -----------------------
+ * This is the base hybrid factor graph.
+ * Everything inside needs to be hybrid factor or hybrid conditional.
+ */
+class HybridFactorGraph : public FactorGraph<HybridFactor> {
+ public:
+  using Base = FactorGraph<HybridFactor>;
+  using This = HybridFactorGraph;              ///< this class
+  using shared_ptr = boost::shared_ptr<This>;  ///< shared_ptr to This
+
+  using Values = gtsam::Values;  ///< backwards compatibility
+  using Indices = KeyVector;     ///> map from keys to values
+
+ protected:
+  /// Check if FACTOR type is derived from DiscreteFactor.
+  template <typename FACTOR>
+  using IsDiscrete = typename std::enable_if<
+      std::is_base_of<DiscreteFactor, FACTOR>::value>::type;
+
+  /// Check if FACTOR type is derived from HybridFactor.
+  template <typename FACTOR>
+  using IsHybrid = typename std::enable_if<
+      std::is_base_of<HybridFactor, FACTOR>::value>::type;
+
+ public:
+  /// @name Constructors
+  /// @{
+
+  /// Default constructor
+  HybridFactorGraph() = default;
+
+  /**
+   * Implicit copy/downcast constructor to override explicit template container
+   * constructor. In BayesTree this is used for:
+   * `cachedSeparatorMarginal_.reset(*separatorMarginal)`
+   * */
+  template <class DERIVEDFACTOR>
+  HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
+
+  /// @}
+
+  // Allow use of selected FactorGraph methods:
+  using Base::empty;
+  using Base::reserve;
+  using Base::size;
+  using Base::operator[];
+  using Base::add;
+  using Base::push_back;
+  using Base::resize;
+
+  /**
+   * Add a discrete factor *pointer* to the internal discrete graph
+   * @param discreteFactor - boost::shared_ptr to the factor to add
+   */
+  template <typename FACTOR>
+  IsDiscrete<FACTOR> push_discrete(
+      const boost::shared_ptr<FACTOR>& discreteFactor) {
+    Base::push_back(boost::make_shared<HybridDiscreteFactor>(discreteFactor));
+  }
+
+  /**
+   * Add a discrete-continuous (Hybrid) factor *pointer* to the graph
+   * @param hybridFactor - boost::shared_ptr to the factor to add
+   */
+  template <typename FACTOR>
+  IsHybrid<FACTOR> push_hybrid(const boost::shared_ptr<FACTOR>& hybridFactor) {
+    Base::push_back(hybridFactor);
+  }
+
+  /// delete emplace_shared.
+  template <class FACTOR, class... Args>
+  void emplace_shared(Args&&... args) = delete;
+
+  /// Construct a factor and add (shared pointer to it) to factor graph.
+  template <class FACTOR, class... Args>
+  IsDiscrete<FACTOR> emplace_discrete(Args&&... args) {
+    auto factor = boost::allocate_shared<FACTOR>(
+        Eigen::aligned_allocator<FACTOR>(), std::forward<Args>(args)...);
+    push_discrete(factor);
+  }
+
+  /// Construct a factor and add (shared pointer to it) to factor graph.
+  template <class FACTOR, class... Args>
+  IsHybrid<FACTOR> emplace_hybrid(Args&&... args) {
+    auto factor = boost::allocate_shared<FACTOR>(
+        Eigen::aligned_allocator<FACTOR>(), std::forward<Args>(args)...);
+    push_hybrid(factor);
+  }
+
+  /**
+   * @brief Add a single factor shared pointer to the hybrid factor graph.
+   * Dynamically handles the factor type and assigns it to the correct
+   * underlying container.
+   *
+   * @param sharedFactor The factor to add to this factor graph.
+   */
+  void push_back(const SharedFactor& sharedFactor) {
+    if (auto p = boost::dynamic_pointer_cast<DiscreteFactor>(sharedFactor)) {
+      push_discrete(p);
+    }
+    if (auto p = boost::dynamic_pointer_cast<HybridFactor>(sharedFactor)) {
+      push_hybrid(p);
+    }
+  }
+
+  /** Constructor from iterator over factors (shared_ptr or plain objects) */
+  template <typename ITERATOR>
+  void push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
+    for (auto&& it = firstFactor; it != lastFactor; it++) {
+      push_back(*it);
+    }
+  }
+};
+
+}  // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h
index 0188aa652..1d49904fe 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.h
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.h
@@ -19,6 +19,8 @@
 #pragma once
 
 #include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/inference/EliminateableFactorGraph.h>
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/Ordering.h>
@@ -53,10 +55,9 @@ struct EliminationTraits<HybridGaussianFactorGraph> {
   typedef HybridBayesNet
       BayesNetType;  ///< Type of Bayes net from sequential elimination
   typedef HybridEliminationTree
-      EliminationTreeType;                ///< Type of elimination tree
-  typedef HybridBayesTree BayesTreeType;  ///< Type of Bayes tree
-  typedef HybridJunctionTree
-      JunctionTreeType;  ///< Type of Junction tree
+      EliminationTreeType;                      ///< Type of elimination tree
+  typedef HybridBayesTree BayesTreeType;        ///< Type of Bayes tree
+  typedef HybridJunctionTree JunctionTreeType;  ///< Type of Junction tree
   /// The default dense elimination function
   static std::pair<boost::shared_ptr<ConditionalType>,
                    boost::shared_ptr<FactorType> >
@@ -72,10 +73,16 @@ struct EliminationTraits<HybridGaussianFactorGraph> {
  * Everything inside needs to be hybrid factor or hybrid conditional.
  */
 class GTSAM_EXPORT HybridGaussianFactorGraph
-    : public FactorGraph<HybridFactor>,
+    : public HybridFactorGraph,
       public EliminateableFactorGraph<HybridGaussianFactorGraph> {
+ protected:
+  /// Check if FACTOR type is derived from GaussianFactor.
+  template <typename FACTOR>
+  using IsGaussian = typename std::enable_if<
+      std::is_base_of<GaussianFactor, FACTOR>::value>::type;
+
  public:
-  using Base = FactorGraph<HybridFactor>;
+  using Base = HybridFactorGraph;
   using This = HybridGaussianFactorGraph;  ///< this class
   using BaseEliminateable =
       EliminateableFactorGraph<This>;          ///< for elimination
@@ -100,7 +107,13 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
 
   /// @}
 
-  using FactorGraph::add;
+  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);
@@ -113,6 +126,40 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
 
   /// Add a DecisionTreeFactor as a shared ptr.
   void add(boost::shared_ptr<DecisionTreeFactor> factor);
+
+  /**
+   * Add a gaussian factor *pointer* to the internal gaussian factor graph
+   * @param gaussianFactor - boost::shared_ptr to the factor to add
+   */
+  template <typename FACTOR>
+  IsGaussian<FACTOR> push_gaussian(
+      const boost::shared_ptr<FACTOR>& gaussianFactor) {
+    Base::Base::push_back(
+        boost::make_shared<HybridGaussianFactor>(gaussianFactor));
+  }
+
+  /// Construct a factor and add (shared pointer to it) to factor graph.
+  template <class FACTOR, class... Args>
+  IsGaussian<FACTOR> emplace_gaussian(Args&&... args) {
+    auto factor = boost::allocate_shared<FACTOR>(
+        Eigen::aligned_allocator<FACTOR>(), std::forward<Args>(args)...);
+    push_gaussian(factor);
+  }
+
+  /**
+   * @brief Add a single factor shared pointer to the hybrid factor graph.
+   * Dynamically handles the factor type and assigns it to the correct
+   * underlying container.
+   *
+   * @param sharedFactor The factor to add to this factor graph.
+   */
+  void push_back(const SharedFactor& sharedFactor) {
+    if (auto p = boost::dynamic_pointer_cast<GaussianFactor>(sharedFactor)) {
+      push_gaussian(p);
+    } else {
+      Base::push_back(sharedFactor);
+    }
+  }
 };
 
 }  // namespace gtsam

From 44079d13b4357ae958e754b6ed3769c133f0f565 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jun 2022 18:38:46 -0400
Subject: [PATCH 07/17] refactor testGaussianHybridFactorGraph to include
 comments and valid tests

---
 .../tests/testGaussianHybridFactorGraph.cpp   | 225 ++++++------------
 1 file changed, 70 insertions(+), 155 deletions(-)

diff --git a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
index 552bb18f5..9e1a2efdd 100644
--- a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
@@ -58,26 +58,30 @@ using gtsam::symbol_shorthand::X;
 using gtsam::symbol_shorthand::Y;
 
 /* ************************************************************************* */
-TEST(HybridGaussianFactorGraph, creation) {
-  HybridConditional test;
+TEST(HybridGaussianFactorGraph, Creation) {
+  HybridConditional conditional;
 
   HybridGaussianFactorGraph hfg;
 
-  hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
+  hfg.add(HybridGaussianFactor(JacobianFactor(X(0), I_3x3, Z_3x1)));
 
-  GaussianMixture clgc(
-      {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
-      GaussianMixture::Conditionals(
-          C(0),
-          boost::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1),
-                                                  I_3x3),
-          boost::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3,
-                                                  X(1), I_3x3)));
-  GTSAM_PRINT(clgc);
+  // Define a gaussian mixture conditional P(x0|x1, c0) and add it to the factor
+  // graph
+  GaussianMixture gm({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
+                     GaussianMixture::Conditionals(
+                         C(0),
+                         boost::make_shared<GaussianConditional>(
+                             X(0), Z_3x1, I_3x3, X(1), I_3x3),
+                         boost::make_shared<GaussianConditional>(
+                             X(0), Vector3::Ones(), I_3x3, X(1), I_3x3)));
+  hfg.add(gm);
+
+  EXPECT_LONGS_EQUAL(2, hfg.size());
 }
 
 /* ************************************************************************* */
-TEST(HybridGaussianFactorGraph, eliminate) {
+TEST(HybridGaussianFactorGraph, EliminateSequential) {
+  // Test elimination of a single variable.
   HybridGaussianFactorGraph hfg;
 
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
@@ -88,11 +92,13 @@ TEST(HybridGaussianFactorGraph, eliminate) {
 }
 
 /* ************************************************************************* */
-TEST(HybridGaussianFactorGraph, eliminateMultifrontal) {
+TEST(HybridGaussianFactorGraph, EliminateMultifrontal) {
+  // Test multifrontal elimination
   HybridGaussianFactorGraph hfg;
 
   DiscreteKey c(C(1), 2);
 
+  // Add priors on x0 and c1
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
   hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
 
@@ -110,9 +116,12 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
 
   DiscreteKey c1(C(1), 2);
 
+  // Add prior on x0
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  // Add factor between x0 and x1
   hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
 
+  // Add a gaussian mixture factor ϕ(x1, c1)
   DecisionTree<Key, GaussianFactor::shared_ptr> dt(
       C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
       boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
@@ -123,9 +132,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
       hfg.eliminateSequential(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
 
   auto dc = result->at(2)->asDiscreteConditional();
-  DiscreteValues dv;
-  dv[C(1)] = 0;
-  EXPECT_DOUBLES_EQUAL(0.6225, dc->operator()(dv), 1e-3);
+  DiscreteValues mode;
+  mode[C(1)] = 0;
+  EXPECT_DOUBLES_EQUAL(0.6225, (*dc)(mode), 1e-3);
 }
 
 /* ************************************************************************* */
@@ -134,26 +143,26 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
 
   DiscreteKey c1(C(1), 2);
 
+  // Add prior on x0
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  // Add factor between x0 and x1
   hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
 
   DecisionTree<Key, GaussianFactor::shared_ptr> dt(
       C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
       boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
-
   hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));
-  // hfg.add(GaussianMixtureFactor({X(0)}, {c1}, dt));
+
+  // Discrete probability table for c1
   hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c1, {2, 8})));
+  // Joint discrete probability table for c1, c2
   hfg.add(HybridDiscreteFactor(
       DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
-  // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1
-  // 2 3 4"))); hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2},
-  // {C(1), 2}}, "1 2 2 1")));
 
   auto result = hfg.eliminateSequential(
       Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));
 
-  GTSAM_PRINT(*result);
+  EXPECT_LONGS_EQUAL(4, result->size());
 }
 
 /* ************************************************************************* */
@@ -165,28 +174,19 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
   hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
 
-  // DecisionTree<Key, GaussianFactor::shared_ptr> dt(
-  //     C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
-  //     boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
-
-  // hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));
   hfg.add(GaussianMixtureFactor::FromFactors(
       {X(1)}, {{C(1), 2}},
       {boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
        boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones())}));
 
-  // hfg.add(GaussianMixtureFactor({X(0)}, {c1}, dt));
   hfg.add(DecisionTreeFactor(c1, {2, 8}));
   hfg.add(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4"));
-  // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1
-  // 2 3 4"))); hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2},
-  // {C(1), 2}}, "1 2 2 1")));
 
   auto result = hfg.eliminateMultifrontal(
       Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));
 
-  GTSAM_PRINT(*result);
-  GTSAM_PRINT(*result->marginalFactor(C(2)));
+  // GTSAM_PRINT(*result);
+  // GTSAM_PRINT(*result->marginalFactor(C(2)));
 }
 
 /* ************************************************************************* */
@@ -195,30 +195,28 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
 
   DiscreteKey c(C(1), 2);
 
+  // Prior on x0
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  // Factor between x0-x1
   hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
 
+  // Decision tree with different modes on x1
   DecisionTree<Key, GaussianFactor::shared_ptr> dt(
       C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
       boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
 
+  // Hybrid factor P(x1|c1)
   hfg.add(GaussianMixtureFactor({X(1)}, {c}, dt));
+  // Prior factor on c1
   hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
-  //  hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1
-  //  2 3 4")));
 
+  // Get a constrained ordering keeping c1 last
   auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {C(1)});
 
+  // Returns a Hybrid Bayes Tree with distribution P(x0|x1)P(x1|c1)P(c1)
   HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full);
 
-  GTSAM_PRINT(*hbt);
-  /*
-  Explanation: the Junction tree will need to reeliminate to get to the marginal
-  on X(1), which is not possible because it involves eliminating discrete before
-  continuous. The solution to this, however, is in Murphy02. TLDR is that this
-  is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
-  should do this.
-  */
+  EXPECT_LONGS_EQUAL(3, hbt->size());
 }
 
 /* ************************************************************************* */
@@ -233,10 +231,6 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
   hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
 
   {
-    // DecisionTree<Key, GaussianFactor::shared_ptr> dt(
-    //     C(0), boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
-    //     boost::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones()));
-
     hfg.add(GaussianMixtureFactor::FromFactors(
         {X(0)}, {{C(0), 2}},
         {boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
@@ -249,7 +243,6 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
     hfg.add(GaussianMixtureFactor({X(2)}, {{C(1), 2}}, dt1));
   }
 
-  // hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
   hfg.add(HybridDiscreteFactor(
       DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
 
@@ -273,26 +266,36 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
   auto ordering_full =
       Ordering::ColamdConstrainedLast(hfg, {C(0), C(1), C(2), C(3)});
 
-  GTSAM_PRINT(ordering_full);
-
   HybridBayesTree::shared_ptr hbt;
   HybridGaussianFactorGraph::shared_ptr remaining;
   std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full);
 
-  GTSAM_PRINT(*hbt);
+  // GTSAM_PRINT(*hbt);
+  // GTSAM_PRINT(*remaining);
 
-  GTSAM_PRINT(*remaining);
-
-  hbt->dot(std::cout);
   /*
-  Explanation: the Junction tree will need to reeliminate to get to the marginal
-  on X(1), which is not possible because it involves eliminating discrete before
-  continuous. The solution to this, however, is in Murphy02. TLDR is that this
-  is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
-  should do this.
+  (Fan) Explanation: the Junction tree will need to reeliminate to get to the
+  marginal on X(1), which is not possible because it involves eliminating
+  discrete before continuous. The solution to this, however, is in Murphy02.
+  TLDR is that this is 1. expensive and 2. inexact. nevertheless it is doable.
+  And I believe that we should do this.
   */
 }
 
+void dotPrint(const HybridGaussianFactorGraph::shared_ptr &hfg,
+              const HybridBayesTree::shared_ptr &hbt,
+              const Ordering &ordering) {
+  DotWriter dw;
+  dw.positionHints['c'] = 2;
+  dw.positionHints['x'] = 1;
+  std::cout << hfg->dot(DefaultKeyFormatter, dw);
+  std::cout << "\n";
+  hbt->dot(std::cout);
+
+  std::cout << "\n";
+  std::cout << hfg->eliminateSequential(ordering)->dot(DefaultKeyFormatter, dw);
+}
+
 /* ************************************************************************* */
 // TODO(fan): make a graph like Varun's paper one
 TEST(HybridGaussianFactorGraph, Switching) {
@@ -326,9 +329,6 @@ TEST(HybridGaussianFactorGraph, Switching) {
     for (auto &l : lvls) {
       l = -l;
     }
-    std::copy(lvls.begin(), lvls.end(),
-              std::ostream_iterator<int>(std::cout, ","));
-    std::cout << "\n";
   }
   {
     std::vector<int> naturalC(N - 1);
@@ -342,63 +342,19 @@ TEST(HybridGaussianFactorGraph, Switching) {
     // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
     std::tie(ndC, lvls) = makeBinaryOrdering(ordC);
     std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
-    std::copy(lvls.begin(), lvls.end(),
-              std::ostream_iterator<int>(std::cout, ","));
   }
   auto ordering_full = Ordering(ordering);
 
-  // auto ordering_full =
-  //     Ordering();
-
-  // for (int i = 1; i <= 9; i++) {
-  //   ordering_full.push_back(X(i));
-  // }
-
-  // for (int i = 1; i < 9; i++) {
-  //   ordering_full.push_back(C(i));
-  // }
-
-  // auto ordering_full =
-  //     Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
-  //     X(5),
-  //                        C(1), C(2), C(3), C(4), C(5), C(6), C(7), C(8)});
-
   // GTSAM_PRINT(*hfg);
-  GTSAM_PRINT(ordering_full);
+  // GTSAM_PRINT(ordering_full);
 
   HybridBayesTree::shared_ptr hbt;
   HybridGaussianFactorGraph::shared_ptr remaining;
   std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
 
   // GTSAM_PRINT(*hbt);
-
   // GTSAM_PRINT(*remaining);
-
-  {
-    DotWriter dw;
-    dw.positionHints['c'] = 2;
-    dw.positionHints['x'] = 1;
-    std::cout << hfg->dot(DefaultKeyFormatter, dw);
-    std::cout << "\n";
-    hbt->dot(std::cout);
-  }
-
-  {
-    DotWriter dw;
-    // dw.positionHints['c'] = 2;
-    // dw.positionHints['x'] = 1;
-    std::cout << "\n";
-    std::cout << hfg->eliminateSequential(ordering_full)
-                     ->dot(DefaultKeyFormatter, dw);
-  }
-  /*
-  Explanation: the Junction tree will need to reeliminate to get to the marginal
-  on X(1), which is not possible because it involves eliminating discrete before
-  continuous. The solution to this, however, is in Murphy02. TLDR is that this
-  is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
-  should do this.
-  */
-  hbt->marginalFactor(C(11))->print("HBT: ");
+  // hbt->marginalFactor(C(11))->print("HBT: ");
 }
 
 /* ************************************************************************* */
@@ -434,9 +390,6 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
     for (auto &l : lvls) {
       l = -l;
     }
-    std::copy(lvls.begin(), lvls.end(),
-              std::ostream_iterator<int>(std::cout, ","));
-    std::cout << "\n";
   }
   {
     std::vector<int> naturalC(N - 1);
@@ -450,40 +403,16 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
     // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
     std::tie(ndC, lvls) = makeBinaryOrdering(ordC);
     std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
-    std::copy(lvls.begin(), lvls.end(),
-              std::ostream_iterator<int>(std::cout, ","));
   }
   auto ordering_full = Ordering(ordering);
 
   // GTSAM_PRINT(*hfg);
-  GTSAM_PRINT(ordering_full);
+  // GTSAM_PRINT(ordering_full);
 
   HybridBayesTree::shared_ptr hbt;
   HybridGaussianFactorGraph::shared_ptr remaining;
   std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
 
-  // GTSAM_PRINT(*hbt);
-
-  // GTSAM_PRINT(*remaining);
-
-  {
-    DotWriter dw;
-    dw.positionHints['c'] = 2;
-    dw.positionHints['x'] = 1;
-    std::cout << hfg->dot(DefaultKeyFormatter, dw);
-    std::cout << "\n";
-    hbt->dot(std::cout);
-  }
-
-  {
-    DotWriter dw;
-    // dw.positionHints['c'] = 2;
-    // dw.positionHints['x'] = 1;
-    std::cout << "\n";
-    std::cout << hfg->eliminateSequential(ordering_full)
-                     ->dot(DefaultKeyFormatter, dw);
-  }
-
   auto new_fg = makeSwitchingChain(12);
   auto isam = HybridGaussianISAM(*hbt);
 
@@ -492,13 +421,14 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
     factorGraph.push_back(new_fg->at(new_fg->size() - 2));
     factorGraph.push_back(new_fg->at(new_fg->size() - 1));
     isam.update(factorGraph);
-    std::cout << isam.dot();
-    isam.marginalFactor(C(11))->print();
+    // std::cout << isam.dot();
+    // isam.marginalFactor(C(11))->print();
   }
 }
 
 /* ************************************************************************* */
-TEST(HybridGaussianFactorGraph, SwitchingTwoVar) {
+// TODO(Varun) Actually test something!
+TEST_DISABLED(HybridGaussianFactorGraph, SwitchingTwoVar) {
   const int N = 7;
   auto hfg = makeSwitchingChain(N, X);
   hfg->push_back(*makeSwitchingChain(N, Y, D));
@@ -517,21 +447,6 @@ TEST(HybridGaussianFactorGraph, SwitchingTwoVar) {
     ordX.emplace_back(Y(i));
   }
 
-  // {
-  //   KeyVector ndX;
-  //   std::vector<int> lvls;
-  //   std::tie(ndX, lvls) = makeBinaryOrdering(naturalX);
-  //   std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
-  //   std::copy(lvls.begin(), lvls.end(),
-  //             std::ostream_iterator<int>(std::cout, ","));
-  //   std::cout << "\n";
-
-  //   for (size_t i = 0; i < N; i++) {
-  //     ordX.emplace_back(X(ndX[i]));
-  //     ordX.emplace_back(Y(ndX[i]));
-  //   }
-  // }
-
   for (size_t i = 1; i <= N - 1; i++) {
     ordX.emplace_back(C(i));
   }

From 374e3cbc7a882c8e8a2f53aa84535ec1fb5fcbd3 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jun 2022 18:39:10 -0400
Subject: [PATCH 08/17] Improved hybrid bayes net and tests

---
 gtsam/hybrid/HybridBayesNet.cpp           | 27 +++++++
 gtsam/hybrid/HybridBayesNet.h             | 22 ++++++
 gtsam/hybrid/tests/testHybridBayesNet.cpp | 92 +++++++++++++++++++++++
 3 files changed, 141 insertions(+)
 create mode 100644 gtsam/hybrid/tests/testHybridBayesNet.cpp

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 1292711d8..b3df73bf2 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -10,7 +10,34 @@
  * @file   HybridBayesNet.cpp
  * @brief  A bayes net of Gaussian Conditionals indexed by discrete keys.
  * @author Fan Jiang
+ * @author Varun Agrawal
  * @date   January 2022
  */
 
 #include <gtsam/hybrid/HybridBayesNet.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+GaussianMixture::shared_ptr HybridBayesNet::atGaussian(size_t i) const {
+  return boost::dynamic_pointer_cast<GaussianMixture>(factors_.at(i)->inner());
+}
+
+/* ************************************************************************* */
+DiscreteConditional::shared_ptr HybridBayesNet::atDiscrete(size_t i) const {
+  return boost::dynamic_pointer_cast<DiscreteConditional>(
+      factors_.at(i)->inner());
+}
+
+/* ************************************************************************* */
+GaussianBayesNet HybridBayesNet::choose(
+    const DiscreteValues &assignment) const {
+  GaussianBayesNet gbn;
+  for (size_t idx = 0; idx < size(); idx++) {
+    GaussianMixture gm = *this->atGaussian(idx);
+    gbn.push_back(gm(assignment));
+  }
+  return gbn;
+}
+
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h
index 43eead280..412b208b9 100644
--- a/gtsam/hybrid/HybridBayesNet.h
+++ b/gtsam/hybrid/HybridBayesNet.h
@@ -19,6 +19,7 @@
 
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/inference/BayesNet.h>
+#include <gtsam/linear/GaussianBayesNet.h>
 
 namespace gtsam {
 
@@ -36,6 +37,27 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
 
   /** Construct empty bayes net */
   HybridBayesNet() = default;
+
+  /// Add a discrete conditional to the Bayes Net.
+  void add(const DiscreteKey &key, const std::string &table) {
+    push_back(
+        HybridConditional(boost::make_shared<DiscreteConditional>(key, table)));
+  }
+
+  /// Get a specific Gaussian mixture by index `i`.
+  GaussianMixture::shared_ptr atGaussian(size_t i) const;
+
+  /// Get a specific discrete conditional by index `i`.
+  DiscreteConditional::shared_ptr atDiscrete(size_t i) const;
+
+  /**
+   * @brief Get the Gaussian Bayes Net which corresponds to a specific discrete
+   * value assignment.
+   *
+   * @param assignment The discrete value assignment for the discrete keys.
+   * @return GaussianBayesNet
+   */
+  GaussianBayesNet choose(const DiscreteValues &assignment) const;
 };
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
new file mode 100644
index 000000000..34133ab0b
--- /dev/null
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -0,0 +1,92 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testHybridBayesNet.cpp
+ * @brief   Unit tests for HybridBayesNet
+ * @author  Varun Agrawal
+ * @author  Fan Jiang
+ * @author  Frank Dellaert
+ * @date    December 2021
+ */
+
+#include <gtsam/hybrid/HybridBayesNet.h>
+#include <gtsam/hybrid/tests/Switching.h>
+
+// Include for test suite
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using noiseModel::Isotropic;
+using symbol_shorthand::M;
+using symbol_shorthand::X;
+
+static const DiscreteKey Asia(0, 2);
+
+/* ****************************************************************************/
+// Test creation
+TEST(HybridBayesNet, Creation) {
+  HybridBayesNet bayesNet;
+
+  bayesNet.add(Asia, "99/1");
+
+  DiscreteConditional expected(Asia, "99/1");
+
+  CHECK(bayesNet.atDiscrete(0));
+  auto& df = *bayesNet.atDiscrete(0);
+  EXPECT(df.equals(expected));
+}
+
+/* ****************************************************************************/
+// Test choosing an assignment of conditionals
+TEST(HybridBayesNet, Choose) {
+  Switching s(4);
+
+  Ordering ordering;
+  for (auto&& kvp : s.linearizationPoint) {
+    ordering += kvp.key;
+  }
+
+  HybridBayesNet::shared_ptr hybridBayesNet;
+  HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;
+  std::tie(hybridBayesNet, remainingFactorGraph) =
+      s.linearizedFactorGraph.eliminatePartialSequential(ordering);
+
+  DiscreteValues assignment;
+  assignment[M(1)] = 1;
+  assignment[M(2)] = 1;
+  assignment[M(3)] = 0;
+
+  GaussianBayesNet gbn = hybridBayesNet->choose(assignment);
+
+  EXPECT_LONGS_EQUAL(4, gbn.size());
+
+  EXPECT(assert_equal(*(*boost::dynamic_pointer_cast<GaussianMixture>(
+                          hybridBayesNet->atGaussian(0)))(assignment),
+                      *gbn.at(0)));
+  EXPECT(assert_equal(*(*boost::dynamic_pointer_cast<GaussianMixture>(
+                          hybridBayesNet->atGaussian(1)))(assignment),
+                      *gbn.at(1)));
+  EXPECT(assert_equal(*(*boost::dynamic_pointer_cast<GaussianMixture>(
+                          hybridBayesNet->atGaussian(2)))(assignment),
+                      *gbn.at(2)));
+  EXPECT(assert_equal(*(*boost::dynamic_pointer_cast<GaussianMixture>(
+                          hybridBayesNet->atGaussian(3)))(assignment),
+                      *gbn.at(3)));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
\ No newline at end of file

From 8d6a225851b5a2751452cedf598c660446b99161 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jun 2022 18:39:20 -0400
Subject: [PATCH 09/17] fix printing

---
 gtsam/hybrid/GaussianMixtureFactor.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h
index 6c90ee6a7..7b7c1899f 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.h
+++ b/gtsam/hybrid/GaussianMixtureFactor.h
@@ -108,7 +108,7 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
   bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
 
   void print(
-      const std::string &s = "HybridFactor\n",
+      const std::string &s = "GaussianMixtureFactor\n",
       const KeyFormatter &formatter = DefaultKeyFormatter) const override;
   /// @}
 

From 00be610e18ced3a1226a5271a6f1ad275d5c3539 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jun 2022 18:39:33 -0400
Subject: [PATCH 10/17] Add Switching class and make it linear

---
 gtsam/hybrid/tests/Switching.h | 139 +++++++++++++++++++++++++++++++--
 1 file changed, 132 insertions(+), 7 deletions(-)

diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
index c081b8e87..85c86f64e 100644
--- a/gtsam/hybrid/tests/Switching.h
+++ b/gtsam/hybrid/tests/Switching.h
@@ -18,18 +18,136 @@
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/discrete/DecisionTreeFactor.h>
+#include <gtsam/discrete/DiscreteDistribution.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
 
 #pragma once
 
-using gtsam::symbol_shorthand::C;
-using gtsam::symbol_shorthand::X;
-
 namespace gtsam {
-inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
+
+using symbol_shorthand::C;
+using symbol_shorthand::M;
+using symbol_shorthand::X;
+
+/* ****************************************************************************/
+// Test fixture with switching network.
+// TODO(Varun) Currently this is only linear. We need to add nonlinear support
+// and then update to
+// https://github.com/borglab/gtsam/pull/973/files#diff-58c02b3b197ebf731694946e87762d252e9eaa2f5c6c4ba22d618085b321ca23
+struct Switching {
+  size_t K;
+  DiscreteKeys modes;
+  HybridGaussianFactorGraph linearizedFactorGraph;
+  Values linearizationPoint;
+
+  using MotionModel = BetweenFactor<double>;
+  // using MotionMixture = MixtureFactor<MotionModel>;
+
+  /// Create with given number of time steps.
+  Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1)
+      : K(K) {
+    // Create DiscreteKeys for binary K modes, modes[0] will not be used.
+    modes = addDiscreteModes(K);
+
+    // Create hybrid factor graph.
+    // Add a prior on X(1).
+    auto prior = boost::make_shared<JacobianFactor>(
+        X(1), Matrix11::Ones() / prior_sigma, Vector1::Zero());
+    linearizedFactorGraph.push_back(prior);
+
+    // Add "motion models".
+    linearizedFactorGraph = addMotionModels(K);
+
+    // Add measurement factors
+    for (size_t k = 1; k <= K; k++) {
+      linearizedFactorGraph.emplace_gaussian<JacobianFactor>(
+          X(k), Matrix11::Ones() / 0.1, Vector1::Zero());
+    }
+
+    // Add "mode chain"
+    linearizedFactorGraph = addModeChain(linearizedFactorGraph);
+
+    // Create the linearization point.
+    for (size_t k = 1; k <= K; k++) {
+      linearizationPoint.insert<double>(X(k), static_cast<double>(k));
+    }
+  }
+
+  /// Create DiscreteKeys for K binary modes.
+  DiscreteKeys addDiscreteModes(size_t K) {
+    DiscreteKeys m;
+    for (size_t k = 0; k <= K; k++) {
+      m.emplace_back(M(k), 2);
+    }
+    return m;
+  }
+
+  /// Helper function to add motion models for each [k, k+1] interval.
+  HybridGaussianFactorGraph addMotionModels(size_t K) {
+    HybridGaussianFactorGraph hgfg;
+    for (size_t k = 1; k < K; k++) {
+      auto keys = {X(k), X(k + 1)};
+      auto components = motionModels(k);
+      hgfg.emplace_hybrid<GaussianMixtureFactor>(keys, DiscreteKeys{modes[k]},
+                                                 components);
+    }
+    return hgfg;
+  }
+
+  // Create motion models for a given time step
+  static std::vector<GaussianFactor::shared_ptr> motionModels(
+      size_t k, double sigma = 1.0) {
+    auto noise_model = noiseModel::Isotropic::Sigma(1, sigma);
+    auto still = boost::make_shared<JacobianFactor>(
+             X(k), -Matrix11::Ones() / sigma, X(k + 1),
+             Matrix11::Ones() / sigma, Vector1::Zero()),
+         moving = boost::make_shared<JacobianFactor>(
+             X(k), -Matrix11::Ones() / sigma, X(k + 1),
+             Matrix11::Ones() / sigma, -Vector1::Ones() / sigma);
+    return {boost::dynamic_pointer_cast<GaussianFactor>(still),
+            boost::dynamic_pointer_cast<GaussianFactor>(moving)};
+  }
+
+  // // Add "mode chain" to NonlinearHybridFactorGraph
+  // void addModeChain(HybridNonlinearFactorGraph& fg) {
+  //   auto prior = boost::make_shared<DiscreteDistribution>(modes[1], "1/1");
+  //   fg.push_discrete(prior);
+  //   for (size_t k = 1; k < K - 1; k++) {
+  //     auto parents = {modes[k]};
+  //     auto conditional = boost::make_shared<DiscreteConditional>(
+  //         modes[k + 1], parents, "1/2 3/2");
+  //     fg.push_discrete(conditional);
+  //   }
+  // }
+
+  // Add "mode chain" to GaussianHybridFactorGraph
+  HybridGaussianFactorGraph addModeChain(HybridGaussianFactorGraph& fg) {
+    auto prior = boost::make_shared<DiscreteDistribution>(modes[1], "1/1");
+    fg.push_discrete(prior);
+    for (size_t k = 1; k < K - 1; k++) {
+      auto parents = {modes[k]};
+      auto conditional = boost::make_shared<DiscreteConditional>(
+          modes[k + 1], parents, "1/2 3/2");
+      fg.push_discrete(conditional);
+    }
+    return fg;
+  }
+};
+
+/**
+ * @brief Create a switching system chain. A switching system is a continuous
+ * system which depends on a discrete mode at each time step of the chain.
+ *
+ * @param n The number of chain elements.
+ * @param keyFunc The functional to help specify the continuous key.
+ * @param dKeyFunc The functional to help specify the discrete key.
+ * @return HybridGaussianFactorGraph::shared_ptr
+ */
+HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
     size_t n, std::function<Key(int)> keyFunc = X,
     std::function<Key(int)> dKeyFunc = C) {
   HybridGaussianFactorGraph hfg;
@@ -54,9 +172,16 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
   return boost::make_shared<HybridGaussianFactorGraph>(std::move(hfg));
 }
 
-inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
-    std::vector<Key> &input) {
+/**
+ * @brief
+ *
+ * @param input The original ordering.
+ * @return std::pair<KeyVector, std::vector<int>>
+ */
+std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
+    std::vector<Key>& input) {
   KeyVector new_order;
+
   std::vector<int> levels(input.size());
   std::function<void(std::vector<Key>::iterator, std::vector<Key>::iterator,
                      int)>
@@ -79,7 +204,7 @@ inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
 
   bsg(input.begin(), input.end(), 0);
   std::reverse(new_order.begin(), new_order.end());
-  // std::reverse(levels.begin(), levels.end());
+
   return {new_order, levels};
 }
 

From 26e0cef48d04f01ff76d5ec09653a6d41b4f25c8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jun 2022 19:56:19 -0400
Subject: [PATCH 11/17] fixes

---
 gtsam/hybrid/HybridBayesNet.h    | 3 +++
 gtsam/hybrid/HybridFactorGraph.h | 8 --------
 2 files changed, 3 insertions(+), 8 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h
index 412b208b9..7fe19c0ea 100644
--- a/gtsam/hybrid/HybridBayesNet.h
+++ b/gtsam/hybrid/HybridBayesNet.h
@@ -38,6 +38,9 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
   /** Construct empty bayes net */
   HybridBayesNet() = default;
 
+  /// Add HybridConditional to Bayes Net
+  using Base::add;
+
   /// Add a discrete conditional to the Bayes Net.
   void add(const DiscreteKey &key, const std::string &table) {
     push_back(
diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h
index ce0241d25..cbbc45590 100644
--- a/gtsam/hybrid/HybridFactorGraph.h
+++ b/gtsam/hybrid/HybridFactorGraph.h
@@ -135,14 +135,6 @@ class HybridFactorGraph : public FactorGraph<HybridFactor> {
       push_hybrid(p);
     }
   }
-
-  /** Constructor from iterator over factors (shared_ptr or plain objects) */
-  template <typename ITERATOR>
-  void push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
-    for (auto&& it = firstFactor; it != lastFactor; it++) {
-      push_back(*it);
-    }
-  }
 };
 
 }  // namespace gtsam
\ No newline at end of file

From 5bf5d0300697afddac74b898502b5226ff30c085 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jun 2022 20:13:42 -0400
Subject: [PATCH 12/17] local include

---
 gtsam/hybrid/tests/testHybridBayesNet.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index 34133ab0b..ec6b67d66 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -19,7 +19,8 @@
  */
 
 #include <gtsam/hybrid/HybridBayesNet.h>
-#include <gtsam/hybrid/tests/Switching.h>
+
+#include "Switching.h"
 
 // Include for test suite
 #include <CppUnitLite/TestHarness.h>

From 8d359257797d502b76eb6500347515a08602eb15 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jun 2022 21:04:21 -0400
Subject: [PATCH 13/17] make makeBinaryOrdering inline to prevent multiple
 definition error

---
 gtsam/hybrid/tests/Switching.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
index 85c86f64e..76c3da498 100644
--- a/gtsam/hybrid/tests/Switching.h
+++ b/gtsam/hybrid/tests/Switching.h
@@ -178,7 +178,7 @@ HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
  * @param input The original ordering.
  * @return std::pair<KeyVector, std::vector<int>>
  */
-std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
+inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
     std::vector<Key>& input) {
   KeyVector new_order;
 

From 622ebdd13ba3841317d533dbbd87771fa9bbd910 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jun 2022 22:21:38 -0400
Subject: [PATCH 14/17] makeSwitchingChain is also inline

---
 gtsam/hybrid/tests/Switching.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
index 76c3da498..545259e48 100644
--- a/gtsam/hybrid/tests/Switching.h
+++ b/gtsam/hybrid/tests/Switching.h
@@ -147,7 +147,7 @@ struct Switching {
  * @param dKeyFunc The functional to help specify the discrete key.
  * @return HybridGaussianFactorGraph::shared_ptr
  */
-HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
+inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
     size_t n, std::function<Key(int)> keyFunc = X,
     std::function<Key(int)> dKeyFunc = C) {
   HybridGaussianFactorGraph hfg;

From 09d512ae4878b767ac1ac5e674795c4ec83f14e6 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 9 Jun 2022 20:11:30 -0400
Subject: [PATCH 15/17] add docstring for makeBinaryOrdering

---
 gtsam/hybrid/tests/Switching.h | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
index 545259e48..38a327a58 100644
--- a/gtsam/hybrid/tests/Switching.h
+++ b/gtsam/hybrid/tests/Switching.h
@@ -173,7 +173,10 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
 }
 
 /**
- * @brief
+ * @brief Return the ordering as a binary tree such that all parent nodes are
+ * above their children.
+ *
+ * This will result in a nested dissection Bayes tree after elimination.
  *
  * @param input The original ordering.
  * @return std::pair<KeyVector, std::vector<int>>

From 7496f2f43afa11d5431cc09d85694a461db0e1cb Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 10 Jun 2022 04:24:13 -0400
Subject: [PATCH 16/17] address review comments

---
 gtsam/hybrid/HybridGaussianFactorGraph.h             | 3 +--
 gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp | 1 +
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h
index 1d49904fe..e1cd2dc5f 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.h
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.h
@@ -134,8 +134,7 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
   template <typename FACTOR>
   IsGaussian<FACTOR> push_gaussian(
       const boost::shared_ptr<FACTOR>& gaussianFactor) {
-    Base::Base::push_back(
-        boost::make_shared<HybridGaussianFactor>(gaussianFactor));
+    Base::push_back(boost::make_shared<HybridGaussianFactor>(gaussianFactor));
   }
 
   /// Construct a factor and add (shared pointer to it) to factor graph.
diff --git a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
index 9e1a2efdd..a96aab6b9 100644
--- a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
@@ -273,6 +273,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
   // GTSAM_PRINT(*hbt);
   // GTSAM_PRINT(*remaining);
 
+  // hbt->marginalFactor(X(1))->print("HBT: ");
   /*
   (Fan) Explanation: the Junction tree will need to reeliminate to get to the
   marginal on X(1), which is not possible because it involves eliminating

From 91da5209fe5801cc845a05828e676cc045774e2e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 20 Jun 2022 09:12:42 -0400
Subject: [PATCH 17/17] add new lines

---
 gtsam/hybrid/HybridFactorGraph.h          | 2 +-
 gtsam/hybrid/tests/testHybridBayesNet.cpp | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h
index cbbc45590..fc730f0c9 100644
--- a/gtsam/hybrid/HybridFactorGraph.h
+++ b/gtsam/hybrid/HybridFactorGraph.h
@@ -137,4 +137,4 @@ class HybridFactorGraph : public FactorGraph<HybridFactor> {
   }
 };
 
-}  // namespace gtsam
\ No newline at end of file
+}  // namespace gtsam
diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index ec6b67d66..f3db83955 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -90,4 +90,4 @@ int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }
-/* ************************************************************************* */
\ No newline at end of file
+/* ************************************************************************* */