diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 7268f36b2..11e4f0961 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -28,7 +28,7 @@ namespace gtsam { template struct TraversalNode { bool expanded; - NODE* const treeNode; + NODE& const treeNode; DATA data; TraversalNode(NODE* _treeNode, const DATA& _data) : expanded(false), treeNode(_treeNode), data(_data) {} @@ -38,16 +38,14 @@ namespace gtsam { template struct PreOrderExpand { VISITOR& visitor_; - DATA* parentData_; + DATA& parentData_; STACK& stack_; - 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_))); + stack_.push(TraversalNode(*nodePointer, visitor_(nodePointer, parentData_))); } }; @@ -82,7 +80,7 @@ namespace gtsam { // 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(visitorPre, &rootData, stack)); + Expander(visitorPre, rootData, stack)); // Traverse while(!stack.empty()) @@ -93,13 +91,13 @@ namespace gtsam { 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); + (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)); + Expander(visitorPre, node.data, stack)); node.expanded = true; } } diff --git a/gtsam/inference/EliminationTreeUnordered-inst.h b/gtsam/inference/EliminationTreeUnordered-inst.h index 589bdcbb2..b9873c058 100644 --- a/gtsam/inference/EliminationTreeUnordered-inst.h +++ b/gtsam/inference/EliminationTreeUnordered-inst.h @@ -37,6 +37,8 @@ namespace gtsam { const boost::shared_ptr& output, const Eliminate& function, const std::vector& childrenResults) const { + // This function eliminates one node (Node::eliminate) - see below eliminate for the whole tree. + assert(childrenResults.size() == children.size()); // Gather factors @@ -146,8 +148,7 @@ namespace gtsam { // Build variable index first const VariableIndexUnordered variableIndex(factorGraph); This temp(factorGraph, variableIndex, order); - roots_.swap(temp.roots_); // Swap in the tree, and temp will be deleted - remainingFactors_.swap(temp.remainingFactors_); + this->swap(temp); // Swap in the tree, and temp will be deleted } /* ************************************************************************* */ @@ -295,4 +296,12 @@ namespace gtsam { return true; } + /* ************************************************************************* */ + template + void EliminationTreeUnordered::swap(This& other) { + roots_.swap(other.roots_); + remainingFactors_.swap(other.remainingFactors_); + } + + } diff --git a/gtsam/inference/EliminationTreeUnordered.h b/gtsam/inference/EliminationTreeUnordered.h index ced271d14..764ced70d 100644 --- a/gtsam/inference/EliminationTreeUnordered.h +++ b/gtsam/inference/EliminationTreeUnordered.h @@ -65,8 +65,8 @@ namespace gtsam { Eliminate; ///< Typedef for an eliminate subroutine struct Node { - typedef FastList Factors; - typedef FastList > Children; + typedef std::vector Factors; + typedef std::vector > Children; Key key; ///< key associated with root Factors factors; ///< factors associated with root @@ -146,6 +146,9 @@ namespace gtsam { const FastList& roots() const { return roots_; } + /** Swap the data of this tree with another one, this operation is very fast. */ + void swap(This& other); + protected: /// Protected default constructor EliminationTreeUnordered() {} diff --git a/gtsam/inference/JunctionTreeUnordered-inst.h b/gtsam/inference/JunctionTreeUnordered-inst.h index 1fc58cbb6..98bafb694 100644 --- a/gtsam/inference/JunctionTreeUnordered-inst.h +++ b/gtsam/inference/JunctionTreeUnordered-inst.h @@ -28,6 +28,71 @@ namespace gtsam { + namespace { + /* ************************************************************************* */ + template + struct ConstructorTraversalData { + const ConstructorTraversalData& parentData; + typename JunctionTreeUnordered::sharedNode myJTNode; + ConstructorTraversalData(const ConstructorTraversalData& _parentData) : parentData(_parentData) {} + }; + + /* ************************************************************************* */ + template + ConstructorTraversalData ConstructorTraversalVisitorPre( + const boost::shared_ptr& node, + const ConstructorTraversalData& parentData) + { + // On the pre-order pass, before children have been visited, we just set up a traversal data + // structure with its own JT node, and create a child pointer in its parent. + ConstructorTraversalData myData = ConstructorTraversalData(parentData); + myData.myJTNode = boost::make_shared::Node>(); + myData.myJTNode->keys.push_back(node->key); + myData.myJTNode->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end()); + parentData.myJTNode->children.push_back(myData.myJTNode); + return myData; + } + + /* ************************************************************************* */ + template + struct ConstructorTraversalVisitorPost + { + // Store and increment an iterator into the Bayes net during the post-order step + FactorGraphUnordered::const_iterator symbolicBayesNetIterator; + + // Constructor that starts at the beginning of the Bayes net + ConstructorTraversalVisitorPost(const FactorGraphUnordered& symbolicBayesNet) : + symbolicBayesNetIterator(symbolicBayesNet.begin()) {} + + // Post-order visitor function + void operator()( + const boost::shared_ptr& node, + const ConstructorTraversalData& myData) + { + // In the post-order step, we check if we are in the same clique with our parent. If so, we + // merge our JT nodes. + if() + } + }; + + } + + /* ************************************************************************* */ + template + template + JunctionTreeUnordered(const ETREE& eliminationTree, + const FactorGraphUnordered& symbolicBayesNet) + { + // Here we rely on the BayesNet having been produced by this elimination tree, such that the + // conditionals are arranged in DFS post-order. We traverse the elimination tree, and inspect + // the symbolic conditional corresponding to each node. The elimination tree node is added to + // the same clique with its parent if it has exactly one more Bayes net conditional parent than + // does its elimination tree parent. + + + } + + /* ************************************************************************* */ template void JunctionTree::construct(const FG& fg, const VariableIndex& variableIndex) { diff --git a/gtsam/inference/JunctionTreeUnordered.h b/gtsam/inference/JunctionTreeUnordered.h index 6de7fd4f7..200ca2e9d 100644 --- a/gtsam/inference/JunctionTreeUnordered.h +++ b/gtsam/inference/JunctionTreeUnordered.h @@ -21,51 +21,11 @@ #include #include +#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. @@ -87,86 +47,58 @@ namespace gtsam { * \addtogroup Multifrontal * \nosubgrouping */ - template::Clique> - class JunctionTree: public ClusterTree { + template + class JunctionTreeUnordered { 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 + 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 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 - /// The BayesTree type produced by elimination - typedef BTCLIQUE BTClique; + struct Node { + typedef std::vector Keys; + typedef std::vector Factors; + typedef std::vector > Children; - /// Shared pointer to this class - typedef boost::shared_ptr > shared_ptr; + Keys keys; ///< Frontal keys of this node + Factors factors; ///< Factors associated with this node + Children children; ///< sub-trees - /// We will frequently refer to a symbolic Bayes tree, used to find the clique structure - typedef gtsam::BayesTree SymbolicBayesTree; + 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: - /// @name Advanced Interface - /// @{ + /** concept check */ + GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); - /// 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); - - /// @} + FastList roots_; + std::vector remainingFactors_; 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; + /** Build the junction tree from an elimination tree and a symbolic Bayes net. */ + template + JunctionTreeUnordered( + const ETREE& eliminationTree, + const FactorGraphUnordered& symbolicBayesNet); /// @} - }; // JunctionTree + }; -} // namespace gtsam - -#include +} \ No newline at end of file