Prototype a type-erased hybrid machinery

gtsam/CMakeLists.txt

@@ -10,6 +10,7 @@ set (gtsam_subdirs
     inference
     symbolic
     discrete
+	hybrid
     linear
     nonlinear
     sam

gtsam/hybrid/CGMixtureFactor.cpp

@@ -0,0 +1,5 @@
+//
+// Created by Fan Jiang on 3/11/22.
+//
+
+#include "CGMixtureFactor.h"

gtsam/hybrid/CGMixtureFactor.h

@@ -0,0 +1,23 @@
+/* ----------------------------------------------------------------------------
+ * Copyright 2021 The Ambitious Folks of the MRG
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   CGMixtureFactor.h
+ * @brief  Custom hybrid factor graph for discrete + continuous factors
+ * @author Kevin Doherty, kdoherty@mit.edu
+ * @author Varun Agrawal
+ * @author Fan Jiang
+ * @date   December 2021
+ */
+
+#include <gtsam/hybrid/HybridFactor.h>
+
+namespace gtsam {
+
+class CGMixtureFactor : HybridFactor {
+
+};
+
+}

gtsam/hybrid/CMakeLists.txt

@@ -0,0 +1,8 @@
+# Install headers
+set(subdir hybrid)
+file(GLOB hybrid_headers "*.h")
+# FIXME: exclude headers
+install(FILES ${hybrid_headers} DESTINATION include/gtsam/hybrid)
+
+# Add all tests
+add_subdirectory(tests)

gtsam/hybrid/HybridBayesNet.cpp

@@ -0,0 +1,16 @@
+/* ----------------------------------------------------------------------------
+ * 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   HybridBayesNet.cpp
+ * @brief  A bayes net of Gaussian Conditionals indexed by discrete keys.
+ * @author Fan Jiang
+ * @date   January 2022
+ */
+
+#include <gtsam/hybrid/HybridBayesNet.h>

gtsam/hybrid/HybridBayesNet.h

@@ -0,0 +1,43 @@
+/* ----------------------------------------------------------------------------
+ * 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    HybridBayesNet.h
+ * @brief   A bayes net of Gaussian Conditionals indexed by discrete keys.
+ * @author  Varun Agrawal
+ * @author  Fan Jiang
+ * @author  Frank Dellaert
+ * @date    December 2021
+ */
+
+#pragma once
+
+#include <gtsam/hybrid/HybridConditional.h>
+#include <gtsam/inference/BayesNet.h>
+
+#include <iostream>  // TODO!
+
+namespace gtsam {
+
+/**
+ * A hybrid Bayes net can have discrete conditionals, Gaussian mixtures,
+ * or pure Gaussian conditionals.
+ */
+class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
+ public:
+  using Base = BayesNet<HybridConditional>;
+  using This = HybridBayesNet;
+  using ConditionalType = HybridConditional;
+  using shared_ptr = boost::shared_ptr<HybridBayesNet>;
+  using sharedConditional = boost::shared_ptr<ConditionalType>;
+
+  /** Construct empty bayes net */
+  HybridBayesNet() : Base() {}
+};
+
+}  // namespace gtsam

gtsam/hybrid/HybridBayesTree.cpp

@@ -0,0 +1,38 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    HybridBayesTree.cpp
+ * @brief   Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree
+ * @brief   HybridBayesTree
+ * @author  Frank Dellaert
+ * @author  Richard Roberts
+ */
+
+#include <gtsam/base/treeTraversal-inst.h>
+#include <gtsam/inference/BayesTree-inst.h>
+#include <gtsam/inference/BayesTreeCliqueBase-inst.h>
+#include <gtsam/hybrid/HybridBayesTree.h>
+#include <gtsam/hybrid/HybridBayesNet.h>
+
+namespace gtsam {
+
+// Instantiate base class
+template class BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph>;
+template class BayesTree<HybridBayesTreeClique>;
+
+/* ************************************************************************* */
+bool HybridBayesTree::equals(const This& other, double tol) const {
+  return Base::equals(other, tol);
+}
+
+/* **************************************************************************/
+}  // namespace gtsam

gtsam/hybrid/HybridBayesTree.h

