From 9f2229fad57e633ac1066415a927435057f8cb08 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 2 Sep 2022 21:30:10 -0400
Subject: [PATCH] remove unused blocks

---
 .../tests/testHybridNonlinearFactorGraph.cpp  | 69 +------------------
 1 file changed, 1 insertion(+), 68 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index 018b017a9..6c07331ab 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -257,14 +257,6 @@ TEST(GaussianElimination, Eliminate_x1) {
   // Add first hybrid factor
   factors.push_back(self.linearizedFactorGraph[1]);
 
-  // TODO(Varun) remove this block since sum is no longer exposed.
-  // // Check that sum works:
-  // auto sum = factors.sum();
-  // Assignment<Key> mode;
-  // mode[M(1)] = 1;
-  // auto actual = sum(mode);               // Selects one of 2 modes.
-  // EXPECT_LONGS_EQUAL(2, actual.size());  // Prior and motion model.
-
   // Eliminate x1
   Ordering ordering;
   ordering += X(1);
@@ -289,15 +281,6 @@ TEST(HybridsGaussianElimination, Eliminate_x2) {
   factors.push_back(self.linearizedFactorGraph[1]);  // involves m1
   factors.push_back(self.linearizedFactorGraph[2]);  // involves m2
 
-  // TODO(Varun) remove this block since sum is no longer exposed.
-  // // Check that sum works:
-  // auto sum = factors.sum();
-  // Assignment<Key> mode;
-  // mode[M(1)] = 0;
-  // mode[M(2)] = 1;
-  // auto actual = sum(mode);               // Selects one of 4 mode
-  // combinations. EXPECT_LONGS_EQUAL(2, actual.size());  // 2 motion models.
-
   // Eliminate x2
   Ordering ordering;
   ordering += X(2);
@@ -366,49 +349,6 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
   EXPECT(discreteFactor->root_->isLeaf() == false);
 }
 
-// /*
-// ****************************************************************************/
-// /// Test the toDecisionTreeFactor method
-// TEST(HybridFactorGraph, ToDecisionTreeFactor) {
-//   size_t K = 3;
-
-//   // Provide tight sigma values so that the errors are visibly different.
-//   double between_sigma = 5e-8, prior_sigma = 1e-7;
-
-//   Switching self(K, between_sigma, prior_sigma);
-
-//   // Clear out discrete factors since sum() cannot hanldle those
-//   HybridGaussianFactorGraph linearizedFactorGraph(
-//       self.linearizedFactorGraph.gaussianGraph(), DiscreteFactorGraph(),
-//       self.linearizedFactorGraph.dcGraph());
-
-//   auto decisionTreeFactor = linearizedFactorGraph.toDecisionTreeFactor();
-
-//   auto allAssignments =
-//       DiscreteValues::CartesianProduct(linearizedFactorGraph.discreteKeys());
-
-//   // Get the error of the discrete assignment m1=0, m2=1.
-//   double actual = (*decisionTreeFactor)(allAssignments[1]);
-
-//   /********************************************/
-//   // Create equivalent factor graph for m1=0, m2=1
-//   GaussianFactorGraph graph = linearizedFactorGraph.gaussianGraph();
-
-//   for (auto &p : linearizedFactorGraph.dcGraph()) {
-//     if (auto mixture =
-//             boost::dynamic_pointer_cast<DCGaussianMixtureFactor>(p)) {
-//       graph.add((*mixture)(allAssignments[1]));
-//     }
-//   }
-
-//   VectorValues values = graph.optimize();
-//   double expected = graph.probPrime(values);
-//   /********************************************/
-//   EXPECT_DOUBLES_EQUAL(expected, actual, 1e-12);
-//   // REGRESSION:
-//   EXPECT_DOUBLES_EQUAL(0.6125, actual, 1e-4);
-// }
-
 /****************************************************************************
  * Test partial elimination
  */
@@ -428,7 +368,6 @@ TEST(HybridFactorGraph, Partial_Elimination) {
       linearizedFactorGraph.eliminatePartialSequential(ordering);
 
   CHECK(hybridBayesNet);
-  //  GTSAM_PRINT(*hybridBayesNet);  // HybridBayesNet
   EXPECT_LONGS_EQUAL(3, hybridBayesNet->size());
   EXPECT(hybridBayesNet->at(0)->frontals() == KeyVector{X(1)});
   EXPECT(hybridBayesNet->at(0)->parents() == KeyVector({X(2), M(1)}));
@@ -438,7 +377,6 @@ TEST(HybridFactorGraph, Partial_Elimination) {
   EXPECT(hybridBayesNet->at(2)->parents() == KeyVector({M(1), M(2)}));
 
   CHECK(remainingFactorGraph);
-  //  GTSAM_PRINT(*remainingFactorGraph);  // HybridFactorGraph
   EXPECT_LONGS_EQUAL(3, remainingFactorGraph->size());
   EXPECT(remainingFactorGraph->at(0)->keys() == KeyVector({M(1)}));
   EXPECT(remainingFactorGraph->at(1)->keys() == KeyVector({M(2), M(1)}));
@@ -721,13 +659,8 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
        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