gtsam  4.1.0
gtsam
inference-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 
20 #pragma once
21 
22 #include <boost/shared_ptr.hpp>
23 #include <boost/bind.hpp>
24 #include <utility>
25 
27 #include <gtsam/base/FastVector.h>
28 
29 namespace gtsam {
30  namespace inference {
31 
32  namespace {
33  /* ************************************************************************* */
34  template<class TREE>
35  struct EliminationData {
36  EliminationData* const parentData;
37  FastVector<typename TREE::sharedFactor> childFactors;
38  EliminationData(EliminationData* _parentData, size_t nChildren) :
39  parentData(_parentData) { childFactors.reserve(nChildren); }
40  };
41 
42  /* ************************************************************************* */
43  template<class TREE>
44  EliminationData<TREE> eliminationPreOrderVisitor(
45  const typename TREE::sharedNode& node, EliminationData<TREE>& parentData)
46  {
47  // This function is called before visiting the children. Here, we create this node's data,
48  // which includes a pointer to the parent data and space for the factors of the children.
49  return EliminationData<TREE>(&parentData, node->children.size());
50  }
51 
52  /* ************************************************************************* */
53  template<class TREE, class RESULT>
54  struct EliminationPostOrderVisitor
55  {
56  RESULT& result;
57  const typename TREE::Eliminate& eliminationFunction;
58  EliminationPostOrderVisitor(RESULT& result, const typename TREE::Eliminate& eliminationFunction) :
59  result(result), eliminationFunction(eliminationFunction) {}
60  void operator()(const typename TREE::sharedNode& node, EliminationData<TREE>& myData)
61  {
62  // Call eliminate on the node and add the result to the parent's gathered factors
63  typename TREE::sharedFactor childFactor = node->eliminate(result, eliminationFunction, myData.childFactors);
64  if(childFactor && !childFactor->empty())
65  myData.parentData->childFactors.push_back(childFactor);
66  }
67  };
68  }
69 
70  /* ************************************************************************* */
74  template<class TREE, class RESULT>
75  FastVector<typename TREE::sharedFactor>
76  EliminateTree(RESULT& result, const TREE& tree, const typename TREE::Eliminate& function)
77  {
78  // Do elimination using a depth-first traversal. During the pre-order visit (see
79  // eliminationPreOrderVisitor), we store a pointer to the parent data (where we'll put the
80  // remaining factor) and reserve a vector of factors to store the children elimination
81  // results. During the post-order visit (see eliminationPostOrderVisitor), we call dense
82  // elimination (using the gathered child factors) and store the result in the parent's
83  // gathered factors.
84  EliminationData<TREE> rootData(0, tree.roots().size());
85  EliminationPostOrderVisitor<TREE,RESULT> visitorPost(result, function);
86  treeTraversal::DepthFirstForest(tree, rootData, eliminationPreOrderVisitor<TREE>, visitorPost);
87 
88  // Return remaining factors
89  return rootData.childFactors;
90  }
91 
92  }
93 }
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
gtsam::inference::EliminateTree
FastVector< typename TREE::sharedFactor > EliminateTree(RESULT &result, const TREE &tree, const typename TREE::Eliminate &function)
Eliminate an elimination tree or a Bayes tree (used internally).
Definition: inference-inst.h:76
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
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
treeTraversal-inst.h