From 0567303ed3334508cf5ab0c19903b22a5d3fd9e1 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 11 Mar 2022 15:52:28 -0500 Subject: [PATCH] Prototype a type-erased hybrid machinery --- gtsam/CMakeLists.txt | 1 + gtsam/hybrid/CGMixtureFactor.cpp | 5 + gtsam/hybrid/CGMixtureFactor.h | 23 +++++ gtsam/hybrid/CMakeLists.txt | 8 ++ gtsam/hybrid/HybridBayesNet.cpp | 16 ++++ gtsam/hybrid/HybridBayesNet.h | 43 +++++++++ gtsam/hybrid/HybridBayesTree.cpp | 38 ++++++++ gtsam/hybrid/HybridBayesTree.h | 77 +++++++++++++++ gtsam/hybrid/HybridConditional.cpp | 23 +++++ gtsam/hybrid/HybridConditional.h | 99 ++++++++++++++++++++ gtsam/hybrid/HybridDiscreteFactor.cpp | 19 ++++ gtsam/hybrid/HybridDiscreteFactor.h | 34 +++++++ gtsam/hybrid/HybridEliminationTree.cpp | 43 +++++++++ gtsam/hybrid/HybridEliminationTree.h | 62 ++++++++++++ gtsam/hybrid/HybridFactor.cpp | 18 ++++ gtsam/hybrid/HybridFactor.h | 83 ++++++++++++++++ gtsam/hybrid/HybridFactorGraph.cpp | 58 ++++++++++++ gtsam/hybrid/HybridFactorGraph.h | 83 ++++++++++++++++ gtsam/hybrid/HybridGaussianFactor.cpp | 23 +++++ gtsam/hybrid/HybridGaussianFactor.h | 39 ++++++++ gtsam/hybrid/HybridJunctionTree.cpp | 34 +++++++ gtsam/hybrid/HybridJunctionTree.h | 67 +++++++++++++ gtsam/hybrid/tests/CMakeLists.txt | 1 + gtsam/hybrid/tests/testHybridConditional.cpp | 73 +++++++++++++++ 24 files changed, 970 insertions(+) create mode 100644 gtsam/hybrid/CGMixtureFactor.cpp create mode 100644 gtsam/hybrid/CGMixtureFactor.h create mode 100644 gtsam/hybrid/CMakeLists.txt create mode 100644 gtsam/hybrid/HybridBayesNet.cpp create mode 100644 gtsam/hybrid/HybridBayesNet.h create mode 100644 gtsam/hybrid/HybridBayesTree.cpp create mode 100644 gtsam/hybrid/HybridBayesTree.h create mode 100644 gtsam/hybrid/HybridConditional.cpp create mode 100644 gtsam/hybrid/HybridConditional.h create mode 100644 gtsam/hybrid/HybridDiscreteFactor.cpp create mode 100644 gtsam/hybrid/HybridDiscreteFactor.h create mode 100644 gtsam/hybrid/HybridEliminationTree.cpp create mode 100644 gtsam/hybrid/HybridEliminationTree.h create mode 100644 gtsam/hybrid/HybridFactor.cpp create mode 100644 gtsam/hybrid/HybridFactor.h create mode 100644 gtsam/hybrid/HybridFactorGraph.cpp create mode 100644 gtsam/hybrid/HybridFactorGraph.h create mode 100644 gtsam/hybrid/HybridGaussianFactor.cpp create mode 100644 gtsam/hybrid/HybridGaussianFactor.h create mode 100644 gtsam/hybrid/HybridJunctionTree.cpp create mode 100644 gtsam/hybrid/HybridJunctionTree.h create mode 100644 gtsam/hybrid/tests/CMakeLists.txt create mode 100644 gtsam/hybrid/tests/testHybridConditional.cpp diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index a293c6ec2..845ac7cd0 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -10,6 +10,7 @@ set (gtsam_subdirs inference symbolic discrete + hybrid linear nonlinear sam diff --git a/gtsam/hybrid/CGMixtureFactor.cpp b/gtsam/hybrid/CGMixtureFactor.cpp new file mode 100644 index 000000000..d789825f7 --- /dev/null +++ b/gtsam/hybrid/CGMixtureFactor.cpp @@ -0,0 +1,5 @@ +// +// Created by Fan Jiang on 3/11/22. +// + +#include "CGMixtureFactor.h" diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/CGMixtureFactor.h new file mode 100644 index 000000000..f2fa1e54a --- /dev/null +++ b/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 + +namespace gtsam { + +class CGMixtureFactor : HybridFactor { + +}; + +} diff --git a/gtsam/hybrid/CMakeLists.txt b/gtsam/hybrid/CMakeLists.txt new file mode 100644 index 000000000..f1cfcd5c4 --- /dev/null +++ b/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) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp new file mode 100644 index 000000000..1292711d8 --- /dev/null +++ b/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 diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h new file mode 100644 index 000000000..53d10518f --- /dev/null +++ b/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 +#include + +#include // TODO! + +namespace gtsam { + +/** + * A hybrid Bayes net can have discrete conditionals, Gaussian mixtures, + * or pure Gaussian conditionals. + */ +class GTSAM_EXPORT HybridBayesNet : public BayesNet { + public: + using Base = BayesNet; + using This = HybridBayesNet; + using ConditionalType = HybridConditional; + using shared_ptr = boost::shared_ptr; + using sharedConditional = boost::shared_ptr; + + /** Construct empty bayes net */ + HybridBayesNet() : Base() {} +}; + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp new file mode 100644 index 000000000..72f3fd794 --- /dev/null +++ b/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 +#include +#include +#include +#include + +namespace gtsam { + +// Instantiate base class +template class BayesTreeCliqueBase; +template class BayesTree; + +/* ************************************************************************* */ +bool HybridBayesTree::equals(const This& other, double tol) const { + return Base::equals(other, tol); +} + +/* **************************************************************************/ +} // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h new file mode 100644 index 000000000..2ea40cecc --- /dev/null +++ b/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 +#include +#include +#include +#include + +#include + +namespace gtsam { + +// Forward declarations +class HybridConditional; +class VectorValues; + +/* ************************************************************************* */ +/** A clique in a HybridBayesTree */ +class GTSAM_EXPORT HybridBayesTreeClique + : public BayesTreeCliqueBase { + public: + typedef HybridBayesTreeClique This; + typedef BayesTreeCliqueBase + Base; + typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; + HybridBayesTreeClique() {} + virtual ~HybridBayesTreeClique() {} + HybridBayesTreeClique( + const boost::shared_ptr& conditional) + : Base(conditional) {} + +}; + +/* ************************************************************************* */ +/** A Bayes tree representing a Hybrid density */ +class GTSAM_EXPORT HybridBayesTree + : public BayesTree { + private: + typedef BayesTree Base; + + public: + typedef HybridBayesTree This; + typedef boost::shared_ptr 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 diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp new file mode 100644 index 000000000..e6b2eb47f --- /dev/null +++ b/gtsam/hybrid/HybridConditional.cpp @@ -0,0 +1,23 @@ +// +// Created by Fan Jiang on 3/11/22. +// + +#include +#include + +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 {}; +} +} + diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h new file mode 100644 index 000000000..d6dd8250f --- /dev/null +++ b/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 +#include +#include + + +#include +#include +#include +#include + +namespace gtsam { + +/** + * Hybrid Conditional Density + * + * As a type-erased variant of: + * - DiscreteConditional + * - GaussianMixture + * - GaussianConditional + */ +class GTSAM_EXPORT HybridConditional +: public HybridFactor, +public Conditional { +public: +// typedefs needed to play nice with gtsam +typedef HybridConditional This; ///< Typedef to this class +typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class +typedef HybridFactor BaseFactor; ///< Typedef to our factor base class +typedef Conditional + BaseConditional; ///< Typedef to our conditional base class + +private: +// Type-erased pointer to the inner type +std::unique_ptr 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 : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp new file mode 100644 index 000000000..fded0a2df --- /dev/null +++ b/gtsam/hybrid/HybridDiscreteFactor.cpp @@ -0,0 +1,19 @@ +// +// Created by Fan Jiang on 3/11/22. +// + +#include + +#include + +namespace gtsam { + +HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other) { + inner = other; +} + +HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) : inner(boost::make_shared(std::move(dtf))) { + +}; + +} \ No newline at end of file diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h new file mode 100644 index 000000000..4b1c00672 --- /dev/null +++ b/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 +#include +#include + +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); +}; +} diff --git a/gtsam/hybrid/HybridEliminationTree.cpp b/gtsam/hybrid/HybridEliminationTree.cpp new file mode 100644 index 000000000..ff106095a --- /dev/null +++ b/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 +#include + +namespace gtsam { + +// Instantiate base class +template class EliminationTree; + +/* ************************************************************************* */ +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); +} + +} diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h new file mode 100644 index 000000000..e218ce9f6 --- /dev/null +++ b/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 +#include +#include + +namespace gtsam { + +class GTSAM_EXPORT HybridEliminationTree : + public EliminationTree +{ + public: + typedef EliminationTree Base; ///< Base class + typedef HybridEliminationTree This; ///< This class + typedef boost::shared_ptr 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&) + * 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; + +}; + +} diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp new file mode 100644 index 000000000..907350e83 --- /dev/null +++ b/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 diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h new file mode 100644 index 000000000..5af1a23a9 --- /dev/null +++ b/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 +#include +#include + +#include +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 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 +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 : public Testable {}; + +}// namespace gtsam diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp new file mode 100644 index 000000000..686fddc51 --- /dev/null +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -0,0 +1,58 @@ +// +// Created by Fan Jiang on 3/11/22. +// + +#include +#include +#include + +#include + +#include + +namespace gtsam { + +template class EliminateableFactorGraph; + +/* ************************************************************************ */ +std::pair // +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(product, *sum, orderedKeys); + gttoc(divide); + +// return std::make_pair(conditional, sum); + return std::make_pair(boost::make_shared(), boost::make_shared(product)); +} + +} diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h new file mode 100644 index 000000000..fb0de644d --- /dev/null +++ b/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 +#include +#include +#include + +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, HybridFactor::shared_ptr> +EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys); + +/* ************************************************************************* */ +template<> struct EliminationTraits +{ + 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 > + DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { + return EliminateHybrid(factors, keys); } +}; + + +class HybridFactorGraph : public FactorGraph, public EliminateableFactorGraph { + public: + using Base = FactorGraph; + using This = HybridFactorGraph; ///< this class + using BaseEliminateable = + EliminateableFactorGraph; ///< for elimination + using shared_ptr = boost::shared_ptr; ///< 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 + explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {} + + /** Implicit copy/downcast constructor to override explicit template container + * constructor */ + template + HybridFactorGraph(const FactorGraph& graph) : Base(graph) {} + +}; + +} + diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp new file mode 100644 index 000000000..1910c3307 --- /dev/null +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -0,0 +1,23 @@ +// +// Created by Fan Jiang on 3/11/22. +// + +#include + +#include + +namespace gtsam { + +HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) : Base(other->keys()){ + inner = other; +} + +HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) : Base(jf.keys()), inner(boost::make_shared(std::move(jf))) { + +} + +bool HybridGaussianFactor::equals(const HybridFactor& lf, double tol) const { + return false; +}; + +} \ No newline at end of file diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h new file mode 100644 index 000000000..e40682dc1 --- /dev/null +++ b/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 +#include +#include + +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; +}; +} diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp new file mode 100644 index 000000000..eabfc2e7c --- /dev/null +++ b/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 +#include +#include + +namespace gtsam { + +// Instantiate base classes +template class EliminatableClusterTree; +template class JunctionTree; + +/* ************************************************************************* */ +HybridJunctionTree::HybridJunctionTree( + const HybridEliminationTree& eliminationTree) : + Base(eliminationTree) {} + +} diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h new file mode 100644 index 000000000..1901e7007 --- /dev/null +++ b/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 +#include +#include + +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 { + public: + typedef JunctionTree Base; ///< Base class + typedef HybridJunctionTree This; ///< This class + typedef boost::shared_ptr 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&) + * named constructor instead. + * @return The elimination tree + */ + HybridJunctionTree(const HybridEliminationTree& eliminationTree); +}; + +} diff --git a/gtsam/hybrid/tests/CMakeLists.txt b/gtsam/hybrid/tests/CMakeLists.txt new file mode 100644 index 000000000..b52e18586 --- /dev/null +++ b/gtsam/hybrid/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(hybrid "test*.cpp" "" "gtsam") \ No newline at end of file diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp new file mode 100644 index 000000000..295d6011d --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +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); +} +/* ************************************************************************* */ +