@@ -0,0 +1,77 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    HybridBayesTree.h
+ * @brief   Hybrid Bayes Tree, the result of eliminating a
+ * HybridJunctionTree
+ * @brief   HybridBayesTree
+ * @author  Frank Dellaert
+ * @author  Richard Roberts
+ */
+
+#pragma once
+
+#include <gtsam/hybrid/HybridBayesNet.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/inference/BayesTree.h>
+#include <gtsam/inference/Conditional.h>
+#include <gtsam/inference/BayesTreeCliqueBase.h>
+
+#include <string>
+
+namespace gtsam {
+
+// Forward declarations
+class HybridConditional;
+class VectorValues;
+
+/* ************************************************************************* */
+/** A clique in a HybridBayesTree */
+class GTSAM_EXPORT HybridBayesTreeClique
+    : public BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph> {
+ public:
+  typedef HybridBayesTreeClique This;
+  typedef BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph>
+      Base;
+  typedef boost::shared_ptr<This> shared_ptr;
+  typedef boost::weak_ptr<This> weak_ptr;
+  HybridBayesTreeClique() {}
+  virtual ~HybridBayesTreeClique() {}
+  HybridBayesTreeClique(
+      const boost::shared_ptr<HybridConditional>& conditional)
+      : Base(conditional) {}
+
+};
+
+/* ************************************************************************* */
+/** A Bayes tree representing a Hybrid density */
+class GTSAM_EXPORT HybridBayesTree
+    : public BayesTree<HybridBayesTreeClique> {
+ private:
+  typedef BayesTree<HybridBayesTreeClique> Base;
+
+ public:
+  typedef HybridBayesTree This;
+  typedef boost::shared_ptr<This> shared_ptr;
+
+  /// @name Standard interface
+  /// @{
+  /** Default constructor, creates an empty Bayes tree */
+  HybridBayesTree() = default;
+
+  /** Check equality */
+  bool equals(const This& other, double tol = 1e-9) const;
+
+  /// @}
+};
+
+}  // namespace gtsam

gtsam/hybrid/HybridConditional.cpp

@@ -0,0 +1,23 @@
+//
+// Created by Fan Jiang on 3/11/22.
+//
+
+#include <gtsam/hybrid/HybridConditional.h>
+#include <gtsam/inference/Conditional-inst.h>
+
+namespace gtsam {
+void HybridConditional::print(const std::string &s,
+                                     const KeyFormatter &formatter) const {
+  Conditional::print(s, formatter);
+}
+
+bool HybridConditional::equals(const HybridFactor &other,
+                                      double tol) const {
+  return false;
+}
+
+HybridConditional HybridConditional::operator*(const HybridConditional &other) const {
+  return {};
+}
+}
+

gtsam/hybrid/HybridConditional.h

@@ -0,0 +1,99 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridConditional.h
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
+
+#pragma once
+
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/discrete/DiscreteConditional.h>
+#include <gtsam/inference/Conditional.h>
+
+
+#include <boost/make_shared.hpp>
+#include <boost/shared_ptr.hpp>
+#include <string>
+#include <vector>
+
+namespace gtsam {
+
+/**
+ * Hybrid Conditional Density
+ *
+ * As a type-erased variant of:
+ * - DiscreteConditional
+ * - GaussianMixture
+ * - GaussianConditional
+ */
+class GTSAM_EXPORT HybridConditional
+: public HybridFactor,
+public Conditional<HybridFactor, HybridConditional> {
+public:
+// typedefs needed to play nice with gtsam
+typedef HybridConditional This;            ///< Typedef to this class
+typedef boost::shared_ptr<This> shared_ptr;  ///< shared_ptr to this class
+typedef HybridFactor BaseFactor;  ///< Typedef to our factor base class
+typedef Conditional<BaseFactor, This>
+    BaseConditional;  ///< Typedef to our conditional base class
+
+private:
+// Type-erased pointer to the inner type
+std::unique_ptr<Factor> inner;
+
+public:
+/// @name Standard Constructors
+/// @{
+
+/// Default constructor needed for serialization.
+HybridConditional() = default;
+
+/**
+ * @brief Combine two conditionals, yielding a new conditional with the union
+ * of the frontal keys, ordered by gtsam::Key.
+ *
+ * The two conditionals must make a valid Bayes net fragment, i.e.,
+ * the frontal variables cannot overlap, and must be acyclic:
+ * Example of correct use:
+ *   P(A,B) = P(A|B) * P(B)
+ *   P(A,B|C) = P(A|B) * P(B|C)
+ *   P(A,B,C) = P(A,B|C) * P(C)
+ * Example of incorrect use:
+ *   P(A|B) * P(A|C) = ?
+ *   P(A|B) * P(B|A) = ?
+ * We check for overlapping frontals, but do *not* check for cyclic.
+ */
+HybridConditional operator*(const HybridConditional& other) const;
+
+/// @}
+/// @name Testable
+/// @{
+
+/// GTSAM-style print
+void print(
+    const std::string& s = "Hybrid Conditional: ",
+    const KeyFormatter& formatter = DefaultKeyFormatter) const override;
+
+/// GTSAM-style equals
+bool equals(const HybridFactor& other, double tol = 1e-9) const override;
+
+/// @}
+};
+// DiscreteConditional
+
+// traits
+template <>
+struct traits<HybridConditional> : public Testable<DiscreteConditional> {};
+
+}  // namespace gtsam

