HybridJunctionTree.cpp
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 
22 #include <gtsam/inference/Key.h>
23 
24 #include <unordered_map>
25 
26 namespace gtsam {
27 
28 // Instantiate base classes
29 template class EliminatableClusterTree<HybridBayesTree,
30  HybridGaussianFactorGraph>;
31 template class JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>;
32 
35  typedef
38 
44 
45  // Small inner class to store symbolic factors
46  class SymbolicFactors : public FactorGraph<Factor> {};
47 
49  : parentData(_parentData) {}
50 
51  // Pre-order visitor function
53  const std::shared_ptr<HybridEliminationTree::Node>& node,
55  // On the pre-order pass, before children have been visited, we just set up
56  // a traversal data structure with its own JT node, and create a child
57  // pointer in its parent.
60  data.junctionTreeNode = std::make_shared<Node>(node->key, node->factors);
61  parentData.junctionTreeNode->addChild(data.junctionTreeNode);
62 
63  // Add all the discrete keys in the hybrid factors to the current data
64  for (const auto& f : node->factors) {
65  if (auto hf = std::dynamic_pointer_cast<HybridFactor>(f)) {
66  for (auto& k : hf->discreteKeys()) {
67  data.discreteKeys.insert(k.first);
68  }
69  } else if (auto hf = std::dynamic_pointer_cast<DiscreteFactor>(f)) {
70  for (auto& k : hf->discreteKeys()) {
71  data.discreteKeys.insert(k.first);
72  }
73  }
74  }
75 
76  return data;
77  }
78 
79  // Post-order visitor function
81  const std::shared_ptr<HybridEliminationTree::Node>& node,
83  // In this post-order visitor, we combine the symbolic elimination results
84  // from the elimination tree children and symbolically eliminate the current
85  // elimination tree node. We then check whether each of our elimination
86  // tree child nodes should be merged with us. The check for this is that
87  // our number of symbolic elimination parents is exactly 1 less than
88  // our child's symbolic elimination parents - this condition indicates that
89  // eliminating the current node did not introduce any parents beyond those
90  // already in the child->
91 
92  // Do symbolic elimination for this node
93  SymbolicFactors symbolicFactors;
94  symbolicFactors.reserve(node->factors.size() +
95  data.childSymbolicFactors.size());
96  // Add ETree node factors
97  symbolicFactors.push_back(node->factors);
98  // Add symbolic factors passed up from children
99  symbolicFactors.push_back(data.childSymbolicFactors);
100 
101  Ordering keyAsOrdering;
102  keyAsOrdering.push_back(node->key);
103  const auto [conditional, separatorFactor] =
104  internal::EliminateSymbolic(symbolicFactors, keyAsOrdering);
105 
106  // Store symbolic elimination results in the parent
107  data.parentData->childSymbolicConditionals.push_back(conditional);
108  data.parentData->childSymbolicFactors.push_back(separatorFactor);
109  data.parentData->discreteKeys.merge(data.discreteKeys);
110 
111  sharedNode jt_node = data.junctionTreeNode;
112  const FastVector<SymbolicConditional::shared_ptr>& childConditionals =
113  data.childSymbolicConditionals;
114  jt_node->problemSize_ = (int)(conditional->size() * symbolicFactors.size());
115 
116  // Merge our children if they are in our clique - if our conditional has
117  // exactly one fewer parent than our child's conditional.
118  const size_t nrParents = conditional->nrParents();
119  const size_t nrChildren = jt_node->nrChildren();
120  assert(childConditionals.size() == nrChildren);
121 
122  // decide which children to merge, as index into children
123  std::vector<size_t> nrChildrenFrontals = jt_node->nrFrontalsOfChildren();
124  std::vector<bool> merge(nrChildren, false);
125  size_t nrFrontals = 1;
126  for (size_t i = 0; i < nrChildren; i++) {
127  // Check if we should merge the i^th child
128  if (nrParents + nrFrontals == childConditionals[i]->nrParents()) {
129  const bool myType =
130  data.discreteKeys.exists(conditional->frontals().front());
131  const bool theirType =
132  data.discreteKeys.exists(childConditionals[i]->frontals().front());
133 
134  if (myType == theirType) {
135  // Increment number of frontal variables
136  nrFrontals += nrChildrenFrontals[i];
137  merge[i] = true;
138  }
139  }
140  }
141 
142  // now really merge
143  jt_node->mergeChildren(merge);
144  }
145 };
146 
147 /* ************************************************************************* */
149  const HybridEliminationTree& eliminationTree) {
150  gttic(JunctionTree_FromEliminationTree);
151  // Here we rely on the BayesNet having been produced by this elimination tree,
152  // such that the conditionals are arranged in DFS post-order. We traverse the
153  // elimination tree, and inspect the symbolic conditional corresponding to
154  // each node. The elimination tree node is added to the same clique with its
155  // parent if it has exactly one more Bayes net conditional parent than
156  // does its elimination tree parent.
157 
158  // Traverse the elimination tree, doing symbolic elimination and merging nodes
159  // as we go. Gather the created junction tree roots in a dummy Node.
160  typedef HybridConstructorTraversalData Data;
161  Data rootData(0);
162  rootData.junctionTreeNode =
163  std::make_shared<typename Base::Node>(); // Make a dummy node to gather
164  // the junction tree roots
165  treeTraversal::DepthFirstForest(eliminationTree, rootData,
166  Data::ConstructorTraversalVisitorPre,
167  Data::ConstructorTraversalVisitorPost);
168 
169  // Assign roots from the dummy node
170  this->addChildrenAsRoots(rootData.junctionTreeNode);
171 
172  // Transfer remaining factors from elimination tree
173  Base::remainingFactors_ = eliminationTree.remainingFactors();
174 }
175 
176 } // namespace gtsam
gtsam::HybridConstructorTraversalData::childSymbolicConditionals
FastVector< SymbolicConditional::shared_ptr > childSymbolicConditionals
Definition: HybridJunctionTree.cpp:41
gtsam.examples.DogLegOptimizerExample.int
int
Definition: DogLegOptimizerExample.py:111
HybridJunctionTree.h
gtsam::HybridConstructorTraversalData::Node
HybridJunctionTree::Node Node
Definition: HybridJunctionTree.cpp:34
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
gtsam::HybridConstructorTraversalData::SymbolicFactors
Definition: HybridJunctionTree.cpp:46
gtsam::HybridConstructorTraversalData::junctionTreeNode
sharedNode junctionTreeNode
Definition: HybridJunctionTree.cpp:40
gtsam::HybridConstructorTraversalData::HybridConstructorTraversalData
HybridConstructorTraversalData(HybridConstructorTraversalData *_parentData)
Definition: HybridJunctionTree.cpp:48
gtsam::HybridBayesTree
Definition: HybridBayesTree.h:62
gtsam::HybridConstructorTraversalData::sharedNode
JunctionTree< HybridBayesTree, HybridGaussianFactorGraph >::sharedNode sharedNode
Definition: HybridJunctionTree.cpp:37
gtsam::HybridConstructorTraversalData::ConstructorTraversalVisitorPre
static HybridConstructorTraversalData ConstructorTraversalVisitorPre(const std::shared_ptr< HybridEliminationTree::Node > &node, HybridConstructorTraversalData &parentData)
Definition: HybridJunctionTree.cpp:52
gtsam::FastSet< Key >
gtsam::HybridConstructorTraversalData
Definition: HybridJunctionTree.cpp:33
gtsam::HybridConstructorTraversalData::childSymbolicFactors
FastVector< SymbolicFactor::shared_ptr > childSymbolicFactors
Definition: HybridJunctionTree.cpp:42
gtsam::FactorGraph
Definition: BayesTree.h:34
Key.h
gtsam::internal::EliminateSymbolic
std::pair< std::shared_ptr< SymbolicConditional >, std::shared_ptr< SymbolicFactor > > EliminateSymbolic(const FactorGraph< FACTOR > &factors, const Ordering &keys)
Definition: SymbolicFactor-inst.h:38
data
int data[]
Definition: Map_placement_new.cpp:1
parentData
DATA & parentData
Definition: treeTraversal-inst.h:45
gtsam::HybridConstructorTraversalData::discreteKeys
KeySet discreteKeys
Definition: HybridJunctionTree.cpp:43
gtsam::treeTraversal::DepthFirstForest
void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost)
Definition: treeTraversal-inst.h:77
gtsam::HybridGaussianFactorGraph
Definition: HybridGaussianFactorGraph.h:105
gtsam::HybridEliminationTree
Definition: HybridEliminationTree.h:31
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
HybridEliminationTree.h
gtsam::HybridConstructorTraversalData::ConstructorTraversalVisitorPost
static void ConstructorTraversalVisitorPost(const std::shared_ptr< HybridEliminationTree::Node > &node, const HybridConstructorTraversalData &data)
Definition: HybridJunctionTree.cpp:80
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
gtsam
traits
Definition: SFMdata.h:40
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::JunctionTree
Definition: JunctionTree.h:50
gtsam::EliminatableClusterTree< HybridBayesTree, HybridGaussianFactorGraph >::remainingFactors_
FastVector< sharedFactor > remainingFactors_
Definition: ClusterTree.h:202
gtsam::HybridConstructorTraversalData::parentData
HybridConstructorTraversalData *const parentData
Definition: HybridJunctionTree.cpp:39
gtsam::FactorGraph::reserve
void reserve(size_t size)
Definition: FactorGraph.h:143
JunctionTree-inst.h
The junction tree, template bodies.
Node
static sharedNode Node(Key key, const SymbolicFactorGraph &factors, const ChildNodes::Result &children)
Definition: testSymbolicEliminationTree.cpp:86
gtsam::Ordering
Definition: inference/Ordering.h:33
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gttic
#define gttic(label)
Definition: timing.h:295
gtsam::HybridJunctionTree::HybridJunctionTree
HybridJunctionTree(const HybridEliminationTree &eliminationTree)
Definition: HybridJunctionTree.cpp:148
gtsam::EliminationTree::remainingFactors
const FastVector< sharedFactor > & remainingFactors() const
Definition: EliminationTree.h:155


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