gtsam  4.1.0
gtsam
treeTraversal-inst.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
17 #pragma once
18 
19 #include <gtsam/base/treeTraversal/parallelTraversalTasks.h>
20 #include <gtsam/base/treeTraversal/statistics.h>
21 
22 #include <gtsam/base/FastList.h>
23 #include <gtsam/base/FastVector.h>
24 #include <gtsam/inference/Key.h>
25 #include <gtsam/config.h> // for GTSAM_USE_TBB
26 
27 #include <stack>
28 #include <vector>
29 #include <string>
30 #include <boost/shared_ptr.hpp>
31 #include <boost/make_shared.hpp>
32 
33 namespace gtsam {
34 
36 namespace treeTraversal {
37 
38 /* ************************************************************************* */
39 namespace {
40 // Internal node used in DFS preorder stack
41 template<typename NODE, typename DATA>
42 struct TraversalNode {
43  bool expanded;
44  const boost::shared_ptr<NODE>& treeNode;
45  DATA& parentData;
46  typename FastList<DATA>::iterator dataPointer;
47  TraversalNode(const boost::shared_ptr<NODE>& _treeNode, DATA& _parentData) :
48  expanded(false), treeNode(_treeNode), parentData(_parentData) {
49  }
50 };
51 
52 // Do nothing - default argument for post-visitor for tree traversal
53 struct no_op {
54  template<typename NODE, typename DATA>
55  void operator()(const boost::shared_ptr<NODE>& node, const DATA& data) {
56  }
57 };
58 
59 }
60 
75 template<class FOREST, typename DATA, typename VISITOR_PRE,
76  typename VISITOR_POST>
77 void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre,
78  VISITOR_POST& visitorPost) {
79  // Typedefs
80  typedef typename FOREST::Node Node;
81  typedef boost::shared_ptr<Node> sharedNode;
82 
83  // Depth first traversal stack
84  typedef TraversalNode<typename FOREST::Node, DATA> TraversalNode;
85  typedef FastList<TraversalNode> Stack;
86  Stack stack;
87  FastList<DATA> dataList; // List to store node data as it is returned from the pre-order visitor
88 
89  // Add roots to stack (insert such that they are visited and processed in order
90  {
91  typename Stack::iterator insertLocation = stack.begin();
92  for(const sharedNode& root: forest.roots())
93  stack.insert(insertLocation, TraversalNode(root, rootData));
94  }
95 
96  // Traverse
97  while (!stack.empty()) {
98  // Get next node
99  TraversalNode& node = stack.front();
100 
101  if (node.expanded) {
102  // If already expanded, then the data stored in the node is no longer needed, so visit
103  // then delete it.
104  (void) visitorPost(node.treeNode, *node.dataPointer);
105  dataList.erase(node.dataPointer);
106  stack.pop_front();
107  } else {
108  // If not already visited, visit the node and add its children (use reverse iterators so
109  // children are processed in the order they appear)
110  node.dataPointer = dataList.insert(dataList.end(),
111  visitorPre(node.treeNode, node.parentData));
112  typename Stack::iterator insertLocation = stack.begin();
113  for(const sharedNode& child: node.treeNode->children)
114  stack.insert(insertLocation, TraversalNode(child, *node.dataPointer));
115  node.expanded = true;
116  }
117  }
118  assert(dataList.empty());
119 }
120 
132 template<class FOREST, typename DATA, typename VISITOR_PRE>
133 void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) {
134  no_op visitorPost;
135  DepthFirstForest(forest, rootData, visitorPre, visitorPost);
136 }
137 
152 template<class FOREST, typename DATA, typename VISITOR_PRE,
153  typename VISITOR_POST>
154 void DepthFirstForestParallel(FOREST& forest, DATA& rootData,
155  VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
156  int problemSizeThreshold = 10) {
157 #ifdef GTSAM_USE_TBB
158  // Typedefs
159  typedef typename FOREST::Node Node;
160 
161  tbb::task::spawn_root_and_wait(
162  internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
163  visitorPost, problemSizeThreshold));
164 #else
165  DepthFirstForest(forest, rootData, visitorPre, visitorPost);
166 #endif
167 }
168 
169 /* ************************************************************************* */
171 namespace {
172 template<typename NODE>
173 boost::shared_ptr<NODE> CloneForestVisitorPre(
174  const boost::shared_ptr<NODE>& node,
175  const boost::shared_ptr<NODE>& parentPointer) {
176  // Clone the current node and add it to its cloned parent
177  boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(*node);
178  clone->children.clear();
179  parentPointer->children.push_back(clone);
180  return clone;
181 }
182 }
183 
189 template<class FOREST>
190 FastVector<boost::shared_ptr<typename FOREST::Node> > CloneForest(
191  const FOREST& forest) {
192  typedef typename FOREST::Node Node;
193  boost::shared_ptr<Node> rootContainer = boost::make_shared<Node>();
194  DepthFirstForest(forest, rootContainer, CloneForestVisitorPre<Node>);
195  return FastVector<boost::shared_ptr<Node> >(rootContainer->children.begin(),
196  rootContainer->children.end());
197 }
198 
199 /* ************************************************************************* */
201 namespace {
202 struct PrintForestVisitorPre {
203  const KeyFormatter& formatter;
204  PrintForestVisitorPre(const KeyFormatter& formatter) :
205  formatter(formatter) {
206  }
207  template<typename NODE> std::string operator()(
208  const boost::shared_ptr<NODE>& node, const std::string& parentString) {
209  // Print the current node
210  node->print(parentString + "-", formatter);
211  // Increment the indentation
212  return parentString + "| ";
213  }
214 };
215 }
216 
219 template<class FOREST>
220 void PrintForest(const FOREST& forest, std::string str,
221  const KeyFormatter& keyFormatter) {
222  PrintForestVisitorPre visitor(keyFormatter);
223  DepthFirstForest(forest, str, visitor);
224 }
225 }
226 
227 }
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
gtsam::treeTraversal::CloneForest
FastVector< boost::shared_ptr< typename FOREST::Node > > CloneForest(const FOREST &forest)
Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child pointers ...
Definition: treeTraversal-inst.h:190
gtsam::treeTraversal::DepthFirstForest
void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost)
Traverse a forest depth-first with pre-order and post-order visits.
Definition: treeTraversal-inst.h:77
gtsam::FastList
Definition: FastList.h:38
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::treeTraversal::PrintForest
void PrintForest(const FOREST &forest, std::string str, const KeyFormatter &keyFormatter)
Print a tree, prefixing each line with str, and formatting keys using keyFormatter.
Definition: treeTraversal-inst.h:220
Key.h
gtsam::KeyFormatter
boost::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
FastList.h
A thin wrapper around std::list that uses boost's fast_pool_allocator.
gtsam::treeTraversal::DepthFirstForestParallel
void DepthFirstForestParallel(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost, int problemSizeThreshold=10)
Traverse a forest depth-first with pre-order and post-order visits.
Definition: treeTraversal-inst.h:154
gtsam::stack
Matrix stack(size_t nrMatrices,...)
create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...
Definition: Matrix.cpp:396