From e6fe9093af38c66325f1bf83eb3c1c421afbc4d3 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 7 Jan 2023 08:15:45 -0800
Subject: [PATCH] Removed emplace_hybrid, can be done with emplace_shared
 now...

---
 gtsam/hybrid/HybridFactorGraph.h              | 47 ++-----------------
 gtsam/hybrid/tests/Switching.h                |  2 +-
 gtsam/hybrid/tests/testHybridBayesNet.cpp     |  2 +-
 gtsam/hybrid/tests/testHybridEstimation.cpp   |  2 +-
 .../tests/testHybridNonlinearFactorGraph.cpp  |  4 +-
 5 files changed, 8 insertions(+), 49 deletions(-)

diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h
index 4d2a113c5..7d30663a3 100644
--- a/gtsam/hybrid/HybridFactorGraph.h
+++ b/gtsam/hybrid/HybridFactorGraph.h
@@ -45,17 +45,6 @@ class HybridFactorGraph : public FactorGraph<Factor> {
   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
   /// @{
@@ -72,38 +61,8 @@ class HybridFactorGraph : public FactorGraph<Factor> {
   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-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);
-  }
-
-  /// Construct a discrete factor and add shared pointer to the factor graph.
-  template <class FACTOR, class... Args>
-  IsDiscrete<FACTOR> emplace_discrete(Args&&... args) {
-    emplace_shared<FACTOR>(std::forward<Args>(args)...);
-  }
-
-  /// 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);
-  }
+  /// @name Extra methods to inspect discrete/continuous keys.
+  /// @{
 
   /// Get all the discrete keys in the factor graph.
   DiscreteKeys discreteKeys() const;
@@ -116,6 +75,8 @@ class HybridFactorGraph : public FactorGraph<Factor> {
 
   /// Get all the continuous keys in the factor graph.
   const KeySet continuousKeySet() const;
+
+  /// @}
 };
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
index 46831c54e..ab7a9a84f 100644
--- a/gtsam/hybrid/tests/Switching.h
+++ b/gtsam/hybrid/tests/Switching.h
@@ -163,7 +163,7 @@ struct Switching {
       for (auto &&f : motion_models) {
         components.push_back(boost::dynamic_pointer_cast<NonlinearFactor>(f));
       }
-      nonlinearFactorGraph.emplace_hybrid<MixtureFactor>(
+      nonlinearFactorGraph.emplace_shared<MixtureFactor>(
           keys, DiscreteKeys{modes[k]}, components);
     }
 
diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index 7f938b434..9c7b3ab5a 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -337,7 +337,7 @@ TEST(HybridBayesNet, Sampling) {
       boost::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
   std::vector<NonlinearFactor::shared_ptr> factors = {zero_motion, one_motion};
   nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
-  nfg.emplace_hybrid<MixtureFactor>(
+  nfg.emplace_shared<MixtureFactor>(
       KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors);
 
   DiscreteKey mode(M(0), 2);
diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp
index 2a99834f1..93da30c6f 100644
--- a/gtsam/hybrid/tests/testHybridEstimation.cpp
+++ b/gtsam/hybrid/tests/testHybridEstimation.cpp
@@ -416,7 +416,7 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
       boost::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
   const auto one_motion =
       boost::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
-  nfg.emplace_hybrid<MixtureFactor>(
+  nfg.emplace_shared<MixtureFactor>(
       KeyVector{X(0), X(1)}, DiscreteKeys{m},
       std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
 
diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index afa5c8710..94a611a9e 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -368,8 +368,6 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
   CHECK(discreteFactor);
   EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size());
   EXPECT(discreteFactor->root_->isLeaf() == false);
-
-  // TODO(Varun) Test emplace_discrete
 }
 
 /****************************************************************************
@@ -687,7 +685,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
        moving = boost::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
                                                       noise_model);
   std::vector<PlanarMotionModel::shared_ptr> motion_models = {still, moving};
-  fg.emplace_hybrid<MixtureFactor>(
+  fg.emplace_shared<MixtureFactor>(
       contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models);
 
   // Add Range-Bearing measurements to from X0 to L0 and X1 to L1.