From 5c5c05370a14141a9ec60628bdb1327cab98f1de Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 7 Jun 2022 18:38:21 -0400 Subject: [PATCH] Add HybridFactorGraph base class and more methods for adding gaussian factors --- gtsam/hybrid/HybridFactorGraph.h | 148 +++++++++++++++++++++++ gtsam/hybrid/HybridGaussianFactorGraph.h | 61 ++++++++-- 2 files changed, 202 insertions(+), 7 deletions(-) create mode 100644 gtsam/hybrid/HybridFactorGraph.h diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h new file mode 100644 index 000000000..ce0241d25 --- /dev/null +++ b/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 +#include +#include +#include +#include + +#include +namespace gtsam { + +using SharedFactor = boost::shared_ptr; + +/** + * Hybrid Factor Graph + * ----------------------- + * This is the base hybrid factor graph. + * Everything inside needs to be hybrid factor or hybrid conditional. + */ +class HybridFactorGraph : public FactorGraph { + public: + using Base = FactorGraph; + using This = HybridFactorGraph; ///< this class + using shared_ptr = boost::shared_ptr; ///< 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 + using IsDiscrete = typename std::enable_if< + std::is_base_of::value>::type; + + /// Check if FACTOR type is derived from HybridFactor. + template + using IsHybrid = typename std::enable_if< + std::is_base_of::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 + HybridFactorGraph(const FactorGraph& 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 + IsDiscrete push_discrete( + const boost::shared_ptr& discreteFactor) { + Base::push_back(boost::make_shared(discreteFactor)); + } + + /** + * Add a discrete-continuous (Hybrid) factor *pointer* to the graph + * @param hybridFactor - boost::shared_ptr to the factor to add + */ + template + IsHybrid push_hybrid(const boost::shared_ptr& hybridFactor) { + Base::push_back(hybridFactor); + } + + /// delete emplace_shared. + template + void emplace_shared(Args&&... args) = delete; + + /// Construct a factor and add (shared pointer to it) to factor graph. + template + IsDiscrete emplace_discrete(Args&&... args) { + auto factor = boost::allocate_shared( + Eigen::aligned_allocator(), std::forward(args)...); + push_discrete(factor); + } + + /// Construct a factor and add (shared pointer to it) to factor graph. + template + IsHybrid emplace_hybrid(Args&&... args) { + auto factor = boost::allocate_shared( + Eigen::aligned_allocator(), std::forward(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(sharedFactor)) { + push_discrete(p); + } + if (auto p = boost::dynamic_pointer_cast(sharedFactor)) { + push_hybrid(p); + } + } + + /** Constructor from iterator over factors (shared_ptr or plain objects) */ + template + void push_back(ITERATOR firstFactor, ITERATOR lastFactor) { + for (auto&& it = firstFactor; it != lastFactor; it++) { + push_back(*it); + } + } +}; + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 0188aa652..1d49904fe 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -19,6 +19,8 @@ #pragma once #include +#include +#include #include #include #include @@ -53,10 +55,9 @@ struct EliminationTraits { 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 + 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 > @@ -72,10 +73,16 @@ struct EliminationTraits { * Everything inside needs to be hybrid factor or hybrid conditional. */ class GTSAM_EXPORT HybridGaussianFactorGraph - : public FactorGraph, + : public HybridFactorGraph, public EliminateableFactorGraph { + protected: + /// Check if FACTOR type is derived from GaussianFactor. + template + using IsGaussian = typename std::enable_if< + std::is_base_of::value>::type; + public: - using Base = FactorGraph; + using Base = HybridFactorGraph; using This = HybridGaussianFactorGraph; ///< this class using BaseEliminateable = EliminateableFactorGraph; ///< 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 factor); + + /** + * Add a gaussian factor *pointer* to the internal gaussian factor graph + * @param gaussianFactor - boost::shared_ptr to the factor to add + */ + template + IsGaussian push_gaussian( + const boost::shared_ptr& gaussianFactor) { + Base::Base::push_back( + boost::make_shared(gaussianFactor)); + } + + /// Construct a factor and add (shared pointer to it) to factor graph. + template + IsGaussian emplace_gaussian(Args&&... args) { + auto factor = boost::allocate_shared( + Eigen::aligned_allocator(), std::forward(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(sharedFactor)) { + push_gaussian(p); + } else { + Base::push_back(sharedFactor); + } + } }; } // namespace gtsam