diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index bac1285e1..cabfd28b8 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -59,6 +59,8 @@ GaussianMixture::GaussianMixture( Conditionals(discreteParents, conditionals)) {} /* *******************************************************************************/ +// TODO(dellaert): This is copy/paste: GaussianMixture should be derived from +// GaussianMixtureFactor, no? GaussianFactorGraphTree GaussianMixture::add( const GaussianFactorGraphTree &sum) const { using Y = GraphAndConstant; diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 4c61085d7..628ac5fb1 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -141,8 +141,8 @@ std::function &, double)> prunerFunc( /* ************************************************************************* */ void HybridBayesNet::updateDiscreteConditionals( - const DecisionTreeFactor::shared_ptr &prunedDecisionTree) { - KeyVector prunedTreeKeys = prunedDecisionTree->keys(); + const DecisionTreeFactor &prunedDecisionTree) { + KeyVector prunedTreeKeys = prunedDecisionTree.keys(); // Loop with index since we need it later. for (size_t i = 0; i < this->size(); i++) { @@ -154,7 +154,7 @@ void HybridBayesNet::updateDiscreteConditionals( auto discreteTree = boost::dynamic_pointer_cast(discrete); DecisionTreeFactor::ADT prunedDiscreteTree = - discreteTree->apply(prunerFunc(*prunedDecisionTree, *conditional)); + discreteTree->apply(prunerFunc(prunedDecisionTree, *conditional)); // Create the new (hybrid) conditional KeyVector frontals(discrete->frontals().begin(), @@ -173,9 +173,7 @@ void HybridBayesNet::updateDiscreteConditionals( HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { // Get the decision tree of only the discrete keys auto discreteConditionals = this->discreteConditionals(); - const DecisionTreeFactor::shared_ptr decisionTree = - boost::make_shared( - discreteConditionals->prune(maxNrLeaves)); + const auto decisionTree = discreteConditionals->prune(maxNrLeaves); this->updateDiscreteConditionals(decisionTree); @@ -194,7 +192,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { if (auto gm = conditional->asMixture()) { // Make a copy of the Gaussian mixture and prune it! auto prunedGaussianMixture = boost::make_shared(*gm); - prunedGaussianMixture->prune(*decisionTree); // imperative :-( + prunedGaussianMixture->prune(decisionTree); // imperative :-( // Type-erase and add to the pruned Bayes Net fragment. prunedBayesNetFragment.push_back(prunedGaussianMixture); diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 8d0671c9d..dd8d38a4c 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -51,33 +51,51 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { /// @{ /// GTSAM-style printing - void print( - const std::string &s = "", - const KeyFormatter &formatter = DefaultKeyFormatter) const override; + void print(const std::string &s = "", const KeyFormatter &formatter = + DefaultKeyFormatter) const override; /// GTSAM-style equals - bool equals(const This& fg, double tol = 1e-9) const; - + bool equals(const This &fg, double tol = 1e-9) const; + /// @} /// @name Standard Interface /// @{ - /// Add HybridConditional to Bayes Net - using Base::emplace_shared; + /** + * @brief Add a hybrid conditional using a shared_ptr. + * + * This is the "native" push back, as this class stores hybrid conditionals. + */ + void push_back(boost::shared_ptr conditional) { + factors_.push_back(conditional); + } - /// Add a conditional directly using a pointer. + /** + * Preferred: add a conditional directly using a pointer. + * + * Examples: + * hbn.emplace_back(new GaussianMixture(...))); + * hbn.emplace_back(new GaussianConditional(...))); + * hbn.emplace_back(new DiscreteConditional(...))); + */ template void emplace_back(Conditional *conditional) { factors_.push_back(boost::make_shared( boost::shared_ptr(conditional))); } - /// Add a conditional directly using a shared_ptr. - void push_back(boost::shared_ptr conditional) { - factors_.push_back(conditional); - } - - /// Add a conditional directly using implicit conversion. + /** + * Add a conditional using a shared_ptr, using implicit conversion to + * a HybridConditional. + * + * This is useful when you create a conditional shared pointer as you need it + * somewhere else. + * + * Example: + * auto shared_ptr_to_a_conditional = + * boost::make_shared(...); + * hbn.push_back(shared_ptr_to_a_conditional); + */ void push_back(HybridConditional &&conditional) { factors_.push_back( boost::make_shared(std::move(conditional))); @@ -214,8 +232,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * * @param prunedDecisionTree */ - void updateDiscreteConditionals( - const DecisionTreeFactor::shared_ptr &prunedDecisionTree); + void updateDiscreteConditionals(const DecisionTreeFactor &prunedDecisionTree); /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 628a453a6..7240edaac 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -132,18 +132,15 @@ struct traits : public Testable {}; * This object stores parent keys in our base type factor so that * eliminating those parent keys will pull this subtree into the * elimination. - * This does special stuff for the hybrid case. * - * @tparam CLIQUE + * This is a template instantiation for hybrid Bayes tree cliques, storing both + * the regular keys *and* discrete keys in the HybridConditional. */ -template -class BayesTreeOrphanWrapper< - CLIQUE, typename std::enable_if< - boost::is_same::value> > - : public CLIQUE::ConditionalType { +template <> +class BayesTreeOrphanWrapper : public HybridConditional { public: - typedef CLIQUE CliqueType; - typedef typename CLIQUE::ConditionalType Base; + typedef HybridBayesTreeClique CliqueType; + typedef HybridConditional Base; boost::shared_ptr clique; diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp deleted file mode 100644 index afdb6472a..000000000 --- a/gtsam/hybrid/HybridDiscreteFactor.cpp +++ /dev/null @@ -1,61 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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.cpp - * @brief Wrapper for a discrete factor - * @date Mar 11, 2022 - * @author Fan Jiang - */ - -#include -#include - -#include - -#include "gtsam/discrete/DecisionTreeFactor.h" - -namespace gtsam { - -/* ************************************************************************ */ -HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other) - : Base(boost::dynamic_pointer_cast(other) - ->discreteKeys()), - inner_(other) {} - -/* ************************************************************************ */ -HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) - : Base(dtf.discreteKeys()), - inner_(boost::make_shared(std::move(dtf))) {} - -/* ************************************************************************ */ -bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const { - const This *e = dynamic_cast(&lf); - if (e == nullptr) return false; - if (!Base::equals(*e, tol)) return false; - return inner_ ? (e->inner_ ? inner_->equals(*(e->inner_), tol) : false) - : !(e->inner_); -} - -/* ************************************************************************ */ -void HybridDiscreteFactor::print(const std::string &s, - const KeyFormatter &formatter) const { - HybridFactor::print(s, formatter); - inner_->print("\n", formatter); -}; - -/* ************************************************************************ */ -double HybridDiscreteFactor::error(const HybridValues &values) const { - return -log((*inner_)(values.discrete())); -} -/* ************************************************************************ */ - -} // namespace gtsam diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h deleted file mode 100644 index 7a43ab3a5..000000000 --- a/gtsam/hybrid/HybridDiscreteFactor.h +++ /dev/null @@ -1,91 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - * @author Varun Agrawal - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - -class HybridValues; - -/** - * A HybridDiscreteFactor is a thin container for DiscreteFactor, which - * allows us to hide the implementation of DiscreteFactor and thus avoid - * diamond inheritance. - * - * @ingroup hybrid - */ -class GTSAM_EXPORT HybridDiscreteFactor : public HybridFactor { - private: - DiscreteFactor::shared_ptr inner_; - - public: - using Base = HybridFactor; - using This = HybridDiscreteFactor; - using shared_ptr = boost::shared_ptr; - - /// @name Constructors - /// @{ - - /// Default constructor - for serialization. - HybridDiscreteFactor() = default; - - // Implicit conversion from a shared ptr of DF - HybridDiscreteFactor(DiscreteFactor::shared_ptr other); - - // Forwarding constructor from concrete DecisionTreeFactor - HybridDiscreteFactor(DecisionTreeFactor &&dtf); - - /// @} - /// @name Testable - /// @{ - virtual bool equals(const HybridFactor &lf, double tol) const override; - - void print( - const std::string &s = "HybridFactor\n", - const KeyFormatter &formatter = DefaultKeyFormatter) const override; - - /// @} - /// @name Standard Interface - /// @{ - - /// Return pointer to the internal discrete factor. - DiscreteFactor::shared_ptr inner() const { return inner_; } - - /// Return the error of the underlying Discrete Factor. - double error(const HybridValues &values) const override; - /// @} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE &ar, const unsigned int /*version*/) { - ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar &BOOST_SERIALIZATION_NVP(inner_); - } -}; - -// traits -template <> -struct traits : public Testable {}; - -} // namespace gtsam diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index 8c1b0dad3..bab38aa07 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -67,11 +67,10 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2); /** - * Base class for hybrid probabilistic factors + * Base class for *truly* hybrid probabilistic factors * * Examples: - * - HybridGaussianFactor - * - HybridDiscreteFactor + * - MixtureFactor * - GaussianMixtureFactor * - GaussianMixture * diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp new file mode 100644 index 000000000..4238925d6 --- /dev/null +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -0,0 +1,78 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2022, 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.cpp + * @brief Factor graph with utilities for hybrid factors. + * @author Varun Agrawal + * @author Frank Dellaert + * @date January, 2023 + */ + +#include +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +DiscreteKeys HybridFactorGraph::discreteKeys() const { + DiscreteKeys keys; + for (auto& factor : factors_) { + if (auto p = boost::dynamic_pointer_cast(factor)) { + for (const DiscreteKey& key : p->discreteKeys()) { + keys.push_back(key); + } + } + if (auto p = boost::dynamic_pointer_cast(factor)) { + for (const DiscreteKey& key : p->discreteKeys()) { + keys.push_back(key); + } + } + } + return keys; +} + +/* ************************************************************************* */ +KeySet HybridFactorGraph::discreteKeySet() const { + KeySet keys; + for (const DiscreteKey& k : discreteKeys()) { + keys.insert(k.first); + } + return keys; +} + +/* ************************************************************************* */ +std::unordered_map HybridFactorGraph::discreteKeyMap() const { + std::unordered_map result; + for (const DiscreteKey& k : discreteKeys()) { + result[k.first] = k; + } + return result; +} + +/* ************************************************************************* */ +const KeySet HybridFactorGraph::continuousKeySet() const { + KeySet keys; + for (auto& factor : factors_) { + if (auto p = boost::dynamic_pointer_cast(factor)) { + for (const Key& key : p->continuousKeys()) { + keys.insert(key); + } + } + } + return keys; +} + +/* ************************************************************************* */ + +} // namespace gtsam diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index 05a17b000..7d30663a3 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -11,50 +11,40 @@ /** * @file HybridFactorGraph.h - * @brief Hybrid factor graph base class that uses type erasure + * @brief Factor graph with utilities for hybrid factors. * @author Varun Agrawal + * @author Frank Dellaert * @date May 28, 2022 */ #pragma once -#include -#include #include #include -#include #include +#include + namespace gtsam { +class DiscreteFactor; +class Ordering; + 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. + * Factor graph with utilities for hybrid factors. */ -class HybridFactorGraph : public FactorGraph { +class HybridFactorGraph : public FactorGraph { public: - using Base = FactorGraph; + 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 /// @{ @@ -71,92 +61,22 @@ class HybridFactorGraph : public FactorGraph { 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); - } - } + /// @name Extra methods to inspect discrete/continuous keys. + /// @{ /// Get all the discrete keys in the factor graph. - const KeySet discreteKeys() const { - KeySet discrete_keys; - for (auto& factor : factors_) { - for (const DiscreteKey& k : factor->discreteKeys()) { - discrete_keys.insert(k.first); - } - } - return discrete_keys; - } + DiscreteKeys discreteKeys() const; + + /// Get all the discrete keys in the factor graph, as a set. + KeySet discreteKeySet() const; + + /// Get a map from Key to corresponding DiscreteKey. + std::unordered_map discreteKeyMap() const; /// Get all the continuous keys in the factor graph. - const KeySet continuousKeys() const { - KeySet keys; - for (auto& factor : factors_) { - for (const Key& key : factor->continuousKeys()) { - keys.insert(key); - } - } - return keys; - } + const KeySet continuousKeySet() const; + + /// @} }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp deleted file mode 100644 index 4fe18bea7..000000000 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ /dev/null @@ -1,70 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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.cpp - * @date Mar 11, 2022 - * @author Fan Jiang - */ - -#include -#include -#include -#include - -#include - -namespace gtsam { - -/* ************************************************************************* */ -HybridGaussianFactor::HybridGaussianFactor( - const boost::shared_ptr &ptr) - : Base(ptr->keys()), inner_(ptr) {} - -HybridGaussianFactor::HybridGaussianFactor( - boost::shared_ptr &&ptr) - : Base(ptr->keys()), inner_(std::move(ptr)) {} - -HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) - : Base(jf.keys()), - inner_(boost::make_shared(std::move(jf))) {} - -HybridGaussianFactor::HybridGaussianFactor(HessianFactor &&hf) - : Base(hf.keys()), - inner_(boost::make_shared(std::move(hf))) {} - -/* ************************************************************************* */ -bool HybridGaussianFactor::equals(const HybridFactor &other, double tol) const { - const This *e = dynamic_cast(&other); - if (e == nullptr) return false; - if (!Base::equals(*e, tol)) return false; - return inner_ ? (e->inner_ ? inner_->equals(*(e->inner_), tol) : false) - : !(e->inner_); -} - -/* ************************************************************************* */ -void HybridGaussianFactor::print(const std::string &s, - const KeyFormatter &formatter) const { - HybridFactor::print(s, formatter); - if (inner_) { - inner_->print("\n", formatter); - } else { - std::cout << "\nGaussian: nullptr" << std::endl; - } -}; - -/* ************************************************************************ */ -double HybridGaussianFactor::error(const HybridValues &values) const { - return inner_->error(values.continuous()); -} -/* ************************************************************************ */ - -} // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h deleted file mode 100644 index 6bb022396..000000000 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ /dev/null @@ -1,123 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - */ - -#pragma once - -#include -#include - -namespace gtsam { - -// Forward declarations -class JacobianFactor; -class HessianFactor; -class HybridValues; - -/** - * A HybridGaussianFactor is a layer over GaussianFactor so that we do not have - * a diamond inheritance i.e. an extra factor type that inherits from both - * HybridFactor and GaussianFactor. - * - * @ingroup hybrid - */ -class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { - private: - GaussianFactor::shared_ptr inner_; - - public: - using Base = HybridFactor; - using This = HybridGaussianFactor; - using shared_ptr = boost::shared_ptr; - - /// @name Constructors - /// @{ - - /// Default constructor - for serialization. - HybridGaussianFactor() = default; - - /** - * Constructor from shared_ptr of GaussianFactor. - * Example: - * auto ptr = boost::make_shared(...); - * HybridGaussianFactor factor(ptr); - */ - explicit HybridGaussianFactor(const boost::shared_ptr &ptr); - - /** - * Forwarding constructor from shared_ptr of GaussianFactor. - * Examples: - * HybridGaussianFactor factor = boost::make_shared(...); - * HybridGaussianFactor factor(boost::make_shared(...)); - */ - explicit HybridGaussianFactor(boost::shared_ptr &&ptr); - - /** - * Forwarding constructor from rvalue reference of JacobianFactor. - * - * Examples: - * HybridGaussianFactor factor = JacobianFactor(...); - * HybridGaussianFactor factor(JacobianFactor(...)); - */ - explicit HybridGaussianFactor(JacobianFactor &&jf); - - /** - * Forwarding constructor from rvalue reference of JacobianFactor. - * - * Examples: - * HybridGaussianFactor factor = HessianFactor(...); - * HybridGaussianFactor factor(HessianFactor(...)); - */ - explicit HybridGaussianFactor(HessianFactor &&hf); - - /// @} - /// @name Testable - /// @{ - - /// Check equality. - virtual bool equals(const HybridFactor &lf, double tol) const override; - - /// GTSAM print utility. - void print( - const std::string &s = "HybridGaussianFactor\n", - const KeyFormatter &formatter = DefaultKeyFormatter) const override; - - /// @} - /// @name Standard Interface - /// @{ - - /// Return pointer to the internal Gaussian factor. - GaussianFactor::shared_ptr inner() const { return inner_; } - - /// Return the error of the underlying Gaussian factor. - double error(const HybridValues &values) const override; - /// @} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE &ar, const unsigned int /*version*/) { - ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar &BOOST_SERIALIZATION_NVP(inner_); - } -}; - -// traits -template <> -struct traits : public Testable {}; - -} // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 3be438e43..d0351afbc 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -26,10 +26,8 @@ #include #include #include -#include #include #include -#include #include #include #include @@ -47,7 +45,6 @@ #include #include #include -#include #include #include @@ -58,19 +55,25 @@ namespace gtsam { /// Specialize EliminateableFactorGraph for HybridGaussianFactorGraph: template class EliminateableFactorGraph; +using OrphanWrapper = BayesTreeOrphanWrapper; + +using boost::dynamic_pointer_cast; + +/* ************************************************************************ */ +// Throw a runtime exception for method specified in string s, and factor f: +static void throwRuntimeError(const std::string &s, + const boost::shared_ptr &f) { + auto &fr = *f; + throw std::runtime_error(s + " not implemented for factor type " + + demangle(typeid(fr).name()) + "."); +} + /* ************************************************************************ */ const Ordering HybridOrdering(const HybridGaussianFactorGraph &graph) { - KeySet discrete_keys = graph.discreteKeys(); - for (auto &factor : graph) { - for (const DiscreteKey &k : factor->discreteKeys()) { - discrete_keys.insert(k.first); - } - } - + KeySet discrete_keys = graph.discreteKeySet(); const VariableIndex index(graph); - Ordering ordering = Ordering::ColamdConstrainedLast( + return Ordering::ColamdConstrainedLast( index, KeyVector(discrete_keys.begin(), discrete_keys.end()), true); - return ordering; } /* ************************************************************************ */ @@ -82,7 +85,6 @@ static GaussianFactorGraphTree addGaussian( GaussianFactorGraph result; result.push_back(factor); return GaussianFactorGraphTree(GraphAndConstant(result, 0.0)); - } else { auto add = [&factor](const GraphAndConstant &graph_z) { auto result = graph_z.graph; @@ -94,9 +96,8 @@ static GaussianFactorGraphTree addGaussian( } /* ************************************************************************ */ -// TODO(dellaert): Implementation-wise, it's probably more efficient to first -// collect the discrete keys, and then loop over all assignments to populate a -// vector. +// TODO(dellaert): it's probably more efficient to first collect the discrete +// keys, and then loop over all assignments to populate a vector. GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { gttic(assembleGraphTree); @@ -104,38 +105,28 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { for (auto &f : factors_) { // TODO(dellaert): just use a virtual method defined in HybridFactor. - if (f->isHybrid()) { - if (auto gm = boost::dynamic_pointer_cast(f)) { + if (auto gf = dynamic_pointer_cast(f)) { + result = addGaussian(result, gf); + } else if (auto gm = dynamic_pointer_cast(f)) { + result = gm->add(result); + } else if (auto hc = dynamic_pointer_cast(f)) { + if (auto gm = hc->asMixture()) { result = gm->add(result); + } else if (auto g = hc->asGaussian()) { + result = addGaussian(result, g); + } else { + // Has to be discrete. + // TODO(dellaert): in C++20, we can use std::visit. + continue; } - if (auto gm = boost::dynamic_pointer_cast(f)) { - result = gm->asMixture()->add(result); - } - - } else if (f->isContinuous()) { - if (auto gf = boost::dynamic_pointer_cast(f)) { - result = addGaussian(result, gf->inner()); - } - if (auto cg = boost::dynamic_pointer_cast(f)) { - result = addGaussian(result, cg->asGaussian()); - } - - } else if (f->isDiscrete()) { + } else if (dynamic_pointer_cast(f)) { // Don't do anything for discrete-only factors // since we want to eliminate continuous values only. continue; - } else { - // We need to handle the case where the object is actually an - // BayesTreeOrphanWrapper! - auto orphan = boost::dynamic_pointer_cast< - BayesTreeOrphanWrapper>(f); - if (!orphan) { - auto &fr = *f; - throw std::invalid_argument( - std::string("factor is discrete in continuous elimination ") + - demangle(typeid(fr).name())); - } + // TODO(dellaert): there was an unattributed comment here: We need to + // handle the case where the object is actually an BayesTreeOrphanWrapper! + throwRuntimeError("gtsam::assembleGraphTree", f); } } @@ -145,49 +136,54 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { } /* ************************************************************************ */ -static std::pair +static std::pair> continuousElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { GaussianFactorGraph gfg; - for (auto &fp : factors) { - if (auto ptr = boost::dynamic_pointer_cast(fp)) { - gfg.push_back(ptr->inner()); - } else if (auto ptr = boost::static_pointer_cast(fp)) { - gfg.push_back( - boost::static_pointer_cast(ptr->inner())); + for (auto &f : factors) { + if (auto gf = dynamic_pointer_cast(f)) { + gfg.push_back(gf); + } else if (auto orphan = dynamic_pointer_cast(f)) { + // Ignore orphaned clique. + // TODO(dellaert): is this correct? If so explain here. + } else if (auto hc = dynamic_pointer_cast(f)) { + auto gc = hc->asGaussian(); + if (!gc) throwRuntimeError("continuousElimination", gc); + gfg.push_back(gc); } else { - // It is an orphan wrapped conditional + throwRuntimeError("continuousElimination", f); } } auto result = EliminatePreferCholesky(gfg, frontalKeys); - return {boost::make_shared(result.first), - boost::make_shared(result.second)}; + return {boost::make_shared(result.first), result.second}; } /* ************************************************************************ */ -static std::pair +static std::pair> discreteElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { DiscreteFactorGraph dfg; - for (auto &factor : factors) { - if (auto p = boost::dynamic_pointer_cast(factor)) { - dfg.push_back(p->inner()); - } else if (auto p = boost::static_pointer_cast(factor)) { - auto discrete_conditional = - boost::static_pointer_cast(p->inner()); - dfg.push_back(discrete_conditional); + for (auto &f : factors) { + if (auto dtf = dynamic_pointer_cast(f)) { + dfg.push_back(dtf); + } else if (auto orphan = dynamic_pointer_cast(f)) { + // Ignore orphaned clique. + // TODO(dellaert): is this correct? If so explain here. + } else if (auto hc = dynamic_pointer_cast(f)) { + auto dc = hc->asDiscrete(); + if (!dc) throwRuntimeError("continuousElimination", dc); + dfg.push_back(hc->asDiscrete()); } else { - // It is an orphan wrapper + throwRuntimeError("continuousElimination", f); } } // NOTE: This does sum-product. For max-product, use EliminateForMPE. auto result = EliminateDiscrete(dfg, frontalKeys); - return {boost::make_shared(result.first), - boost::make_shared(result.second)}; + return {boost::make_shared(result.first), result.second}; } /* ************************************************************************ */ @@ -205,7 +201,7 @@ GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) { } /* ************************************************************************ */ -static std::pair +static std::pair> hybridElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys, const KeyVector &continuousSeparator, @@ -307,7 +303,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, boost::make_shared(discreteSeparator, fdt); return {boost::make_shared(gaussianMixture), - boost::make_shared(discreteFactor)}; + discreteFactor}; } else { // Create a resulting GaussianMixtureFactor on the separator. return {boost::make_shared(gaussianMixture), @@ -330,7 +326,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, * eliminate a discrete variable (as specified in the ordering), the result will * be INCORRECT and there will be NO error raised. */ -std::pair // +std::pair> // EliminateHybrid(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { // NOTE: Because we are in the Conditional Gaussian regime there are only @@ -390,14 +386,7 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, } // Build a map from keys to DiscreteKeys - std::unordered_map mapFromKeyToDiscreteKey; - for (auto &&factor : factors) { - if (!factor->isContinuous()) { - for (auto &k : factor->discreteKeys()) { - mapFromKeyToDiscreteKey[k.first] = k; - } - } - } + auto mapFromKeyToDiscreteKey = factors.discreteKeyMap(); // Fill in discrete frontals and continuous frontals. std::set discreteFrontals; @@ -443,59 +432,31 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, } } -/* ************************************************************************ */ -void HybridGaussianFactorGraph::add(JacobianFactor &&factor) { - FactorGraph::add(boost::make_shared(std::move(factor))); -} - -/* ************************************************************************ */ -void HybridGaussianFactorGraph::add(boost::shared_ptr &factor) { - FactorGraph::add(boost::make_shared(factor)); -} - -/* ************************************************************************ */ -void HybridGaussianFactorGraph::add(DecisionTreeFactor &&factor) { - FactorGraph::add(boost::make_shared(std::move(factor))); -} - -/* ************************************************************************ */ -void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { - FactorGraph::add(boost::make_shared(factor)); -} - /* ************************************************************************ */ AlgebraicDecisionTree HybridGaussianFactorGraph::error( const VectorValues &continuousValues) const { AlgebraicDecisionTree error_tree(0.0); // Iterate over each factor. - for (size_t idx = 0; idx < size(); idx++) { + for (auto &f : factors_) { // TODO(dellaert): just use a virtual method defined in HybridFactor. AlgebraicDecisionTree factor_error; - if (factors_.at(idx)->isHybrid()) { - // If factor is hybrid, select based on assignment. - GaussianMixtureFactor::shared_ptr gaussianMixture = - boost::static_pointer_cast(factors_.at(idx)); + if (auto gaussianMixture = dynamic_pointer_cast(f)) { // Compute factor error and add it. error_tree = error_tree + gaussianMixture->error(continuousValues); - - } else if (factors_.at(idx)->isContinuous()) { + } else if (auto gaussian = dynamic_pointer_cast(f)) { // If continuous only, get the (double) error // and add it to the error_tree - auto hybridGaussianFactor = - boost::static_pointer_cast(factors_.at(idx)); - GaussianFactor::shared_ptr gaussian = hybridGaussianFactor->inner(); - - // Compute the error of the gaussian factor. double error = gaussian->error(continuousValues); // Add the gaussian factor error to every leaf of the error tree. error_tree = error_tree.apply( [error](double leaf_value) { return leaf_value + error; }); - - } else if (factors_.at(idx)->isDiscrete()) { + } else if (dynamic_pointer_cast(f)) { // If factor at `idx` is discrete-only, we skip. continue; + } else { + throwRuntimeError("HybridGaussianFactorGraph::error(VV)", f); } } @@ -505,8 +466,17 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::error( /* ************************************************************************ */ double HybridGaussianFactorGraph::error(const HybridValues &values) const { double error = 0.0; - for (auto &factor : factors_) { - error += factor->error(values); + for (auto &f : factors_) { + if (auto hf = dynamic_pointer_cast(f)) { + error += hf->error(values.continuous()); + } else if (auto hf = dynamic_pointer_cast(f)) { + // TODO(dellaert): needs to change when we discard other wrappers. + error += hf->error(values); + } else if (auto dtf = dynamic_pointer_cast(f)) { + error -= log((*dtf)(values.discrete())); + } else { + throwRuntimeError("HybridGaussianFactorGraph::error(HV)", f); + } } return error; } diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 44ef7d784..0dc737250 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include @@ -50,7 +49,7 @@ class HybridValues; * @ingroup hybrid */ GTSAM_EXPORT -std::pair, HybridFactor::shared_ptr> +std::pair, boost::shared_ptr> EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys); /** @@ -65,7 +64,7 @@ HybridOrdering(const HybridGaussianFactorGraph& graph); /* ************************************************************************* */ template <> struct EliminationTraits { - typedef HybridFactor FactorType; ///< Type of factors in factor graph + typedef Factor FactorType; ///< Type of factors in factor graph typedef HybridGaussianFactorGraph FactorGraphType; ///< Type of the factor graph (e.g. ///< HybridGaussianFactorGraph) @@ -79,7 +78,7 @@ struct EliminationTraits { typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree /// The default dense elimination function static std::pair, - boost::shared_ptr > + boost::shared_ptr> DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { return EliminateHybrid(factors, keys); } @@ -95,7 +94,6 @@ struct EliminationTraits { * Hybrid Gaussian Factor Graph * ----------------------- * This is the linearized version of a hybrid factor graph. - * Everything inside needs to be hybrid factor or hybrid conditional. * * @ingroup hybrid */ @@ -133,59 +131,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph HybridGaussianFactorGraph(const FactorGraph& graph) : Base(graph) {} - /// @} - /// @name Adding factors. - /// @{ - - using Base::add; - using Base::push_back; - using Base::reserve; - - /// Add a Jacobian factor to the factor graph. - void add(JacobianFactor&& factor); - - /// Add a Jacobian factor as a shared ptr. - void add(boost::shared_ptr& factor); - - /// Add a DecisionTreeFactor to the factor graph. - void add(DecisionTreeFactor&& factor); - - /// Add a DecisionTreeFactor as a shared ptr. - void add(DecisionTreeFactor::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::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); - } - } - /// @} /// @name Testable /// @{ @@ -200,11 +145,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph /// @name Standard Interface /// @{ - using Base::empty; - using Base::size; - using Base::operator[]; - using Base::resize; - /** * @brief Compute error for each discrete assignment, * and return as a tree. diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index aa6b3f266..3f63cb089 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -43,7 +43,7 @@ Ordering HybridGaussianISAM::GetOrdering( HybridGaussianFactorGraph& factors, const HybridGaussianFactorGraph& newFactors) { // Get all the discrete keys from the factors - KeySet allDiscrete = factors.discreteKeys(); + const KeySet allDiscrete = factors.discreteKeySet(); // Create KeyVector with continuous keys followed by discrete keys. KeyVector newKeysDiscreteLast; diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 422c200a4..a463c625f 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -61,9 +61,15 @@ struct HybridConstructorTraversalData { parentData.junctionTreeNode->addChild(data.junctionTreeNode); // Add all the discrete keys in the hybrid factors to the current data - for (HybridFactor::shared_ptr& f : node->factors) { - for (auto& k : f->discreteKeys()) { - data.discreteKeys.insert(k.first); + for (const auto& f : node->factors) { + if (auto hf = boost::dynamic_pointer_cast(f)) { + for (auto& k : hf->discreteKeys()) { + data.discreteKeys.insert(k.first); + } + } else if (auto hf = boost::dynamic_pointer_cast(f)) { + for (auto& k : hf->discreteKeys()) { + data.discreteKeys.insert(k.first); + } } } diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp deleted file mode 100644 index 5a1833d39..000000000 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 HybridNonlinearFactor.cpp - * @date May 28, 2022 - * @author Varun Agrawal - */ - -#include - -#include - -namespace gtsam { - -/* ************************************************************************* */ -HybridNonlinearFactor::HybridNonlinearFactor( - const NonlinearFactor::shared_ptr &other) - : Base(other->keys()), inner_(other) {} - -/* ************************************************************************* */ -bool HybridNonlinearFactor::equals(const HybridFactor &lf, double tol) const { - return Base::equals(lf, tol); -} - -/* ************************************************************************* */ -void HybridNonlinearFactor::print(const std::string &s, - const KeyFormatter &formatter) const { - HybridFactor::print(s, formatter); - inner_->print("\n", formatter); -}; - -} // namespace gtsam diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h deleted file mode 100644 index 56e30ba74..000000000 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ /dev/null @@ -1,73 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 HybridNonlinearFactor.h - * @date May 28, 2022 - * @author Varun Agrawal - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - -/** - * A HybridNonlinearFactor is a layer over NonlinearFactor so that we do not - * have a diamond inheritance. - */ -class HybridNonlinearFactor : public HybridFactor { - private: - NonlinearFactor::shared_ptr inner_; - - public: - using Base = HybridFactor; - using This = HybridNonlinearFactor; - using shared_ptr = boost::shared_ptr; - - // Explicit conversion from a shared ptr of NonlinearFactor - explicit HybridNonlinearFactor(const NonlinearFactor::shared_ptr &other); - - /// @name Testable - /// @{ - - /// Check equality. - virtual bool equals(const HybridFactor &lf, double tol) const override; - - /// GTSAM print utility. - void print( - const std::string &s = "HybridNonlinearFactor\n", - const KeyFormatter &formatter = DefaultKeyFormatter) const override; - - /// @} - /// @name Standard Interface - /// @{ - - /// Return pointer to the internal nonlinear factor. - NonlinearFactor::shared_ptr inner() const { return inner_; } - - /// Error for HybridValues is not provided for nonlinear factor. - double error(const HybridValues &values) const override { - throw std::runtime_error( - "HybridNonlinearFactor::error(HybridValues) not implemented."); - } - - /// Linearize to a HybridGaussianFactor at the linearization point `c`. - boost::shared_ptr linearize(const Values &c) const { - return boost::make_shared(inner_->linearize(c)); - } - - /// @} -}; -} // namespace gtsam diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index 3a3bf720b..71b064eb6 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -16,21 +16,14 @@ * @date May 28, 2022 */ +#include +#include #include +#include +#include namespace gtsam { -/* ************************************************************************* */ -void HybridNonlinearFactorGraph::add( - boost::shared_ptr factor) { - FactorGraph::add(boost::make_shared(factor)); -} - -/* ************************************************************************* */ -void HybridNonlinearFactorGraph::add(boost::shared_ptr factor) { - FactorGraph::add(boost::make_shared(factor)); -} - /* ************************************************************************* */ void HybridNonlinearFactorGraph::print(const std::string& s, const KeyFormatter& keyFormatter) const { @@ -50,47 +43,38 @@ void HybridNonlinearFactorGraph::print(const std::string& s, /* ************************************************************************* */ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize( const Values& continuousValues) const { + using boost::dynamic_pointer_cast; + // create an empty linear FG auto linearFG = boost::make_shared(); linearFG->reserve(size()); // linearize all hybrid factors - for (auto&& factor : factors_) { + for (auto& f : factors_) { // First check if it is a valid factor - if (factor) { - // Check if the factor is a hybrid factor. - // It can be either a nonlinear MixtureFactor or a linear - // GaussianMixtureFactor. - if (factor->isHybrid()) { - // Check if it is a nonlinear mixture factor - if (auto nlmf = boost::dynamic_pointer_cast(factor)) { - linearFG->push_back(nlmf->linearize(continuousValues)); - } else { - linearFG->push_back(factor); - } - - // Now check if the factor is a continuous only factor. - } else if (factor->isContinuous()) { - // In this case, we check if factor->inner() is nonlinear since - // HybridFactors wrap over continuous factors. - auto nlhf = boost::dynamic_pointer_cast(factor); - if (auto nlf = - boost::dynamic_pointer_cast(nlhf->inner())) { - auto hgf = boost::make_shared( - nlf->linearize(continuousValues)); - linearFG->push_back(hgf); - } else { - linearFG->push_back(factor); - } - // Finally if nothing else, we are discrete-only which doesn't need - // lineariztion. - } else { - linearFG->push_back(factor); - } - - } else { + if (!f) { + // TODO(dellaert): why? linearFG->push_back(GaussianFactor::shared_ptr()); + continue; + } + // Check if it is a nonlinear mixture factor + if (auto mf = dynamic_pointer_cast(f)) { + const GaussianMixtureFactor::shared_ptr& gmf = + mf->linearize(continuousValues); + linearFG->push_back(gmf); + } else if (auto nlf = dynamic_pointer_cast(f)) { + const GaussianFactor::shared_ptr& gf = nlf->linearize(continuousValues); + linearFG->push_back(gf); + } else if (dynamic_pointer_cast(f)) { + // If discrete-only: doesn't need linearization. + linearFG->push_back(f); + } else { + auto& fr = *f; + throw std::invalid_argument( + std::string("HybridNonlinearFactorGraph::linearize: factor type " + "not handled: ") + + demangle(typeid(fr).name())); } } return linearFG; diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.h b/gtsam/hybrid/HybridNonlinearFactorGraph.h index b48e8bb5c..60aee431b 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.h +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.h @@ -18,17 +18,12 @@ #pragma once -#include #include -#include -#include -#include -#include -#include -#include namespace gtsam { +class HybridGaussianFactorGraph; + /** * Nonlinear Hybrid Factor Graph * ----------------------- @@ -37,21 +32,6 @@ namespace gtsam { */ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { protected: - /// Check if FACTOR type is derived from NonlinearFactor. - template - using IsNonlinear = typename std::enable_if< - std::is_base_of::value>::type; - - /// Check if T has a value_type derived from FactorType. - template - using HasDerivedValueType = typename std::enable_if< - std::is_base_of::value>::type; - - /// Check if T has a pointer type derived from FactorType. - template - using HasDerivedElementType = typename std::enable_if::value>::type; - public: using Base = HybridFactorGraph; using This = HybridNonlinearFactorGraph; ///< this class @@ -76,70 +56,6 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { /// @} - // Allow use of selected FactorGraph methods: - using Base::empty; - using Base::reserve; - using Base::size; - using Base::operator[]; - using Base::add; - using Base::resize; - - /** - * Add a nonlinear factor *pointer* to the internal nonlinear factor graph - * @param nonlinearFactor - boost::shared_ptr to the factor to add - */ - template - IsNonlinear push_nonlinear( - const boost::shared_ptr& nonlinearFactor) { - Base::push_back(boost::make_shared(nonlinearFactor)); - } - - /// Construct a factor and add (shared pointer to it) to factor graph. - template - IsNonlinear emplace_nonlinear(Args&&... args) { - auto factor = boost::allocate_shared( - Eigen::aligned_allocator(), std::forward(args)...); - push_nonlinear(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. - * - * @tparam FACTOR The factor type template - * @param sharedFactor The factor to add to this factor graph. - */ - template - void push_back(const boost::shared_ptr& sharedFactor) { - if (auto p = boost::dynamic_pointer_cast(sharedFactor)) { - push_nonlinear(p); - } else { - Base::push_back(sharedFactor); - } - } - - /** - * Push back many factors as shared_ptr's in a container (factors are not - * copied) - */ - template - HasDerivedElementType push_back(const CONTAINER& container) { - Base::push_back(container.begin(), container.end()); - } - - /// Push back non-pointer objects in a container (factors are copied). - template - HasDerivedValueType push_back(const CONTAINER& container) { - Base::push_back(container.begin(), container.end()); - } - - /// Add a nonlinear factor as a shared ptr. - void add(boost::shared_ptr factor); - - /// Add a discrete factor as a shared ptr. - void add(boost::shared_ptr factor); - /// Print the factor graph. void print( const std::string& s = "HybridNonlinearFactorGraph", diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index 5285dd191..38905b8a2 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -21,7 +21,6 @@ #include #include -#include #include #include #include diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index e877e5ee7..012f707e4 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -68,17 +68,6 @@ virtual class HybridConditional { double error(const gtsam::HybridValues& values) const; }; -#include -virtual class HybridDiscreteFactor { - HybridDiscreteFactor(gtsam::DecisionTreeFactor dtf); - void print(string s = "HybridDiscreteFactor\n", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::HybridDiscreteFactor& other, double tol = 1e-9) const; - gtsam::Factor* inner(); - double error(const gtsam::HybridValues &values) const; -}; - #include class GaussianMixtureFactor : gtsam::HybridFactor { GaussianMixtureFactor( @@ -217,9 +206,7 @@ class HybridNonlinearFactorGraph { HybridNonlinearFactorGraph(const gtsam::HybridNonlinearFactorGraph& graph); void push_back(gtsam::HybridFactor* factor); void push_back(gtsam::NonlinearFactor* factor); - void push_back(gtsam::HybridDiscreteFactor* factor); - void add(gtsam::NonlinearFactor* factor); - void add(gtsam::DiscreteFactor* factor); + void push_back(gtsam::DiscreteFactor* factor); gtsam::HybridGaussianFactorGraph linearize(const gtsam::Values& continuousValues) const; bool empty() const; diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index b232efbf2..ab7a9a84f 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -136,6 +136,8 @@ struct Switching { std::vector measurements = {}, std::string discrete_transition_prob = "1/2 3/2") : K(K) { + using noiseModel::Isotropic; + // Create DiscreteKeys for binary K modes. for (size_t k = 0; k < K; k++) { modes.emplace_back(M(k), 2); @@ -150,9 +152,8 @@ struct Switching { // Create hybrid factor graph. // Add a prior on X(0). - auto prior = boost::make_shared>( - X(0), measurements.at(0), noiseModel::Isotropic::Sigma(1, prior_sigma)); - nonlinearFactorGraph.push_nonlinear(prior); + nonlinearFactorGraph.emplace_shared>( + X(0), measurements.at(0), Isotropic::Sigma(1, prior_sigma)); // Add "motion models". for (size_t k = 0; k < K - 1; k++) { @@ -162,14 +163,14 @@ struct Switching { for (auto &&f : motion_models) { components.push_back(boost::dynamic_pointer_cast(f)); } - nonlinearFactorGraph.emplace_hybrid( + nonlinearFactorGraph.emplace_shared( keys, DiscreteKeys{modes[k]}, components); } // Add measurement factors - auto measurement_noise = noiseModel::Isotropic::Sigma(1, prior_sigma); + auto measurement_noise = Isotropic::Sigma(1, prior_sigma); for (size_t k = 1; k < K; k++) { - nonlinearFactorGraph.emplace_nonlinear>( + nonlinearFactorGraph.emplace_shared>( X(k), measurements.at(k), measurement_noise); } @@ -205,13 +206,11 @@ struct Switching { */ void addModeChain(HybridNonlinearFactorGraph *fg, std::string discrete_transition_prob = "1/2 3/2") { - auto prior = boost::make_shared(modes[0], "1/1"); - fg->push_discrete(prior); + fg->emplace_shared(modes[0], "1/1"); for (size_t k = 0; k < K - 2; k++) { auto parents = {modes[k]}; - auto conditional = boost::make_shared( - modes[k + 1], parents, discrete_transition_prob); - fg->push_discrete(conditional); + fg->emplace_shared(modes[k + 1], parents, + discrete_transition_prob); } } @@ -223,13 +222,11 @@ struct Switching { */ void addModeChain(HybridGaussianFactorGraph *fg, std::string discrete_transition_prob = "1/2 3/2") { - auto prior = boost::make_shared(modes[0], "1/1"); - fg->push_discrete(prior); + fg->emplace_shared(modes[0], "1/1"); for (size_t k = 0; k < K - 2; k++) { auto parents = {modes[k]}; - auto conditional = boost::make_shared( - modes[k + 1], parents, discrete_transition_prob); - fg->push_discrete(conditional); + fg->emplace_shared(modes[k + 1], parents, + discrete_transition_prob); } } }; diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 3badc34a4..bdf858b22 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -93,8 +93,7 @@ TEST(HybridBayesNet, evaluateHybrid) { // Create hybrid Bayes net. HybridBayesNet bayesNet; - bayesNet.push_back(GaussianConditional::sharedMeanAndStddev( - X(0), 2 * I_1x1, X(1), Vector1(-4.0), 5.0)); + bayesNet.push_back(continuousConditional); bayesNet.emplace_back( new GaussianMixture({X(1)}, {}, {Asia}, {conditional0, conditional1})); bayesNet.emplace_back(new DiscreteConditional(Asia, "99/1")); @@ -333,13 +332,12 @@ TEST(HybridBayesNet, Sampling) { auto one_motion = boost::make_shared>(X(0), X(1), 1, noise_model); std::vector factors = {zero_motion, one_motion}; - nfg.emplace_nonlinear>(X(0), 0.0, noise_model); - nfg.emplace_hybrid( + nfg.emplace_shared>(X(0), 0.0, noise_model); + nfg.emplace_shared( KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors); DiscreteKey mode(M(0), 2); - auto discrete_prior = boost::make_shared(mode, "1/1"); - nfg.push_discrete(discrete_prior); + nfg.emplace_shared(mode, "1/1"); Values initial; double z0 = 0.0, z1 = 1.0; diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index 08f94d88e..2c2bdc3c0 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -150,9 +150,9 @@ TEST(HybridBayesTree, Optimize) { DiscreteFactorGraph dfg; for (auto&& f : *remainingFactorGraph) { - auto factor = dynamic_pointer_cast(f); - dfg.push_back( - boost::dynamic_pointer_cast(factor->inner())); + auto discreteFactor = dynamic_pointer_cast(f); + assert(discreteFactor); + dfg.push_back(discreteFactor); } // Add the probabilities for each branch diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 86cf3fad6..a0195d0c5 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -46,7 +46,7 @@ Ordering getOrdering(HybridGaussianFactorGraph& factors, const HybridGaussianFactorGraph& newFactors) { factors += newFactors; // Get all the discrete keys from the factors - KeySet allDiscrete = factors.discreteKeys(); + KeySet allDiscrete = factors.discreteKeySet(); // Create KeyVector with continuous keys followed by discrete keys. KeyVector newKeysDiscreteLast; @@ -241,7 +241,7 @@ AlgebraicDecisionTree getProbPrimeTree( const HybridGaussianFactorGraph& graph) { HybridBayesNet::shared_ptr bayesNet; HybridGaussianFactorGraph::shared_ptr remainingGraph; - Ordering continuous(graph.continuousKeys()); + Ordering continuous(graph.continuousKeySet()); std::tie(bayesNet, remainingGraph) = graph.eliminatePartialSequential(continuous); @@ -296,14 +296,14 @@ TEST(HybridEstimation, Probability) { auto graph = switching.linearizedFactorGraph; // Continuous elimination - Ordering continuous_ordering(graph.continuousKeys()); + Ordering continuous_ordering(graph.continuousKeySet()); HybridBayesNet::shared_ptr bayesNet; HybridGaussianFactorGraph::shared_ptr discreteGraph; std::tie(bayesNet, discreteGraph) = graph.eliminatePartialSequential(continuous_ordering); // Discrete elimination - Ordering discrete_ordering(graph.discreteKeys()); + Ordering discrete_ordering(graph.discreteKeySet()); auto discreteBayesNet = discreteGraph->eliminateSequential(discrete_ordering); // Add the discrete conditionals to make it a full bayes net. @@ -346,7 +346,7 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { AlgebraicDecisionTree expected_probPrimeTree = getProbPrimeTree(graph); // Eliminate continuous - Ordering continuous_ordering(graph.continuousKeys()); + Ordering continuous_ordering(graph.continuousKeySet()); HybridBayesTree::shared_ptr bayesTree; HybridGaussianFactorGraph::shared_ptr discreteGraph; std::tie(bayesTree, discreteGraph) = @@ -358,7 +358,7 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { auto last_conditional = (*bayesTree)[last_continuous_key]->conditional(); DiscreteKeys discrete_keys = last_conditional->discreteKeys(); - Ordering discrete(graph.discreteKeys()); + Ordering discrete(graph.discreteKeySet()); auto discreteBayesTree = discreteGraph->eliminateMultifrontal(discrete); EXPECT_LONGS_EQUAL(1, discreteBayesTree->size()); @@ -407,8 +407,8 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() { const auto noise_model = noiseModel::Isotropic::Sigma(1, sigma); // Add "measurement" factors: - nfg.emplace_nonlinear>(X(0), 0.0, noise_model); - nfg.emplace_nonlinear>(X(1), 1.0, noise_model); + nfg.emplace_shared>(X(0), 0.0, noise_model); + nfg.emplace_shared>(X(1), 1.0, noise_model); // Add mixture factor: DiscreteKey m(M(0), 2); @@ -416,7 +416,7 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() { boost::make_shared>(X(0), X(1), 0, noise_model); const auto one_motion = boost::make_shared>(X(0), X(1), 1, noise_model); - nfg.emplace_hybrid( + nfg.emplace_shared( KeyVector{X(0), X(1)}, DiscreteKeys{m}, std::vector{zero_motion, one_motion}); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index d8bf77762..f904ee5ba 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -26,9 +26,7 @@ #include #include #include -#include #include -#include #include #include #include @@ -65,7 +63,7 @@ TEST(HybridGaussianFactorGraph, Creation) { HybridGaussianFactorGraph hfg; - hfg.add(HybridGaussianFactor(JacobianFactor(X(0), I_3x3, Z_3x1))); + hfg.emplace_shared(X(0), I_3x3, Z_3x1); // Define a gaussian mixture conditional P(x0|x1, c0) and add it to the factor // graph @@ -86,7 +84,7 @@ TEST(HybridGaussianFactorGraph, EliminateSequential) { // Test elimination of a single variable. HybridGaussianFactorGraph hfg; - hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); + hfg.emplace_shared(0, I_3x3, Z_3x1); auto result = hfg.eliminatePartialSequential(KeyVector{0}); @@ -102,7 +100,7 @@ TEST(HybridGaussianFactorGraph, EliminateMultifrontal) { // Add priors on x0 and c1 hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); - hfg.add(HybridDiscreteFactor(DecisionTreeFactor(m, {2, 8}))); + hfg.add(DecisionTreeFactor(m, {2, 8})); Ordering ordering; ordering.push_back(X(0)); @@ -116,23 +114,23 @@ TEST(HybridGaussianFactorGraph, EliminateMultifrontal) { TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { HybridGaussianFactorGraph hfg; - DiscreteKey m1(M(1), 2); - // Add prior on x0 hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); + // Add factor between x0 and x1 hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); // Add a gaussian mixture factor ϕ(x1, c1) + DiscreteKey m1(M(1), 2); DecisionTree dt( M(1), boost::make_shared(X(1), I_3x3, Z_3x1), boost::make_shared(X(1), I_3x3, Vector3::Ones())); - hfg.add(GaussianMixtureFactor({X(1)}, {m1}, dt)); auto result = hfg.eliminateSequential(); auto dc = result->at(2)->asDiscrete(); + CHECK(dc); DiscreteValues dv; dv[M(1)] = 0; // Regression test @@ -212,7 +210,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { // Hybrid factor P(x1|c1) hfg.add(GaussianMixtureFactor({X(1)}, {m}, dt)); // Prior factor on c1 - hfg.add(HybridDiscreteFactor(DecisionTreeFactor(m, {2, 8}))); + hfg.add(DecisionTreeFactor(m, {2, 8})); // Get a constrained ordering keeping c1 last auto ordering_full = HybridOrdering(hfg); @@ -247,8 +245,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { hfg.add(GaussianMixtureFactor({X(2)}, {{M(1), 2}}, dt1)); } - hfg.add(HybridDiscreteFactor( - DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4"))); + hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); hfg.add(JacobianFactor(X(3), I_3x3, X(4), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1)); diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 1ce10b452..4f135f84f 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -380,7 +380,7 @@ TEST(HybridGaussianISAM, NonTrivial) { Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin auto priorNoise = noiseModel::Diagonal::Sigmas( Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - fg.emplace_nonlinear>(X(0), prior, priorNoise); + fg.emplace_shared>(X(0), prior, priorNoise); // create a noise model for the landmark measurements auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1); @@ -389,11 +389,11 @@ TEST(HybridGaussianISAM, NonTrivial) { // where X is the base link and W is the foot link. // Add connecting poses similar to PoseFactors in GTD - fg.emplace_nonlinear>(X(0), Y(0), Pose2(0, 1.0, 0), + fg.emplace_shared>(X(0), Y(0), Pose2(0, 1.0, 0), poseNoise); - fg.emplace_nonlinear>(Y(0), Z(0), Pose2(0, 1.0, 0), + fg.emplace_shared>(Y(0), Z(0), Pose2(0, 1.0, 0), poseNoise); - fg.emplace_nonlinear>(Z(0), W(0), Pose2(0, 1.0, 0), + fg.emplace_shared>(Z(0), W(0), Pose2(0, 1.0, 0), poseNoise); // Create initial estimate @@ -432,14 +432,14 @@ TEST(HybridGaussianISAM, NonTrivial) { fg.push_back(mixtureFactor); // Add equivalent of ImuFactor - fg.emplace_nonlinear>(X(0), X(1), Pose2(1.0, 0.0, 0), + fg.emplace_shared>(X(0), X(1), Pose2(1.0, 0.0, 0), poseNoise); // PoseFactors-like at k=1 - fg.emplace_nonlinear>(X(1), Y(1), Pose2(0, 1, 0), + fg.emplace_shared>(X(1), Y(1), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Y(1), Z(1), Pose2(0, 1, 0), + fg.emplace_shared>(Y(1), Z(1), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Z(1), W(1), Pose2(-1, 1, 0), + fg.emplace_shared>(Z(1), W(1), Pose2(-1, 1, 0), poseNoise); initial.insert(X(1), Pose2(1.0, 0.0, 0.0)); @@ -472,14 +472,14 @@ TEST(HybridGaussianISAM, NonTrivial) { fg.push_back(mixtureFactor); // Add equivalent of ImuFactor - fg.emplace_nonlinear>(X(1), X(2), Pose2(1.0, 0.0, 0), + fg.emplace_shared>(X(1), X(2), Pose2(1.0, 0.0, 0), poseNoise); // PoseFactors-like at k=1 - fg.emplace_nonlinear>(X(2), Y(2), Pose2(0, 1, 0), + fg.emplace_shared>(X(2), Y(2), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Y(2), Z(2), Pose2(0, 1, 0), + fg.emplace_shared>(Y(2), Z(2), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Z(2), W(2), Pose2(-2, 1, 0), + fg.emplace_shared>(Z(2), W(2), Pose2(-2, 1, 0), poseNoise); initial.insert(X(2), Pose2(2.0, 0.0, 0.0)); @@ -515,14 +515,14 @@ TEST(HybridGaussianISAM, NonTrivial) { fg.push_back(mixtureFactor); // Add equivalent of ImuFactor - fg.emplace_nonlinear>(X(2), X(3), Pose2(1.0, 0.0, 0), + fg.emplace_shared>(X(2), X(3), Pose2(1.0, 0.0, 0), poseNoise); // PoseFactors-like at k=3 - fg.emplace_nonlinear>(X(3), Y(3), Pose2(0, 1, 0), + fg.emplace_shared>(X(3), Y(3), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Y(3), Z(3), Pose2(0, 1, 0), + fg.emplace_shared>(Y(3), Z(3), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Z(3), W(3), Pose2(-3, 1, 0), + fg.emplace_shared>(Z(3), W(3), Pose2(-3, 1, 0), poseNoise); initial.insert(X(3), Pose2(3.0, 0.0, 0.0)); diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index d84f4b352..94a611a9e 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -54,7 +54,7 @@ TEST(HybridFactorGraph, GaussianFactorGraph) { HybridNonlinearFactorGraph fg; // Add a simple prior factor to the nonlinear factor graph - fg.emplace_nonlinear>(X(0), 0, Isotropic::Sigma(1, 0.1)); + fg.emplace_shared>(X(0), 0, Isotropic::Sigma(1, 0.1)); // Linearization point Values linearizationPoint; @@ -311,8 +311,7 @@ TEST(HybridsGaussianElimination, Eliminate_x1) { Ordering ordering; ordering += X(1); - std::pair result = - EliminateHybrid(factors, ordering); + auto result = EliminateHybrid(factors, ordering); CHECK(result.first); EXPECT_LONGS_EQUAL(1, result.first->nrFrontals()); CHECK(result.second); @@ -350,7 +349,7 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { ordering += X(1); HybridConditional::shared_ptr hybridConditionalMixture; - HybridFactor::shared_ptr factorOnModes; + boost::shared_ptr factorOnModes; std::tie(hybridConditionalMixture, factorOnModes) = EliminateHybrid(factors, ordering); @@ -364,17 +363,11 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { // 1 parent, which is the mode EXPECT_LONGS_EQUAL(1, gaussianConditionalMixture->nrParents()); - // This is now a HybridDiscreteFactor - auto hybridDiscreteFactor = - dynamic_pointer_cast(factorOnModes); - // Access the type-erased inner object and convert to DecisionTreeFactor - auto discreteFactor = - dynamic_pointer_cast(hybridDiscreteFactor->inner()); + // This is now a discreteFactor + auto discreteFactor = dynamic_pointer_cast(factorOnModes); CHECK(discreteFactor); EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size()); EXPECT(discreteFactor->root_->isLeaf() == false); - - // TODO(Varun) Test emplace_discrete } /**************************************************************************** @@ -435,9 +428,10 @@ TEST(HybridFactorGraph, Full_Elimination) { DiscreteFactorGraph discrete_fg; // TODO(Varun) Make this a function of HybridGaussianFactorGraph? - for (HybridFactor::shared_ptr& factor : (*remainingFactorGraph_partial)) { - auto df = dynamic_pointer_cast(factor); - discrete_fg.push_back(df->inner()); + for (auto& factor : (*remainingFactorGraph_partial)) { + auto df = dynamic_pointer_cast(factor); + assert(df); + discrete_fg.push_back(df); } ordering.clear(); @@ -500,8 +494,7 @@ TEST(HybridFactorGraph, Printing) { string expected_hybridFactorGraph = R"( size: 7 -factor 0: Continuous [x0] - +factor 0: A[x0] = [ 10 ] @@ -553,26 +546,22 @@ factor 2: Hybrid [x1 x2; m1]{ No noise model } -factor 3: Continuous [x1] - +factor 3: A[x1] = [ 10 ] b = [ -10 ] No noise model -factor 4: Continuous [x2] - +factor 4: A[x2] = [ 10 ] b = [ -10 ] No noise model -factor 5: Discrete [m0] - P( m0 ): +factor 5: P( m0 ): Leaf 0.5 -factor 6: Discrete [m1 m0] - P( m1 | m0 ): +factor 6: P( m1 | m0 ): Choice(m1) 0 Choice(m0) 0 0 Leaf 0.33333333 @@ -683,7 +672,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin auto priorNoise = noiseModel::Diagonal::Sigmas( Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - fg.emplace_nonlinear>(X(0), prior, priorNoise); + fg.emplace_shared>(X(0), prior, priorNoise); using PlanarMotionModel = BetweenFactor; @@ -696,7 +685,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { moving = boost::make_shared(X(0), X(1), odometry, noise_model); std::vector motion_models = {still, moving}; - fg.emplace_hybrid( + fg.emplace_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models); // Add Range-Bearing measurements to from X0 to L0 and X1 to L1. @@ -708,9 +697,9 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { double range11 = std::sqrt(4.0 + 4.0), range22 = 2.0; // Add Bearing-Range factors - fg.emplace_nonlinear>( + fg.emplace_shared>( X(0), L(0), bearing11, range11, measurementNoise); - fg.emplace_nonlinear>( + fg.emplace_shared>( X(1), L(1), bearing22, range22, measurementNoise); // Create (deliberately inaccurate) initial estimate diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 68a55abdd..d89909122 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -409,7 +409,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin auto priorNoise = noiseModel::Diagonal::Sigmas( Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - fg.emplace_nonlinear>(X(0), prior, priorNoise); + fg.emplace_shared>(X(0), prior, priorNoise); // create a noise model for the landmark measurements auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1); @@ -418,11 +418,11 @@ TEST(HybridNonlinearISAM, NonTrivial) { // where X is the base link and W is the foot link. // Add connecting poses similar to PoseFactors in GTD - fg.emplace_nonlinear>(X(0), Y(0), Pose2(0, 1.0, 0), + fg.emplace_shared>(X(0), Y(0), Pose2(0, 1.0, 0), poseNoise); - fg.emplace_nonlinear>(Y(0), Z(0), Pose2(0, 1.0, 0), + fg.emplace_shared>(Y(0), Z(0), Pose2(0, 1.0, 0), poseNoise); - fg.emplace_nonlinear>(Z(0), W(0), Pose2(0, 1.0, 0), + fg.emplace_shared>(Z(0), W(0), Pose2(0, 1.0, 0), poseNoise); // Create initial estimate @@ -451,14 +451,14 @@ TEST(HybridNonlinearISAM, NonTrivial) { fg.push_back(mixtureFactor); // Add equivalent of ImuFactor - fg.emplace_nonlinear>(X(0), X(1), Pose2(1.0, 0.0, 0), + fg.emplace_shared>(X(0), X(1), Pose2(1.0, 0.0, 0), poseNoise); // PoseFactors-like at k=1 - fg.emplace_nonlinear>(X(1), Y(1), Pose2(0, 1, 0), + fg.emplace_shared>(X(1), Y(1), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Y(1), Z(1), Pose2(0, 1, 0), + fg.emplace_shared>(Y(1), Z(1), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Z(1), W(1), Pose2(-1, 1, 0), + fg.emplace_shared>(Z(1), W(1), Pose2(-1, 1, 0), poseNoise); initial.insert(X(1), Pose2(1.0, 0.0, 0.0)); @@ -491,14 +491,14 @@ TEST(HybridNonlinearISAM, NonTrivial) { fg.push_back(mixtureFactor); // Add equivalent of ImuFactor - fg.emplace_nonlinear>(X(1), X(2), Pose2(1.0, 0.0, 0), + fg.emplace_shared>(X(1), X(2), Pose2(1.0, 0.0, 0), poseNoise); // PoseFactors-like at k=1 - fg.emplace_nonlinear>(X(2), Y(2), Pose2(0, 1, 0), + fg.emplace_shared>(X(2), Y(2), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Y(2), Z(2), Pose2(0, 1, 0), + fg.emplace_shared>(Y(2), Z(2), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Z(2), W(2), Pose2(-2, 1, 0), + fg.emplace_shared>(Z(2), W(2), Pose2(-2, 1, 0), poseNoise); initial.insert(X(2), Pose2(2.0, 0.0, 0.0)); @@ -534,14 +534,14 @@ TEST(HybridNonlinearISAM, NonTrivial) { fg.push_back(mixtureFactor); // Add equivalent of ImuFactor - fg.emplace_nonlinear>(X(2), X(3), Pose2(1.0, 0.0, 0), + fg.emplace_shared>(X(2), X(3), Pose2(1.0, 0.0, 0), poseNoise); // PoseFactors-like at k=3 - fg.emplace_nonlinear>(X(3), Y(3), Pose2(0, 1, 0), + fg.emplace_shared>(X(3), Y(3), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Y(3), Z(3), Pose2(0, 1, 0), + fg.emplace_shared>(Y(3), Z(3), Pose2(0, 1, 0), poseNoise); - fg.emplace_nonlinear>(Z(3), W(3), Pose2(-3, 1, 0), + fg.emplace_shared>(Z(3), W(3), Pose2(-3, 1, 0), poseNoise); initial.insert(X(3), Pose2(3.0, 0.0, 0.0)); diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index 94f614bc7..bfa8b7514 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -23,8 +23,6 @@ #include #include #include -#include -#include #include #include @@ -73,28 +71,6 @@ BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); BOOST_CLASS_EXPORT_GUID(HybridBayesNet, "gtsam_HybridBayesNet"); -/* ****************************************************************************/ -// Test HybridGaussianFactor serialization. -TEST(HybridSerialization, HybridGaussianFactor) { - const HybridGaussianFactor factor(JacobianFactor(X(0), I_3x3, Z_3x1)); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - -/* ****************************************************************************/ -// Test HybridDiscreteFactor serialization. -TEST(HybridSerialization, HybridDiscreteFactor) { - DiscreteKeys discreteKeys{{M(0), 2}}; - const HybridDiscreteFactor factor( - DecisionTreeFactor(discreteKeys, std::vector{0.4, 0.6})); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - /* ****************************************************************************/ // Test GaussianMixtureFactor serialization. TEST(HybridSerialization, GaussianMixtureFactor) { diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 27b85ef67..1fb2b8df1 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -34,22 +34,17 @@ typedef FastVector FactorIndices; typedef FastSet FactorIndexSet; /** - * This is the base class for all factor types. It is templated on a KEY type, - * which will be the type used to label variables. Key types currently in use - * in gtsam are Index with symbolic (IndexFactor, SymbolicFactorGraph) and - * Gaussian factors (GaussianFactor, JacobianFactor, HessianFactor, GaussianFactorGraph), - * and Key with nonlinear factors (NonlinearFactor, NonlinearFactorGraph). - * though currently only IndexFactor and IndexConditional derive from this - * class, using Index keys. This class does not store any data other than its - * keys. Derived classes store data such as matrices and probability tables. + * This is the base class for all factor types. This class does not store any + * data other than its keys. Derived classes store data such as matrices and + * probability tables. * - * Note that derived classes *must* redefine the ConditionalType and shared_ptr - * typedefs to refer to the associated conditional and shared_ptr types of the - * derived class. See IndexFactor, JacobianFactor, etc. for examples. + * Note that derived classes *must* redefine the `This` and `shared_ptr` + * typedefs. See JacobianFactor, etc. for examples. * - * This class is \b not virtual for performance reasons - derived symbolic classes, - * IndexFactor and IndexConditional, need to be created and destroyed quickly - * during symbolic elimination. GaussianFactor and NonlinearFactor are virtual. + * This class is \b not virtual for performance reasons - the derived class + * SymbolicFactor needs to be created and destroyed quickly during symbolic + * elimination. GaussianFactor and NonlinearFactor are virtual. + * * \nosubgrouping */ class GTSAM_EXPORT Factor diff --git a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py index 171fae60f..ed26a0c81 100644 --- a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py +++ b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py @@ -17,6 +17,7 @@ import unittest import numpy as np from gtsam.symbol_shorthand import C, X from gtsam.utils.test_case import GtsamTestCase +from gtsam import BetweenFactorPoint3, noiseModel, PriorFactorPoint3, Point3 import gtsam @@ -27,25 +28,22 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): nlfg = gtsam.HybridNonlinearFactorGraph() dk = gtsam.DiscreteKeys() dk.push_back((10, 2)) - nlfg.add( - gtsam.BetweenFactorPoint3( - 1, 2, gtsam.Point3(1, 2, 3), - gtsam.noiseModel.Diagonal.Variances([1, 1, 1]))) - nlfg.add( - gtsam.PriorFactorPoint3( - 2, gtsam.Point3(1, 2, 3), - gtsam.noiseModel.Diagonal.Variances([0.5, 0.5, 0.5]))) + nlfg.push_back(BetweenFactorPoint3(1, 2, Point3( + 1, 2, 3), noiseModel.Diagonal.Variances([1, 1, 1]))) + nlfg.push_back( + PriorFactorPoint3(2, Point3(1, 2, 3), + noiseModel.Diagonal.Variances([0.5, 0.5, 0.5]))) nlfg.push_back( gtsam.MixtureFactor([1], dk, [ - gtsam.PriorFactorPoint3(1, gtsam.Point3(0, 0, 0), - gtsam.noiseModel.Unit.Create(3)), - gtsam.PriorFactorPoint3(1, gtsam.Point3(1, 2, 1), - gtsam.noiseModel.Unit.Create(3)) + PriorFactorPoint3(1, Point3(0, 0, 0), + noiseModel.Unit.Create(3)), + PriorFactorPoint3(1, Point3(1, 2, 1), + noiseModel.Unit.Create(3)) ])) - nlfg.add(gtsam.DecisionTreeFactor((10, 2), "1 3")) + nlfg.push_back(gtsam.DecisionTreeFactor((10, 2), "1 3")) values = gtsam.Values() - values.insert_point3(1, gtsam.Point3(0, 0, 0)) - values.insert_point3(2, gtsam.Point3(2, 3, 1)) + values.insert_point3(1, Point3(0, 0, 0)) + values.insert_point3(2, Point3(2, 3, 1)) hfg = nlfg.linearize(values) hbn = hfg.eliminateSequential() hbv = hbn.optimize()