gtsam/hybrid/HybridDiscreteFactor.cpp

@@ -0,0 +1,19 @@
+//
+// Created by Fan Jiang on 3/11/22.
+//
+
+#include <gtsam/hybrid/HybridDiscreteFactor.h>
+
+#include <boost/make_shared.hpp>
+
+namespace gtsam {
+
+HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other) {
+  inner = other;
+}
+
+HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) : inner(boost::make_shared<DecisionTreeFactor>(std::move(dtf))) {
+
+};
+
+}

gtsam/hybrid/HybridDiscreteFactor.h

@@ -0,0 +1,34 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridDiscreteFactor.h
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
+
+#include <gtsam/discrete/DiscreteFactor.h>
+#include <gtsam/discrete/DecisionTreeFactor.h>
+#include <gtsam/hybrid/HybridFactor.h>
+
+namespace gtsam {
+
+class HybridDiscreteFactor : public HybridFactor {
+ public:
+  DiscreteFactor::shared_ptr inner;
+
+  // Implicit conversion from a shared ptr of GF
+  HybridDiscreteFactor(DiscreteFactor::shared_ptr other);
+
+  // Forwarding constructor from concrete JacobianFactor
+  HybridDiscreteFactor(DecisionTreeFactor &&dtf);
+};
+}

gtsam/hybrid/HybridEliminationTree.cpp

@@ -0,0 +1,43 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridEliminationTree.cpp
+ * @date Mar 11, 2022
+ * @author Fan Jiang
+ */
+
+#include <gtsam/inference/EliminationTree-inst.h>
+#include <gtsam/hybrid/HybridEliminationTree.h>
+
+namespace gtsam {
+
+// Instantiate base class
+template class EliminationTree<HybridBayesNet, HybridFactorGraph>;
+
+/* ************************************************************************* */
+HybridEliminationTree::HybridEliminationTree(
+    const HybridFactorGraph& factorGraph, const VariableIndex& structure,
+    const Ordering& order) :
+    Base(factorGraph, structure, order) {}
+
+/* ************************************************************************* */
+HybridEliminationTree::HybridEliminationTree(
+    const HybridFactorGraph& factorGraph, const Ordering& order) :
+    Base(factorGraph, order) {}
+
+/* ************************************************************************* */
+bool HybridEliminationTree::equals(const This& other, double tol) const
+{
+  return Base::equals(other, tol);
+}
+
+}

gtsam/hybrid/HybridEliminationTree.h

@@ -0,0 +1,62 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridEliminationTree.h
+ * @date Mar 11, 2022
+ * @author Fan Jiang
+ */
+
+#pragma once
+
+#include <gtsam/hybrid/HybridBayesNet.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/inference/EliminationTree.h>
+
+namespace gtsam {
+
+class GTSAM_EXPORT HybridEliminationTree :
+    public EliminationTree<HybridBayesNet, HybridFactorGraph>
+{
+ public:
+  typedef EliminationTree<HybridBayesNet, HybridFactorGraph> Base; ///< Base class
+  typedef HybridEliminationTree This; ///< This class
+  typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
+
+  /**
+  * Build the elimination tree of a factor graph using pre-computed column structure.
+  * @param factorGraph The factor graph for which to build the elimination tree
+  * @param structure The set of factors involving each variable.  If this is not
+  * precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
+  * named constructor instead.
+  * @return The elimination tree
+  */
+  HybridEliminationTree(const HybridFactorGraph& factorGraph,
+                          const VariableIndex& structure, const Ordering& order);
+
+  /** Build the elimination tree of a factor graph.  Note that this has to compute the column
+  * structure as a VariableIndex, so if you already have this precomputed, use the other
+  * constructor instead.
+  * @param factorGraph The factor graph for which to build the elimination tree
+  */
+  HybridEliminationTree(const HybridFactorGraph& factorGraph,
+                          const Ordering& order);
+
+  /** Test whether the tree is equal to another */
+  bool equals(const This& other, double tol = 1e-9) const;
+
+ private:
+
+  friend class ::EliminationTreeTester;
+
+};
+
+}

