From 2a974a4ca7cdb2769db27007bd7c551ea37f7044 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 15 Aug 2022 08:49:51 -0400
Subject: [PATCH] Address review comments

---
 gtsam/hybrid/HybridGaussianFactor.h           |  4 ---
 gtsam/hybrid/HybridNonlinearFactor.h          |  1 -
 gtsam/hybrid/HybridNonlinearFactorGraph.cpp   |  6 ----
 gtsam/hybrid/HybridNonlinearFactorGraph.h     |  8 -----
 .../tests/testHybridNonlinearFactorGraph.cpp  | 31 ++++++++++++++++++-
 5 files changed, 30 insertions(+), 20 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 7ffe39e79..e645e63e5 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -39,10 +39,6 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
 
   HybridGaussianFactor() = default;
 
-  HybridGaussianFactor(const KeyVector &continuousKeys,
-                       const DiscreteKeys &discreteKeys)
-      : Base(continuousKeys, discreteKeys) {}
-
   // Explicit conversion from a shared ptr of GF
   explicit HybridGaussianFactor(GaussianFactor::shared_ptr other);
 
diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h
index 4a0c0fd9c..7776347b3 100644
--- a/gtsam/hybrid/HybridNonlinearFactor.h
+++ b/gtsam/hybrid/HybridNonlinearFactor.h
@@ -39,7 +39,6 @@ class HybridNonlinearFactor : public HybridFactor {
   // Explicit conversion from a shared ptr of NonlinearFactor
   explicit HybridNonlinearFactor(const NonlinearFactor::shared_ptr &other);
 
- public:
   /// @name Testable
   /// @{
 
diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp
index 233badc55..a6abce15a 100644
--- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp
@@ -42,12 +42,6 @@ void HybridNonlinearFactorGraph::print(const std::string& s,
   }
 }
 
-/* ************************************************************************* */
-bool HybridNonlinearFactorGraph::equals(const HybridNonlinearFactorGraph& other,
-                                        double tol) const {
-  return false;
-}
-
 /* ************************************************************************* */
 HybridGaussianFactorGraph HybridNonlinearFactorGraph::linearize(
     const Values& continuousValues) const {
diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.h b/gtsam/hybrid/HybridNonlinearFactorGraph.h
index 073581da2..2ddb8bcea 100644
--- a/gtsam/hybrid/HybridNonlinearFactorGraph.h
+++ b/gtsam/hybrid/HybridNonlinearFactorGraph.h
@@ -109,11 +109,6 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
     }
   }
 
-  // /// Add a nonlinear factor to the factor graph.
-  // void add(NonlinearFactor&& factor) {
-  //   Base::add(boost::make_shared<HybridNonlinearFactor>(std::move(factor)));
-  // }
-
   /// Add a nonlinear factor as a shared ptr.
   void add(boost::shared_ptr<NonlinearFactor> factor);
 
@@ -122,9 +117,6 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
       const std::string& s = "HybridNonlinearFactorGraph",
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
-  /// Check if this factor graph is equal to `other`.
-  bool equals(const HybridNonlinearFactorGraph& other, double tol = 1e-9) const;
-
   /**
    * @brief Linearize all the continuous factors in the
    * HybridNonlinearFactorGraph.
diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index 814492686..a3debb3a9 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -68,6 +68,30 @@ TEST(HybridFactorGraph, GaussianFactorGraph) {
   EXPECT_LONGS_EQUAL(2, ghfg.size());
 }
 
+/***************************************************************************
+ * Test equality for Hybrid Nonlinear Factor Graph.
+ */
+TEST(HybridNonlinearFactorGraph, Equals) {
+  HybridNonlinearFactorGraph graph1;
+  HybridNonlinearFactorGraph graph2;
+
+  // Test empty factor graphs
+  EXPECT(assert_equal(graph1, graph2));
+
+  auto f0 = boost::make_shared<PriorFactor<Pose2>>(
+      1, Pose2(), noiseModel::Isotropic::Sigma(3, 0.001));
+  graph1.push_back(f0);
+  graph2.push_back(f0);
+
+  auto f1 = boost::make_shared<BetweenFactor<Pose2>>(
+      1, 2, Pose2(), noiseModel::Isotropic::Sigma(3, 0.1));
+  graph1.push_back(f1);
+  graph2.push_back(f1);
+
+  // Test non-empty graphs
+  EXPECT(assert_equal(graph1, graph2));
+}
+
 /***************************************************************************
  * Test that the resize method works correctly for a HybridNonlinearFactorGraph.
  */
@@ -682,8 +706,13 @@ 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)}, motion_models);
+      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
 
   // Add Range-Bearing measurements to from X0 to L0 and X1 to L1.
   // create a noise model for the landmark measurements