From 85f4b4892586ad166dffa83d8e0b914c87868097 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 30 May 2022 08:21:46 -0400
Subject: [PATCH] Improvements to GaussianHybridFactorGraph, make MixtureFactor
 a subclass of HybridFactor

---
 gtsam/hybrid/GaussianHybridFactorGraph.h | 51 ++++++++++++++++++++++++
 gtsam/hybrid/HybridNonlinearFactor.cpp   |  3 +-
 gtsam/hybrid/HybridNonlinearFactor.h     |  4 +-
 gtsam/hybrid/MixtureFactor.h             |  8 ++--
 4 files changed, 59 insertions(+), 7 deletions(-)

diff --git a/gtsam/hybrid/GaussianHybridFactorGraph.h b/gtsam/hybrid/GaussianHybridFactorGraph.h
index 341a0838e..b38a1ebd8 100644
--- a/gtsam/hybrid/GaussianHybridFactorGraph.h
+++ b/gtsam/hybrid/GaussianHybridFactorGraph.h
@@ -20,9 +20,11 @@
 
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/inference/EliminateableFactorGraph.h>
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/Ordering.h>
+#include <gtsam/linear/GaussianFactor.h>
 
 namespace gtsam {
 
@@ -74,6 +76,12 @@ struct EliminationTraits<GaussianHybridFactorGraph> {
 class GaussianHybridFactorGraph
     : public HybridFactorGraph,
       public EliminateableFactorGraph<GaussianHybridFactorGraph> {
+ protected:
+  /// Check if FACTOR type is derived from GaussianFactor.
+  template <typename FACTOR>
+  using IsGaussian = typename std::enable_if<
+      std::is_base_of<GaussianFactor, FACTOR>::value>::type;
+
  public:
   using Base = HybridFactorGraph;
   using This = GaussianHybridFactorGraph;  ///< this class
@@ -119,6 +127,49 @@ class GaussianHybridFactorGraph
 
   /// Add a DecisionTreeFactor as a shared ptr.
   void add(boost::shared_ptr<DecisionTreeFactor> factor);
+
+  /**
+   * Add a gaussian factor *pointer* to the internal gaussian factor graph
+   * @param gaussianFactor - boost::shared_ptr to the factor to add
+   */
+  template <typename FACTOR>
+  IsGaussian<FACTOR> push_gaussian(
+      const boost::shared_ptr<FACTOR>& gaussianFactor) {
+    Base::Base::push_back(
+        boost::make_shared<HybridGaussianFactor>(gaussianFactor));
+  }
+
+  /// Construct a factor and add (shared pointer to it) to factor graph.
+  template <class FACTOR, class... Args>
+  IsGaussian<FACTOR> emplace_gaussian(Args&&... args) {
+    auto factor = boost::allocate_shared<FACTOR>(
+        Eigen::aligned_allocator<FACTOR>(), std::forward<Args>(args)...);
+    push_gaussian(factor);
+  }
+
+  /**
+   * @brief Add a single factor shared pointer to the hybrid factor graph.
+   * Dynamically handles the factor type and assigns it to the correct
+   * underlying container.
+   *
+   * @param sharedFactor The factor to add to this factor graph.
+   */
+  void push_back(const SharedFactor& sharedFactor) {
+    if (auto p = boost::dynamic_pointer_cast<GaussianFactor>(sharedFactor)) {
+      push_gaussian(p);
+    } else {
+      Base::push_back(sharedFactor);
+    }
+  }
+
+  /**
+   * @brief Push back for Gaussian Factor specifically.
+   *
+   * @param sharedFactor Shared ptr to a gaussian factor.
+   */
+  void push_back(const GaussianFactor::shared_ptr& sharedFactor) {
+    push_gaussian(sharedFactor);
+  }
 };
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp
index 69a976790..0938fd2b1 100644
--- a/gtsam/hybrid/HybridNonlinearFactor.cpp
+++ b/gtsam/hybrid/HybridNonlinearFactor.cpp
@@ -22,7 +22,8 @@
 namespace gtsam {
 
 /* ************************************************************************* */
-HybridNonlinearFactor::HybridNonlinearFactor(NonlinearFactor::shared_ptr other)
+HybridNonlinearFactor::HybridNonlinearFactor(
+    const NonlinearFactor::shared_ptr &other)
     : Base(other->keys()), inner_(other) {}
 
 /* ************************************************************************* */
diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h
index 504992e22..4a0c0fd9c 100644
--- a/gtsam/hybrid/HybridNonlinearFactor.h
+++ b/gtsam/hybrid/HybridNonlinearFactor.h
@@ -36,8 +36,8 @@ class HybridNonlinearFactor : public HybridFactor {
   using This = HybridNonlinearFactor;
   using shared_ptr = boost::shared_ptr<This>;
 
-  // Explicit conversion from a shared ptr of GF
-  explicit HybridNonlinearFactor(NonlinearFactor::shared_ptr other);
+  // Explicit conversion from a shared ptr of NonlinearFactor
+  explicit HybridNonlinearFactor(const NonlinearFactor::shared_ptr &other);
 
  public:
   /// @name Testable
diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h
index b2423d20e..d2d1a8d74 100644
--- a/gtsam/hybrid/MixtureFactor.h
+++ b/gtsam/hybrid/MixtureFactor.h
@@ -38,7 +38,7 @@ namespace gtsam {
  * one of (NonlinearFactor, GaussianFactor) which can then be checked to perform
  * the correct operation.
  */
-class MixtureFactor : public HybridNonlinearFactor {
+class MixtureFactor : public HybridFactor {
  public:
   using Base = HybridFactor;
   using This = MixtureFactor;
@@ -213,7 +213,7 @@ class MixtureFactor : public HybridNonlinearFactor {
     // If this is a NoiseModelFactor, we'll use its noiseModel to
     // otherwise noiseModelFactor will be nullptr
     if (auto noiseModelFactor =
-            boost::dynamic_pointer_cast<NoiseModelFactor>(factor);) {
+            boost::dynamic_pointer_cast<NoiseModelFactor>(factor)) {
       // If dynamic cast to NoiseModelFactor succeeded, see if the noise model
       // is Gaussian
       auto noiseModel = noiseModelFactor->noiseModel();
@@ -228,13 +228,13 @@ class MixtureFactor : public HybridNonlinearFactor {
         // something with a normalized noise model
         // TODO(kevin): does this make sense to do? I think maybe not in
         // general? Should we just yell at the user?
-        auto gaussianFactor = factor.linearize(values);
+        auto gaussianFactor = factor->linearize(values);
         infoMat = gaussianFactor->information();
       }
     }
 
     // Compute the (negative) log of the normalizing constant
-    return -(factor.dim() * log(2.0 * M_PI) / 2.0) -
+    return -(factor->dim() * log(2.0 * M_PI) / 2.0) -
            (log(infoMat.determinant()) / 2.0);
   }
 };