gtsam/hybrid/HybridFactor.cpp

@@ -0,0 +1,18 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridFactor.cpp
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
+
+#include <gtsam/hybrid/HybridFactor.h>

gtsam/hybrid/HybridFactor.h

@@ -0,0 +1,83 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridFactor.h
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/inference/Factor.h>
+#include <gtsam/base/Testable.h>
+
+#include <string>
+namespace gtsam {
+
+/**
+ * Base class for hybrid probabilistic factors
+ */
+class GTSAM_EXPORT HybridFactor: public Factor {
+
+public:
+
+// typedefs needed to play nice with gtsam
+typedef HybridFactor This; ///< This class
+typedef boost::shared_ptr<HybridFactor> shared_ptr; ///< shared_ptr to this class
+typedef Factor Base; ///< Our base class
+
+using Values = Values; ///< backwards compatibility
+
+public:
+
+/// @name Standard Constructors
+/// @{
+
+/** Default constructor creates empty factor */
+HybridFactor() {}
+
+/** Construct from container of keys.  This constructor is used internally from derived factor
+ *  constructors, either from a container of keys or from a boost::assign::list_of. */
+template<typename CONTAINER>
+HybridFactor(const CONTAINER& keys) : Base(keys) {}
+
+/// Virtual destructor
+virtual ~HybridFactor() {
+}
+
+/// @}
+/// @name Testable
+/// @{
+
+/// equals
+virtual bool equals(const HybridFactor& lf, double tol = 1e-9) const = 0;
+
+/// print
+void print(
+    const std::string& s = "HybridFactor\n",
+    const KeyFormatter& formatter = DefaultKeyFormatter) const override {
+Base::print(s, formatter);
+}
+
+/// @}
+/// @name Standard Interface
+/// @{
+
+/// @}
+};
+// HybridFactor
+
+// traits
+template<> struct traits<HybridFactor> : public Testable<HybridFactor> {};
+
+}// namespace gtsam

gtsam/hybrid/HybridFactorGraph.cpp

@@ -0,0 +1,58 @@
+//
+// Created by Fan Jiang on 3/11/22.
+//
+
+#include <gtsam/hybrid/HybridEliminationTree.h>
+#include <gtsam/hybrid/HybridJunctionTree.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
+
+#include <gtsam/hybrid/HybridGaussianFactor.h>
+
+#include <gtsam/inference/EliminateableFactorGraph-inst.h>
+
+namespace gtsam {
+
+template class EliminateableFactorGraph<HybridFactorGraph>;
+
+/* ************************************************************************ */
+std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
+EliminateHybrid(const HybridFactorGraph& factors,
+                  const Ordering& frontalKeys) {
+  // NOTE(fan): Because we are in the Conditional Gaussian regime there are only
+  // few cases: continuous variable, we make a GM if there are hybrid factors;
+  // continuous variable, we make a GF if there are no hybrid factors;
+  // discrete variable, no continuous factor is allowed (escapes CG regime), so
+  // we panic, if discrete only we do the discrete elimination.
+
+  // The issue here is that, how can we know which variable is discrete if we
+  // unify Values? Obviously we can tell using the factors, but is that fast?
+
+  // PRODUCT: multiply all factors
+  gttic(product);
+  HybridGaussianFactor product(JacobianFactor(0, I_3x3, Z_3x1));
+//  for (auto&& factor : factors) product = (*factor) * product;
+  gttoc(product);
+
+  // sum out frontals, this is the factor on the separator
+  gttic(sum);
+//  HybridFactor::shared_ptr sum = product.sum(frontalKeys);
+  gttoc(sum);
+
+  // Ordering keys for the conditional so that frontalKeys are really in front
+//  Ordering orderedKeys;
+//  orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(),
+//                     frontalKeys.end());
+//  orderedKeys.insert(orderedKeys.end(), sum->keys().begin(),
+//                     sum->keys().end());
+
+  // now divide product/sum to get conditional
+  gttic(divide);
+//  auto conditional =
+//      boost::make_shared<HybridConditional>(product, *sum, orderedKeys);
+  gttoc(divide);
+
+//  return std::make_pair(conditional, sum);
+  return std::make_pair(boost::make_shared<HybridConditional>(), boost::make_shared<HybridGaussianFactor>(product));
+}
+
+}

