From f2fbd14f964c840a5961972ec47142b19ab687c9 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 6 Jun 2013 15:36:43 +0000 Subject: [PATCH] More work on BayesTree, replaced some print functions with generic tree print, moved Key, and some formatting fixes. --- gtsam/base/treeTraversal-inst.h | 26 +- gtsam/base/types.cpp | 11 + gtsam/base/types.h | 16 + gtsam/inference/BayesNetUnordered-inst.h | 53 ++ gtsam/inference/BayesNetUnordered.h | 35 +- .../inference/BayesTreeCliqueBaseUnordered.h | 14 +- gtsam/inference/BayesTreeCliqueDefault.h | 60 ++ gtsam/inference/BayesTreeUnordered-inst.h | 765 +++++++----------- gtsam/inference/BayesTreeUnordered.h | 231 ++---- .../inference/EliminateableFactorGraph-inst.h | 20 +- gtsam/inference/EliminateableFactorGraph.h | 41 +- .../inference/EliminationTreeUnordered-inst.h | 31 +- gtsam/inference/EliminationTreeUnordered.h | 7 +- gtsam/inference/FactorGraphUnordered.h | 20 +- gtsam/inference/JunctionTreeUnordered-inst.h | 2 +- gtsam/inference/JunctionTreeUnordered.h | 13 +- gtsam/inference/Key.cpp | 8 - gtsam/inference/Key.h | 13 - gtsam/inference/OrderingUnordered.h | 6 +- gtsam/symbolic/SymbolicBayesNetUnordered.cpp | 28 + gtsam/symbolic/SymbolicBayesNetUnordered.h | 13 +- gtsam/symbolic/SymbolicBayesTreeUnordered.cpp | 98 +++ gtsam/symbolic/SymbolicBayesTreeUnordered.h | 46 ++ .../symbolic/SymbolicFactorGraphUnordered.cpp | 3 + gtsam/symbolic/SymbolicFactorGraphUnordered.h | 23 +- gtsam/symbolic/tests/testSymbolicBayesNet.cpp | 99 +++ .../tests/testSymbolicFactorGraph.cpp | 116 +++ 27 files changed, 1048 insertions(+), 750 deletions(-) create mode 100644 gtsam/inference/BayesTreeCliqueDefault.h create mode 100644 gtsam/symbolic/SymbolicBayesNetUnordered.cpp create mode 100644 gtsam/symbolic/SymbolicBayesTreeUnordered.cpp create mode 100644 gtsam/symbolic/SymbolicBayesTreeUnordered.h create mode 100644 gtsam/symbolic/tests/testSymbolicBayesNet.cpp create mode 100644 gtsam/symbolic/tests/testSymbolicFactorGraph.cpp diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 4721ba50b..afd12cc51 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -23,6 +23,7 @@ namespace gtsam { /** Internal functions used for traversing trees */ namespace treeTraversal { + /* ************************************************************************* */ namespace { // Internal node used in DFS preorder stack template @@ -96,7 +97,7 @@ namespace gtsam { } else { // If not already visited, visit the node and add its children (use reverse iterators so // children are processed in the order they appear) - (void) std::for_each(node.treeNode->children.rbegin(), node.treeNode->children.rend(), + (void) std::for_each(node.treeNode.children.rbegin(), node.treeNode.children.rend(), Expander(visitorPre, node.data, stack)); node.expanded = true; } @@ -121,6 +122,8 @@ namespace gtsam { forest, rootData, visitorPre, no_op); } + + /* ************************************************************************* */ /** Traversal function for CloneForest */ namespace { template @@ -147,6 +150,27 @@ namespace gtsam { return std::vector >(rootContainer->children.begin(), rootContainer->children.end()); } + + /* ************************************************************************* */ + /** Traversal function for PrintForest */ + namespace { + template + std::string PrintForestVisitorPre(const NODE& node, const std::string& parentString, const KeyFormatter& formatter) + { + // Print the current node + node.print(parentString + "-", formatter); + // Increment the indentation + return parentString + "| "; + } + } + + /** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter. + * To print each node, this function calls the \c print function of the tree nodes. */ + template + void PrintForest(const FOREST& forest, const std::string& str, const KeyFormatter& keyFormatter) { + typedef typename FOREST::Node Node; + DepthFirstForest(forest, str, boost::bind(PrintForestVisitorPre, _1, _2, formatter)); + } } } \ No newline at end of file diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp index ea4db72c8..8b783d512 100644 --- a/gtsam/base/types.cpp +++ b/gtsam/base/types.cpp @@ -20,11 +20,22 @@ #include #include +#include namespace gtsam { + /* ************************************************************************* */ std::string _defaultIndexFormatter(Index j) { return boost::lexical_cast(j); } + /* ************************************************************************* */ + std::string _defaultKeyFormatter(Key key) { + const Symbol asSymbol(key); + if(asSymbol.chr() > 0) + return (std::string)asSymbol; + else + return boost::lexical_cast(key); + } + } \ No newline at end of file diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 94fcb4b27..4f97db7e1 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -40,6 +40,22 @@ namespace gtsam { /** The default IndexFormatter outputs the index */ static const IndexFormatter DefaultIndexFormatter = &_defaultIndexFormatter; + + /// Integer nonlinear key type + typedef size_t Key; + + /// Typedef for a function to format a key, i.e. to convert it to a string + typedef boost::function KeyFormatter; + + // Helper function for DefaultKeyFormatter + GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); + + /// The default KeyFormatter, which is used if no KeyFormatter is passed to + /// a nonlinear 'print' function. Automatically detects plain integer keys + /// and Symbol keys. + static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; + + /** * Helper class that uses templates to select between two types based on * whether TEST_TYPE is const or not. diff --git a/gtsam/inference/BayesNetUnordered-inst.h b/gtsam/inference/BayesNetUnordered-inst.h index e69de29bb..2d336bea6 100644 --- a/gtsam/inference/BayesNetUnordered-inst.h +++ b/gtsam/inference/BayesNetUnordered-inst.h @@ -0,0 +1,53 @@ +/* ---------------------------------------------------------------------------- + +* 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 BayesNet.h +* @brief Bayes network +* @author Frank Dellaert +* @author Richard Roberts +*/ + +#pragma once + +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + void BayesNetUnordered::print(const std::string& s, const KeyFormatter& formatter) const + { + Base::print(s, formatter); + } + + /* ************************************************************************* */ + template + void BayesNetUnordered::saveGraph( + const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) + { + std::ofstream of(s.c_str()); + of << "digraph G{\n"; + + BOOST_REVERSE_FOREACH(const sharedConditional& conditional, *this) { + typename CONDITIONAL::Frontals frontals = conditional->frontals(); + Index me = frontals.front(); + typename CONDITIONAL::Parents parents = conditional->parents(); + BOOST_FOREACH(Index p, parents) + of << p << "->" << me << std::endl; + } + + of << "}"; + of.close(); + } + +} diff --git a/gtsam/inference/BayesNetUnordered.h b/gtsam/inference/BayesNetUnordered.h index c4d4e5a17..0552576dc 100644 --- a/gtsam/inference/BayesNetUnordered.h +++ b/gtsam/inference/BayesNetUnordered.h @@ -15,10 +15,13 @@ * @author Frank Dellaert * @author Richard Roberts */ + #pragma once #include +#include + namespace gtsam { /** @@ -28,32 +31,36 @@ namespace gtsam { * \nosubgrouping */ template - class BayesNetUnordered { + class BayesNetUnordered : public FactorGraphUnordered { public: + typedef FactorGraphUnordered Base; typedef typename boost::shared_ptr sharedConditional; ///< A shared pointer to a conditional - /// Internal tree node that stores the conditional and the elimination parent - struct Node { - sharedConditional conditional; - boost::shared_ptr parent; - }; - - typedef boost::shared_ptr sharedNode; ///< A shared pointer to a node (used internally) - - protected: - - sharedNode roots_; ///< Tree roots - public: /// @name Standard Constructors /// @{ /** Default constructor as an empty BayesNet */ - BayesNet() {}; + BayesNetUnordered() {}; + /// @} + + /// @name Testable + /// @{ + + /** print out graph */ + void print(const std::string& s = "BayesNet", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /// @} + + /// @name Standard Interface + /// @{ + + void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; }; } \ No newline at end of file diff --git a/gtsam/inference/BayesTreeCliqueBaseUnordered.h b/gtsam/inference/BayesTreeCliqueBaseUnordered.h index 96f6fd6b7..e1aff08a6 100644 --- a/gtsam/inference/BayesTreeCliqueBaseUnordered.h +++ b/gtsam/inference/BayesTreeCliqueBaseUnordered.h @@ -131,7 +131,7 @@ namespace gtsam { const boost::optional& cachedSeparatorMarginal() const { return cachedSeparatorMarginal_; } - friend class BayesTreeUnordered; + friend class BayesTreeUnordered; protected: @@ -182,17 +182,5 @@ namespace gtsam { /// @} }; - // \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/BayesTreeCliqueDefault.h b/gtsam/inference/BayesTreeCliqueDefault.h new file mode 100644 index 000000000..d6f4c006f --- /dev/null +++ b/gtsam/inference/BayesTreeCliqueDefault.h @@ -0,0 +1,60 @@ +/* ---------------------------------------------------------------------------- + + * 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 BayesTreeCliqueDefault.h + * @brief A standard BayesTree clique. iSAM2 uses a specialized clique. + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include + +namespace gtsam { + + /* ************************************************************************* */ + /** + * A Clique in the tree is an incomplete Bayes net: the variables + * in the Bayes net are the frontal nodes, and the variables conditioned + * on are the separator. We also have pointers up and down the tree. + * + * Since our Conditional class already handles multiple frontal variables, + * this Clique contains exactly 1 conditional. + * + * This is the default clique type in a BayesTree, but some algorithms, like + * iSAM2 (see ISAM2Clique), use a different clique type in order to store + * extra data along with the clique. + */ + template + struct BayesTreeCliqueDefaultUnordered : + public BayesTreeCliqueBaseUnordered, FACTORGRAPH, BAYESNET> { + public: + typedef typename BAYESNET::ConditionalType ConditionalType; + typedef BayesTreeCliqueDefaultUnordered This; + typedef BayesTreeCliqueBaseUnordered Base; + typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; + BayesTreeCliqueDefaultUnordered() {} + BayesTreeCliqueDefaultUnordered(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {} + BayesTreeCliqueDefaultUnordered(const std::pair& result) : Base(result) {} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } + }; + +} diff --git a/gtsam/inference/BayesTreeUnordered-inst.h b/gtsam/inference/BayesTreeUnordered-inst.h index 3ebb502f2..db0f118bc 100644 --- a/gtsam/inference/BayesTreeUnordered-inst.h +++ b/gtsam/inference/BayesTreeUnordered-inst.h @@ -20,36 +20,24 @@ #pragma once -#include -#include -#include -#include -#include -#include - -#include -#include -#include +#include #include -#include // for operator += -using boost::assign::operator+=; -#include namespace gtsam { /* ************************************************************************* */ - template - typename BayesTree::CliqueData - BayesTree::getCliqueData() const { + template + typename BayesTreeUnordered::CliqueData + BayesTreeUnordered::getCliqueData() const { CliqueData data; getCliqueData(data, root_); return data; } /* ************************************************************************* */ - template - void BayesTree::getCliqueData(CliqueData& data, sharedClique clique) const { + template + void BayesTreeUnordered::getCliqueData(CliqueData& data, sharedClique clique) const { data.conditionalSizes.push_back((*clique)->nrFrontals()); data.separatorSizes.push_back((*clique)->nrParents()); BOOST_FOREACH(sharedClique c, clique->children_) { @@ -58,25 +46,25 @@ namespace gtsam { } /* ************************************************************************* */ - template - size_t BayesTree::numCachedSeparatorMarginals() const { + template + size_t BayesTreeUnordered::numCachedSeparatorMarginals() const { return (root_) ? root_->numCachedSeparatorMarginals() : 0; } /* ************************************************************************* */ - template - void BayesTree::saveGraph(const std::string &s, const IndexFormatter& indexFormatter) const { + template + void BayesTreeUnordered::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const { if (!root_.get()) throw std::invalid_argument("the root of Bayes tree has not been initialized!"); std::ofstream of(s.c_str()); of<< "digraph G{\n"; - saveGraph(of, root_, indexFormatter); + saveGraph(of, root_, keyFormatter); of<<"}"; of.close(); } /* ************************************************************************* */ - template - void BayesTree::saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, int parentnum) const { + template + void BayesTreeUnordered::saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& indexFormatter, int parentnum) const { static int num = 0; bool first = true; std::stringstream out; @@ -84,7 +72,7 @@ namespace gtsam { std::string parent = out.str(); parent += "[label=\""; - BOOST_FOREACH(Index index, clique->conditional_->frontals()) { + BOOST_FOREACH(Key index, clique->conditional_->frontals()) { if(!first) parent += ","; first = false; parent += indexFormatter(index); } @@ -95,7 +83,7 @@ namespace gtsam { } first = true; - BOOST_FOREACH(Index sep, clique->conditional_->parents()) { + BOOST_FOREACH(Key sep, clique->conditional_->parents()) { if(!first) parent += ","; first = false; parent += indexFormatter(sep); } @@ -111,19 +99,20 @@ namespace gtsam { /* ************************************************************************* */ - template - void BayesTree::CliqueStats::print(const std::string& s) const { + template + void BayesTreeUnordered::CliqueStats::print(const std::string& s) const { std::cout << s - <<"avg Conditional Size: " << avgConditionalSize << std::endl + << "avg Conditional Size: " << avgConditionalSize << std::endl << "max Conditional Size: " << maxConditionalSize << std::endl - << "avg Separator Size: " << avgSeparatorSize << std::endl - << "max Separator Size: " << maxSeparatorSize << std::endl; + << "avg Separator Size: " << avgSeparatorSize << std::endl + << "max Separator Size: " << maxSeparatorSize << std::endl; } /* ************************************************************************* */ - template - typename BayesTree::CliqueStats - BayesTree::CliqueData::getStats() const { + template + typename BayesTreeUnordered::CliqueStats + BayesTreeUnordered::CliqueData::getStats() const + { CliqueStats stats; double sum = 0.0; @@ -148,31 +137,31 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::Cliques::print(const std::string& s, const IndexFormatter& indexFormatter) const { + template + void BayesTreeUnordered::Cliques::print(const std::string& s, const KeyFormatter& keyFormatter) const { std::cout << s << ":\n"; - BOOST_FOREACH(sharedClique clique, *this) - clique->printTree("", indexFormatter); + BOOST_FOREACH(sharedClique clique, *this) { + clique->printTree("", keyFormatter); } } /* ************************************************************************* */ - template - bool BayesTree::Cliques::equals(const Cliques& other, double tol) const { + template + bool BayesTreeUnordered::Cliques::equals(const Cliques& other, double tol) const { return other == *this; } /* ************************************************************************* */ - template - typename BayesTree::sharedClique - BayesTree::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) { + template + typename BayesTreeUnordered::sharedClique + BayesTreeUnordered::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) { sharedClique new_clique(new Clique(conditional)); addClique(new_clique, parent_clique); return new_clique; } /* ************************************************************************* */ - template - void BayesTree::addClique(const sharedClique& clique, const sharedClique& parent_clique) { + template + void BayesTreeUnordered::addClique(const sharedClique& clique, const sharedClique& parent_clique) { nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size())); BOOST_FOREACH(Index j, (*clique)->frontals()) nodes_[j] = clique; @@ -187,9 +176,10 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename BayesTree::sharedClique BayesTree::addClique( - const sharedConditional& conditional, std::list& child_cliques) { + template + typename BayesTreeUnordered::sharedClique BayesTreeUnordered::addClique( + const sharedConditional& conditional, std::list& child_cliques) + { sharedClique new_clique(new Clique(conditional)); nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size())); BOOST_FOREACH(Index j, conditional->frontals()) @@ -202,60 +192,14 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::permuteWithInverse(const Permutation& inversePermutation) { - // recursively permute the cliques and internal conditionals - if (root_) - root_->permuteWithInverse(inversePermutation); - - // need to know what the largest key is to get the right number of cliques - Index maxIndex = *std::max_element(inversePermutation.begin(), inversePermutation.end()); - - // Update the nodes structure - typename BayesTree::Nodes newNodes(maxIndex+1); -// inversePermutation.applyToCollection(newNodes, nodes_); // Uses the forward, rather than inverse permutation - for(size_t i = 0; i < nodes_.size(); ++i) - newNodes[inversePermutation[i]] = nodes_[i]; - - nodes_ = newNodes; - } - - /* ************************************************************************* */ - template - inline void BayesTree::addToCliqueFront(BayesTree& bayesTree, const sharedConditional& conditional, const sharedClique& clique) { - static const bool debug = false; -#ifndef NDEBUG - // Debug check to make sure the conditional variable is ordered lower than - // its parents and that all of its parents are present either in this - // clique or its separator. - BOOST_FOREACH(Index parent, conditional->parents()) { - assert(parent > conditional->lastFrontalKey()); - const Clique& cliquer(*clique); - assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end()); - } -#endif - if(debug) conditional->print("Adding conditional "); - if(debug) clique->print("To clique "); - Index j = conditional->lastFrontalKey(); - bayesTree.nodes_.resize(std::max(j+1, bayesTree.nodes_.size())); - bayesTree.nodes_[j] = clique; - FastVector newIndices((*clique)->size() + 1); - newIndices[0] = j; - std::copy((*clique)->begin(), (*clique)->end(), newIndices.begin()+1); - clique->conditional_ = CONDITIONAL::FromKeys(newIndices, (*clique)->nrFrontals() + 1); - if(debug) clique->print("Expanded clique is "); - clique->assertInvariants(); - } - - /* ************************************************************************* */ - template - void BayesTree::removeClique(sharedClique clique) { - + template + void BayesTreeUnordered::removeClique(sharedClique clique) + { if (clique->isRoot()) root_.reset(); else { // detach clique from parent sharedClique parent = clique->parent_.lock(); - typename FastList::iterator child = std::find(parent->children().begin(), parent->children().end(), clique); + typename FastList::iterator child = std::find(parent->children().begin(), parent->children().end(), clique); assert(child != parent->children().end()); parent->children().erase(child); } @@ -264,223 +208,153 @@ namespace gtsam { BOOST_FOREACH(sharedClique child, clique->children_) child->parent_ = typename Clique::weak_ptr(); - BOOST_FOREACH(Index j, clique->conditional()->frontals()) { - nodes_[j].reset(); + BOOST_FOREACH(Key j, clique->conditional()->frontals()) { + nodes_.erase(j); } } /* ************************************************************************* */ - template - void BayesTree::recursiveTreeBuild(const boost::shared_ptr >& symbolic, - const std::vector >& conditionals, - const typename BayesTree::sharedClique& parent) { + //template + //void BayesTreeUnordered::recursiveTreeBuild(const boost::shared_ptr >& symbolic, + // const std::vector >& conditionals, + // const typename BayesTreeUnordered::sharedClique& parent) { - // Helper function to build a non-symbolic tree (e.g. Gaussian) using a - // symbolic tree, used in the BT(BN) constructor. + // // Helper function to build a non-symbolic tree (e.g. Gaussian) using a + // // symbolic tree, used in the BT(BN) constructor. - // Build the current clique - FastList cliqueConditionals; - BOOST_FOREACH(Index j, symbolic->conditional()->frontals()) { - cliqueConditionals.push_back(conditionals[j]); } - typename BayesTree::sharedClique thisClique(new CLIQUE(CONDITIONAL::Combine(cliqueConditionals.begin(), cliqueConditionals.end()))); + // // Build the current clique + // FastList cliqueConditionals; + // BOOST_FOREACH(Index j, symbolic->conditional()->frontals()) { + // cliqueConditionals.push_back(conditionals[j]); } + // typename BayesTreeUnordered::sharedClique thisClique(new CLIQUE(CONDITIONAL::Combine(cliqueConditionals.begin(), cliqueConditionals.end()))); - // Add the new clique with the current parent - this->addClique(thisClique, parent); + // // Add the new clique with the current parent + // this->addClique(thisClique, parent); - // Build the children, whose parent is the new clique - BOOST_FOREACH(const BayesTree::sharedClique& child, symbolic->children()) { - this->recursiveTreeBuild(child, conditionals, thisClique); } - } + // // Build the children, whose parent is the new clique + // BOOST_FOREACH(const BayesTreeUnordered::sharedClique& child, symbolic->children()) { + // this->recursiveTreeBuild(child, conditionals, thisClique); } + //} /* ************************************************************************* */ - template - BayesTree::BayesTree(const BayesNet& bayesNet) { - // First generate symbolic BT to determine clique structure - BayesTree sbt(bayesNet); + //template + //BayesTreeUnordered::BayesTreeUnordered(const BayesNet& bayesNet) { + // // First generate symbolic BT to determine clique structure + // BayesTreeUnordered sbt(bayesNet); - // Build index of variables to conditionals - std::vector > conditionals(sbt.root()->conditional()->frontals().back() + 1); - BOOST_FOREACH(const boost::shared_ptr& c, bayesNet) { - if(c->nrFrontals() != 1) - throw std::invalid_argument("BayesTree constructor from BayesNet only supports single frontal variable conditionals"); - if(c->firstFrontalKey() >= conditionals.size()) - throw std::invalid_argument("An inconsistent BayesNet was passed into the BayesTree constructor!"); - if(conditionals[c->firstFrontalKey()]) - throw std::invalid_argument("An inconsistent BayesNet with duplicate frontal variables was passed into the BayesTree constructor!"); + // // Build index of variables to conditionals + // std::vector > conditionals(sbt.root()->conditional()->frontals().back() + 1); + // BOOST_FOREACH(const boost::shared_ptr& c, bayesNet) { + // if(c->nrFrontals() != 1) + // throw std::invalid_argument("BayesTreeUnordered constructor from BayesNet only supports single frontal variable conditionals"); + // if(c->firstFrontalKey() >= conditionals.size()) + // throw std::invalid_argument("An inconsistent BayesNet was passed into the BayesTreeUnordered constructor!"); + // if(conditionals[c->firstFrontalKey()]) + // throw std::invalid_argument("An inconsistent BayesNet with duplicate frontal variables was passed into the BayesTreeUnordered constructor!"); - conditionals[c->firstFrontalKey()] = c; - } + // conditionals[c->firstFrontalKey()] = c; + // } - // Build the new tree - this->recursiveTreeBuild(sbt.root(), conditionals, sharedClique()); - } + // // Build the new tree + // this->recursiveTreeBuild(sbt.root(), conditionals, sharedClique()); + //} /* ************************************************************************* */ - template<> - inline BayesTree::BayesTree(const BayesNet& bayesNet) { - BayesNet::const_reverse_iterator rit; - for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) - insert(*this, *rit); - } + //template<> + //inline BayesTreeUnordered::BayesTreeUnordered(const BayesNet& bayesNet) { + // BayesNet::const_reverse_iterator rit; + // for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) + // insert(*this, *rit); + //} /* ************************************************************************* */ - template - BayesTree::BayesTree(const BayesNet& bayesNet, std::list > subtrees) { - if (bayesNet.size() == 0) - throw std::invalid_argument("BayesTree::insert: empty bayes net!"); + //template + //BayesTreeUnordered::BayesTreeUnordered(const BayesNet& bayesNet, std::list > subtrees) { + // if (bayesNet.size() == 0) + // throw std::invalid_argument("BayesTreeUnordered::insert: empty bayes net!"); - // get the roots of child subtrees and merge their nodes_ - std::list childRoots; - typedef BayesTree Tree; - BOOST_FOREACH(const Tree& subtree, subtrees) { - nodes_.assign(subtree.nodes_.begin(), subtree.nodes_.end()); - childRoots.push_back(subtree.root()); - } + // // get the roots of child subtrees and merge their nodes_ + // std::list childRoots; + // typedef BayesTreeUnordered Tree; + // BOOST_FOREACH(const Tree& subtree, subtrees) { + // nodes_.assign(subtree.nodes_.begin(), subtree.nodes_.end()); + // childRoots.push_back(subtree.root()); + // } - // create a new clique and add all the conditionals to the clique - sharedClique new_clique; - typename BayesNet::sharedConditional conditional; - BOOST_REVERSE_FOREACH(conditional, bayesNet) { - if (!new_clique.get()) - new_clique = addClique(conditional,childRoots); - else - addToCliqueFront(*this, conditional, new_clique); - } + // // create a new clique and add all the conditionals to the clique + // sharedClique new_clique; + // typename BayesNet::sharedConditional conditional; + // BOOST_REVERSE_FOREACH(conditional, bayesNet) { + // if (!new_clique.get()) + // new_clique = addClique(conditional,childRoots); + // else + // addToCliqueFront(*this, conditional, new_clique); + // } - root_ = new_clique; - } + // root_ = new_clique; + //} /* ************************************************************************* */ - template - BayesTree::BayesTree(const This& other) { + template + BayesTreeUnordered::BayesTreeUnordered(const This& other) { *this = other; } /* ************************************************************************* */ - template - BayesTree& BayesTree::operator=(const This& other) { + template + BayesTreeUnordered& BayesTreeUnordered::operator=(const This& other) { this->clear(); other.cloneTo(*this); return *this; } /* ************************************************************************* */ - template - void BayesTree::print(const std::string& s, const IndexFormatter& indexFormatter) const { - if (root_.use_count() == 0) { - std::cout << "WARNING: BayesTree.print encountered a forest..." << std::endl; - return; - } - std::cout << s << ": clique size == " << size() << ", node size == " << nodes_.size() << std::endl; - if (nodes_.empty()) return; - root_->printTree("", indexFormatter); + template + void BayesTreeUnordered::print(const std::string& s, const KeyFormatter& keyFormatter) const { + std::cout << s << ": cliques: " << size() << ", variables: " << nodes_.size() << std::endl; + treeTraversal::PrintForest(*this, s, keyFormatter) } /* ************************************************************************* */ // binary predicate to test equality of a pair for use in equals - template + template bool check_sharedCliques( - const typename BayesTree::sharedClique& v1, - const typename BayesTree::sharedClique& v2 + const typename BayesTreeUnordered::sharedClique& v1, + const typename BayesTreeUnordered::sharedClique& v2 ) { return (!v1 && !v2) || (v1 && v2 && v1->equals(*v2)); } /* ************************************************************************* */ - template - bool BayesTree::equals(const BayesTree& other, - double tol) const { + template + bool BayesTreeUnordered::equals(const BayesTreeUnordered& other, double tol) const { return size()==other.size() && - std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques); + std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques); } /* ************************************************************************* */ - template + template template - inline Index BayesTree::findParentClique(const CONTAINER& parents) const { + Key BayesTreeUnordered::findParentClique(const CONTAINER& parents) const { typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end()); assert(lowestOrderedParent != parents.end()); return *lowestOrderedParent; } /* ************************************************************************* */ - template - void BayesTree::insert(BayesTree& bayesTree, const sharedConditional& conditional) - { - static const bool debug = false; - - // get indices and parents - const typename CONDITIONAL::Parents& parents = conditional->parents(); - - if(debug) conditional->print("Adding conditional "); - - // if no parents, start a new root clique - if (parents.empty()) { - if(debug) std::cout << "No parents so making root" << std::endl; - bayesTree.root_ = bayesTree.addClique(conditional); - return; - } - - // otherwise, find the parent clique by using the index data structure - // to find the lowest-ordered parent - Index parentRepresentative = bayesTree.findParentClique(parents); - if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl; - sharedClique parent_clique = bayesTree[parentRepresentative]; - if(debug) parent_clique->print("Parent clique is "); - - // if the parents and parent clique have the same size, add to parent clique - if ((*parent_clique)->size() == size_t(parents.size())) { - if(debug) std::cout << "Adding to parent clique" << std::endl; -#ifndef NDEBUG - // Debug check that the parent indices of the new conditional match the indices - // currently in the clique. -// list::const_iterator parent = parents.begin(); -// typename Clique::const_iterator cond = parent_clique->begin(); -// while(parent != parents.end()) { -// assert(cond != parent_clique->end()); -// assert(*parent == (*cond)->key()); -// ++ parent; -// ++ cond; -// } -#endif - addToCliqueFront(bayesTree, conditional, parent_clique); - } else { - if(debug) std::cout << "Starting new clique" << std::endl; - // otherwise, start a new clique and add it to the tree - bayesTree.addClique(conditional,parent_clique); - } - } - - /* ************************************************************************* */ - //TODO: remove this function after removing TSAM.cpp - template - typename BayesTree::sharedClique BayesTree::insert( - const sharedConditional& clique, std::list& children, bool isRootClique) { - - if (clique->nrFrontals() == 0) - throw std::invalid_argument("BayesTree::insert: empty clique!"); - - // create a new clique and add all the conditionals to the clique - sharedClique new_clique = addClique(clique, children); - if (isRootClique) root_ = new_clique; - - return new_clique; - } - - /* ************************************************************************* */ - template - void BayesTree::fillNodesIndex(const sharedClique& subtree) { + template + void BayesTreeUnordered::fillNodesIndex(const sharedClique& subtree) { // Add each frontal variable of this root node - BOOST_FOREACH(const Index& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; } + BOOST_FOREACH(const Key& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; } // Fill index for each child - typedef typename BayesTree::sharedClique sharedClique; + typedef typename BayesTreeUnordered::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& child, subtree->children_) { fillNodesIndex(child); } } /* ************************************************************************* */ - template - void BayesTree::insert(const sharedClique& subtree) { + template + void BayesTreeUnordered::insert(const sharedClique& subtree) { if(subtree) { // Find the parent clique of the new subtree. By the running intersection // property, those separator variables in the subtree that are ordered @@ -490,17 +364,13 @@ namespace gtsam { assert(!root_); root_ = subtree; } else { - Index parentRepresentative = findParentClique(subtree->conditional()->parents()); + Key parentRepresentative = findParentClique(subtree->conditional()->parents()); sharedClique parent = (*this)[parentRepresentative]; parent->children_ += subtree; subtree->parent_ = parent; // set new parent! } // Now fill in the nodes index - if(nodes_.size() == 0 || - *std::max_element(subtree->conditional()->beginFrontals(), subtree->conditional()->endFrontals()) > (nodes_.size() - 1)) { - nodes_.resize(subtree->conditional()->lastFrontalKey() + 1); - } fillNodesIndex(subtree); } } @@ -508,209 +378,211 @@ namespace gtsam { /* ************************************************************************* */ // First finds clique marginal then marginalizes that /* ************************************************************************* */ - template - typename CONDITIONAL::FactorType::shared_ptr BayesTree::marginalFactor( - Index j, Eliminate function) const + template + typename BayesTreeUnordered::sharedFactor BayesTreeUnordered::marginalFactor( + Key j, Eliminate function) const { - gttic(BayesTree_marginalFactor); - - // get clique containing Index j - sharedClique clique = (*this)[j]; - - // calculate or retrieve its marginal P(C) = P(F,S) -#ifdef OLD_SHORTCUT_MARGINALS - FactorGraph cliqueMarginal = clique->marginal(root_,function); -#else - FactorGraph cliqueMarginal = clique->marginal2(root_,function); -#endif - - // Reduce the variable indices to start at zero - gttic(Reduce); - const Permutation reduction = internal::createReducingPermutation(cliqueMarginal.keys()); - internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, cliqueMarginal) { - if(factor) factor->reduceWithInverse(inverseReduction); } - gttoc(Reduce); - - // now, marginalize out everything that is not variable j - GenericSequentialSolver solver(cliqueMarginal); - boost::shared_ptr result = solver.marginalFactor(inverseReduction[j], function); - - // Undo the reduction - gttic(Undo_Reduce); - result->permuteWithInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, cliqueMarginal) { - if(factor) factor->permuteWithInverse(reduction); } - gttoc(Undo_Reduce); - return result; + return boost::make_shared(); +// gttic(BayesTree_marginalFactor); +// +// // get clique containing Index j +// sharedClique clique = this->clique(j); +// +// // calculate or retrieve its marginal P(C) = P(F,S) +//#ifdef OLD_SHORTCUT_MARGINALS +// FactorGraph cliqueMarginal = clique->marginal(root_,function); +//#else +// FactorGraph cliqueMarginal = clique->marginal2(root_,function); +//#endif +// +// // Reduce the variable indices to start at zero +// gttic(Reduce); +// const Permutation reduction = internal::createReducingPermutation(cliqueMarginal.keys()); +// internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); +// BOOST_FOREACH(const boost::shared_ptr& factor, cliqueMarginal) { +// if(factor) factor->reduceWithInverse(inverseReduction); } +// gttoc(Reduce); +// +// // now, marginalize out everything that is not variable j +// GenericSequentialSolver solver(cliqueMarginal); +// boost::shared_ptr result = solver.marginalFactor(inverseReduction[j], function); +// +// // Undo the reduction +// gttic(Undo_Reduce); +// result->permuteWithInverse(reduction); +// BOOST_FOREACH(const boost::shared_ptr& factor, cliqueMarginal) { +// if(factor) factor->permuteWithInverse(reduction); } +// gttoc(Undo_Reduce); +// return result; } /* ************************************************************************* */ - template - typename BayesNet::shared_ptr BayesTree::marginalBayesNet( - Index j, Eliminate function) const + template + typename BayesTreeUnordered::sharedBayesNet BayesTreeUnordered::marginalBayesNet( + Key j, Eliminate function) const { - gttic(BayesTree_marginalBayesNet); + return boost::make_shared(); + //gttic(BayesTree_marginalBayesNet); - // calculate marginal as a factor graph - FactorGraph fg; - fg.push_back(this->marginalFactor(j,function)); + //// calculate marginal as a factor graph + //FactorGraph fg; + //fg.push_back(this->marginalFactor(j,function)); - // Reduce the variable indices to start at zero - gttic(Reduce); - const Permutation reduction = internal::createReducingPermutation(fg.keys()); - internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, fg) { - if(factor) factor->reduceWithInverse(inverseReduction); } - gttoc(Reduce); + //// Reduce the variable indices to start at zero + //gttic(Reduce); + //const Permutation reduction = internal::createReducingPermutation(fg.keys()); + //internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); + //BOOST_FOREACH(const boost::shared_ptr& factor, fg) { + // if(factor) factor->reduceWithInverse(inverseReduction); } + //gttoc(Reduce); - // eliminate factor graph marginal to a Bayes net - boost::shared_ptr > bn = GenericSequentialSolver(fg).eliminate(function); + //// eliminate factor graph marginal to a Bayes net + //boost::shared_ptr > bn = GenericSequentialSolver(fg).eliminate(function); - // Undo the reduction - gttic(Undo_Reduce); - bn->permuteWithInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, fg) { - if(factor) factor->permuteWithInverse(reduction); } - gttoc(Undo_Reduce); - return bn; + //// Undo the reduction + //gttic(Undo_Reduce); + //bn->permuteWithInverse(reduction); + //BOOST_FOREACH(const boost::shared_ptr& factor, fg) { + // if(factor) factor->permuteWithInverse(reduction); } + //gttoc(Undo_Reduce); + //return bn; } /* ************************************************************************* */ // Find two cliques, their joint, then marginalizes /* ************************************************************************* */ - template - typename FactorGraph::shared_ptr - BayesTree::joint(Index j1, Index j2, Eliminate function) const { - gttic(BayesTree_joint); + template + typename BayesTreeUnordered::sharedFactorGraph + BayesTreeUnordered::joint(Key j1, Key j2, Eliminate function) const { + return boost::make_shared(); + //gttic(BayesTree_joint); - // get clique C1 and C2 - sharedClique C1 = (*this)[j1], C2 = (*this)[j2]; + //// get clique C1 and C2 + //sharedClique C1 = (*this)[j1], C2 = (*this)[j2]; - gttic(Lowest_common_ancestor); - // Find lowest common ancestor clique - sharedClique B; { - // Build two paths to the root - FastList path1, path2; { - sharedClique p = C1; - while(p) { - path1.push_front(p); - p = p->parent(); - } - } { - sharedClique p = C2; - while(p) { - path2.push_front(p); - p = p->parent(); - } - } - // Find the path intersection - B = this->root(); - typename FastList::const_iterator p1 = path1.begin(), p2 = path2.begin(); - while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) { - B = *p1; - ++p1; - ++p2; - } - } - gttoc(Lowest_common_ancestor); + //gttic(Lowest_common_ancestor); + //// Find lowest common ancestor clique + //sharedClique B; { + // // Build two paths to the root + // FastList path1, path2; { + // sharedClique p = C1; + // while(p) { + // path1.push_front(p); + // p = p->parent(); + // } + // } { + // sharedClique p = C2; + // while(p) { + // path2.push_front(p); + // p = p->parent(); + // } + // } + // // Find the path intersection + // B = this->root(); + // typename FastList::const_iterator p1 = path1.begin(), p2 = path2.begin(); + // while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) { + // B = *p1; + // ++p1; + // ++p2; + // } + //} + //gttoc(Lowest_common_ancestor); - // Compute marginal on lowest common ancestor clique - gttic(LCA_marginal); - FactorGraph p_B = B->marginal2(this->root(), function); - gttoc(LCA_marginal); + //// Compute marginal on lowest common ancestor clique + //gttic(LCA_marginal); + //FactorGraph p_B = B->marginal2(this->root(), function); + //gttoc(LCA_marginal); - // Compute shortcuts of the requested cliques given the lowest common ancestor - gttic(Clique_shortcuts); - BayesNet p_C1_Bred = C1->shortcut(B, function); - BayesNet p_C2_Bred = C2->shortcut(B, function); - gttoc(Clique_shortcuts); + //// Compute shortcuts of the requested cliques given the lowest common ancestor + //gttic(Clique_shortcuts); + //BayesNet p_C1_Bred = C1->shortcut(B, function); + //BayesNet p_C2_Bred = C2->shortcut(B, function); + //gttoc(Clique_shortcuts); - // Factor the shortcuts to be conditioned on the full root - // Get the set of variables to eliminate, which is C1\B. - gttic(Full_root_factoring); - sharedConditional p_C1_B; { - std::vector C1_minus_B; { - FastSet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); - BOOST_FOREACH(const Index j, *B->conditional()) { - C1_minus_B_set.erase(j); } - C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end()); - } - // Factor into C1\B | B. - FactorGraph temp_remaining; - boost::tie(p_C1_B, temp_remaining) = FactorGraph(p_C1_Bred).eliminate(C1_minus_B, function); - } - sharedConditional p_C2_B; { - std::vector C2_minus_B; { - FastSet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); - BOOST_FOREACH(const Index j, *B->conditional()) { - C2_minus_B_set.erase(j); } - C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end()); - } - // Factor into C2\B | B. - FactorGraph temp_remaining; - boost::tie(p_C2_B, temp_remaining) = FactorGraph(p_C2_Bred).eliminate(C2_minus_B, function); - } - gttoc(Full_root_factoring); + //// Factor the shortcuts to be conditioned on the full root + //// Get the set of variables to eliminate, which is C1\B. + //gttic(Full_root_factoring); + //sharedConditional p_C1_B; { + // std::vector C1_minus_B; { + // FastSet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); + // BOOST_FOREACH(const Index j, *B->conditional()) { + // C1_minus_B_set.erase(j); } + // C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end()); + // } + // // Factor into C1\B | B. + // FactorGraph temp_remaining; + // boost::tie(p_C1_B, temp_remaining) = FactorGraph(p_C1_Bred).eliminate(C1_minus_B, function); + //} + //sharedConditional p_C2_B; { + // std::vector C2_minus_B; { + // FastSet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); + // BOOST_FOREACH(const Index j, *B->conditional()) { + // C2_minus_B_set.erase(j); } + // C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end()); + // } + // // Factor into C2\B | B. + // FactorGraph temp_remaining; + // boost::tie(p_C2_B, temp_remaining) = FactorGraph(p_C2_Bred).eliminate(C2_minus_B, function); + //} + //gttoc(Full_root_factoring); - gttic(Variable_joint); - // Build joint on all involved variables - FactorGraph p_BC1C2; - p_BC1C2.push_back(p_B); - p_BC1C2.push_back(p_C1_B->toFactor()); - p_BC1C2.push_back(p_C2_B->toFactor()); - if(C1 != B) - p_BC1C2.push_back(C1->conditional()->toFactor()); - if(C2 != B) - p_BC1C2.push_back(C2->conditional()->toFactor()); + //gttic(Variable_joint); + //// Build joint on all involved variables + //FactorGraph p_BC1C2; + //p_BC1C2.push_back(p_B); + //p_BC1C2.push_back(p_C1_B->toFactor()); + //p_BC1C2.push_back(p_C2_B->toFactor()); + //if(C1 != B) + // p_BC1C2.push_back(C1->conditional()->toFactor()); + //if(C2 != B) + // p_BC1C2.push_back(C2->conditional()->toFactor()); - // Reduce the variable indices to start at zero - gttic(Reduce); - const Permutation reduction = internal::createReducingPermutation(p_BC1C2.keys()); - internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, p_BC1C2) { - if(factor) factor->reduceWithInverse(inverseReduction); } - std::vector js; js.push_back(inverseReduction[j1]); js.push_back(inverseReduction[j2]); - gttoc(Reduce); + //// Reduce the variable indices to start at zero + //gttic(Reduce); + //const Permutation reduction = internal::createReducingPermutation(p_BC1C2.keys()); + //internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); + //BOOST_FOREACH(const boost::shared_ptr& factor, p_BC1C2) { + // if(factor) factor->reduceWithInverse(inverseReduction); } + //std::vector js; js.push_back(inverseReduction[j1]); js.push_back(inverseReduction[j2]); + //gttoc(Reduce); - // now, marginalize out everything that is not variable j - GenericSequentialSolver solver(p_BC1C2); - boost::shared_ptr > result = solver.jointFactorGraph(js, function); - - // Undo the reduction - gttic(Undo_Reduce); - BOOST_FOREACH(const boost::shared_ptr& factor, *result) { - if(factor) factor->permuteWithInverse(reduction); } - BOOST_FOREACH(const boost::shared_ptr& factor, p_BC1C2) { - if(factor) factor->permuteWithInverse(reduction); } - gttoc(Undo_Reduce); - return result; + //// now, marginalize out everything that is not variable j + //GenericSequentialSolver solver(p_BC1C2); + //boost::shared_ptr > result = solver.jointFactorGraph(js, function); + //// Undo the reduction + //gttic(Undo_Reduce); + //BOOST_FOREACH(const boost::shared_ptr& factor, *result) { + // if(factor) factor->permuteWithInverse(reduction); } + //BOOST_FOREACH(const boost::shared_ptr& factor, p_BC1C2) { + // if(factor) factor->permuteWithInverse(reduction); } + //gttoc(Undo_Reduce); + //return result; } /* ************************************************************************* */ - template - typename BayesNet::shared_ptr BayesTree::jointBayesNet( - Index j1, Index j2, Eliminate function) const { - - // eliminate factor graph marginal to a Bayes net - return GenericSequentialSolver ( - *this->joint(j1, j2, function)).eliminate(function); + template + typename BayesTreeUnordered::sharedBayesNet BayesTreeUnordered::jointBayesNet( + Key j1, Key j2, Eliminate function) const + { + return boost::make_shared(); + //// eliminate factor graph marginal to a Bayes net + //return GenericSequentialSolver ( + // *this->joint(j1, j2, function)).eliminate(function); } /* ************************************************************************* */ - template - void BayesTree::clear() { + template + void BayesTreeUnordered::clear() { // Remove all nodes and clear the root pointer nodes_.clear(); root_.reset(); } /* ************************************************************************* */ - template - void BayesTree::removePath(sharedClique clique, - BayesNet& bn, typename BayesTree::Cliques& orphans) { - + template + void BayesTreeUnordered::removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans) + { // base case is NULL, if so we do nothing and return empties above if (clique!=NULL) { @@ -732,13 +604,12 @@ namespace gtsam { } /* ************************************************************************* */ - template + template template - void BayesTree::removeTop(const CONTAINER& keys, - BayesNet& bn, typename BayesTree::Cliques& orphans) { - + void BayesTreeUnordered::removeTop(const CONTAINER& keys, BayesNetType& bn, Cliques& orphans) + { // process each key of the new factor - BOOST_FOREACH(const Index& j, keys) { + BOOST_FOREACH(const Key& j, keys) { // get the clique if(j < nodes_.size()) { @@ -757,8 +628,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename BayesTree::Cliques BayesTree::removeSubtree( + template + typename BayesTreeUnordered::Cliques BayesTreeUnordered::removeSubtree( const sharedClique& subtree) { // Result clique list @@ -782,8 +653,8 @@ namespace gtsam { (*clique)->deleteCachedShortcutsNonRecursive(); // Remove this node from the nodes index - BOOST_FOREACH(Index j, (*clique)->conditional()->frontals()) { - nodes_[j].reset(); } + BOOST_FOREACH(Key j, (*clique)->conditional()->frontals()) { + nodes_.erase(j); } // Erase the parent and children pointers (*clique)->parent_.reset(); @@ -793,23 +664,5 @@ namespace gtsam { return cliques; } - /* ************************************************************************* */ - template - void BayesTree::cloneTo(This& newTree) const { - if(root()) - cloneTo(newTree, root(), sharedClique()); - } - - /* ************************************************************************* */ - template - void BayesTree::cloneTo( - This& newTree, const sharedClique& subtree, const sharedClique& parent) const { - sharedClique newClique(subtree->clone()); - newTree.addClique(newClique, parent); - BOOST_FOREACH(const sharedClique& childClique, subtree->children()) { - cloneTo(newTree, childClique, newClique); - } - } - } /// namespace gtsam diff --git a/gtsam/inference/BayesTreeUnordered.h b/gtsam/inference/BayesTreeUnordered.h index 5e35b3f09..7411bc4de 100644 --- a/gtsam/inference/BayesTreeUnordered.h +++ b/gtsam/inference/BayesTreeUnordered.h @@ -20,26 +20,14 @@ #pragma once #include -#include -#include -#include -#include -#include -#include +#include #include #include -#include -#include -#include -#include -#include +#include namespace gtsam { - // Forward declaration of BayesTreeClique which is defined below BayesTree in this file - template struct BayesTreeClique; - /** * Bayes tree * @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain, @@ -50,28 +38,29 @@ namespace gtsam { * \addtogroup Multifrontal * \nosubgrouping */ - template > - class BayesTree { + template + class BayesTreeUnordered { public: - typedef BayesTree This; - typedef boost::shared_ptr > shared_ptr; - typedef boost::shared_ptr sharedConditional; - typedef boost::shared_ptr > sharedBayesNet; - typedef CONDITIONAL ConditionalType; - typedef typename CONDITIONAL::FactorType FactorType; - typedef typename FactorGraph::Eliminate Eliminate; - + typedef BayesTreeUnordered This; + typedef boost::shared_ptr shared_ptr; typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique + typedef boost::shared_ptr sharedClique; ///< Shared pointer to a clique + typedef typename CLIQUE::ConditionalType ConditionalType; + typedef boost::shared_ptr sharedConditional; + typedef typename CLIQUE::BayesNetType BayesNetType; + typedef boost::shared_ptr sharedBayesNet; + typedef typename CLIQUE::FactorType FactorType; + typedef boost::shared_ptr sharedFactor; + typedef typename CLIQUE::FactorGraphType FactorGraphType; + typedef boost::shared_ptr sharedFactorGraph; + typedef typename FactorGraphType::Eliminate Eliminate; - // typedef for shared pointers to cliques - typedef boost::shared_ptr sharedClique; - - // A convenience class for a list of shared cliques + /** A convenience class for a list of shared cliques */ struct Cliques : public FastList { void print(const std::string& s = "Cliques", - const IndexFormatter& indexFormatter = DefaultIndexFormatter) const; + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; bool equals(const Cliques& other, double tol = 1e-9) const; }; @@ -91,16 +80,16 @@ namespace gtsam { CliqueStats getStats() const; }; - /** Map from indices to Clique */ - typedef std::vector Nodes; + /** Map from keys to Clique */ + typedef FastMap Nodes; protected: /** Map from indices to Clique */ Nodes nodes_; - /** Root clique */ - sharedClique root_; + /** Root cliques */ + std::vector roots_; public: @@ -108,13 +97,10 @@ namespace gtsam { /// @{ /** Create an empty Bayes Tree */ - BayesTree() {} - - /** Create a Bayes Tree from a Bayes Net (requires CONDITIONAL is IndexConditional *or* CONDITIONAL::Combine) */ - explicit BayesTree(const BayesNet& bayesNet); + BayesTreeUnordered() {} /** Copy constructor */ - BayesTree(const This& other); + BayesTreeUnordered(const This& other); /** Assignment operator */ This& operator=(const This& other); @@ -123,37 +109,24 @@ namespace gtsam { /// @name Advanced Constructors /// @{ - /** - * Create a Bayes Tree from a Bayes Net and some subtrees. The Bayes net corresponds to the - * new root clique and the subtrees are connected to the root clique. - */ - BayesTree(const BayesNet& bayesNet, std::list > subtrees); - /** Destructor */ - virtual ~BayesTree() {} + virtual ~BayesTreeUnordered() {} /// @} /// @name Testable /// @{ /** check equality */ - bool equals(const BayesTree& other, double tol = 1e-9) const; + bool equals(const This& other, double tol = 1e-9) const; /** print */ void print(const std::string& s = "", - const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const; + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /// @} /// @name Standard Interface /// @{ - /** - * Find parent clique of a conditional. It will look at all parents and - * return the one with the lowest index in the ordering. - */ - template - Index findParentClique(const CONTAINER& parents) const; - /** number of cliques */ inline size_t size() const { if(root_) @@ -170,17 +143,16 @@ namespace gtsam { /** return nodes */ const Nodes& nodes() const { return nodes_; } - /** return root clique */ - const sharedClique& root() const { return root_; } - - /** find the clique that contains the variable with Index j */ - inline sharedClique operator[](Index j) const { - return nodes_.at(j); - } + /** return root cliques */ + const std::vector& roots() const { return roots_; } /** alternate syntax for matlab: find the clique that contains the variable with Index j */ - inline sharedClique clique(Index j) const { - return nodes_.at(j); + const sharedClique& clique(Key j) const { + Nodes::const_iterator c = nodes_.find(j); + if(c == nodes_.end()) + throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree"); + else + return *c; } /** Gather data on all cliques */ @@ -190,43 +162,44 @@ namespace gtsam { size_t numCachedSeparatorMarginals() const; /** return marginal on any variable */ - typename FactorType::shared_ptr marginalFactor(Index j, Eliminate function) const; + sharedFactor marginalFactor(Key j, Eliminate function) const; /** * return marginal on any variable, as a Bayes Net * NOTE: this function calls marginal, and then eliminates it into a Bayes Net * This is more expensive than the above function */ - typename BayesNet::shared_ptr marginalBayesNet(Index j, Eliminate function) const; + sharedBayesNet marginalBayesNet(Key j, Eliminate function) const; /** * return joint on two variables * Limitation: can only calculate joint if cliques are disjoint or one of them is root */ - typename FactorGraph::shared_ptr joint(Index j1, Index j2, Eliminate function) const; + sharedFactorGraph joint(Index j1, Index j2, Eliminate function) const; /** * return joint on two variables as a BayesNet * Limitation: can only calculate joint if cliques are disjoint or one of them is root */ - typename BayesNet::shared_ptr jointBayesNet(Index j1, Index j2, Eliminate function) const; + sharedBayesNet jointBayesNet(Index j1, Index j2, Eliminate function) const; /** * Read only with side effects */ /** saves the Tree to a text file in GraphViz format */ - void saveGraph(const std::string& s, const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const; + void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /// @} /// @name Advanced Interface /// @{ - - /** Access the root clique (non-const version) */ - sharedClique& root() { return root_; } - - /** Access the nodes (non-cost version) */ - Nodes& nodes() { return nodes_; } + + /** + * Find parent clique of a conditional. It will look at all parents and + * return the one with the lowest index in the ordering. + */ + template + Index findParentClique(const CONTAINER& parents) const; /** Remove all nodes */ void clear(); @@ -236,21 +209,18 @@ namespace gtsam { root_->deleteCachedShortcuts(); } - /** Apply a permutation to the full tree - also updates the nodes structure */ - void permuteWithInverse(const Permutation& inversePermutation); - /** * Remove path from clique to root and return that path as factors * plus a list of orphaned subtree roots. Used in removeTop below. */ - void removePath(sharedClique clique, BayesNet& bn, Cliques& orphans); + void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans); /** * Given a list of indices, turn "contaminated" part of the tree back into a factor graph. * Factors and orphans are added to the in/out arguments. */ template - void removeTop(const CONTAINER& indices, BayesNet& bn, Cliques& orphans); + void removeTop(const CONTAINER& keys, BayesNetType& bn, Cliques& orphans); /** * Remove the requested subtree. */ @@ -265,20 +235,6 @@ namespace gtsam { */ void insert(const sharedClique& subtree); - /** Insert a new conditional - * This function only applies for Symbolic case with IndexCondtional, - * We make it static so that it won't be compiled in GaussianConditional case. - * */ - static void insert(BayesTree& bayesTree, const sharedConditional& conditional); - - /** - * Insert a new clique corresponding to the given Bayes net. - * It is the caller's responsibility to decide whether the given Bayes net is a valid clique, - * i.e. all the variables (frontal and separator) are connected - */ - sharedClique insert(const sharedConditional& clique, - std::list& children, bool isRootClique = false); - /** * Create a clone of this object as a shared pointer * Necessary for inheritance in matlab interface @@ -290,7 +246,7 @@ namespace gtsam { protected: /** private helper method for saving the Tree to a text file in GraphViz format */ - void saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, + void saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter, int parentnum = 0) const; /** Gather data on a single clique */ @@ -308,32 +264,17 @@ namespace gtsam { /** add a clique (bottom up) */ sharedClique addClique(const sharedConditional& conditional, std::list& child_cliques); - /** - * Add a conditional to the front of a clique, i.e. a conditional whose - * parents are already in the clique or its separators. This function does - * not check for this condition, it just updates the data structures. - */ - static void addToCliqueFront(BayesTree& bayesTree, - const sharedConditional& conditional, const sharedClique& clique); - /** Fill the nodes index for a subtree */ void fillNodesIndex(const sharedClique& subtree); /** Helper function to build a non-symbolic tree (e.g. Gaussian) using a * symbolic tree, used in the BT(BN) constructor. */ - void recursiveTreeBuild(const boost::shared_ptr >& symbolic, - const std::vector >& conditionals, - const typename BayesTree::sharedClique& parent); + //void recursiveTreeBuild(const boost::shared_ptr >& symbolic, + // const std::vector >& conditionals, + // const typename BayesTree::sharedClique& parent); private: - - /** deep copy to another tree */ - void cloneTo(This& newTree) const; - - /** deep copy to another tree */ - void cloneTo(This& newTree, const sharedClique& subtree, const sharedClique& parent) const; - /** Serialization function */ friend class boost::serialization::access; template @@ -346,70 +287,4 @@ namespace gtsam { }; // BayesTree - - /* ************************************************************************* */ - template - void _BayesTree_dim_adder( - std::vector& dims, - const typename BayesTree::sharedClique& clique) { - - if(clique) { - // Add dims from this clique - for(typename CONDITIONAL::const_iterator it = (*clique)->beginFrontals(); it != (*clique)->endFrontals(); ++it) - dims[*it] = (*clique)->dim(it); - - // Traverse children - typedef typename BayesTree::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& child, clique->children()) { - _BayesTree_dim_adder(dims, child); - } - } - } - - /* ************************************************************************* */ - template - boost::shared_ptr allocateVectorValues(const BayesTree& bt) { - std::vector dimensions(bt.nodes().size(), 0); - _BayesTree_dim_adder(dimensions, bt.root()); - return boost::shared_ptr(new VectorValues(dimensions)); - } - - - /* ************************************************************************* */ - /** - * A Clique in the tree is an incomplete Bayes net: the variables - * in the Bayes net are the frontal nodes, and the variables conditioned - * on are the separator. We also have pointers up and down the tree. - * - * Since our Conditional class already handles multiple frontal variables, - * this Clique contains exactly 1 conditional. - * - * This is the default clique type in a BayesTree, but some algorithms, like - * iSAM2 (see ISAM2Clique), use a different clique type in order to store - * extra data along with the clique. - */ - template - struct BayesTreeClique : public BayesTreeCliqueBase, CONDITIONAL> { - public: - typedef CONDITIONAL ConditionalType; - typedef BayesTreeClique This; - typedef BayesTreeCliqueBase Base; - typedef boost::shared_ptr shared_ptr; - typedef boost::weak_ptr weak_ptr; - BayesTreeClique() {} - BayesTreeClique(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {} - BayesTreeClique(const std::pair& result) : Base(result) {} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - } - }; - } /// namespace gtsam - -#include -#include diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 5d45a796c..b842a4644 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -24,13 +24,15 @@ namespace gtsam { /* ************************************************************************* */ - template - boost::shared_ptr - EliminateableFactorGraph::eliminateSequential( + template + boost::shared_ptr + EliminateableFactorGraph:: + eliminateSequential( const Eliminate& function, OptionalOrdering ordering, const VariableIndexUnordered& variableIndex) const { // Do elimination - std::pair, boost::shared_ptr > result; + std::pair, boost::shared_ptr > result; if(ordering) { // Do elimination with given ordering result = EliminationTreeType(*this, variableIndex, *ordering).eliminate(function); @@ -49,13 +51,15 @@ namespace gtsam { } /* ************************************************************************* */ - template - boost::shared_ptr - EliminateableFactorGraph::eliminateMultifrontal( + template + boost::shared_ptr + EliminateableFactorGraph:: + eliminateMultifrontal( const Eliminate& function, OptionalOrdering ordering, const VariableIndexUnordered& variableIndex) const { // Do elimination - std::pair, boost::shared_ptr > result; + std::pair, boost::shared_ptr > result; if(ordering) { // Do elimination with given ordering result = JunctionTreeType(*this, variableIndex, *ordering).eliminate(function); diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index ab28014af..16042c7ec 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -30,23 +30,16 @@ namespace gtsam { * algorithms. Any factor graph holding eliminateable factors can derive from this class to * expose functions for computing marginals, conditional marginals, doing multifrontal and * sequential elimination, etc. */ - template + template class EliminateableFactorGraph { public: - typedef EliminateableFactorGraph This; - typedef DERIVED FactorGraphType; - typedef ELIMINATIONTREE EliminationTreeType; - typedef JUNCTIONTREE JunctionTreeType; - typedef typename EliminationTreeType::FactorType FactorType; - typedef typename EliminationTreeType::BayesNetType BayesNetType; - typedef typename JunctionTreeType::BayesTreeType 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 EliminateableFactorGraph This; typedef boost::optional OptionalOrdering; + typedef boost::function, boost::shared_ptr >( + std::vector >, std::vector)> + Eliminate; ///< Typedef for an eliminate subroutine + /** 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. @@ -68,7 +61,7 @@ namespace gtsam { * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, boost::none, varIndex); * \endcode * */ - boost::shared_ptr + boost::shared_ptr eliminateSequential(const Eliminate& function, OptionalOrdering ordering = boost::none, const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)) const; @@ -92,7 +85,7 @@ namespace gtsam { * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, boost::none, varIndex); * \endcode * */ - boost::shared_ptr + boost::shared_ptr eliminateMultifrontal(const Eliminate& function, OptionalOrdering ordering = boost::none, const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)) const; @@ -100,32 +93,32 @@ namespace gtsam { * 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, const Ordering& ordering, + std::pair, boost::shared_ptr > + eliminatePartialSequential(const Eliminate& function, const OrderingUnordered& ordering, const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); /** Do sequential elimination of the given \c variables in an ordering computed by COLAMD 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, const std::vector& variables, + std::pair, boost::shared_ptr > + eliminatePartialSequential(const Eliminate& function, const std::vector& variables, const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); /** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD 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 > - eliminatePartialMultifrontal(const Eliminate& function, const Ordering& ordering, + std::pair, boost::shared_ptr > + eliminatePartialMultifrontal(const Eliminate& function, const OrderingUnordered& ordering, const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); /** Do multifrontal elimination of some variables in the given \c ordering 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, const std::vector& ordering, + std::pair, boost::shared_ptr > + eliminatePartialMultifrontal(const Eliminate& function, const std::vector& variables, const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); }; diff --git a/gtsam/inference/EliminationTreeUnordered-inst.h b/gtsam/inference/EliminationTreeUnordered-inst.h index ec3d600cc..ed0d6d472 100644 --- a/gtsam/inference/EliminationTreeUnordered-inst.h +++ b/gtsam/inference/EliminationTreeUnordered-inst.h @@ -59,6 +59,20 @@ namespace gtsam { return eliminationResult.second; } + /* ************************************************************************* */ + template + void EliminationTreeUnordered::Node::print( + const std::string& str, const KeyFormatter& keyFormatter) const + { + std::cout << str << "(" << formatter(node->key) << ")\n"; + BOOST_FOREACH(const typename ETREE::sharedFactor& factor, node->factors) { + if(factor) + factor->print(str + "| "); + else + std::cout << str << "| null factor\n"; + } + } + /* ************************************************************************* */ template @@ -186,26 +200,11 @@ namespace gtsam { return std::make_pair(result, allRemainingFactors); } - /* ************************************************************************* */ - namespace { - template - std::string printVisitor(const typename ETREE::sharedNode& node, const std::string& myString, const KeyFormatter& formatter) { - std::cout << myString << "-(" << formatter(node->key) << ")\n"; - BOOST_FOREACH(const typename ETREE::sharedFactor& factor, node->factors) { - if(factor) - factor->print(myString + "| "); - else - std::cout << myString << "| null factor\n"; - } - return myString + "| "; - } - } - /* ************************************************************************* */ template void EliminationTreeUnordered::print(const std::string& name, const KeyFormatter& formatter) const { - treeTraversal::DepthFirstForest(*this, name, boost::bind(&printVisitor, _1, _2, formatter)); + treeTraversal::PrintForest(*this, name, formatter); } /* ************************************************************************* */ diff --git a/gtsam/inference/EliminationTreeUnordered.h b/gtsam/inference/EliminationTreeUnordered.h index f0c858baa..9eb0454a1 100644 --- a/gtsam/inference/EliminationTreeUnordered.h +++ b/gtsam/inference/EliminationTreeUnordered.h @@ -21,7 +21,6 @@ #include #include -#include class EliminationTreeUnorderedTester; // for unit tests, see testEliminationTree @@ -59,9 +58,7 @@ 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)> - Eliminate; ///< Typedef for an eliminate subroutine + typedef typename GRAPH::Eliminate Eliminate; struct Node { typedef std::vector Factors; @@ -73,6 +70,8 @@ namespace gtsam { sharedFactor eliminate(const boost::shared_ptr& output, const Eliminate& function, const std::vector& childrenFactors) const; + + void print(const std::string& str, const KeyFormatter& keyFormatter) const; }; typedef boost::shared_ptr sharedNode; ///< Shared pointer to Node diff --git a/gtsam/inference/FactorGraphUnordered.h b/gtsam/inference/FactorGraphUnordered.h index e5de1e3ba..eddc43781 100644 --- a/gtsam/inference/FactorGraphUnordered.h +++ b/gtsam/inference/FactorGraphUnordered.h @@ -22,6 +22,8 @@ #pragma once #include +#include +#include #include #include @@ -38,6 +40,7 @@ namespace gtsam { public: + typedef FactorGraphUnordered This; typedef FACTOR FactorType; ///< factor type typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional @@ -103,12 +106,18 @@ namespace gtsam { // TODO: are these needed? - /** Add a factor */ + /** Add a factor directly using a shared_ptr */ template void push_back(const boost::shared_ptr& factor) { factors_.push_back(boost::shared_ptr(factor)); } + /** Add a factor, will be copy-constructed into a shared_ptr (use push_back to avoid the copy). */ + template + void add(const DERIVEDFACTOR& factor) { + factors_.push_back(boost::make_shared(factor)); + } + /** push back many factors */ void push_back(const This& factors) { factors_.insert(end(), factors.begin(), factors.end()); @@ -119,6 +128,15 @@ namespace gtsam { void push_back(ITERATOR firstFactor, ITERATOR lastFactor) { factors_.insert(end(), firstFactor, lastFactor); } + + /** += syntax for push_back, e.g. graph += f1, f2, f3 */ + boost::assign::list_inserter, sharedFactor> + operator+=(const sharedFactor& factor) + { + return boost::assign::make_list_inserter( + boost::assign_detail::call_push_back(*this))(factor); + } + /** * @brief Add a vector of derived factors diff --git a/gtsam/inference/JunctionTreeUnordered-inst.h b/gtsam/inference/JunctionTreeUnordered-inst.h index 400b05ea4..18b01efd9 100644 --- a/gtsam/inference/JunctionTreeUnordered-inst.h +++ b/gtsam/inference/JunctionTreeUnordered-inst.h @@ -158,7 +158,7 @@ namespace gtsam { /* ************************************************************************* */ template - JunctionTreeUnordered& JunctionTreeUnordered::operator=(const This& other) + JunctionTreeUnordered& JunctionTreeUnordered::operator=(const This& other) { // Start by duplicating the tree. roots_ = treeTraversal::CloneForest(other); diff --git a/gtsam/inference/JunctionTreeUnordered.h b/gtsam/inference/JunctionTreeUnordered.h index 1cce0c345..6f8936292 100644 --- a/gtsam/inference/JunctionTreeUnordered.h +++ b/gtsam/inference/JunctionTreeUnordered.h @@ -57,12 +57,11 @@ namespace gtsam { typedef typename GRAPH::FactorType FactorType; ///< The type of factors typedef JunctionTreeUnordered This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - typedef typename boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals - typedef typename boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional - typedef boost::function(std::vector, std::vector)> - Eliminate; ///< Typedef for an eliminate subroutine + typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional + typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine struct Node { typedef std::vector Keys; @@ -92,7 +91,7 @@ namespace gtsam { /// @name Standard Constructors /// @{ - /** Build the junction tree from an elimination tree and a symbolic Bayes net. */ + /** Build the junction tree from an elimination tree. */ template JunctionTreeUnordered(const ETREE& eliminationTree); @@ -114,8 +113,8 @@ namespace gtsam { * in GaussianFactorGraph.h * @return The Bayes net and factor graph resulting from elimination */ - std::pair, boost::shared_ptr > - eliminate(Eliminate function) const; + std::pair, boost::shared_ptr > + eliminate(const Eliminate& function) const; /// @} diff --git a/gtsam/inference/Key.cpp b/gtsam/inference/Key.cpp index 521ca5261..332a3688b 100644 --- a/gtsam/inference/Key.cpp +++ b/gtsam/inference/Key.cpp @@ -27,14 +27,6 @@ namespace gtsam { -/* ************************************************************************* */ -std::string _defaultKeyFormatter(Key key) { - const Symbol asSymbol(key); - if(asSymbol.chr() > 0) - return (std::string)asSymbol; - else - return boost::lexical_cast(key); - } /* ************************************************************************* */ std::string _multirobotKeyFormatter(gtsam::Key key) { diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index bcc08d9cd..87ec08393 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -27,19 +27,6 @@ namespace gtsam { - /// Integer nonlinear key type - typedef size_t Key; - - /// Typedef for a function to format a key, i.e. to convert it to a string - typedef boost::function KeyFormatter; - - // Helper function for DefaultKeyFormatter - GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); - - /// The default KeyFormatter, which is used if no KeyFormatter is passed to - /// a nonlinear 'print' function. Automatically detects plain integer keys - /// and Symbol keys. - static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; // Helper function for Multi-robot Key Formatter GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key); diff --git a/gtsam/inference/OrderingUnordered.h b/gtsam/inference/OrderingUnordered.h index d22cfde0b..1fe364be9 100644 --- a/gtsam/inference/OrderingUnordered.h +++ b/gtsam/inference/OrderingUnordered.h @@ -22,6 +22,8 @@ #include namespace gtsam { - class OrderingUnordered : - typedef std::vector OrderingUnordered; + class OrderingUnordered : std::vector { + public: + OrderingUnordered() {} + }; } diff --git a/gtsam/symbolic/SymbolicBayesNetUnordered.cpp b/gtsam/symbolic/SymbolicBayesNetUnordered.cpp new file mode 100644 index 000000000..429f7bbe0 --- /dev/null +++ b/gtsam/symbolic/SymbolicBayesNetUnordered.cpp @@ -0,0 +1,28 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicBayesNet.cpp + * @date Oct 29, 2009 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include + +namespace gtsam { + + /* ************************************************************************* */ + void SymbolicBayesNetUnordered::noop() const { + } + + +} diff --git a/gtsam/symbolic/SymbolicBayesNetUnordered.h b/gtsam/symbolic/SymbolicBayesNetUnordered.h index 523ea094b..d4002daee 100644 --- a/gtsam/symbolic/SymbolicBayesNetUnordered.h +++ b/gtsam/symbolic/SymbolicBayesNetUnordered.h @@ -19,7 +19,7 @@ #pragma once #include -#include +#include #include namespace gtsam { @@ -27,12 +27,14 @@ namespace gtsam { /** Symbolic Bayes Net * \nosubgrouping */ - class SymbolicBayesNetUnordered: public SymbolicFactorGraphUnordered { + class SymbolicBayesNetUnordered: public BayesNetUnordered { public: - typedef SymbolicFactorGraphUnordered Base; + typedef BayesNetUnordered Base; + typedef SymbolicBayesNetUnordered This; typedef SymbolicConditionalUnordered ConditionalType; + typedef boost::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ @@ -43,6 +45,11 @@ namespace gtsam { /// @} /// @name Standard Interface /// @{ + + /// @} + + private: + void noop() const; // Function defined in cpp file so that compiler instantiates the base class }; } // namespace gtsam diff --git a/gtsam/symbolic/SymbolicBayesTreeUnordered.cpp b/gtsam/symbolic/SymbolicBayesTreeUnordered.cpp new file mode 100644 index 000000000..0a50e44a2 --- /dev/null +++ b/gtsam/symbolic/SymbolicBayesTreeUnordered.cpp @@ -0,0 +1,98 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicBayesTree.h + * @date Oct 29, 2009 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include + +#include + +namespace gtsam { + + /* ************************************************************************* */ + void SymbolicBayesTreeUnordered::insert(const sharedConditional& conditional) + { + static const bool debug = false; + + // get indices and parents + const typename CONDITIONAL::Parents& parents = conditional->parents(); + + if(debug) conditional->print("Adding conditional "); + + // if no parents, start a new root clique + if (parents.empty()) { + if(debug) std::cout << "No parents so making root" << std::endl; + bayesTree.root_ = bayesTree.addClique(conditional); + return; + } + + // otherwise, find the parent clique by using the index data structure + // to find the lowest-ordered parent + Index parentRepresentative = bayesTree.findParentClique(parents); + if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl; + sharedClique parent_clique = bayesTree[parentRepresentative]; + if(debug) parent_clique->print("Parent clique is "); + + // if the parents and parent clique have the same size, add to parent clique + if ((*parent_clique)->size() == size_t(parents.size())) { + if(debug) std::cout << "Adding to parent clique" << std::endl; +#ifndef NDEBUG + // Debug check that the parent indices of the new conditional match the indices + // currently in the clique. + // list::const_iterator parent = parents.begin(); + // typename Clique::const_iterator cond = parent_clique->begin(); + // while(parent != parents.end()) { + // assert(cond != parent_clique->end()); + // assert(*parent == (*cond)->key()); + // ++ parent; + // ++ cond; + // } +#endif + addToCliqueFront(bayesTree, conditional, parent_clique); + } else { + if(debug) std::cout << "Starting new clique" << std::endl; + // otherwise, start a new clique and add it to the tree + bayesTree.addClique(conditional,parent_clique); + } + } + + /* ************************************************************************* */ + void SymbolicBayesTreeUnordered::addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique) { + static const bool debug = false; +#ifndef NDEBUG + // Debug check to make sure the conditional variable is ordered lower than + // its parents and that all of its parents are present either in this + // clique or its separator. + BOOST_FOREACH(Key parent, conditional->parents()) { + assert(parent > conditional->lastFrontalKey()); + const Clique& cliquer(*clique); + assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end()); + } +#endif + if(debug) conditional->print("Adding conditional "); + if(debug) clique->print("To clique "); + Index j = conditional->lastFrontalKey(); + this->nodes_.resize(std::max(j+1, this->nodes_.size())); + this->nodes_[j] = clique; + FastVector newIndices(clique->conditional()->size() + 1); + newIndices[0] = j; + std::copy(clique->conditional()->begin(), clique->conditional()->end(), newIndices.begin()+1); + clique->conditional_ = ConditionalType::FromKeys(newIndices, (*clique)->nrFrontals() + 1); + if(debug) clique->print("Expanded clique is "); + clique->assertInvariants(); + } + +} diff --git a/gtsam/symbolic/SymbolicBayesTreeUnordered.h b/gtsam/symbolic/SymbolicBayesTreeUnordered.h new file mode 100644 index 000000000..a627ae403 --- /dev/null +++ b/gtsam/symbolic/SymbolicBayesTreeUnordered.h @@ -0,0 +1,46 @@ +/* ---------------------------------------------------------------------------- + + * 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 SymbolicBayesTree.h + * @date Oct 29, 2009 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + class SymbolicBayesTreeUnordered : + public BayesTreeUnordered + { + + public: + /** Insert a new conditional */ + void insert(const sharedConditional& conditional); + + protected: + + /** + * Add a conditional to the front of a clique, i.e. a conditional whose + * parents are already in the clique or its separators. This function does + * not check for this condition, it just updates the data structures. + */ + void addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique); + + }; + +} diff --git a/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp b/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp index bfa8dc268..67ef4bd4c 100644 --- a/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp +++ b/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp @@ -18,7 +18,10 @@ #include #include +#include #include +#include +#include namespace gtsam { diff --git a/gtsam/symbolic/SymbolicFactorGraphUnordered.h b/gtsam/symbolic/SymbolicFactorGraphUnordered.h index b06b65139..628fe3156 100644 --- a/gtsam/symbolic/SymbolicFactorGraphUnordered.h +++ b/gtsam/symbolic/SymbolicFactorGraphUnordered.h @@ -20,19 +20,36 @@ #include #include +#include #include +namespace gtsam { class SymbolicConditionalUnordered; } +namespace gtsam { class SymbolicBayesNet; } +namespace gtsam { class SymbolicEliminationTreeUnordered; } +namespace gtsam { class SymbolicBayesTree; } +namespace gtsam { class SymbolicJunctionTreeUnordered; } + namespace gtsam { /** Symbolic Factor Graph * \nosubgrouping */ - class GTSAM_EXPORT SymbolicFactorGraphUnordered: public FactorGraphUnordered { + class GTSAM_EXPORT SymbolicFactorGraphUnordered: + public FactorGraphUnordered, + public EliminateableFactorGraph< + SymbolicFactorGraphUnordered, SymbolicFactorUnordered, SymbolicConditionalUnordered, + SymbolicBayesNet, SymbolicEliminationTreeUnordered, SymbolicBayesTree, SymbolicJunctionTreeUnordered> + { public: typedef SymbolicFactorGraphUnordered This; typedef FactorGraphUnordered Base; + typedef EliminateableFactorGraph< + SymbolicFactorGraphUnordered, SymbolicFactorUnordered, SymbolicConditionalUnordered, + SymbolicBayesNet, SymbolicEliminationTreeUnordered, SymbolicBayesTree, SymbolicJunctionTreeUnordered> + BaseEliminateable; + typedef BaseEliminateable::Eliminate Eliminate; /// @name Standard Constructors /// @{ @@ -40,6 +57,10 @@ namespace gtsam { /** Construct empty factor graph */ SymbolicFactorGraphUnordered() {} + /** Construct from any factor graph with factors derived from SymbolicFactor. */ + template + SymbolicFactorGraphUnordered(const FactorGraphUnordered& graph) : Base(graph.begin(), graph.end()) {} + /** Constructor from iterator over factors */ template SymbolicFactorGraphUnordered(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {} diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp new file mode 100644 index 000000000..afa494f7e --- /dev/null +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -0,0 +1,99 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSymbolicBayesNet.cpp + * @brief Unit tests for a symbolic Bayes chain + * @author Frank Dellaert + */ + +#include + +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +static const Key _L_ = 0; +static const Key _A_ = 1; +static const Key _B_ = 2; +static const Key _C_ = 3; +static const Key _D_ = 4; +static const Key _E_ = 5; + +static SymbolicConditionalUnordered::shared_ptr + B(new SymbolicConditionalUnordered(_B_)), + L(new SymbolicConditionalUnordered(_L_, _B_)); + +/* ************************************************************************* */ +TEST( SymbolicBayesNet, equals ) +{ + SymbolicBayesNetUnordered f1; + f1.push_back(B); + f1.push_back(L); + SymbolicBayesNetUnordered f2; + f2.push_back(L); + f2.push_back(B); + CHECK(f1.equals(f1)); + CHECK(!f1.equals(f2)); +} + +/* ************************************************************************* */ +TEST( SymbolicBayesNet, combine ) +{ + SymbolicConditionalUnordered::shared_ptr + A(new SymbolicConditionalUnordered(_A_,_B_,_C_)), + B(new SymbolicConditionalUnordered(_B_,_C_)), + C(new SymbolicConditionalUnordered(_C_)); + + // p(A|BC) + SymbolicBayesNetUnordered p_ABC; + p_ABC.push_back(A); + + // P(BC)=P(B|C)P(C) + SymbolicBayesNetUnordered p_BC; + p_BC.push_back(B); + p_BC.push_back(C); + + // P(ABC) = P(A|BC) P(BC) + p_ABC.push_back(p_BC); + + SymbolicBayesNetUnordered expected; + expected.push_back(A); + expected.push_back(B); + expected.push_back(C); + + CHECK(assert_equal(expected,p_ABC)); +} + +/* ************************************************************************* */ +TEST(SymbolicBayesNet, saveGraph) { + SymbolicBayesNetUnordered bn; + bn.add(SymbolicConditionalUnordered(_A_, _B_)); + std::vector keys; + keys.push_back(_B_); + keys.push_back(_C_); + keys.push_back(_D_); + bn.add(SymbolicConditionalUnordered::FromKeys(keys,2)); + bn.add(SymbolicConditionalUnordered(_D_)); + + bn.saveGraph("SymbolicBayesNet.dot"); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp new file mode 100644 index 000000000..97f627726 --- /dev/null +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -0,0 +1,116 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSymbolicFactorGraph.cpp + * @brief Unit tests for symbolic factor graphs + * @author Christian Potthast + **/ + +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +//TEST(SymbolicFactorGraph, eliminateFrontals) { +// +// SymbolicFactorGraph sfgOrig; +// sfgOrig.push_factor(0,1); +// sfgOrig.push_factor(0,2); +// sfgOrig.push_factor(1,3); +// sfgOrig.push_factor(1,4); +// sfgOrig.push_factor(2,3); +// sfgOrig.push_factor(4,5); +// +// SymbolicConditionalUnordered::shared_ptr actualCond; +// SymbolicFactorGraph::shared_ptr actualSfg; +// boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2); +// +// vector condIndices; +// condIndices += 0,1,2,3,4; +// IndexConditional expectedCond(condIndices, 2); +// +// SymbolicFactorGraph expectedSfg; +// expectedSfg.push_factor(2,3); +// expectedSfg.push_factor(4,5); +// expectedSfg.push_factor(2,3,4); +// +// EXPECT(assert_equal(expectedSfg, actualSfg)); +// EXPECT(assert_equal(expectedCond, *actualCond)); +//} + +///* ************************************************************************* */ +//TEST( SymbolicFactorGraph, EliminateOne ) +//{ +// // create a test graph +// SymbolicFactorGraph fg; +// fg.push_factor(vx2, vx1); +// +// SymbolicSequentialSolver::EliminateUntil(fg, vx2+1); +// SymbolicFactorGraph expected; +// expected.push_back(boost::shared_ptr()); +// expected.push_factor(vx1); +// +// CHECK(assert_equal(expected, fg)); +//} + +/* ************************************************************************* */ +TEST( SymbolicFactorGraph, constructFromBayesNet ) +{ + // create expected factor graph + SymbolicFactorGraphUnordered expected; + expected.push_factor(0, 1, 2); + expected.push_factor(1, 2); + expected.push_factor(1); + + // create Bayes Net + SymbolicBayesNetUnordered bayesNet; + bayesNet.add(SymbolicConditionalUnordered(0, 1, 2)); + bayesNet.add(SymbolicConditionalUnordered(1, 2)); + bayesNet.add(SymbolicConditionalUnordered(1)); + + // create actual factor graph from a Bayes Net + SymbolicFactorGraphUnordered actual(bayesNet); + + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST( SymbolicFactorGraph, push_back ) +{ + // Create two factor graphs and expected combined graph + SymbolicFactorGraphUnordered fg1, fg2, expected; + + fg1.push_factor(1); + fg1.push_factor(0, 1); + + fg2.push_factor(1, 2); + fg2.push_factor(0, 2); + + expected.push_factor(1); + expected.push_factor(0, 1); + expected.push_factor(1, 2); + expected.push_factor(0, 2); + + // combine + SymbolicFactorGraphUnordered actual; + actual.push_back(fg1); + actual.push_back(fg2); + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */