Add HybridFactorGraph base class and more methods for adding gaussian factors

gtsam/hybrid/HybridFactorGraph.h

@@ -0,0 +1,148 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   HybridFactorGraph.h
+ * @brief  Hybrid factor graph base class that uses type erasure
+ * @author Varun Agrawal
+ * @date   May 28, 2022
+ */
+
+#pragma once
+
+#include <gtsam/discrete/DiscreteFactor.h>
+#include <gtsam/hybrid/HybridDiscreteFactor.h>
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/inference/FactorGraph.h>
+#include <gtsam/inference/Ordering.h>
+
+#include <boost/format.hpp>
+namespace gtsam {
+
+using SharedFactor = boost::shared_ptr<Factor>;
+
+/**
+ * Hybrid Factor Graph
+ * -----------------------
+ * This is the base hybrid factor graph.
+ * Everything inside needs to be hybrid factor or hybrid conditional.
+ */
+class HybridFactorGraph : public FactorGraph<HybridFactor> {
+ public:
+  using Base = FactorGraph<HybridFactor>;
+  using This = HybridFactorGraph;              ///< this class
+  using shared_ptr = boost::shared_ptr<This>;  ///< shared_ptr to This
+
+  using Values = gtsam::Values;  ///< backwards compatibility
+  using Indices = KeyVector;     ///> map from keys to values
+
+ protected:
+  /// Check if FACTOR type is derived from DiscreteFactor.
+  template <typename FACTOR>
+  using IsDiscrete = typename std::enable_if<
+      std::is_base_of<DiscreteFactor, FACTOR>::value>::type;
+
+  /// Check if FACTOR type is derived from HybridFactor.
+  template <typename FACTOR>
+  using IsHybrid = typename std::enable_if<
+      std::is_base_of<HybridFactor, FACTOR>::value>::type;
+
+ public:
+  /// @name Constructors
+  /// @{
+
+  /// Default constructor
+  HybridFactorGraph() = default;
+
+  /**
+   * Implicit copy/downcast constructor to override explicit template container
+   * constructor. In BayesTree this is used for:
+   * `cachedSeparatorMarginal_.reset(*separatorMarginal)`
+   * */
+  template <class DERIVEDFACTOR>
+  HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
+
+  /// @}
+
+  // Allow use of selected FactorGraph methods:
+  using Base::empty;
+  using Base::reserve;
+  using Base::size;
+  using Base::operator[];
+  using Base::add;
+  using Base::push_back;
+  using Base::resize;
+
+  /**
+   * Add a discrete factor *pointer* to the internal discrete graph
+   * @param discreteFactor - boost::shared_ptr to the factor to add
+   */
+  template <typename FACTOR>
+  IsDiscrete<FACTOR> push_discrete(
+      const boost::shared_ptr<FACTOR>& discreteFactor) {
+    Base::push_back(boost::make_shared<HybridDiscreteFactor>(discreteFactor));
+  }
+
+  /**
+   * Add a discrete-continuous (Hybrid) factor *pointer* to the graph
+   * @param hybridFactor - boost::shared_ptr to the factor to add
+   */
+  template <typename FACTOR>
+  IsHybrid<FACTOR> push_hybrid(const boost::shared_ptr<FACTOR>& hybridFactor) {
+    Base::push_back(hybridFactor);
+  }
+
+  /// delete emplace_shared.
+  template <class FACTOR, class... Args>
+  void emplace_shared(Args&&... args) = delete;
+
+  /// Construct a factor and add (shared pointer to it) to factor graph.
+  template <class FACTOR, class... Args>
+  IsDiscrete<FACTOR> emplace_discrete(Args&&... args) {
+    auto factor = boost::allocate_shared<FACTOR>(
+        Eigen::aligned_allocator<FACTOR>(), std::forward<Args>(args)...);
+    push_discrete(factor);
+  }
+
+  /// Construct a factor and add (shared pointer to it) to factor graph.
+  template <class FACTOR, class... Args>
+  IsHybrid<FACTOR> emplace_hybrid(Args&&... args) {
+    auto factor = boost::allocate_shared<FACTOR>(
+        Eigen::aligned_allocator<FACTOR>(), std::forward<Args>(args)...);
+    push_hybrid(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<DiscreteFactor>(sharedFactor)) {
+      push_discrete(p);
+    }
+    if (auto p = boost::dynamic_pointer_cast<HybridFactor>(sharedFactor)) {
+      push_hybrid(p);
+    }
+  }
+
+  /** Constructor from iterator over factors (shared_ptr or plain objects) */
+  template <typename ITERATOR>
+  void push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
+    for (auto&& it = firstFactor; it != lastFactor; it++) {
+      push_back(*it);
+    }
+  }
+};
+
+}  // namespace gtsam

gtsam/hybrid/HybridGaussianFactorGraph.h

@@ -19,6 +19,8 @@
 #pragma once
 
 #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>
@@ -55,8 +57,7 @@ struct EliminationTraits<HybridGaussianFactorGraph> {
   typedef HybridEliminationTree
       EliminationTreeType;                      ///< Type of elimination tree
   typedef HybridBayesTree BayesTreeType;        ///< Type of Bayes tree
-  typedef HybridJunctionTree
+  typedef HybridJunctionTree JunctionTreeType;  ///< Type of Junction tree
-      JunctionTreeType;  ///< Type of Junction tree
   /// The default dense elimination function
   static std::pair<boost::shared_ptr<ConditionalType>,
                    boost::shared_ptr<FactorType> >
@@ -72,10 +73,16 @@ struct EliminationTraits<HybridGaussianFactorGraph> {
  * Everything inside needs to be hybrid factor or hybrid conditional.
  */
 class GTSAM_EXPORT HybridGaussianFactorGraph
-    : public FactorGraph<HybridFactor>,
+    : public HybridFactorGraph,
       public EliminateableFactorGraph<HybridGaussianFactorGraph> {
+ 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 = FactorGraph<HybridFactor>;
+  using Base = HybridFactorGraph;
   using This = HybridGaussianFactorGraph;  ///< this class
   using BaseEliminateable =
       EliminateableFactorGraph<This>;          ///< for elimination
@@ -100,7 +107,13 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
 
   /// @}
 
-  using FactorGraph::add;
+  using Base::empty;
+  using Base::reserve;
+  using Base::size;
+  using Base::operator[];
+  using Base::add;
+  using Base::push_back;
+  using Base::resize;
 
   /// Add a Jacobian factor to the factor graph.
   void add(JacobianFactor&& factor);
@@ -113,6 +126,40 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
 
   /// 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);
+    }
+  }
 };
 
 }  // namespace gtsam