EliminationTree-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 
18 #pragma once
19 
20 #include <stack>
21 #include <queue>
22 
23 #include <gtsam/base/timing.h>
29 
30 namespace gtsam {
31 
32  /* ************************************************************************* */
33  template<class BAYESNET, class GRAPH>
36  const std::shared_ptr<BayesNetType>& output,
37  const Eliminate& function, const FastVector<sharedFactor>& childrenResults) const
38  {
39  // This function eliminates one node (Node::eliminate) - see below eliminate for the whole tree.
40 
41  assert(childrenResults.size() == children.size());
42 
43  // Gather factors
44  FactorGraphType gatheredFactors;
45  gatheredFactors.reserve(factors.size() + children.size());
46  gatheredFactors.push_back(factors.begin(), factors.end());
47  gatheredFactors.push_back(childrenResults.begin(), childrenResults.end());
48 
49  // Do dense elimination step
50  KeyVector keyAsVector(1); keyAsVector[0] = key;
51  auto eliminationResult = function(gatheredFactors, Ordering(keyAsVector));
52 
53  // Add conditional to BayesNet
54  output->push_back(eliminationResult.first);
55 
56  // Return result
57  return eliminationResult.second;
58  }
59 
60  /* ************************************************************************* */
61  template<class BAYESNET, class GRAPH>
63  const std::string& str, const KeyFormatter& keyFormatter) const
64  {
65  std::cout << str << "(" << keyFormatter(key) << ")\n";
66  for(const sharedFactor& factor: factors) {
67  if(factor)
68  factor->print(str);
69  else
70  std::cout << str << "null factor\n";
71  }
72  }
73 
74 
75  /* ************************************************************************* */
76  template<class BAYESNET, class GRAPH>
78  const VariableIndex& structure, const Ordering& order)
79  {
80  gttic(EliminationTree_Contructor);
81 
82  // Number of factors and variables - NOTE in the case of partial elimination, n here may
83  // be fewer variables than are actually present in the graph.
84  const size_t m = graph.size();
85  const size_t n = order.size();
86 
87  static const size_t none = std::numeric_limits<size_t>::max();
88 
89  // Allocate result parent vector and vector of last factor columns
91  FastVector<size_t> parents(n, none);
92  FastVector<size_t> prevCol(m, none);
93  FastVector<bool> factorUsed(m, false);
94 
95  try {
96  // for column j \in 1 to n do
97  for (size_t j = 0; j < n; j++)
98  {
99  // Retrieve the factors involving this variable and create the current node
100  const FactorIndices& factors = structure[order[j]];
101  const sharedNode node = std::make_shared<Node>();
102  node->key = order[j];
103 
104  // for row i \in Struct[A*j] do
105  node->children.reserve(factors.size());
106  node->factors.reserve(factors.size());
107  for(const size_t i: factors) {
108  // If we already hit a variable in this factor, make the subtree containing the previous
109  // variable in this factor a child of the current node. This means that the variables
110  // eliminated earlier in the factor depend on the later variables in the factor. If we
111  // haven't yet hit a variable in this factor, we add the factor to the current node.
112  // TODO: Store root shortcuts instead of parents.
113  if (prevCol[i] != none) {
114  size_t k = prevCol[i];
115  // Find root r of the current tree that contains k. Use raw pointers in computing the
116  // parents to avoid changing the reference counts while traversing up the tree.
117  size_t r = k;
118  while (parents[r] != none)
119  r = parents[r];
120  // If the root of the subtree involving this node is actually the current node,
121  // TODO: what does this mean? forest?
122  if (r != j) {
123  // Now that we found the root, hook up parent and child pointers in the nodes.
124  parents[r] = j;
125  node->children.push_back(nodes[r]);
126  }
127  } else {
128  // Add the factor to the current node since we are at the first variable in this factor.
129  node->factors.push_back(graph[i]);
130  factorUsed[i] = true;
131  }
132  prevCol[i] = j;
133  }
134  nodes[j] = node;
135  }
136  } catch(std::invalid_argument& e) {
137  // If this is thrown from structure[order[j]] above, it means that it was requested to
138  // eliminate a variable not present in the graph, so throw a more informative error message.
139  (void)e; // Prevent unused variable warning
140  throw std::invalid_argument("EliminationTree: given ordering contains variables that are not involved in the factor graph");
141  } catch(...) {
142  throw;
143  }
144 
145  // Find roots
146  assert(parents.empty() || parents.back() == none); // We expect the last-eliminated node to be a root no matter what
147  for(size_t j = 0; j < n; ++j)
148  if(parents[j] == none)
149  roots_.push_back(nodes[j]);
150 
151  // Gather remaining factors (exclude null factors)
152  for(size_t i = 0; i < m; ++i)
153  if(!factorUsed[i] && graph[i])
154  remainingFactors_.push_back(graph[i]);
155  }
156 
157  /* ************************************************************************* */
158  template<class BAYESNET, class GRAPH>
160  const FactorGraphType& factorGraph, const Ordering& order)
161  {
162  gttic(ET_Create2);
163  // Build variable index first
164  const VariableIndex variableIndex(factorGraph);
165  This temp(factorGraph, variableIndex, order);
166  this->swap(temp); // Swap in the tree, and temp will be deleted
167  }
168 
169  /* ************************************************************************* */
170  template<class BAYESNET, class GRAPH>
173  {
174  // Start by duplicating the tree.
176 
177  // Assign the remaining factors - these are pointers to factors in the original factor graph and
178  // we do not clone them.
179  remainingFactors_ = other.remainingFactors_;
180 
181  return *this;
182  }
183 
184  /* ************************************************************************* */
185 
191  template<class BAYESNET, class GRAPH>
193  {
194  // For each tree, we first move the root into a queue; then we do a BFS on the tree with the queue;
195 
196  for (auto&& root : roots_) {
197  std::queue<sharedNode> bfs_queue;
198 
199  // first, move the root to the queue
200  bfs_queue.push(root);
201  root = nullptr; // now the root node is owned by the queue
202 
203  // for each node iterated, if its reference count is 1, it will be deleted while its children are still in the queue.
204  // so that the recursive deletion will not happen.
205  while (!bfs_queue.empty()) {
206  // move the ownership of the front node from the queue to the current variable
207  auto node = bfs_queue.front();
208  bfs_queue.pop();
209 
210  // add the children of the current node to the queue, so that the queue will also own the children nodes.
211  for (auto&& child : node->children) {
212  bfs_queue.push(child);
213  } // leaving the scope of current will decrease the reference count of the current node by 1, and if the reference count is 0,
214  // the node will be deleted. Because the children are in the queue, the deletion of the node will not trigger a recursive
215  // deletion of the children.
216  }
217  }
218  }
219 
220 
221  /* ************************************************************************* */
222  template<class BAYESNET, class GRAPH>
223  std::pair<std::shared_ptr<BAYESNET>, std::shared_ptr<GRAPH> >
225  {
226  gttic(EliminationTree_eliminate);
227  // Allocate result
228  auto result = std::make_shared<BayesNetType>();
229 
230  // Run tree elimination algorithm
232 
233  // Add remaining factors that were not involved with eliminated variables
234  auto allRemainingFactors = std::make_shared<FactorGraphType>();
235  allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end());
236  allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end());
237 
238  // Return result
239  return {result, allRemainingFactors};
240  }
241 
242  /* ************************************************************************* */
243  template<class BAYESNET, class GRAPH>
244  void EliminationTree<BAYESNET,GRAPH>::print(const std::string& name, const KeyFormatter& formatter) const
245  {
247  }
248 
249  /* ************************************************************************* */
250  template<class BAYESNET, class GRAPH>
252  {
253  // Depth-first-traversal stacks
254  std::stack<sharedNode, FastVector<sharedNode> > stack1, stack2;
255 
256  // Add roots in sorted order
257  {
259  for(const sharedNode& root: this->roots_) { keys.emplace(root->key, root); }
260  typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
261  for(const Key_Node& key_node: keys) { stack1.push(key_node.second); }
262  }
263  {
265  for(const sharedNode& root: expected.roots_) { keys.emplace(root->key, root); }
266  typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
267  for(const Key_Node& key_node: keys) { stack2.push(key_node.second); }
268  }
269 
270  // Traverse, adding children in sorted order
271  while(!stack1.empty() && !stack2.empty()) {
272  // Pop nodes
273  sharedNode node1 = stack1.top();
274  stack1.pop();
275  sharedNode node2 = stack2.top();
276  stack2.pop();
277 
278  // Compare nodes
279  if(node1->key != node2->key)
280  return false;
281  if(node1->factors.size() != node2->factors.size()) {
282  return false;
283  } else {
284  for(typename Node::Factors::const_iterator it1 = node1->factors.begin(), it2 = node2->factors.begin();
285  it1 != node1->factors.end(); ++it1, ++it2) // Only check it1 == end because we already returned false for different counts
286  {
287  if(*it1 && *it2) {
288  if(!(*it1)->equals(**it2, tol))
289  return false;
290  } else if((*it1 && !*it2) || (*it2 && !*it1)) {
291  return false;
292  }
293  }
294  }
295 
296  // Add children in sorted order
297  {
299  for(const sharedNode& node: node1->children) { keys.emplace(node->key, node); }
300  typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
301  for(const Key_Node& key_node: keys) { stack1.push(key_node.second); }
302  }
303  {
305  for(const sharedNode& node: node2->children) { keys.emplace(node->key, node); }
306  typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
307  for(const Key_Node& key_node: keys) { stack2.push(key_node.second); }
308  }
309  }
310 
311  // If either stack is not empty, the number of nodes differed
312  if(!stack1.empty() || !stack2.empty())
313  return false;
314 
315  return true;
316  }
317 
318  /* ************************************************************************* */
319  template<class BAYESNET, class GRAPH>
321  roots_.swap(other.roots_);
322  remainingFactors_.swap(other.remainingFactors_);
323  }
324 
325 
326 }
timing.h
Timing utilities.
gtsam::EliminationTree::Node::factors
Factors factors
factors associated with root
Definition: EliminationTree.h:71
name
Annotation for function names.
Definition: attr.h:51
gtsam::EliminationTree::roots_
FastVector< sharedNode > roots_
Definition: EliminationTree.h:86
gtsam::EliminationTree::Eliminate
GRAPH::Eliminate Eliminate
Definition: EliminationTree.h:64
treeTraversal-inst.h
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::EliminationTree::remainingFactors_
FastVector< sharedFactor > remainingFactors_
Definition: EliminationTree.h:87
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
inference-inst.h
Contains generic inference algorithms that convert between templated graphical models,...
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
gtsam::EliminationTree::~EliminationTree
~EliminationTree()
Definition: EliminationTree-inst.h:192
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
gtsam::EliminationTree::swap
void swap(This &other)
Definition: EliminationTree-inst.h:320
gtsam::FastMap
Definition: FastMap.h:39
gtsam::EliminationTree::print
void print(const std::string &name="EliminationTree: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
Definition: EliminationTree-inst.h:244
formatter
const KeyFormatter & formatter
Definition: treeTraversal-inst.h:204
Ordering.h
Variable ordering for the elimination algorithm.
gtsam::EliminationTree< GaussianBayesNet, GaussianFactorGraph >
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
gtsam::EliminationTree::Node::children
Children children
sub-trees
Definition: EliminationTree.h:72
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::EliminationTree::Node::eliminate
sharedFactor eliminate(const std::shared_ptr< BayesNetType > &output, const Eliminate &function, const FastVector< sharedFactor > &childrenFactors) const
Definition: EliminationTree-inst.h:35
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::KeyFormatter
std::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::EliminationTree::Node::key
Key key
key associated with root
Definition: EliminationTree.h:70
gtsam::EliminationTree::EliminationTree
EliminationTree()
Protected default constructor.
Definition: EliminationTree.h:162
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::EliminationTree::eliminate
std::pair< std::shared_ptr< BayesNetType >, std::shared_ptr< FactorGraphType > > eliminate(Eliminate function) const
Definition: EliminationTree-inst.h:224
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::VariableIndex
Definition: VariableIndex.h:41
gtsam::EliminationTree< GaussianBayesNet, GaussianFactorGraph >::sharedNode
std::shared_ptr< Node > sharedNode
Shared pointer to Node.
Definition: EliminationTree.h:80
str
Definition: pytypes.h:1558
key
const gtsam::Symbol key('X', 0)
gtsam::inference::EliminateTree
FastVector< typename TREE::sharedFactor > EliminateTree(RESULT &result, const TREE &tree, const typename TREE::Eliminate &function)
Definition: inference-inst.h:74
nodes
KeyVector nodes
Definition: testMFAS.cpp:28
gtsam::EliminationTree::FactorGraphType
GRAPH FactorGraphType
The factor graph type.
Definition: EliminationTree.h:58
gtsam::treeTraversal::PrintForest
void PrintForest(const FOREST &forest, std::string str, const KeyFormatter &keyFormatter)
Definition: treeTraversal-inst.h:221
gtsam
traits
Definition: SFMdata.h:40
gtsam::EliminationTree::Node::print
void print(const std::string &str, const KeyFormatter &keyFormatter) const
Definition: EliminationTree-inst.h:62
gtsam::treeTraversal::CloneForest
FastVector< std::shared_ptr< typename FOREST::Node > > CloneForest(const FOREST &forest)
Definition: treeTraversal-inst.h:191
VariableIndex.h
gtsam::EliminationTree::operator=
This & operator=(const This &other)
Definition: EliminationTree-inst.h:172
gtsam::EliminationTree::sharedFactor
std::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition: EliminationTree.h:60
gtsam::tol
const G double tol
Definition: Group.h:79
EliminationTree.h
none
Definition: pytypes.h:1786
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
max
#define max(a, b)
Definition: datatypes.h:20
gtsam::EliminationTree::equals
bool equals(const This &other, double tol=1e-9) const
Definition: EliminationTree-inst.h:251
gtsam::Ordering
Definition: inference/Ordering.h:33
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
gttic
#define gttic(label)
Definition: timing.h:295
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:37
gtsam::EliminationTree::remainingFactors
const FastVector< sharedFactor > & remainingFactors() const
Definition: EliminationTree.h:155


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:29