From 626d66bdf4ee2c859f312bb2952cd09bb5ed7d4c Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 6 Jun 2013 15:36:37 +0000 Subject: [PATCH] Working on BayesTreeUnordered --- .../BayesTreeCliqueBaseUnordered-inst.h | 241 ++++++++++++++++++ .../inference/BayesTreeCliqueBaseUnordered.h | 198 ++++++++++++++ gtsam/inference/EliminateableFactorGraph.h | 73 ++++++ gtsam/inference/EliminationTreeUnordered.h | 4 +- gtsam/inference/OrderingUnordered.h | 27 ++ 5 files changed, 541 insertions(+), 2 deletions(-) create mode 100644 gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h create mode 100644 gtsam/inference/BayesTreeCliqueBaseUnordered.h create mode 100644 gtsam/inference/EliminateableFactorGraph.h create mode 100644 gtsam/inference/OrderingUnordered.h diff --git a/gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h b/gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h new file mode 100644 index 000000000..1e09509ab --- /dev/null +++ b/gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h @@ -0,0 +1,241 @@ +/* ---------------------------------------------------------------------------- + + * 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 BayesTreeCliqueBase-inst.h + * @brief Base class for cliques of a BayesTree + * @author Richard Roberts and Frank Dellaert + */ + +#pragma once + +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + std::vector + BayesTreeCliqueBaseUnordered::separator_setminus_B(derived_ptr B) const + { + FastSet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); + FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); + std::vector S_setminus_B; + std::set_difference(p_F_S->beginParents(), p_F_S->endParents(), + indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); + return S_setminus_B; + } + + /* ************************************************************************* */ + template + std::vector BayesTreeCliqueBaseUnordered::shortcut_indices( + derived_ptr B, const FactorGraphUnordered& p_Cp_B) const + { + gttic(shortcut_indices); + FastSet allKeys = p_Cp_B.keys(); + FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); + std::vector S_setminus_B = separator_setminus_B(B); + std::vector keep; + // keep = S\B intersect allKeys (S_setminus_B is already sorted) + std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), // + allKeys.begin(), allKeys.end(), back_inserter(keep)); + // keep += B intersect allKeys + std::set_intersection(indicesB.begin(), indicesB.end(), // + allKeys.begin(), allKeys.end(), back_inserter(keep)); + return keep; + } + + /* ************************************************************************* */ + template + void BayesTreeCliqueBaseUnordered::print( + const std::string& s, const KeyFormatter& keyFormatter) const + { + conditional_->print(s, keyFormatter); + } + + /* ************************************************************************* */ + template + size_t BayesTreeCliqueBaseUnordered::treeSize() const { + size_t size = 1; + BOOST_FOREACH(const derived_ptr& child, children_) + size += child->treeSize(); + return size; + } + + /* ************************************************************************* */ + template + size_t BayesTreeCliqueBaseUnordered::numCachedSeparatorMarginals() const + { + if (!cachedSeparatorMarginal_) + return 0; + + size_t subtree_count = 1; + BOOST_FOREACH(const derived_ptr& child, children_) + subtree_count += child->numCachedSeparatorMarginals(); + + return subtree_count; + } + + /* ************************************************************************* */ + // The shortcut density is a conditional P(S|R) of the separator of this + // clique on the root. We can compute it recursively from the parent shortcut + // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p + /* ************************************************************************* */ + template + BayesNetUnordered BayesTreeCliqueBaseUnordered::shortcut( + derived_ptr B, Eliminate function) const + { + gttic(BayesTreeCliqueBaseUnordered_shortcut); + + // We only calculate the shortcut when this clique is not B + // and when the S\B is not empty + std::vector S_setminus_B = separator_setminus_B(B); + if (B.get() != this && !S_setminus_B.empty()) { + + // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph + derived_ptr parent(parent_.lock()); + gttoc(BayesTreeCliqueBaseUnordered_shortcut); + FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B) + gttic(BayesTreeCliqueBaseUnordered_shortcut); + p_Cp_B.push_back(parent->conditional()->toFactor()); // P(Fp|Sp) + + // Determine the variables we want to keepSet, S union B + std::vector keep = shortcut_indices(B, p_Cp_B); + + // Reduce the variable indices to start at zero + gttic(Reduce); + const Permutation reduction = internal::createReducingPermutation(p_Cp_B.keys()); + internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); + BOOST_FOREACH(const boost::shared_ptr& factor, p_Cp_B) { + if(factor) factor->reduceWithInverse(inverseReduction); } + inverseReduction.applyInverse(keep); + gttoc(Reduce); + + // Create solver that will marginalize for us + GenericSequentialSolver solver(p_Cp_B); + + // Finally, we only want to have S\B variables in the Bayes net, so + size_t nrFrontals = S_setminus_B.size(); + BayesNet result = *solver.conditionalBayesNet(keep, nrFrontals, function); + + // Undo the reduction + gttic(Undo_Reduce); + BOOST_FOREACH(const typename boost::shared_ptr& factor, p_Cp_B) { + if (factor) factor->permuteWithInverse(reduction); } + result.permuteWithInverse(reduction); + gttoc(Undo_Reduce); + + assertInvariants(); + + return result; + } else { + return BayesNet(); + } + } + + /* ************************************************************************* */ + // separator marginal, uses separator marginal of parent recursively + // P(C) = P(F|S) P(S) + /* ************************************************************************* */ + template + FactorGraph::FactorType> BayesTreeCliqueBaseUnordered< + DERIVED, CONDITIONAL>::separatorMarginal(derived_ptr R, Eliminate function) const + { + gttic(BayesTreeCliqueBaseUnordered_separatorMarginal); + // Check if the Separator marginal was already calculated + if (!cachedSeparatorMarginal_) { + gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); + // If this is the root, there is no separator + if (R.get() == this) { + // we are root, return empty + FactorGraph empty; + cachedSeparatorMarginal_ = empty; + } else { + // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp) + // initialize P(Cp) with the parent separator marginal + derived_ptr parent(parent_.lock()); + gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); // Flatten recursion in timing outline + gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal); + FactorGraph p_Cp(parent->separatorMarginal(R, function)); // P(Sp) + gttic(BayesTreeCliqueBaseUnordered_separatorMarginal); + gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); + // now add the parent conditional + p_Cp.push_back(parent->conditional()->toFactor()); // P(Fp|Sp) + + // Reduce the variable indices to start at zero + gttic(Reduce); + const Permutation reduction = internal::createReducingPermutation(p_Cp.keys()); + internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); + BOOST_FOREACH(const boost::shared_ptr& factor, p_Cp) { + if(factor) factor->reduceWithInverse(inverseReduction); } + + // The variables we want to keepSet are exactly the ones in S + sharedConditional p_F_S = this->conditional(); + std::vector indicesS(p_F_S->beginParents(), p_F_S->endParents()); + inverseReduction.applyInverse(indicesS); + gttoc(Reduce); + + // Create solver that will marginalize for us + GenericSequentialSolver solver(p_Cp); + + cachedSeparatorMarginal_ = *(solver.jointBayesNet(indicesS, function)); + + // Undo the reduction + gttic(Undo_Reduce); + BOOST_FOREACH(const typename boost::shared_ptr& factor, p_Cp) { + if (factor) factor->permuteWithInverse(reduction); } + BOOST_FOREACH(const typename boost::shared_ptr& factor, *cachedSeparatorMarginal_) { + if (factor) factor->permuteWithInverse(reduction); } + gttoc(Undo_Reduce); + } + } else { + gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachehit); + } + + // return the shortcut P(S||B) + return *cachedSeparatorMarginal_; // return the cached version + } + + /* ************************************************************************* */ + // marginal2, uses separator marginal of parent recursively + // P(C) = P(F|S) P(S) + /* ************************************************************************* */ + template + FactorGraph::FactorType> BayesTreeCliqueBaseUnordered< + DERIVED, CONDITIONAL>::marginal2(derived_ptr R, Eliminate function) const + { + gttic(BayesTreeCliqueBaseUnordered_marginal2); + // initialize with separator marginal P(S) + FactorGraph p_C(this->separatorMarginal(R, function)); + // add the conditional P(F|S) + p_C.push_back(this->conditional()->toFactor()); + return p_C; + } + + /* ************************************************************************* */ + template + void BayesTreeCliqueBaseUnordered::deleteCachedShortcuts() { + + // When a shortcut is requested, all of the shortcuts between it and the + // root are also generated. So, if this clique's cached shortcut is set, + // recursively call over all child cliques. Otherwise, it is unnecessary. + if (cachedSeparatorMarginal_) { + BOOST_FOREACH(derived_ptr& child, children_) { + child->deleteCachedShortcuts(); + } + + //Delete CachedShortcut for this clique + cachedSeparatorMarginal_ = boost::none; + } + + } + +} diff --git a/gtsam/inference/BayesTreeCliqueBaseUnordered.h b/gtsam/inference/BayesTreeCliqueBaseUnordered.h new file mode 100644 index 000000000..96f6fd6b7 --- /dev/null +++ b/gtsam/inference/BayesTreeCliqueBaseUnordered.h @@ -0,0 +1,198 @@ +/* ---------------------------------------------------------------------------- + + * 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 BayesTreeCliqueBase.h + * @brief Base class for cliques of a BayesTree + * @author Richard Roberts and Frank Dellaert + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + template class BayesTreeUnordered; +} + +namespace gtsam { + + /** + * This is the base class for BayesTree cliques. The default and standard derived type is + * BayesTreeClique, but some algorithms, like iSAM2, use a different clique type in order to store + * extra data along with the clique. + * + * This class is templated on the derived class (i.e. the curiously recursive template pattern). + * The advantage of this over using virtual classes is that it avoids the need for casting to get + * the derived type. This is possible because all cliques in a BayesTree are the same type - if + * they were not then we'd need a virtual class. + * + * @tparam DERIVED The derived clique type. + * @tparam CONDITIONAL The conditional type. + * \nosubgrouping */ + template + struct BayesTreeCliqueBaseUnordered { + + public: + typedef BayesTreeCliqueBaseUnordered This; + typedef DERIVED DerivedType; + typedef FACTORGRAPH FactorGraphType; + typedef BAYESNET BayesNetType; + typedef BayesNetType::ConditionalType ConditionalType; + typedef boost::shared_ptr sharedConditional; + typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; + typedef boost::shared_ptr derived_ptr; + typedef boost::weak_ptr derived_weak_ptr; + typedef FactorGraphType::FactorType FactorType; + + protected: + + /// @name Standard Constructors + /// @{ + + /** Default constructor */ + BayesTreeCliqueBaseUnordered() {} + + /** Construct from a conditional, leaving parent and child pointers uninitialized */ + BayesTreeCliqueBaseUnordered(const sharedConditional& conditional) : conditional_(conditional) {} + + /// @} + + /// This stores the Cached separator margnal P(S) + mutable boost::optional cachedSeparatorMarginal_; + + public: + sharedConditional conditional_; + derived_weak_ptr parent_; + std::vector children; + + /// @name Testable + /// @{ + + /** check equality */ + bool equals(const This& other, double tol = 1e-9) const { + return (!conditional_ && !other.conditional()) + || conditional_->equals(*other.conditional(), tol); + } + + /** print this node */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; + + /// @} + /// @name Standard Interface + /// @{ + + /** Access the conditional */ + const sharedConditional& conditional() const { return conditional_; } + + /** is this the root of a Bayes tree ? */ + inline bool isRoot() const { return parent_.expired(); } + + /** The size of subtree rooted at this clique, i.e., nr of Cliques */ + size_t treeSize() const; + + /** Collect number of cliques with cached separator marginals */ + size_t numCachedSeparatorMarginals() const; + + /** return a shared_ptr to the parent clique */ + derived_ptr parent() const { return parent_.lock(); } + + /// @} + /// @name Advanced Interface + /// @{ + + /** return the conditional P(S|Root) on the separator given the root */ + BayesNetType shortcut(derived_ptr root, Eliminate function) const; + + /** return the marginal P(S) on the separator */ + FactorGraphType separatorMarginal(derived_ptr root, Eliminate function) const; + + /** return the marginal P(C) of the clique, using marginal caching */ + FactorGraphType marginal2(derived_ptr root, Eliminate function) const; + + /** + * This deletes the cached shortcuts of all cliques (subtree) below this clique. + * This is performed when the bayes tree is modified. + */ + void deleteCachedShortcuts(); + + const boost::optional& cachedSeparatorMarginal() const { + return cachedSeparatorMarginal_; } + + friend class BayesTreeUnordered; + + protected: + + /// Calculate set \f$ S \setminus B \f$ for shortcut calculations + std::vector separator_setminus_B(derived_ptr B) const; + + /// Calculate set \f$ S_p \cap B \f$ for shortcut calculations + std::vector parent_separator_intersection_B(derived_ptr B) const; + + /** + * Determine variable indices to keep in recursive separator shortcut calculation + * The factor graph p_Cp_B has keys from the parent clique Cp and from B. + * But we only keep the variables not in S union B. + */ + std::vector shortcut_indices(derived_ptr B, + const FactorGraph& p_Cp_B) const; + + /** Non-recursive delete cached shortcuts and marginals - internal only. */ + void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; } + + private: + + /** + * Cliques cannot be copied except by the clone() method, which does not + * copy the parent and child pointers. + */ + BayesTreeCliqueBaseUnordered(const This& other) { + assert(false); + } + + /** Cliques cannot be copied except by the clone() method, which does not + * copy the parent and child pointers. + */ + This& operator=(const This& other) { + assert(false); + return *this; + } + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(conditional_); + ar & BOOST_SERIALIZATION_NVP(parent_); + ar & BOOST_SERIALIZATION_NVP(children_); + } + + /// @} + + }; + // \struct Clique + + template + const DERIVED* asDerived( + const BayesTreeCliqueBase* base) { + return static_cast(base); + } + + template + DERIVED* asDerived(BayesTreeCliqueBase* base) { + return static_cast(base); + } + +} diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h new file mode 100644 index 000000000..ad58b6c45 --- /dev/null +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -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 EliminateableFactorGraph.h + * @brief Variable elimination algorithms for factor graphs + * @author Richard Roberts + * @date Oct 21, 2010 + */ + +#pragma once + +#include +#include + +#include + +namespace gtsam { + + /** EliminateableFactorGraph is a base class for factor graphs that contains elimination + * algorithms. Any factor graph holding eliminateable factors can derive from this class to + * expose functions for computing marginals, conditional marginals, doing full multifrontal and + * sequential elimination, etc. */ + template + class EliminateableFactorGraph { + public: + typedef EliminateableFactorGraph This; + typedef DERIVED FactorGraphType; + typedef FACTOR FactorType; + typedef BAYESNET BayesNetType; + typedef BAYESTREE BayesTreeType; + typedef typename BayesNetType::ConditionalType ConditionalType; + typedef boost::shared_ptr sharedFactor; + typedef boost::shared_ptr sharedConditional; + typedef boost::function( + std::vector, std::vector)> + Eliminate; ///< Typedef for a dense eliminate subroutine + typedef boost::optional OptionalOrdering; + + /** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not + * provided, the ordering provided by COLAMD will be used. */ + boost::shared_ptr + eliminateSequential(const Eliminate& function, OptionalOrdering ordering = boost::none) const; + + /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not + * provided, the ordering provided by COLAMD will be used. */ + boost::shared_ptr + eliminateMultifrontal(const Eliminate& function, OptionalOrdering ordering = boost::none) const; + + /** Do sequential elimination of some variables to produce a Bayes net and a remaining factor + * graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, where \f$ A = \f$ \c + * variables, \f$ X \f$ is all the variables in the factor graph, and \f$ B = X\backslash A + * \f$. */ + std::pair, boost::shared_ptr > + eliminatePartialSequential(const Eliminate& function, OptionalOrdering ordering = boost::none); + + /** Do multifrontal elimination of some variables to produce a Bayes tree and a remaining factor + * graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, where \f$ A = \f$ \c + * variables, \f$ X \f$ is all the variables in the factor graph, and \f$ B = X\backslash A + * \f$. */ + std::pair, boost::shared_ptr > + eliminatePartialMultifrontal(const Eliminate& function, OptionalOrdering ordering = boost::none); + }; + +} diff --git a/gtsam/inference/EliminationTreeUnordered.h b/gtsam/inference/EliminationTreeUnordered.h index f18188975..f0c858baa 100644 --- a/gtsam/inference/EliminationTreeUnordered.h +++ b/gtsam/inference/EliminationTreeUnordered.h @@ -19,7 +19,6 @@ #include #include -#include #include #include @@ -60,7 +59,8 @@ namespace gtsam { typedef BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals typedef typename boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional - typedef boost::function(std::vector, std::vector)> + typedef boost::function( + std::vector, std::vector)> Eliminate; ///< Typedef for an eliminate subroutine struct Node { diff --git a/gtsam/inference/OrderingUnordered.h b/gtsam/inference/OrderingUnordered.h new file mode 100644 index 000000000..d22cfde0b --- /dev/null +++ b/gtsam/inference/OrderingUnordered.h @@ -0,0 +1,27 @@ +/* ---------------------------------------------------------------------------- + + * 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 Ordering.h + * @author Richard Roberts + * @date Sep 2, 2010 + */ + +#pragma once + +#include + +#include + +namespace gtsam { + class OrderingUnordered : + typedef std::vector OrderingUnordered; +}