gtsam/hybrid/HybridFactorGraph.h

@@ -0,0 +1,83 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 that uses type erasure
+ * @author Fan Jiang
+ * @date   Mar 11, 2022
+ */
+
+#pragma once
+
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/inference/EliminateableFactorGraph.h>
+#include <gtsam/inference/FactorGraph.h>
+#include <gtsam/inference/Ordering.h>
+
+namespace gtsam {
+
+// Forward declarations
+class HybridFactorGraph;
+class HybridConditional;
+class HybridBayesNet;
+class HybridEliminationTree;
+class HybridBayesTree;
+class HybridJunctionTree;
+
+/** Main elimination function for HybridFactorGraph */
+GTSAM_EXPORT std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
+EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys);
+
+/* ************************************************************************* */
+template<> struct EliminationTraits<HybridFactorGraph>
+{
+  typedef HybridFactor FactorType;                   ///< Type of factors in factor graph
+  typedef HybridFactorGraph FactorGraphType;         ///< Type of the factor graph (e.g. HybridFactorGraph)
+  typedef HybridConditional ConditionalType;         ///< Type of conditionals from elimination
+  typedef HybridBayesNet BayesNetType;               ///< Type of Bayes net from sequential elimination
+  typedef HybridEliminationTree EliminationTreeType; ///< Type of elimination tree
+  typedef HybridBayesTree BayesTreeType;             ///< Type of Bayes tree
+  typedef HybridJunctionTree JunctionTreeType;       ///< Type of Junction tree
+  /// The default dense elimination function
+  static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> >
+  DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
+    return EliminateHybrid(factors, keys); }
+};
+
+
+class HybridFactorGraph : public FactorGraph<HybridFactor>, public EliminateableFactorGraph<HybridFactorGraph> {
+ public:
+  using Base = FactorGraph<HybridFactor>;
+  using This = HybridFactorGraph;          ///< this class
+  using BaseEliminateable =
+  EliminateableFactorGraph<This>;          ///< for elimination
+  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
+
+ public:
+  HybridFactorGraph() = default;
+
+  /** Construct from container of factors (shared_ptr or plain objects) */
+  template <class CONTAINER>
+  explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {}
+
+  /** Implicit copy/downcast constructor to override explicit template container
+   * constructor */
+  template <class DERIVEDFACTOR>
+  HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
+
+};
+
+}
+

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -0,0 +1,23 @@
+//
+// Created by Fan Jiang on 3/11/22.
+//
+
+#include <gtsam/hybrid/HybridGaussianFactor.h>
+
+#include <boost/make_shared.hpp>
+
+namespace gtsam {
+
+HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) : Base(other->keys()){
+  inner = other;
+}
+
+HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) : Base(jf.keys()),  inner(boost::make_shared<JacobianFactor>(std::move(jf))) {
+
+}
+
+bool HybridGaussianFactor::equals(const HybridFactor& lf, double tol) const {
+  return false;
+};
+
+}

gtsam/hybrid/HybridGaussianFactor.h

@@ -0,0 +1,39 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridGaussianFactor.h
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
+
+#include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/hybrid/HybridFactor.h>
+
+namespace gtsam {
+
+class HybridGaussianFactor : public HybridFactor {
+ public:
+  using Base = HybridFactor;
+
+  GaussianFactor::shared_ptr inner;
+
+  // Implicit conversion from a shared ptr of GF
+  HybridGaussianFactor(GaussianFactor::shared_ptr other);
+
+  // Forwarding constructor from concrete JacobianFactor
+  HybridGaussianFactor(JacobianFactor &&jf);
+
+ public:
+  virtual bool equals(const HybridFactor& lf, double tol) const override;
+};
+}

