From 0e4db307136f6d82425d85cf95db8d643add1d53 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 10 Aug 2022 18:37:24 -0400
Subject: [PATCH] use templetized constructor for MixtureFactor

---
 gtsam/hybrid/MixtureFactor.h                  | 15 ++++++++++---
 .../tests/testHybridNonlinearFactorGraph.cpp  | 21 +++++++------------
 2 files changed, 19 insertions(+), 17 deletions(-)

diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h
index 8c25457ef..3cd21e32e 100644
--- a/gtsam/hybrid/MixtureFactor.h
+++ b/gtsam/hybrid/MixtureFactor.h
@@ -86,17 +86,26 @@ class MixtureFactor : public HybridFactor {
    * elements based on the number of discrete keys and the cardinality of the
    * keys, so that the decision tree is constructed appropriately.
    *
+   * @tparam FACTOR The type of the factor shared pointers being passed in. Will
+   * be typecast to NonlinearFactor shared pointers.
    * @param keys Vector of keys for continuous factors.
    * @param discreteKeys Vector of discrete keys.
    * @param factors Vector of shared pointers to factors.
    * @param normalized Flag indicating if the factor error is already
    * normalized.
    */
+  template <typename FACTOR>
   MixtureFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
-                const std::vector<sharedFactor>& factors,
+                const std::vector<boost::shared_ptr<FACTOR>>& factors,
                 bool normalized = false)
-      : MixtureFactor(keys, discreteKeys, Factors(discreteKeys, factors),
-                      normalized) {}
+      : Base(keys, discreteKeys), normalized_(normalized) {
+    std::vector<NonlinearFactor::shared_ptr> nonlinear_factors;
+    for (auto&& f : factors) {
+      nonlinear_factors.push_back(
+          boost::dynamic_pointer_cast<NonlinearFactor>(f));
+    }
+    factors_ = Factors(discreteKeys, nonlinear_factors);
+  }
 
   ~MixtureFactor() = default;
 
diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index b471079b0..814492686 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -105,9 +105,7 @@ TEST(HybridGaussianFactorGraph, Resize) {
   auto still = boost::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
        moving = boost::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model);
 
-  // TODO(Varun) This is declared as NonlinearFactor instead of MotionModel, aka
-  // not clear!!
-  std::vector<NonlinearFactor::shared_ptr> components = {still, moving};
+  std::vector<MotionModel::shared_ptr> components = {still, moving};
   auto dcFactor = boost::make_shared<MixtureFactor>(
       contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
   nhfg.push_back(dcFactor);
@@ -431,14 +429,15 @@ TEST(HybridFactorGraph, Full_Elimination) {
         linearizedFactorGraph.eliminatePartialSequential(ordering);
 
     DiscreteFactorGraph discrete_fg;
-    //TODO(Varun) Make this a function of HybridGaussianFactorGraph?
-    for(HybridFactor::shared_ptr& factor: (*remainingFactorGraph_partial)) {
+    // TODO(Varun) Make this a function of HybridGaussianFactorGraph?
+    for (HybridFactor::shared_ptr& factor : (*remainingFactorGraph_partial)) {
       auto df = dynamic_pointer_cast<HybridDiscreteFactor>(factor);
       discrete_fg.push_back(df->inner());
     }
     ordering.clear();
     for (size_t k = 1; k < self.K; k++) ordering += M(k);
-    discreteBayesNet = *discrete_fg.eliminateSequential(ordering, EliminateForMPE);
+    discreteBayesNet =
+        *discrete_fg.eliminateSequential(ordering, EliminateForMPE);
   }
 
   // Create ordering.
@@ -678,19 +677,13 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
   Pose2 odometry(2.0, 0.0, 0.0);
   KeyVector contKeys = {X(0), X(1)};
   auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
-  auto still = boost::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0,
-  0),
+  auto still = boost::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, 0),
                                                      noise_model),
        moving = boost::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
                                                       noise_model);
   std::vector<PlanarMotionModel::shared_ptr> motion_models = {still, moving};
-  //TODO(Varun) Make a templated constructor for MixtureFactor which does this?
-  std::vector<NonlinearFactor::shared_ptr> components;
-  for (auto&& f : motion_models) {
-    components.push_back(boost::dynamic_pointer_cast<NonlinearFactor>(f));
-  }
   fg.emplace_hybrid<MixtureFactor>(
-      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
+      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models);
 
   // Add Range-Bearing measurements to from X0 to L0 and X1 to L1.
   // create a noise model for the landmark measurements