gtsam  4.1.0
gtsam
EliminationTree-inst.h
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 
18 #pragma once
19 
20 #include <boost/make_shared.hpp>
21 #include <boost/bind.hpp>
22 #include <stack>
23 
24 #include <gtsam/base/timing.h>
30 
31 namespace gtsam {
32 
33  /* ************************************************************************* */
34  template<class BAYESNET, class GRAPH>
36  EliminationTree<BAYESNET,GRAPH>::Node::eliminate(
37  const boost::shared_ptr<BayesNetType>& output,
38  const Eliminate& function, const FastVector<sharedFactor>& childrenResults) const
39  {
40  // This function eliminates one node (Node::eliminate) - see below eliminate for the whole tree.
41 
42  assert(childrenResults.size() == children.size());
43 
44  // Gather factors
45  FactorGraphType gatheredFactors;
46  gatheredFactors.reserve(factors.size() + children.size());
47  gatheredFactors.push_back(factors.begin(), factors.end());
48  gatheredFactors.push_back(childrenResults.begin(), childrenResults.end());
49 
50  // Do dense elimination step
51  KeyVector keyAsVector(1); keyAsVector[0] = key;
52  auto eliminationResult = function(gatheredFactors, Ordering(keyAsVector));
53 
54  // Add conditional to BayesNet
55  output->push_back(eliminationResult.first);
56 
57  // Return result
58  return eliminationResult.second;
59  }
60 
61  /* ************************************************************************* */
62  template<class BAYESNET, class GRAPH>
63  void EliminationTree<BAYESNET,GRAPH>::Node::print(
64  const std::string& str, const KeyFormatter& keyFormatter) const
65  {
66  std::cout << str << "(" << keyFormatter(key) << ")\n";
67  for(const sharedFactor& factor: factors) {
68  if(factor)
69  factor->print(str);
70  else
71  std::cout << str << "null factor\n";
72  }
73  }
74 
75 
76  /* ************************************************************************* */
77  template<class BAYESNET, class GRAPH>
79  const VariableIndex& structure, const Ordering& order)
80  {
81  gttic(EliminationTree_Contructor);
82 
83  // Number of factors and variables - NOTE in the case of partial elimination, n here may
84  // be fewer variables than are actually present in the graph.
85  const size_t m = graph.size();
86  const size_t n = order.size();
87 
88  static const size_t none = std::numeric_limits<size_t>::max();
89 
90  // Allocate result parent vector and vector of last factor columns
91  FastVector<sharedNode> nodes(n);
92  FastVector<size_t> parents(n, none);
93  FastVector<size_t> prevCol(m, none);
94  FastVector<bool> factorUsed(m, false);
95 
96  try {
97  // for column j \in 1 to n do
98  for (size_t j = 0; j < n; j++)
99  {
100  // Retrieve the factors involving this variable and create the current node
101  const FactorIndices& factors = structure[order[j]];
102  const sharedNode node = boost::make_shared<Node>();
103  node->key = order[j];
104 
105  // for row i \in Struct[A*j] do
106  node->children.reserve(factors.size());
107  node->factors.reserve(factors.size());
108  for(const size_t i: factors) {
109  // If we already hit a variable in this factor, make the subtree containing the previous
110  // variable in this factor a child of the current node. This means that the variables
111  // eliminated earlier in the factor depend on the later variables in the factor. If we
112  // haven't yet hit a variable in this factor, we add the factor to the current node.
113  // TODO: Store root shortcuts instead of parents.
114  if (prevCol[i] != none) {
115  size_t k = prevCol[i];
116  // Find root r of the current tree that contains k. Use raw pointers in computing the
117  // parents to avoid changing the reference counts while traversing up the tree.
118  size_t r = k;
119  while (parents[r] != none)
120  r = parents[r];
121  // If the root of the subtree involving this node is actually the current node,
122  // TODO: what does this mean? forest?
123  if (r != j) {
124  // Now that we found the root, hook up parent and child pointers in the nodes.
125  parents[r] = j;
126  node->children.push_back(nodes[r]);
127  }
128  } else {
129  // Add the factor to the current node since we are at the first variable in this factor.
130  node->factors.push_back(graph[i]);
131  factorUsed[i] = true;
132  }
133  prevCol[i] = j;
134  }
135  nodes[j] = node;
136  }
137  } catch(std::invalid_argument& e) {
138  // If this is thrown from structure[order[j]] above, it means that it was requested to
139  // eliminate a variable not present in the graph, so throw a more informative error message.
140  (void)e; // Prevent unused variable warning
141  throw std::invalid_argument("EliminationTree: given ordering contains variables that are not involved in the factor graph");
142  } catch(...) {
143  throw;
144  }
145 
146  // Find roots
147  assert(parents.empty() || parents.back() == none); // We expect the last-eliminated node to be a root no matter what
148  for(size_t j = 0; j < n; ++j)
149  if(parents[j] == none)
150  roots_.push_back(nodes[j]);
151 
152  // Gather remaining factors (exclude null factors)
153  for(size_t i = 0; i < m; ++i)
154  if(!factorUsed[i] && graph[i])
155  remainingFactors_.push_back(graph[i]);
156  }
157 
158  /* ************************************************************************* */
159  template<class BAYESNET, class GRAPH>
161  const FactorGraphType& factorGraph, const Ordering& order)
162  {
163  gttic(ET_Create2);
164  // Build variable index first
165  const VariableIndex variableIndex(factorGraph);
166  This temp(factorGraph, variableIndex, order);
167  this->swap(temp); // Swap in the tree, and temp will be deleted
168  }
169 
170  /* ************************************************************************* */
171  template<class BAYESNET, class GRAPH>
174  {
175  // Start by duplicating the tree.
176  roots_ = treeTraversal::CloneForest(other);
177 
178  // Assign the remaining factors - these are pointers to factors in the original factor graph and
179  // we do not clone them.
180  remainingFactors_ = other.remainingFactors_;
181 
182  return *this;
183  }
184 
185  /* ************************************************************************* */
186  template<class BAYESNET, class GRAPH>
187  std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<GRAPH> >
189  {
190  gttic(EliminationTree_eliminate);
191  // Allocate result
192  auto result = boost::make_shared<BayesNetType>();
193 
194  // Run tree elimination algorithm
195  FastVector<sharedFactor> remainingFactors = inference::EliminateTree(result, *this, function);
196 
197  // Add remaining factors that were not involved with eliminated variables
198  auto allRemainingFactors = boost::make_shared<FactorGraphType>();
199  allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end());
200  allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end());
201 
202  // Return result
203  return std::make_pair(result, allRemainingFactors);
204  }
205 
206  /* ************************************************************************* */
207  template<class BAYESNET, class GRAPH>
208  void EliminationTree<BAYESNET,GRAPH>::print(const std::string& name, const KeyFormatter& formatter) const
209  {
210  treeTraversal::PrintForest(*this, name, formatter);
211  }
212 
213  /* ************************************************************************* */
214  template<class BAYESNET, class GRAPH>
215  bool EliminationTree<BAYESNET,GRAPH>::equals(const This& expected, double tol) const
216  {
217  // Depth-first-traversal stacks
218  std::stack<sharedNode, FastVector<sharedNode> > stack1, stack2;
219 
220  // Add roots in sorted order
221  {
223  for(const sharedNode& root: this->roots_) { keys.insert(std::make_pair(root->key, root)); }
224  typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
225  for(const Key_Node& key_node: keys) { stack1.push(key_node.second); }
226  }
227  {
229  for(const sharedNode& root: expected.roots_) { keys.insert(std::make_pair(root->key, root)); }
230  typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
231  for(const Key_Node& key_node: keys) { stack2.push(key_node.second); }
232  }
233 
234  // Traverse, adding children in sorted order
235  while(!stack1.empty() && !stack2.empty()) {
236  // Pop nodes
237  sharedNode node1 = stack1.top();
238  stack1.pop();
239  sharedNode node2 = stack2.top();
240  stack2.pop();
241 
242  // Compare nodes
243  if(node1->key != node2->key)
244  return false;
245  if(node1->factors.size() != node2->factors.size()) {
246  return false;
247  } else {
248  for(typename Node::Factors::const_iterator it1 = node1->factors.begin(), it2 = node2->factors.begin();
249  it1 != node1->factors.end(); ++it1, ++it2) // Only check it1 == end because we already returned false for different counts
250  {
251  if(*it1 && *it2) {
252  if(!(*it1)->equals(**it2, tol))
253  return false;
254  } else if((*it1 && !*it2) || (*it2 && !*it1)) {
255  return false;
256  }
257  }
258  }
259 
260  // Add children in sorted order
261  {
263  for(const sharedNode& node: node1->children) { keys.insert(std::make_pair(node->key, node)); }
264  typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
265  for(const Key_Node& key_node: keys) { stack1.push(key_node.second); }
266  }
267  {
269  for(const sharedNode& node: node2->children) { keys.insert(std::make_pair(node->key, node)); }
270  typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
271  for(const Key_Node& key_node: keys) { stack2.push(key_node.second); }
272  }
273  }
274 
275  // If either stack is not empty, the number of nodes differed
276  if(!stack1.empty() || !stack2.empty())
277  return false;
278 
279  return true;
280  }
281 
282  /* ************************************************************************* */
283  template<class BAYESNET, class GRAPH>
285  roots_.swap(other.roots_);
286  remainingFactors_.swap(other.remainingFactors_);
287  }
288 
289 
290 }
timing.h
Timing utilities.
gtsam::EliminationTree::equals
bool equals(const This &other, double tol=1e-9) const
Test whether the tree is equal to another.
Definition: EliminationTree-inst.h:215
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::Ordering
Definition: Ordering.h:34
VariableIndex.h
gtsam::EliminationTree::sharedFactor
boost::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition: EliminationTree.h:60
gtsam::EliminationTree::Node::key
Key key
key associated with root
Definition: EliminationTree.h:70
gtsam::EliminationTree::operator=
This & operator=(const This &other)
Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are copie...
Definition: EliminationTree-inst.h:173
gtsam::EliminationTree::Node::factors
Factors factors
factors associated with root
Definition: EliminationTree.h:71
EliminationTree.h
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::EliminationTree< GaussianBayesNet, GaussianFactorGraph >::sharedNode
boost::shared_ptr< Node > sharedNode
Shared pointer to Node.
Definition: EliminationTree.h:80
inference-inst.h
Contains generic inference algorithms that convert between templated graphical models,...
gtsam::EliminationTree::Node::children
Children children
sub-trees
Definition: EliminationTree.h:72
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:32
gtsam::EliminationTree
An elimination tree is a data structure used intermediately during elimination.
Definition: EliminationTree.h:52
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
gtsam::EliminationTree::remainingFactors
const FastVector< sharedFactor > & remainingFactors() const
Return the remaining factors that are not pulled into elimination.
Definition: EliminationTree.h:154
gtsam::EliminationTree::eliminate
std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< FactorGraphType > > eliminate(Eliminate function) const
Eliminate the factors to a Bayes net and remaining factor graph.
Definition: EliminationTree-inst.h:188
gtsam::EliminationTree::swap
void swap(This &other)
Swap the data of this tree with another one, this operation is very fast.
Definition: EliminationTree-inst.h:284
gtsam::EliminationTree::print
void print(const std::string &name="EliminationTree: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print the tree to cout.
Definition: EliminationTree-inst.h:208
treeTraversal-inst.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
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::FastMap
Definition: FastMap.h:38
Ordering.h
Variable ordering for the elimination algorithm.
gtsam::EliminationTree::EliminationTree
EliminationTree()
Protected default constructor.
Definition: EliminationTree.h:161
gtsam::EliminationTree::FactorGraphType
GRAPH FactorGraphType
The factor graph type.
Definition: EliminationTree.h:58
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition: VariableIndex.h:43