Address review comments

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);
 

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
   /// @{
 

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 {

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.

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