From df728a969c7b4fae664ad44bf9e5cc57bf1eb663 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 6 Jun 2013 15:36:18 +0000 Subject: [PATCH] In the middle of converting to generic DFS code but committing to save history --- gtsam/base/treeTraversal-inst.h | 110 ++++++++++ gtsam/dllexport.h | 11 +- gtsam/inference/ConditionalUnordered-inst.h | 41 ++++ gtsam/inference/ConditionalUnordered.h | 15 -- ...-inl.h => EliminationTreeUnordered-inst.h} | 156 ++++++-------- gtsam/inference/EliminationTreeUnordered.cpp | 27 --- gtsam/inference/EliminationTreeUnordered.h | 26 +-- ...ered-inl.h => FactorGraphUnordered-inst.h} | 1 + gtsam/inference/FactorGraphUnordered.h | 2 - gtsam/inference/JunctionTreeUnordered-inst.h | 203 ++++++++++++++++++ gtsam/inference/JunctionTreeUnordered.h | 172 +++++++++++++++ gtsam/inference/inference-inst.h | 141 ++++++++++++ .../symbolic/SymbolicConditionalUnordered.cpp | 1 + .../SymbolicEliminationTreeUnordered.cpp | 28 +++ .../SymbolicEliminationTreeUnordered.h | 10 +- .../symbolic/SymbolicFactorGraphUnordered.cpp | 1 + .../tests/testSymbolicEliminationTree.cpp | 13 +- 17 files changed, 798 insertions(+), 160 deletions(-) create mode 100644 gtsam/base/treeTraversal-inst.h create mode 100644 gtsam/inference/ConditionalUnordered-inst.h rename gtsam/inference/{EliminationTreeUnordered-inl.h => EliminationTreeUnordered-inst.h} (65%) delete mode 100644 gtsam/inference/EliminationTreeUnordered.cpp rename gtsam/inference/{FactorGraphUnordered-inl.h => FactorGraphUnordered-inst.h} (98%) create mode 100644 gtsam/inference/JunctionTreeUnordered-inst.h create mode 100644 gtsam/inference/JunctionTreeUnordered.h create mode 100644 gtsam/inference/inference-inst.h create mode 100644 gtsam/symbolic/SymbolicEliminationTreeUnordered.cpp diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h new file mode 100644 index 000000000..bcc0c281d --- /dev/null +++ b/gtsam/base/treeTraversal-inst.h @@ -0,0 +1,110 @@ +/* ---------------------------------------------------------------------------- + +* 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 treeTraversal-inst.h +* @author Richard Roberts +* @date April 9, 2013 +*/ +#pragma once + +#include + +namespace gtsam { + + /** Internal functions used for traversing trees */ + namespace treeTraversal { + + namespace { + // Internal node used in DFS preorder stack + template + struct TraversalNode { + bool expanded; + NODE* const treeNode; + DATA data; + TraversalNode(NODE* _treeNode, const DATA& _data) : + expanded(false), treeNode(_treeNode), data(_data) {} + }; + + // Functional object to visit a node and then add it to the traversal stack + template + struct PreOrderExpand { + VISITOR& visitor_; + DATA* parentData_; + STACK& stack_; + PreOrderExpand(VISITOR& visitor, DATA* parentData, STACK& stack) : + visitor_(visitor), parentData_(parentData), stack_(stack) {} + template + void operator()(const P& nodePointer) { + // Get raw pointer (in case nodePointer is a shared_ptr) + NODE* rawNodePointer = &(*nodePointer); + // Add node + stack_.push(TraversalNode(rawNodePointer, visitor_(nodePointer, *parentData_))); + } + }; + + /// Do nothing - default argument for post-visitor for tree traversal + template + void no_op(const NODE& node, const DATA& data) {} + } + + /** Traverse a forest depth-first. + * @param forest The forest of trees to traverse. The method \c forest.roots() should exist + * and return a collection of (shared) pointers to \c FOREST::Node. + * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before + * visiting its children, and will be passed, by reference, the \c DATA object returned + * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object + * to pass to the children. The returned \c DATA object will be copy-constructed only + * upon returning to store internally, thus may be modified by visiting the children. + * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. + * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting + * its children, and will be passed, by reference, the \c DATA object returned by the + * call to \c visitorPre (the \c DATA object may be modified by visiting the children). + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. */ + template + void DepthFirstForest(FOREST& forest, DATA& rootData, + VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost = no_op) + { + // Depth first traversal stack + typedef TraversalNode TraversalNode; + typedef std::stack > Stack; + typedef PreOrderExpand Expander; + Stack stack; + + // Add roots to stack (use reverse iterators so children are processed in the order they + // appear) + (void) std::for_each(forest.roots().rbegin(), forest.roots().rend(), + Expander(visitor, &rootData, stack)); + + // Traverse + while(!stack.empty()) + { + // Get next node + TraversalNode& node = stack.top(); + + if(node.expanded) { + // If already expanded, then the data stored in the node is no longer needed, so visit + // then delete it. + (void) visitorPost(*node.treeNode, node.data); + stack.pop(); + } 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(), + Expander(visitorPre, &node.data, stack)); + node.expanded = true; + } + } + } + } + +} \ No newline at end of file diff --git a/gtsam/dllexport.h b/gtsam/dllexport.h index 0e327fc14..008d59c92 100644 --- a/gtsam/dllexport.h +++ b/gtsam/dllexport.h @@ -16,6 +16,16 @@ * @date Mar 9, 2013 */ +// Macros for exporting DLL symbols on Windows +// Usage example: +// In header file: +// class GTSAM_EXPORT MyClass { ... }; +// +// Results in the following declarations: +// When included while compiling the GTSAM library itself: +// class __declspec(dllexport) MyClass { ... }; +// When included while compiling other code against GTSAM: +// class __declspec(dllimport) MyClass { ... }; #ifdef _WIN32 # ifdef GTSAM_EXPORTS # define GTSAM_EXPORT __declspec(dllexport) @@ -33,4 +43,3 @@ # define GTSAM_EXPORT # define GTSAM_EXTERN_EXPORT extern #endif - diff --git a/gtsam/inference/ConditionalUnordered-inst.h b/gtsam/inference/ConditionalUnordered-inst.h new file mode 100644 index 000000000..0167271d0 --- /dev/null +++ b/gtsam/inference/ConditionalUnordered-inst.h @@ -0,0 +1,41 @@ +/* ---------------------------------------------------------------------------- + + * 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 Conditional.h + * @brief Base class for conditional densities + * @author Frank Dellaert + */ + +// \callgraph +#pragma once + +#include +#include + +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + void ConditionalUnordered::print(const std::string& s, const KeyFormatter& formatter) const { + std::cout << s << " P("; + BOOST_FOREACH(Key key, frontals()) + std::cout << " " << formatter(key); + if (nrParents() > 0) + std::cout << " |"; + BOOST_FOREACH(Key parent, parents()) + std::cout << " " << formatter(parent); + std::cout << ")" << std::endl; + } + +} diff --git a/gtsam/inference/ConditionalUnordered.h b/gtsam/inference/ConditionalUnordered.h index 0e1ea274b..4665820bf 100644 --- a/gtsam/inference/ConditionalUnordered.h +++ b/gtsam/inference/ConditionalUnordered.h @@ -19,8 +19,6 @@ #pragma once #include -#include -#include #include @@ -152,17 +150,4 @@ namespace gtsam { }; - /* ************************************************************************* */ - template - void ConditionalUnordered::print(const std::string& s, const KeyFormatter& formatter) const { - std::cout << s << " P("; - BOOST_FOREACH(Key key, frontals()) - std::cout << " " << formatter(key); - if (nrParents() > 0) - std::cout << " |"; - BOOST_FOREACH(Key parent, parents()) - std::cout << " " << formatter(parent); - std::cout << ")" << std::endl; - } - } // gtsam diff --git a/gtsam/inference/EliminationTreeUnordered-inl.h b/gtsam/inference/EliminationTreeUnordered-inst.h similarity index 65% rename from gtsam/inference/EliminationTreeUnordered-inl.h rename to gtsam/inference/EliminationTreeUnordered-inst.h index 95a7f640e..589bdcbb2 100644 --- a/gtsam/inference/EliminationTreeUnordered-inl.h +++ b/gtsam/inference/EliminationTreeUnordered-inst.h @@ -18,14 +18,46 @@ #pragma once #include +#include +#include #include #include +#include #include #include +#include namespace gtsam { + /* ************************************************************************* */ + template + typename EliminationTreeUnordered::sharedFactor + EliminationTreeUnordered::Node::eliminate( + const boost::shared_ptr& output, + const Eliminate& function, const std::vector& childrenResults) const + { + assert(childrenResults.size() == children.size()); + + // Gather factors + std::vector gatheredFactors; + gatheredFactors.reserve(factors.size() + children.size()); + gatheredFactors.insert(gatheredFactors.end(), factors.begin(), factors.end()); + gatheredFactors.insert(gatheredFactors.end(), childrenResults.begin(), childrenResults.end()); + + // Do dense elimination step + std::vector keyAsVector(1); keyAsVector[0] = key; + std::pair, boost::shared_ptr > eliminationResult = + function(gatheredFactors, keyAsVector); + + // Add conditional to BayesNet + output->push_back(eliminationResult.first); + + // Return result + return eliminationResult.second; + } + + /* ************************************************************************* */ template EliminationTreeUnordered::EliminationTreeUnordered(const FactorGraphType& graph, @@ -51,7 +83,7 @@ namespace gtsam { for (size_t j = 0; j < n; j++) { // Retrieve the factors involving this variable and create the current node - const VariableIndex::Factors& factors = structure[order[j]]; + const VariableIndexUnordered::Factors& factors = structure[order[j]]; nodes[j] = boost::make_shared(); nodes[j]->key = order[j]; @@ -74,7 +106,7 @@ namespace gtsam { if (r != j) { // Now that we found the root, hook up parent and child pointers in the nodes. parents[r] = j; - nodes[j]->subTrees.push_back(nodes[r]); + nodes[j]->children.push_back(nodes[r]); } } else { // Add the current factor to the current node since we are at the first variable in this @@ -136,7 +168,7 @@ namespace gtsam { while(!stack.empty()) { sharedNode node = stack.top(); stack.pop(); - BOOST_FOREACH(sharedNode& child, node->subTrees) { + BOOST_FOREACH(sharedNode& child, node->children) { // Important: since we are working with a *reference* to a shared pointer in this // BOOST_FOREACH loop, the next statement modifies the pointer in the current node's child // list - it replaces it with a pointer to a copy of the child. @@ -152,112 +184,46 @@ namespace gtsam { return *this; } - /* ************************************************************************* */ - namespace { - template - struct EliminationNode { - bool expanded; - const typename ELIMINATIONTREE::Node* const treeNode; - std::vector factors; - EliminationNode* const parent; - template EliminationNode(const typename ELIMINATIONTREE::Node* _treeNode, size_t nFactorsToReserve, - ITERATOR firstFactor, ITERATOR lastFactor, EliminationNode* _parent) : - expanded(false), treeNode(_treeNode), parent(_parent) { - factors.reserve(nFactorsToReserve); - factors.insert(factors.end(), firstFactor, lastFactor); - } - }; - } - /* ************************************************************************* */ template std::pair, boost::shared_ptr > EliminationTreeUnordered::eliminate(Eliminate function) const { - // Stack for eliminating nodes. We use this stack instead of recursive function calls to - // avoid call stack overflow due to very long trees that arise from chain-like graphs. - // TODO: Check whether this is faster as a vector (then use indices instead of parent pointers). - typedef EliminationNode EliminationNode; - std::stack > eliminationStack; + // Allocate result + boost::shared_ptr result = boost::make_shared(); - // Create empty Bayes net and factor graph to hold result - boost::shared_ptr bayesNet = boost::make_shared(); - // Initialize remaining factors with the factors remaining from creation of the - // EliminationTree - these are the factors that were not included in the partial elimination - // at all. - boost::shared_ptr remainingFactors = - boost::make_shared(remainingFactors_.begin(), remainingFactors_.end()); + // Run tree elimination algorithm + std::vector remainingFactors = inference::EliminateTree(result, *this, function); - // Add roots to the stack (use reverse foreach so conditionals to appear in elimination order - - // doesn't matter for computation but can make printouts easier to interpret by hand) - BOOST_REVERSE_FOREACH(const sharedNode& root, roots_) { - eliminationStack.push( - EliminationNode(root.get(), root->factors.size() + root->subTrees.size(), - root->factors.begin(), root->factors.end(), 0)); } + // Add remaining factors that were not involved with eliminated variables + boost::shared_ptr allRemainingFactors = boost::make_shared(); + allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end()); + allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end()); - // Until the stack is empty - while(!eliminationStack.empty()) { - // Process the next node. If it has children, add its children to the stack and mark it - // expanded - we'll come back and eliminate it later after the children have been processed. - EliminationNode& node = eliminationStack.top(); - if(node.expanded) { - // Do a dense elimination step - std::vector keyAsVector(1); keyAsVector[0] = node.treeNode->key; - std::pair, boost::shared_ptr > eliminationResult = - function(node.factors, keyAsVector); + // Return result + return std::make_pair(result, allRemainingFactors); + } - // Add conditional to BayesNet and remaining factor to parent - bayesNet->push_back(eliminationResult.first); - - // TODO: Don't add null factor? - if(node.parent) - node.parent->factors.push_back(eliminationResult.second); + /* ************************************************************************* */ + 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 - remainingFactors->push_back(eliminationResult.second); - - // Remove from stack - eliminationStack.pop(); - } else { - // Expand children and mark as expanded (use reverse foreach so conditionals to appear in - // elimination order - doesn't matter for computation but can make printouts easier to - // interpret by hand) - node.expanded = true; - BOOST_REVERSE_FOREACH(const sharedNode& child, node.treeNode->subTrees) { - eliminationStack.push( - EliminationNode(child.get(), child->factors.size() + child->subTrees.size(), - child->factors.begin(), child->factors.end(), &node)); } + std::cout << myString << "| null factor\n"; } + return myString + "| "; } - - // Return results - return std::make_pair(bayesNet, remainingFactors); } /* ************************************************************************* */ template void EliminationTreeUnordered::print(const std::string& name, const KeyFormatter& formatter) const { - // Depth-first-traversal stack - std::stack > stack; - - // Add roots - BOOST_FOREACH(const sharedNode& node, roots_) { stack.push(std::make_pair(node, " ")); } - - // Traverse - while(!stack.empty()) { - std::pair node_string = stack.top(); - stack.pop(); - std::cout << node_string.second << " (" << formatter(node_string.first->key) << ")\n"; - BOOST_FOREACH(const sharedFactor& factor, node_string.first->factors) { - if(factor) - factor->print(node_string.second + " "); - else - std::cout << node_string.second << " null factor\n"; - } - BOOST_FOREACH(const sharedNode& child, node_string.first->subTrees) { - stack.push(std::make_pair(child, node_string.second + " ")); - } - } + treeTraversal::DepthFirstForest(*this, name, boost::bind(&printVisitor, _1, _2, formatter)); } /* ************************************************************************* */ @@ -270,13 +236,13 @@ namespace gtsam { // Add roots in sorted order { FastMap keys; - BOOST_FOREACH(const sharedNode& root, this->roots_) { keys.insert(make_pair(root->key, root)); } + BOOST_FOREACH(const sharedNode& root, this->roots_) { keys.insert(std::make_pair(root->key, root)); } typedef FastMap::value_type Key_Node; BOOST_FOREACH(const Key_Node& key_node, keys) { stack1.push(key_node.second); } } { FastMap keys; - BOOST_FOREACH(const sharedNode& root, expected.roots_) { keys.insert(make_pair(root->key, root)); } + BOOST_FOREACH(const sharedNode& root, expected.roots_) { keys.insert(std::make_pair(root->key, root)); } typedef FastMap::value_type Key_Node; BOOST_FOREACH(const Key_Node& key_node, keys) { stack2.push(key_node.second); } } @@ -310,13 +276,13 @@ namespace gtsam { // Add children in sorted order { FastMap keys; - BOOST_FOREACH(const sharedNode& node, node1->subTrees) { keys.insert(make_pair(node->key, node)); } + BOOST_FOREACH(const sharedNode& node, node1->children) { keys.insert(std::make_pair(node->key, node)); } typedef FastMap::value_type Key_Node; BOOST_FOREACH(const Key_Node& key_node, keys) { stack1.push(key_node.second); } } { FastMap keys; - BOOST_FOREACH(const sharedNode& node, node2->subTrees) { keys.insert(make_pair(node->key, node)); } + BOOST_FOREACH(const sharedNode& node, node2->children) { keys.insert(std::make_pair(node->key, node)); } typedef FastMap::value_type Key_Node; BOOST_FOREACH(const Key_Node& key_node, keys) { stack2.push(key_node.second); } } diff --git a/gtsam/inference/EliminationTreeUnordered.cpp b/gtsam/inference/EliminationTreeUnordered.cpp deleted file mode 100644 index 93c31b23e..000000000 --- a/gtsam/inference/EliminationTreeUnordered.cpp +++ /dev/null @@ -1,27 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file EliminationTree.cpp - * @author Frank Dellaert - * @author Richard Roberts - * @date Oct 13, 2010 - */ - -#include - -namespace gtsam { - namespace internal { - - } - -} - diff --git a/gtsam/inference/EliminationTreeUnordered.h b/gtsam/inference/EliminationTreeUnordered.h index 2c33266b6..ced271d14 100644 --- a/gtsam/inference/EliminationTreeUnordered.h +++ b/gtsam/inference/EliminationTreeUnordered.h @@ -26,7 +26,6 @@ #include class EliminationTreeUnorderedTester; // for unit tests, see testEliminationTree -namespace { template struct EliminationNode; } namespace gtsam { @@ -56,7 +55,7 @@ namespace gtsam { typedef GRAPH FactorGraphType; ///< The factor graph type typedef typename GRAPH::FactorType FactorType; ///< The type of factors - typedef EliminationTreeUnordered This; ///< This class + typedef EliminationTreeUnordered 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 BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR @@ -65,20 +64,22 @@ namespace gtsam { typedef boost::function(std::vector, std::vector)> Eliminate; ///< Typedef for an eliminate subroutine - private: - - class Node { - public: + struct Node { typedef FastList Factors; - typedef FastList > SubTrees; + typedef FastList > Children; Key key; ///< key associated with root Factors factors; ///< factors associated with root - SubTrees subTrees; ///< sub-trees + Children children; ///< sub-trees + + sharedFactor eliminate(const boost::shared_ptr& output, + const Eliminate& function, const std::vector& childrenFactors) const; }; typedef boost::shared_ptr sharedNode; ///< Shared pointer to Node + private: + /** concept check */ GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); @@ -140,6 +141,10 @@ namespace gtsam { bool equals(const This& other, double tol = 1e-9) const; /// @} + /// @name Advanced Interface + /// @{ + + const FastList& roots() const { return roots_; } protected: /// Protected default constructor @@ -148,11 +153,6 @@ namespace gtsam { private: /// Allow access to constructor and add methods for testing purposes friend class ::EliminationTreeUnorderedTester; - - friend struct EliminationNode; - }; } - -#include diff --git a/gtsam/inference/FactorGraphUnordered-inl.h b/gtsam/inference/FactorGraphUnordered-inst.h similarity index 98% rename from gtsam/inference/FactorGraphUnordered-inl.h rename to gtsam/inference/FactorGraphUnordered-inst.h index 272995409..bc07760d6 100644 --- a/gtsam/inference/FactorGraphUnordered-inl.h +++ b/gtsam/inference/FactorGraphUnordered-inst.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include diff --git a/gtsam/inference/FactorGraphUnordered.h b/gtsam/inference/FactorGraphUnordered.h index 9cb8db921..e5de1e3ba 100644 --- a/gtsam/inference/FactorGraphUnordered.h +++ b/gtsam/inference/FactorGraphUnordered.h @@ -228,5 +228,3 @@ namespace gtsam { }; // FactorGraph } // namespace gtsam - -#include diff --git a/gtsam/inference/JunctionTreeUnordered-inst.h b/gtsam/inference/JunctionTreeUnordered-inst.h new file mode 100644 index 000000000..1fc58cbb6 --- /dev/null +++ b/gtsam/inference/JunctionTreeUnordered-inst.h @@ -0,0 +1,203 @@ +/* ---------------------------------------------------------------------------- + + * 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 JunctionTree-inl.h + * @date Feb 4, 2010 + * @author Kai Ni + * @author Frank Dellaert + * @brief The junction tree, template bodies + */ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + void JunctionTree::construct(const FG& fg, const VariableIndex& variableIndex) { + gttic(JT_symbolic_ET); + const typename EliminationTree::shared_ptr symETree = + EliminationTree::Create(fg, variableIndex); + assert(symETree.get()); + gttoc(JT_symbolic_ET); + gttic(JT_symbolic_eliminate); + SymbolicBayesNet::shared_ptr sbn = symETree->eliminate(&EliminateSymbolic); + assert(sbn.get()); + gttoc(JT_symbolic_eliminate); + gttic(symbolic_BayesTree); + SymbolicBayesTree sbt(*sbn); + gttoc(symbolic_BayesTree); + + // distribute factors + gttic(distributeFactors); + this->root_ = distributeFactors(fg, sbt.root()); + gttoc(distributeFactors); + } + + /* ************************************************************************* */ + template + JunctionTree::JunctionTree(const FG& fg) { + gttic(VariableIndex); + VariableIndex varIndex(fg); + gttoc(VariableIndex); + construct(fg, varIndex); + } + + /* ************************************************************************* */ + template + JunctionTree::JunctionTree(const FG& fg, const VariableIndex& variableIndex) { + construct(fg, variableIndex); + } + + /* ************************************************************************* */ + template + typename JunctionTree::sharedClique JunctionTree::distributeFactors( + const FG& fg, const SymbolicBayesTree::sharedClique& bayesClique) { + + // Build "target" index. This is an index for each variable of the factors + // that involve this variable as their *lowest-ordered* variable. For each + // factor, it is the lowest-ordered variable of that factor that pulls the + // factor into elimination, after which all of the information in the + // factor is contained in the eliminated factors that are passed up the + // tree as elimination continues. + + // Two stages - first build an array of the lowest-ordered variable in each + // factor and find the last variable to be eliminated. + std::vector lowestOrdered(fg.size(), std::numeric_limits::max()); + Index maxVar = 0; + for(size_t i=0; ibegin(), fg[i]->end()); + if(min != fg[i]->end()) { + lowestOrdered[i] = *min; + maxVar = std::max(maxVar, *min); + } + } + + // Now add each factor to the list corresponding to its lowest-ordered + // variable. + std::vector > targets(maxVar+1); + for(size_t i=0; i::max()) + targets[lowestOrdered[i]].push_back(i); + + // Now call the recursive distributeFactors + return distributeFactors(fg, targets, bayesClique); + } + + /* ************************************************************************* */ + template + typename JunctionTree::sharedClique JunctionTree::distributeFactors(const FG& fg, + const std::vector >& targets, + const SymbolicBayesTree::sharedClique& bayesClique) { + + if(bayesClique) { + // create a new clique in the junction tree + sharedClique clique(new Clique((*bayesClique)->beginFrontals(), (*bayesClique)->endFrontals(), + (*bayesClique)->beginParents(), (*bayesClique)->endParents())); + + // count the factors for this cluster to pre-allocate space + { + size_t nFactors = 0; + BOOST_FOREACH(const Index frontal, clique->frontal) { + // There may be less variables in "targets" than there really are if + // some of the highest-numbered variables do not pull in any factors. + if(frontal < targets.size()) + nFactors += targets[frontal].size(); } + clique->reserve(nFactors); + } + // add the factors to this cluster + BOOST_FOREACH(const Index frontal, clique->frontal) { + if(frontal < targets.size()) { + BOOST_FOREACH(const size_t factorI, targets[frontal]) { + clique->push_back(fg[factorI]); } } } + + // recursively call the children + BOOST_FOREACH(const typename SymbolicBayesTree::sharedClique bayesChild, bayesClique->children()) { + sharedClique child = distributeFactors(fg, targets, bayesChild); + clique->addChild(child); + child->parent() = clique; + } + return clique; + } else + return sharedClique(); + } + + /* ************************************************************************* */ + template + std::pair::BTClique::shared_ptr, + typename FG::sharedFactor> JunctionTree::eliminateOneClique( + typename FG::Eliminate function, + const boost::shared_ptr& current) const { + + FG fg; // factor graph will be assembled from local factors and marginalized children + fg.reserve(current->size() + current->children().size()); + fg.push_back(*current); // add the local factors + + // receive the factors from the child and its clique point + std::list children; + BOOST_FOREACH(const boost::shared_ptr& child, current->children()) { + std::pair tree_factor( + eliminateOneClique(function, child)); + children.push_back(tree_factor.first); + fg.push_back(tree_factor.second); + } + + // eliminate the combined factors + // warning: fg is being eliminated in-place and will contain marginal afterwards + + // Now that we know which factors and variables, and where variables + // come from and go to, create and eliminate the new joint factor. + gttic(CombineAndEliminate); + typename FG::EliminationResult eliminated(function(fg, + current->frontal.size())); + gttoc(CombineAndEliminate); + + assert(std::equal(eliminated.second->begin(), eliminated.second->end(), current->separator.begin())); + + gttic(Update_tree); + // create a new clique corresponding the combined factors + typename BTClique::shared_ptr new_clique(BTClique::Create(eliminated)); + new_clique->children_ = children; + + BOOST_FOREACH(typename BTClique::shared_ptr& childRoot, children) { + childRoot->parent_ = new_clique; + } + gttoc(Update_tree); + + return std::make_pair(new_clique, eliminated.second); + } + + /* ************************************************************************* */ + template + typename BTCLIQUE::shared_ptr JunctionTree::eliminate( + typename FG::Eliminate function) const { + if (this->root()) { + gttic(JT_eliminate); + std::pair ret = + this->eliminateOneClique(function, this->root()); + if (ret.second->size() != 0) throw std::runtime_error( + "JuntionTree::eliminate: elimination failed because of factors left over!"); + gttoc(JT_eliminate); + return ret.first; + } else + return typename BTClique::shared_ptr(); + } + +} //namespace gtsam diff --git a/gtsam/inference/JunctionTreeUnordered.h b/gtsam/inference/JunctionTreeUnordered.h new file mode 100644 index 000000000..6de7fd4f7 --- /dev/null +++ b/gtsam/inference/JunctionTreeUnordered.h @@ -0,0 +1,172 @@ +/* ---------------------------------------------------------------------------- + + * 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 JunctionTree.h + * @date Feb 4, 2010 + * @author Kai Ni + * @author Frank Dellaert + * @brief: The junction tree + */ + +#pragma once + +#include +#include + +namespace gtsam { + + template + class JunctionTreeUnordered { + + public: + + typedef GRAPH FactorGraphType; ///< The factor graph type + 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 BAYESTREE BayesTreeType; ///< The BayesNet corresponding to FACTOR + 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 + + struct Node { + typedef FastList Factors; + typedef FastList > Children; + + Key key; ///< key associated with root + Factors factors; ///< factors associated with root + Children children; ///< sub-trees + + sharedFactor eliminate(const boost::shared_ptr& output, + const Eliminate& function, const std::vector& childrenFactors) const; + }; + + typedef boost::shared_ptr sharedNode; ///< Shared pointer to Node + + private: + + /** concept check */ + GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); + + FastList roots_; + std::vector remainingFactors_; + + public: + + }; + + /** + * A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with + * the additional property that it represents the clique tree associated with a Bayes net. + * + * In GTSAM a junction tree is an intermediate data structure in multifrontal + * variable elimination. Each node is a cluster of factors, along with a + * clique of variables that are eliminated all at once. In detail, every node k represents + * a clique (maximal fully connected subset) of an associated chordal graph, such as a + * chordal Bayes net resulting from elimination. + * + * The difference with the BayesTree is that a JunctionTree stores factors, whereas a + * BayesTree stores conditionals, that are the product of eliminating the factors in the + * corresponding JunctionTree cliques. + * + * The tree structure and elimination method are exactly analagous to the EliminationTree, + * except that in the JunctionTree, at each node multiple variables are eliminated at a time. + * + * + * \addtogroup Multifrontal + * \nosubgrouping + */ + template::Clique> + class JunctionTree: public ClusterTree { + + public: + + /// In a junction tree each cluster is associated with a clique + typedef typename ClusterTree::Cluster Clique; + typedef typename Clique::shared_ptr sharedClique; ///< Shared pointer to a clique + + /// The BayesTree type produced by elimination + typedef BTCLIQUE BTClique; + + /// Shared pointer to this class + typedef boost::shared_ptr > shared_ptr; + + /// We will frequently refer to a symbolic Bayes tree, used to find the clique structure + typedef gtsam::BayesTree SymbolicBayesTree; + + private: + + /// @name Advanced Interface + /// @{ + + /// distribute the factors along the cluster tree + sharedClique distributeFactors(const FG& fg, + const SymbolicBayesTree::sharedClique& clique); + + /// distribute the factors along the cluster tree + sharedClique distributeFactors(const FG& fg, const std::vector >& targets, + const SymbolicBayesTree::sharedClique& clique); + + /// recursive elimination function + std::pair + eliminateOneClique(typename FG::Eliminate function, + const boost::shared_ptr& clique) const; + + /// internal constructor + void construct(const FG& fg, const VariableIndex& variableIndex); + + /// @} + + public: + + /// @name Standard Constructors + /// @{ + + /** Default constructor */ + JunctionTree() {} + + /** Named constructor to build the junction tree of a factor graph. Note + * that this has to compute the column structure as a VariableIndex, so if you + * already have this precomputed, use the JunctionTree(const FG&, const VariableIndex&) + * constructor instead. + * @param factorGraph The factor graph for which to build the elimination tree + */ + JunctionTree(const FG& factorGraph); + + /** Construct from a factor graph and pre-computed variable index. + * @param fg The factor graph for which to build the junction tree + * @param structure The set of factors involving each variable. If this is not + * precomputed, you can call the JunctionTree(const FG&) + * constructor instead. + */ + JunctionTree(const FG& fg, const VariableIndex& variableIndex); + + /// @} + /// @name Standard Interface + /// @{ + + /** Eliminate the factors in the subgraphs to produce a BayesTree. + * @param function The function used to eliminate, see the namespace functions + * in GaussianFactorGraph.h + * @return The BayesTree resulting from elimination + */ + typename BTClique::shared_ptr eliminate(typename FG::Eliminate function) const; + + /// @} + + }; // JunctionTree + +} // namespace gtsam + +#include diff --git a/gtsam/inference/inference-inst.h b/gtsam/inference/inference-inst.h new file mode 100644 index 000000000..7d37b06eb --- /dev/null +++ b/gtsam/inference/inference-inst.h @@ -0,0 +1,141 @@ +/* ---------------------------------------------------------------------------- + +* 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 inference-inst.h +* @brief Contains *generic* inference algorithms that convert between templated +* graphical models, i.e., factor graphs, Bayes nets, and Bayes trees +* @author Frank Dellaert +* @author Richard Roberts +*/ + +#pragma once + +#include +#include + +#include + +namespace gtsam { + namespace inference { + + /* ************************************************************************* */ + namespace { + template + struct EliminationNode { + bool expanded; + const typename ELIMINATIONTREE::Node* const treeNode; + std::vector childrenFactors; + EliminationNode* const parent; + EliminationNode(const typename ELIMINATIONTREE::Node* _treeNode, EliminationNode* _parent) : + expanded(false), treeNode(_treeNode), parent(_parent) { + childrenFactors.reserve(treeNode->children.size()); } + }; + + /* ************************************************************************* */ + template + struct EliminationData { + EliminationData* const parentData; + std::vector childFactors; + EliminationData(EliminationData* _parentData, size_t nChildren) : + parentData(_parentData) { childFactors.reserve(nChildren); } + }; + + /* ************************************************************************* */ + template + EliminationData eliminationPreOrderVisitor( + const typename TREE::sharedNode& node, EliminationData* parentData) + { + // This function is called before visiting the children. Here, we create this node's data, + // which includes a pointer to the parent data and space for the factors of the children. + return EliminationData(parentData, node->children.size()); + } + + /* ************************************************************************* */ + template + void eliminationPostOrderVisitor(const TREE::Node* const node, EliminationData& myData, + RESULT& result, const typename TREE::Eliminate& eliminationFunction) + { + // Call eliminate on the node and add the result to the parent's gathered factors + myData.parentData->childFactors.push_back(node->eliminate(result, eliminationFunction, myData.childFactors)); + } + } + + /* ************************************************************************* */ + /** Eliminate an elimination tree or a Bayes tree (used internally). Requires + * TREE::BayesNetType, TREE::FactorGraphType, TREE::sharedConditional, TREE::sharedFactor, + * TREE::Node, TREE::sharedNode, TREE::Node::factors, TREE::Node::children. */ + template + std::vector + EliminateTree(RESULT& result, const TREE& tree, const typename TREE::Eliminate& function) + { + // Typedefs + typedef typename TREE::sharedNode sharedNode; + typedef typename TREE::sharedFactor sharedFactor; + + // Allocate remaining factors + std::vector remainingFactors; + remainingFactors.reserve(tree.roots().size()); + + treeTraversal::DepthFirstForest(tree, remainingFactors, ) + + // Stack for eliminating nodes. We use this stack instead of recursive function calls to + // avoid call stack overflow due to very long trees that arise from chain-like graphs. + // TODO: Check whether this is faster as a vector (then use indices instead of parent pointers). + typedef EliminationNode EliminationNode; + std::stack > eliminationStack; + + // Allocate remaining factors + std::vector remainingFactors; + remainingFactors.reserve(tree.roots().size()); + + // Add roots to the stack (use reverse foreach so conditionals to appear in elimination order - + // doesn't matter for computation but can make printouts easier to interpret by hand). + BOOST_REVERSE_FOREACH(const sharedNode& root, tree.roots()) { + eliminationStack.push( + EliminationNode(root.get(), 0)); } + + // Until the stack is empty + while(!eliminationStack.empty()) { + // Process the next node. If it has children, add its children to the stack and mark it + // expanded - we'll come back and eliminate it later after the children have been processed. + EliminationNode& node = eliminationStack.top(); + if(node.expanded) + { + // Do elimination step + sharedFactor remainingFactor = node.treeNode->eliminate(result, function, node.childrenFactors); + + // TODO: Don't add null factor? + if(node.parent) + node.parent->childrenFactors.push_back(remainingFactor); + else + remainingFactors.push_back(remainingFactor); + + // Remove from stack + eliminationStack.pop(); + } else + { + // Expand children and mark as expanded (use reverse foreach so conditionals to appear in + // elimination order - doesn't matter for computation but can make printouts easier to + // interpret by hand). + node.expanded = true; + BOOST_REVERSE_FOREACH(const sharedNode& child, node.treeNode->children) { + eliminationStack.push( + EliminationNode(child.get(), &node)); } + } + } + + // Return remaining factors + return remainingFactors; + } + + } +} \ No newline at end of file diff --git a/gtsam/symbolic/SymbolicConditionalUnordered.cpp b/gtsam/symbolic/SymbolicConditionalUnordered.cpp index 07492f312..7c014b587 100644 --- a/gtsam/symbolic/SymbolicConditionalUnordered.cpp +++ b/gtsam/symbolic/SymbolicConditionalUnordered.cpp @@ -15,6 +15,7 @@ * @date Oct 17, 2010 */ +#include #include namespace gtsam { diff --git a/gtsam/symbolic/SymbolicEliminationTreeUnordered.cpp b/gtsam/symbolic/SymbolicEliminationTreeUnordered.cpp new file mode 100644 index 000000000..225e36b99 --- /dev/null +++ b/gtsam/symbolic/SymbolicEliminationTreeUnordered.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 SymbolicEliminationTreeUnordered.cpp + * @date Mar 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include + +namespace gtsam { + + SymbolicEliminationTreeUnordered::SymbolicEliminationTreeUnordered() { + + } + +} diff --git a/gtsam/symbolic/SymbolicEliminationTreeUnordered.h b/gtsam/symbolic/SymbolicEliminationTreeUnordered.h index 5aa8b12e3..67f0095eb 100644 --- a/gtsam/symbolic/SymbolicEliminationTreeUnordered.h +++ b/gtsam/symbolic/SymbolicEliminationTreeUnordered.h @@ -22,7 +22,7 @@ namespace gtsam { - class SymbolicEliminationTreeUnordered : + class GTSAM_EXPORT SymbolicEliminationTreeUnordered : public EliminationTreeUnordered { public: typedef EliminationTreeUnordered Base; ///< Base class @@ -60,10 +60,16 @@ namespace gtsam { private: /// Private default constructor - SymbolicEliminationTreeUnordered() {} + SymbolicEliminationTreeUnordered(); friend class ::EliminationTreeUnorderedTester; }; +//#ifdef GTSAM_INSTANTIATE_SymbolicEliminationTreeUnordered +// template GTSAM_EXPORT class EliminationTreeUnordered; +//#else +// extern template GTSAM_EXPORT class EliminationTreeUnordered; +//#endif + } diff --git a/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp b/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp index 5843a681b..bfa8dc268 100644 --- a/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp +++ b/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp @@ -17,6 +17,7 @@ #include +#include #include namespace gtsam { diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index b62cf7166..1721e01e4 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -18,7 +18,10 @@ #include +#include #include +using namespace boost::assign; +#include #include #include @@ -39,12 +42,12 @@ public: SymbolicEliminationTreeUnordered::sharedNode node1(new SymbolicEliminationTreeUnordered::Node); node1->key = 1; node1->factors.push_back(fg[2]); - node1->subTrees.push_back(leaf0); + node1->children.push_back(leaf0); SymbolicEliminationTreeUnordered::sharedNode node2(new SymbolicEliminationTreeUnordered::Node); node2->key = 2; node2->factors.push_back(fg[3]); - node2->subTrees.push_back(node1); + node2->children.push_back(node1); SymbolicEliminationTreeUnordered::sharedNode leaf3(new SymbolicEliminationTreeUnordered::Node); leaf3->key = 3; @@ -52,8 +55,8 @@ public: SymbolicEliminationTreeUnordered::sharedNode root(new SymbolicEliminationTreeUnordered::Node); root->key = 4; - root->subTrees.push_back(leaf3); - root->subTrees.push_back(node2); + root->children.push_back(leaf3); + root->children.push_back(node2); SymbolicEliminationTreeUnordered tree; tree.roots_.push_back(root); @@ -131,7 +134,7 @@ TEST(EliminationTree, disconnected_graph) { vector order; order += 0,1,2,3,4; SymbolicBayesNetUnordered actual = *SymbolicEliminationTreeUnordered(fg, order).eliminate(EliminateSymbolicUnordered).first; - + EXPECT(assert_equal(expected,actual)); }