diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 3edd7d076..669186e3f 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -1,19 +1,19 @@ /* ---------------------------------------------------------------------------- -* 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) + * 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 + * See LICENSE for the license information -* -------------------------------------------------------------------------- */ + * -------------------------------------------------------------------------- */ /** -* @file treeTraversal-inst.h -* @author Richard Roberts -* @date April 9, 2013 -*/ + * @file treeTraversal-inst.h + * @author Richard Roberts + * @date April 9, 2013 + */ #pragma once #include @@ -33,192 +33,197 @@ namespace gtsam { - /** Internal functions used for traversing trees */ - namespace treeTraversal { +/** Internal functions used for traversing trees */ +namespace treeTraversal { - /* ************************************************************************* */ - namespace { - // Internal node used in DFS preorder stack - template - struct TraversalNode { - bool expanded; - const boost::shared_ptr& treeNode; - DATA& parentData; - typename FastList::iterator dataPointer; - TraversalNode(const boost::shared_ptr& _treeNode, DATA& _parentData) : - expanded(false), treeNode(_treeNode), parentData(_parentData) {} - }; - - // Do nothing - default argument for post-visitor for tree traversal - struct no_op { - template - void operator()(const boost::shared_ptr& node, const DATA& data) {} - }; - - } - - /** Traverse a forest depth-first with pre-order and post-order visits. - * @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) - { - // Typedefs - typedef typename FOREST::Node Node; - typedef boost::shared_ptr sharedNode; - - // Depth first traversal stack - typedef TraversalNode TraversalNode; - typedef FastList Stack; - Stack stack; - FastList dataList; // List to store node data as it is returned from the pre-order visitor - - // Add roots to stack (insert such that they are visited and processed in order - { - typename Stack::iterator insertLocation = stack.begin(); - BOOST_FOREACH(const sharedNode& root, forest.roots()) - stack.insert(insertLocation, TraversalNode(root, rootData)); - } - - // Traverse - while(!stack.empty()) - { - // Get next node - TraversalNode& node = stack.front(); - - 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.dataPointer); - dataList.erase(node.dataPointer); - stack.pop_front(); - } else { - // If not already visited, visit the node and add its children (use reverse iterators so - // children are processed in the order they appear) - node.dataPointer = dataList.insert(dataList.end(), visitorPre(node.treeNode, node.parentData)); - typename Stack::iterator insertLocation = stack.begin(); - BOOST_FOREACH(const sharedNode& child, node.treeNode->children) - stack.insert(insertLocation, TraversalNode(child, *node.dataPointer)); - node.expanded = true; - } - } - assert(dataList.empty()); - } - - /** Traverse a forest depth-first, with a pre-order visit but no post-order visit. - * @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 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) - { - no_op visitorPost; - DepthFirstForest(forest, rootData, visitorPre, visitorPost); - } - - /** Traverse a forest depth-first with pre-order and post-order visits. - * @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 DepthFirstForestParallel(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold = 10) - { -#ifdef GTSAM_USE_TBB - // Typedefs - typedef typename FOREST::Node Node; - typedef boost::shared_ptr sharedNode; - - tbb::task::spawn_root_and_wait(internal::CreateRootTask( - forest.roots(), rootData, visitorPre, visitorPost, problemSizeThreshold)); -#else - DepthFirstForest(forest, rootData, visitorPre, visitorPost); -#endif - } - - - /* ************************************************************************* */ - /** Traversal function for CloneForest */ - namespace { - template - boost::shared_ptr - CloneForestVisitorPre(const boost::shared_ptr& node, const boost::shared_ptr& parentPointer) - { - // Clone the current node and add it to its cloned parent - boost::shared_ptr clone = boost::make_shared(*node); - clone->children.clear(); - parentPointer->children.push_back(clone); - return clone; - } - } - - /** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child - * pointers for a clone of the original tree. - * @param forest The forest of trees to clone. The method \c forest.roots() should exist and - * return a collection of shared pointers to \c FOREST::Node. - * @return The new collection of roots. */ - template - FastVector > CloneForest(const FOREST& forest) - { - typedef typename FOREST::Node Node; - boost::shared_ptr rootContainer = boost::make_shared(); - DepthFirstForest(forest, rootContainer, CloneForestVisitorPre); - return FastVector >(rootContainer->children.begin(), rootContainer->children.end()); - } - - - /* ************************************************************************* */ - /** Traversal function for PrintForest */ - namespace { - struct PrintForestVisitorPre - { - const KeyFormatter& formatter; - PrintForestVisitorPre(const KeyFormatter& formatter) : formatter(formatter) {} - template std::string operator()(const boost::shared_ptr& node, const std::string& parentString) - { - // 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, std::string str, const KeyFormatter& keyFormatter) { - PrintForestVisitorPre visitor(keyFormatter); - DepthFirstForest(forest, str, visitor); - } +/* ************************************************************************* */ +namespace { +// Internal node used in DFS preorder stack +template +struct TraversalNode { + bool expanded; + const boost::shared_ptr& treeNode; + DATA& parentData; + typename FastList::iterator dataPointer; + TraversalNode(const boost::shared_ptr& _treeNode, DATA& _parentData) : + expanded(false), treeNode(_treeNode), parentData(_parentData) { } +}; + +// Do nothing - default argument for post-visitor for tree traversal +struct no_op { + template + void operator()(const boost::shared_ptr& node, const DATA& data) { + } +}; + +} + +/** Traverse a forest depth-first with pre-order and post-order visits. + * @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) { + // Typedefs + typedef typename FOREST::Node Node; + typedef boost::shared_ptr sharedNode; + + // Depth first traversal stack + typedef TraversalNode TraversalNode; + typedef FastList Stack; + Stack stack; + FastList dataList; // List to store node data as it is returned from the pre-order visitor + + // Add roots to stack (insert such that they are visited and processed in order + { + typename Stack::iterator insertLocation = stack.begin(); + BOOST_FOREACH(const sharedNode& root, forest.roots()) + stack.insert(insertLocation, TraversalNode(root, rootData)); + } + + // Traverse + while (!stack.empty()) { + // Get next node + TraversalNode& node = stack.front(); + + 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.dataPointer); + dataList.erase(node.dataPointer); + stack.pop_front(); + } else { + // If not already visited, visit the node and add its children (use reverse iterators so + // children are processed in the order they appear) + node.dataPointer = dataList.insert(dataList.end(), + visitorPre(node.treeNode, node.parentData)); + typename Stack::iterator insertLocation = stack.begin(); + BOOST_FOREACH(const sharedNode& child, node.treeNode->children) + stack.insert(insertLocation, TraversalNode(child, *node.dataPointer)); + node.expanded = true; + } + } + assert(dataList.empty()); +} + +/** Traverse a forest depth-first, with a pre-order visit but no post-order visit. + * @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 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) { + no_op visitorPost; + DepthFirstForest(forest, rootData, visitorPre, visitorPost); +} + +/** Traverse a forest depth-first with pre-order and post-order visits. + * @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 DepthFirstForestParallel(FOREST& forest, DATA& rootData, + VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, + int problemSizeThreshold = 10) { +#ifdef GTSAM_USE_TBB + // Typedefs + typedef typename FOREST::Node Node; + typedef boost::shared_ptr sharedNode; + + tbb::task::spawn_root_and_wait( + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold)); +#else + DepthFirstForest(forest, rootData, visitorPre, visitorPost); +#endif +} + +/* ************************************************************************* */ +/** Traversal function for CloneForest */ +namespace { +template +boost::shared_ptr CloneForestVisitorPre( + const boost::shared_ptr& node, + const boost::shared_ptr& parentPointer) { + // Clone the current node and add it to its cloned parent + boost::shared_ptr clone = boost::make_shared(*node); + clone->children.clear(); + parentPointer->children.push_back(clone); + return clone; +} +} + +/** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child + * pointers for a clone of the original tree. + * @param forest The forest of trees to clone. The method \c forest.roots() should exist and + * return a collection of shared pointers to \c FOREST::Node. + * @return The new collection of roots. */ +template +FastVector > CloneForest( + const FOREST& forest) { + typedef typename FOREST::Node Node; + boost::shared_ptr rootContainer = boost::make_shared(); + DepthFirstForest(forest, rootContainer, CloneForestVisitorPre); + return FastVector >(rootContainer->children.begin(), + rootContainer->children.end()); +} + +/* ************************************************************************* */ +/** Traversal function for PrintForest */ +namespace { +struct PrintForestVisitorPre { + const KeyFormatter& formatter; + PrintForestVisitorPre(const KeyFormatter& formatter) : + formatter(formatter) { + } + template std::string operator()( + const boost::shared_ptr& node, const std::string& parentString) { + // 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, std::string str, + const KeyFormatter& keyFormatter) { + PrintForestVisitorPre visitor(keyFormatter); + DepthFirstForest(forest, str, visitor); +} +} }