ClusterTree-inst.h
Go to the documentation of this file.
1 
10 #pragma once
11 
15 #include <gtsam/base/timing.h>
17 
18 #ifdef GTSAM_USE_TBB
19 #include <mutex>
20 #endif
21 #include <queue>
22 #include <cassert>
23 
24 namespace gtsam {
25 
26 /* ************************************************************************* */
27 template<class GRAPH>
28 void ClusterTree<GRAPH>::Cluster::print(const std::string& s,
29  const KeyFormatter& keyFormatter) const {
30  std::cout << s << " (" << problemSize_ << ")";
32 }
33 
34 /* ************************************************************************* */
35 template <class GRAPH>
37  std::vector<size_t> nrFrontals;
38  nrFrontals.reserve(nrChildren());
39  for (const sharedNode& child : children)
40  nrFrontals.push_back(child->nrFrontals());
41  return nrFrontals;
42 }
43 
44 /* ************************************************************************* */
45 template <class GRAPH>
46 void ClusterTree<GRAPH>::Cluster::merge(const std::shared_ptr<Cluster>& cluster) {
47  // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after..
48  orderedFrontalKeys.insert(orderedFrontalKeys.end(), cluster->orderedFrontalKeys.rbegin(),
49  cluster->orderedFrontalKeys.rend());
50  factors.push_back(cluster->factors);
51  children.insert(children.end(), cluster->children.begin(), cluster->children.end());
52  // Increment problem size
53  problemSize_ = std::max(problemSize_, cluster->problemSize_);
54 }
55 
56 /* ************************************************************************* */
57 template<class GRAPH>
59  const std::vector<bool>& merge) {
60  gttic(Cluster_mergeChildren);
61  assert(merge.size() == this->children.size());
62 
63  // Count how many keys, factors and children we'll end up with
64  size_t nrKeys = orderedFrontalKeys.size();
65  size_t nrFactors = factors.size();
66  size_t nrNewChildren = 0;
67  // Loop over children
68  size_t i = 0;
69  for(const sharedNode& child: this->children) {
70  if (merge[i]) {
71  nrKeys += child->orderedFrontalKeys.size();
72  nrFactors += child->factors.size();
73  nrNewChildren += child->nrChildren();
74  } else {
75  nrNewChildren += 1; // we keep the child
76  }
77  ++i;
78  }
79 
80  // now reserve space, and really merge
81  auto oldChildren = this->children;
82  this->children.clear();
83  this->children.reserve(nrNewChildren);
84  orderedFrontalKeys.reserve(nrKeys);
85  factors.reserve(nrFactors);
86  i = 0;
87  for (const sharedNode& child : oldChildren) {
88  if (merge[i]) {
89  this->merge(child);
90  } else {
91  this->addChild(child); // we keep the child
92  }
93  ++i;
94  }
95  std::reverse(orderedFrontalKeys.begin(), orderedFrontalKeys.end());
96 }
97 
98 /* ************************************************************************* */
99 template <class GRAPH>
100 void ClusterTree<GRAPH>::print(const std::string& s, const KeyFormatter& keyFormatter) const {
101  treeTraversal::PrintForest(*this, s, keyFormatter);
102 }
103 
104 /* ************************************************************************* */
105 
106 /* Destructor.
107  * Using default destructor causes stack overflow for large trees due to recursive destruction of nodes;
108  * so we manually decrease the reference count of each node in the tree through a BFS, and the nodes with
109  * reference count 0 will be deleted. Please see [PR-1441](https://github.com/borglab/gtsam/pull/1441) for more details.
110  */
111 template <class GRAPH>
113  // For each tree, we first move the root into a queue; then we do a BFS on the tree with the queue;
114 
115  for (auto&& root : roots_) {
116  std::queue<sharedNode> bfs_queue;
117  // first, move the root to the queue
118  bfs_queue.push(root);
119  root = nullptr; // now the root node is owned by the queue
120 
121  // for each node iterated, if its reference count is 1, it will be deleted while its children are still in the queue.
122  // so that the recursive deletion will not happen.
123  while (!bfs_queue.empty()) {
124  // move the ownership of the front node from the queue to the current variable
125  auto node = bfs_queue.front();
126  bfs_queue.pop();
127 
128  // add the children of the current node to the queue, so that the queue will also own the children nodes.
129  for (auto child : node->children) {
130  bfs_queue.push(child);
131  } // leaving the scope of current will decrease the reference count of the current node by 1, and if the reference count is 0,
132  // the node will be deleted. Because the children are in the queue, the deletion of the node will not trigger a recursive
133  // deletion of the children.
134  }
135  }
136 
137 }
138 
139 /* ************************************************************************* */
140 template <class GRAPH>
142  // Start by duplicating the tree.
144  return *this;
145 }
146 
147 /* ************************************************************************* */
148 // Elimination traversal data - stores a pointer to the parent data and collects
149 // the factors resulting from elimination of the children. Also sets up BayesTree
150 // cliques with parent and child pointers.
151 template<class CLUSTERTREE>
153  // Typedefs
154  typedef typename CLUSTERTREE::sharedFactor sharedFactor;
155  typedef typename CLUSTERTREE::FactorType FactorType;
156  typedef typename CLUSTERTREE::FactorGraphType FactorGraphType;
157  typedef typename CLUSTERTREE::ConditionalType ConditionalType;
159 
163  std::shared_ptr<BTNode> bayesTreeNode;
164 #ifdef GTSAM_USE_TBB
165  std::shared_ptr<std::mutex> writeLock;
166 #endif
167 
168  EliminationData(EliminationData* _parentData, size_t nChildren) :
169  parentData(_parentData), bayesTreeNode(std::make_shared<BTNode>())
170 #ifdef GTSAM_USE_TBB
171  , writeLock(std::make_shared<std::mutex>())
172 #endif
173  {
174  if (parentData) {
175 #ifdef GTSAM_USE_TBB
176  parentData->writeLock->lock();
177 #endif
178  myIndexInParent = parentData->childFactors.size();
179  parentData->childFactors.push_back(sharedFactor());
180 #ifdef GTSAM_USE_TBB
181  parentData->writeLock->unlock();
182 #endif
183  } else {
184  myIndexInParent = 0;
185  }
186  // Set up BayesTree parent and child pointers
187  if (parentData) {
188  if (parentData->parentData) // If our parent is not the dummy node
189  bayesTreeNode->parent_ = parentData->bayesTreeNode;
190  parentData->bayesTreeNode->children.push_back(bayesTreeNode);
191  }
192  }
193 
194  // Elimination pre-order visitor - creates the EliminationData structure for the visited node.
196  const typename CLUSTERTREE::sharedNode& node,
198  assert(node);
199  EliminationData myData(&parentData, node->nrChildren());
200  myData.bayesTreeNode->problemSize_ = node->problemSize();
201  return myData;
202  }
203 
204  // Elimination post-order visitor - combine the child factors with our own factors, add the
205  // resulting conditional to the BayesTree, and add the remaining factor to the parent.
207  const typename CLUSTERTREE::Eliminate& eliminationFunction_;
208  typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex_;
209 
210  public:
211  // Construct functor
213  const typename CLUSTERTREE::Eliminate& eliminationFunction,
214  typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) :
215  eliminationFunction_(eliminationFunction), nodesIndex_(nodesIndex) {
216  }
217 
218  // Function that does the HEAVY lifting
219  void operator()(const typename CLUSTERTREE::sharedNode& node, EliminationData& myData) {
220  assert(node);
221 
222  // Gather factors
223  FactorGraphType gatheredFactors;
224  gatheredFactors.reserve(node->factors.size() + node->nrChildren());
225  gatheredFactors.push_back(node->factors);
226  gatheredFactors.push_back(myData.childFactors);
227 
228  // Check for Bayes tree orphan subtrees, and add them to our children
229  // TODO(frank): should this really happen here?
230  for (const sharedFactor& factor: node->factors) {
231  auto asSubtree = dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(factor.get());
232  if (asSubtree) {
233  myData.bayesTreeNode->children.push_back(asSubtree->clique);
234  asSubtree->clique->parent_ = myData.bayesTreeNode;
235  }
236  }
237 
238  // >>>>>>>>>>>>>> Do dense elimination step >>>>>>>>>>>>>>>>>>>>>>>>>>>>>
239  auto eliminationResult = eliminationFunction_(gatheredFactors, node->orderedFrontalKeys);
240  // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
241 
242  // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the
243  // remaining factor
244  myData.bayesTreeNode->setEliminationResult(eliminationResult);
245 
246  // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid
247  // putting orphan subtrees in the index - they'll already be in the index of the ISAM2
248  // object they're added to.
249  for (const Key& j : myData.bayesTreeNode->conditional()->frontals()) {
250 #ifdef GTSAM_USE_TBB
251  nodesIndex_.insert({j, myData.bayesTreeNode});
252 #else
253  nodesIndex_.emplace(j, myData.bayesTreeNode);
254 #endif
255  }
256  // Store remaining factor in parent's gathered factors
257  if (!eliminationResult.second->empty()) {
258 #ifdef GTSAM_USE_TBB
259  myData.parentData->writeLock->lock();
260 #endif
261  myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second;
262 #ifdef GTSAM_USE_TBB
263  myData.parentData->writeLock->unlock();
264 #endif
265  }
266  }
267  };
268 };
269 
270 /* ************************************************************************* */
271 template<class BAYESTREE, class GRAPH>
273  const This& other) {
275 
276  // Assign the remaining factors - these are pointers to factors in the original factor graph and
277  // we do not clone them.
278  remainingFactors_ = other.remainingFactors_;
279 
280  return *this;
281 }
282 
283 /* ************************************************************************* */
284 template <class BAYESTREE, class GRAPH>
285 std::pair<std::shared_ptr<BAYESTREE>, std::shared_ptr<GRAPH> >
287  gttic(ClusterTree_eliminate);
288  // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node
289  // that contains all of the roots as its children. rootsContainer also stores the remaining
290  // un-eliminated factors passed up from the roots.
291  std::shared_ptr<BayesTreeType> result = std::make_shared<BayesTreeType>();
292 
293  typedef EliminationData<This> Data;
294  Data rootsContainer(0, this->nrRoots());
295 
296  typename Data::EliminationPostOrderVisitor visitorPost(function, result->nodes_);
297  {
298  TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP
299  treeTraversal::DepthFirstForestParallel(*this, rootsContainer, Data::EliminationPreOrderVisitor,
300  visitorPost, 10);
301  }
302 
303  // Create BayesTree from roots stored in the dummy BayesTree node.
304  result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(),
305  rootsContainer.bayesTreeNode->children.end());
306 
307  // Add remaining factors that were not involved with eliminated variables
308  std::shared_ptr<FactorGraphType> remaining = std::make_shared<FactorGraphType>();
309  remaining->reserve(remainingFactors_.size() + rootsContainer.childFactors.size());
310  remaining->push_back(remainingFactors_.begin(), remainingFactors_.end());
311  for (const sharedFactor& factor : rootsContainer.childFactors) {
312  if (factor)
313  remaining->push_back(factor);
314  }
315 
316  // Return result
317  return {result, remaining};
318 }
319 
320 } // namespace gtsam
ClusterTree.h
Collects factorgraph fragments defined on variable clusters, arranged in a tree.
gtsam::ClusterTree::Cluster::nrFrontalsOfChildren
std::vector< size_t > nrFrontalsOfChildren() const
Return a vector with nrFrontal keys for each child.
Definition: ClusterTree-inst.h:36
timing.h
Timing utilities.
gtsam::BayesTreeOrphanWrapper
Definition: BayesTree.h:281
gtsam::EliminationData::myIndexInParent
size_t myIndexInParent
Definition: ClusterTree-inst.h:161
gtsam::ClusterTree::sharedFactor
std::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition: ClusterTree.h:32
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
treeTraversal-inst.h
gtsam::EliminationData::EliminationPreOrderVisitor
static EliminationData EliminationPreOrderVisitor(const typename CLUSTERTREE::sharedNode &node, EliminationData &parentData)
Definition: ClusterTree-inst.h:195
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::ClusterTree::Cluster::mergeChildren
void mergeChildren(const std::vector< bool > &merge)
Merge all children for which bit is set into this node.
Definition: ClusterTree-inst.h:58
gtsam::EliminatableClusterTree< SymbolicBayesTree, SymbolicFactorGraph >::sharedFactor
std::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition: ClusterTree.h:199
BayesTree.h
Bayes Tree is a tree of cliques of a Bayes Chain.
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
nrKeys
static const size_t nrKeys
Definition: testRegularJacobianFactor.cpp:31
pybind_wrapper_test_script.this
this
Definition: pybind_wrapper_test_script.py:38
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
gtsam::PrintKeyVector
void PrintKeyVector(const KeyVector &keys, const string &s, const KeyFormatter &keyFormatter)
Utility function to print sets of keys with optional prefix.
Definition: Key.cpp:83
gtsam::EliminationData::FactorGraphType
CLUSTERTREE::FactorGraphType FactorGraphType
Definition: ClusterTree-inst.h:156
gtsam::ClusterTree::Cluster::orderedFrontalKeys
Keys orderedFrontalKeys
Frontal keys of this node.
Definition: ClusterTree.h:41
gtsam::EliminationData::EliminationPostOrderVisitor::eliminationFunction_
const CLUSTERTREE::Eliminate & eliminationFunction_
Definition: ClusterTree-inst.h:207
Ordering.h
Variable ordering for the elimination algorithm.
gtsam::EliminationData::ConditionalType
CLUSTERTREE::ConditionalType ConditionalType
Definition: ClusterTree-inst.h:157
result
Values result
Definition: OdometryOptimize.cpp:8
pruning_fixture::factor
DecisionTreeFactor factor(D &C &B &A, "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0")
gtsam::EliminationData::EliminationPostOrderVisitor::EliminationPostOrderVisitor
EliminationPostOrderVisitor(const typename CLUSTERTREE::Eliminate &eliminationFunction, typename CLUSTERTREE::BayesTreeType::Nodes &nodesIndex)
Definition: ClusterTree-inst.h:212
gtsam::EliminatableClusterTree
Definition: BayesTree.h:35
gtsam::EliminationData::EliminationPostOrderVisitor::operator()
void operator()(const typename CLUSTERTREE::sharedNode &node, EliminationData &myData)
Definition: ClusterTree-inst.h:219
gtsam::ClusterTree::Cluster::print
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print this node
Definition: ClusterTree-inst.h:28
parentData
DATA & parentData
Definition: treeTraversal-inst.h:45
gtsam::ClusterTree::roots_
FastVector< sharedNode > roots_
Definition: ClusterTree.h:116
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::EliminationData::EliminationPostOrderVisitor::nodesIndex_
CLUSTERTREE::BayesTreeType::Nodes & nodesIndex_
Definition: ClusterTree-inst.h:208
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::EliminationData::bayesTreeNode
std::shared_ptr< BTNode > bayesTreeNode
Definition: ClusterTree-inst.h:163
gtsam::EliminationData::EliminationPostOrderVisitor
Definition: ClusterTree-inst.h:206
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, std::shared_ptr< T > > make_shared(Args &&... args)
Definition: make_shared.h:56
eliminationFunction
const TREE::Eliminate & eliminationFunction
Definition: inference-inst.h:55
gtsam::EliminationData::FactorType
CLUSTERTREE::FactorType FactorType
Definition: ClusterTree-inst.h:155
gtsam::ClusterTree::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: ClusterTree-inst.h:100
gtsam::EliminationData::parentData
EliminationData *const parentData
Definition: ClusterTree-inst.h:160
gtsam::ClusterTree::sharedNode
sharedCluster sharedNode
Definition: ClusterTree.h:110
gtsam::EliminationData::sharedFactor
CLUSTERTREE::sharedFactor sharedFactor
Definition: ClusterTree-inst.h:154
gtsam::EliminationData
Definition: ClusterTree-inst.h:152
gtsam::ClusterTree::Cluster::problemSize_
int problemSize_
Definition: ClusterTree.h:45
gtsam::treeTraversal::DepthFirstForestParallel
void DepthFirstForestParallel(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost, int problemSizeThreshold=10)
Definition: treeTraversal-inst.h:156
gtsam::EliminatableClusterTree< SymbolicBayesTree, SymbolicFactorGraph >::Eliminate
SymbolicFactorGraph ::Eliminate Eliminate
Typedef for an eliminate subroutine.
Definition: ClusterTree.h:197
gtsam::ClusterTree::operator=
This & operator=(const This &other)
Definition: ClusterTree-inst.h:141
gtsam::ClusterTree::~ClusterTree
~ClusterTree()
Definition: ClusterTree-inst.h:112
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
std
Definition: BFloat16.h:88
gtsam::treeTraversal::CloneForest
FastVector< std::shared_ptr< typename FOREST::Node > > CloneForest(const FOREST &forest)
Definition: treeTraversal-inst.h:191
gtsam::EliminationData::childFactors
FastVector< sharedFactor > childFactors
Definition: ClusterTree-inst.h:162
gtsam::ClusterTree::Cluster::merge
void merge(const std::shared_ptr< Cluster > &cluster)
Merge in given cluster.
Definition: ClusterTree-inst.h:46
gtsam::ClusterTree
Definition: ClusterTree.h:25
gtsam::EliminatableClusterTree::eliminate
std::pair< std::shared_ptr< BayesTreeType >, std::shared_ptr< FactorGraphType > > eliminate(const Eliminate &function) const
Definition: ClusterTree-inst.h:286
gtsam::EliminationData::EliminationData
EliminationData(EliminationData *_parentData, size_t nChildren)
Definition: ClusterTree-inst.h:168
reverse
void reverse(const MatrixType &m)
Definition: array_reverse.cpp:16
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
max
#define max(a, b)
Definition: datatypes.h:20
Node
static sharedNode Node(Key key, const SymbolicFactorGraph &factors, const ChildNodes::Result &children)
Definition: testSymbolicEliminationTree.cpp:86
gtsam::TbbOpenMPMixedScope
Definition: types.h:162
gtsam::EliminatableClusterTree::operator=
This & operator=(const This &other)
Definition: ClusterTree-inst.h:272
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
sharedNode
SymbolicEliminationTree::sharedNode sharedNode
Definition: testSymbolicEliminationTree.cpp:31
gttic
#define gttic(label)
Definition: timing.h:295
gtsam::ClusterTree::nrRoots
size_t nrRoots() const
Definition: ClusterTree.h:154
gtsam::EliminationData::BTNode
CLUSTERTREE::BayesTreeType::Node BTNode
Definition: ClusterTree-inst.h:158


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:15