gtsam/hybrid/HybridJunctionTree.cpp

@@ -0,0 +1,34 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridJunctionTree.cpp
+ * @date Mar 29, 2013
+ * @author Frank Dellaert
+ * @author Richard Roberts
+ */
+
+#include <gtsam/inference/JunctionTree-inst.h>
+#include <gtsam/hybrid/HybridJunctionTree.h>
+#include <gtsam/hybrid/HybridEliminationTree.h>
+
+namespace gtsam {
+
+// Instantiate base classes
+template class EliminatableClusterTree<HybridBayesTree, HybridFactorGraph>;
+template class JunctionTree<HybridBayesTree, HybridFactorGraph>;
+
+/* ************************************************************************* */
+HybridJunctionTree::HybridJunctionTree(
+    const HybridEliminationTree& eliminationTree) :
+    Base(eliminationTree) {}
+
+}

gtsam/hybrid/HybridJunctionTree.h

@@ -0,0 +1,67 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridJunctionTree.h
+ * @date Mar 11, 2022
+ * @author Fan Jiang
+ */
+
+#pragma once
+
+#include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/hybrid/HybridBayesTree.h>
+#include <gtsam/inference/JunctionTree.h>
+
+namespace gtsam {
+
+// Forward declarations
+class HybridEliminationTree;
+
+/**
+ * An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,
+ * with the additional property that it represents the clique tree associated with a Bayes net.
+ *
+ * In GTSAM a junction tree is an intermediate data structure in multifrontal
+ * variable elimination.  Each node is a cluster of factors, along with a
+ * clique of variables that are eliminated all at once. In detail, every node k represents
+ * a clique (maximal fully connected subset) of an associated chordal graph, such as a
+ * chordal Bayes net resulting from elimination.
+ *
+ * The difference with the BayesTree is that a JunctionTree stores factors, whereas a
+ * BayesTree stores conditionals, that are the product of eliminating the factors in the
+ * corresponding JunctionTree cliques.
+ *
+ * The tree structure and elimination method are exactly analogous to the EliminationTree,
+ * except that in the JunctionTree, at each node multiple variables are eliminated at a time.
+ *
+ * \addtogroup Multifrontal
+ * \nosubgrouping
+ */
+class GTSAM_EXPORT HybridJunctionTree :
+    public JunctionTree<HybridBayesTree, HybridFactorGraph> {
+ public:
+  typedef JunctionTree<HybridBayesTree, HybridFactorGraph> Base; ///< Base class
+  typedef HybridJunctionTree This; ///< This class
+  typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
+
+  /**
+  * Build the elimination tree of a factor graph using precomputed column structure.
+  * @param factorGraph The factor graph for which to build the elimination tree
+  * @param structure The set of factors involving each variable.  If this is not
+  * precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
+  * named constructor instead.
+  * @return The elimination tree
+  */
+  HybridJunctionTree(const HybridEliminationTree& eliminationTree);
+};
+
+}

gtsam/hybrid/tests/CMakeLists.txt

@@ -0,0 +1 @@
+gtsamAddTestsGlob(hybrid "test*.cpp" "" "gtsam")

gtsam/hybrid/tests/testHybridConditional.cpp

@@ -0,0 +1,73 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testHybridConditional.cpp
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
+
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/hybrid/HybridConditional.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/hybrid/HybridGaussianFactor.h>
+#include <gtsam/hybrid/HybridBayesNet.h>
+#include <gtsam/hybrid/HybridBayesTree.h>
+
+#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/inference/BayesNet.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/assign/std/map.hpp>
+using namespace boost::assign;
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST_UNSAFE(HybridFactorGraph, test) {
+  HybridConditional test;
+  GTSAM_PRINT(test);
+
+  HybridFactorGraph hfg;
+
+  hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
+  GTSAM_PRINT(hfg);
+}
+
+TEST_UNSAFE(HybridFactorGraph, eliminate) {
+  HybridFactorGraph hfg;
+
+  hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
+
+  auto result = hfg.eliminatePartialSequential({0});
+
+  GTSAM_PRINT(*result.first);
+}
+
+TEST(HybridFactorGraph, eliminateMultifrontal) {
+  HybridFactorGraph hfg;
+
+  hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
+
+  auto result = hfg.eliminatePartialMultifrontal({0});
+
+  GTSAM_PRINT(*result.first);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+