gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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
22namespace gtsam {
23
24/* ************************************************************************* */
25template<class GRAPH>
26void ClusterTree<GRAPH>::Cluster::print(const std::string& s,
27 const KeyFormatter& keyFormatter) const {
28 std::cout << s << " (" << problemSize_ << ")";
30}
31
32/* ************************************************************************* */
33template <class GRAPH>
35 std::vector<size_t> nrFrontals;
36 nrFrontals.reserve(nrChildren());
37 for (const sharedNode& child : children)
38 nrFrontals.push_back(child->nrFrontals());
39 return nrFrontals;
40}
41
42/* ************************************************************************* */
43template <class GRAPH>
44void ClusterTree<GRAPH>::Cluster::merge(const boost::shared_ptr<Cluster>& cluster) {
45 // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after..
46 orderedFrontalKeys.insert(orderedFrontalKeys.end(), cluster->orderedFrontalKeys.rbegin(),
47 cluster->orderedFrontalKeys.rend());
48 factors.push_back(cluster->factors);
49 children.insert(children.end(), cluster->children.begin(), cluster->children.end());
50 // Increment problem size
51 problemSize_ = std::max(problemSize_, cluster->problemSize_);
52}
53
54/* ************************************************************************* */
55template<class GRAPH>
57 const std::vector<bool>& merge) {
58 gttic(Cluster_mergeChildren);
59 assert(merge.size() == this->children.size());
60
61 // Count how many keys, factors and children we'll end up with
62 size_t nrKeys = orderedFrontalKeys.size();
63 size_t nrFactors = factors.size();
64 size_t nrNewChildren = 0;
65 // Loop over children
66 size_t i = 0;
67 for(const sharedNode& child: this->children) {
68 if (merge[i]) {
69 nrKeys += child->orderedFrontalKeys.size();
70 nrFactors += child->factors.size();
71 nrNewChildren += child->nrChildren();
72 } else {
73 nrNewChildren += 1; // we keep the child
74 }
75 ++i;
76 }
77
78 // now reserve space, and really merge
79 auto oldChildren = this->children;
80 this->children.clear();
81 this->children.reserve(nrNewChildren);
82 orderedFrontalKeys.reserve(nrKeys);
83 factors.reserve(nrFactors);
84 i = 0;
85 for (const sharedNode& child : oldChildren) {
86 if (merge[i]) {
87 this->merge(child);
88 } else {
89 this->addChild(child); // we keep the child
90 }
91 ++i;
92 }
93 std::reverse(orderedFrontalKeys.begin(), orderedFrontalKeys.end());
94}
95
96/* ************************************************************************* */
97template <class GRAPH>
98void ClusterTree<GRAPH>::print(const std::string& s, const KeyFormatter& keyFormatter) const {
99 treeTraversal::PrintForest(*this, s, keyFormatter);
101
102/* ************************************************************************* */
103template <class GRAPH>
105 // Start by duplicating the tree.
107 return *this;
108}
109
110/* ************************************************************************* */
111// Elimination traversal data - stores a pointer to the parent data and collects
112// the factors resulting from elimination of the children. Also sets up BayesTree
113// cliques with parent and child pointers.
114template<class CLUSTERTREE>
116 // Typedefs
117 typedef typename CLUSTERTREE::sharedFactor sharedFactor;
118 typedef typename CLUSTERTREE::FactorType FactorType;
119 typedef typename CLUSTERTREE::FactorGraphType FactorGraphType;
120 typedef typename CLUSTERTREE::ConditionalType ConditionalType;
121 typedef typename CLUSTERTREE::BayesTreeType::Node BTNode;
122
123 EliminationData* const parentData;
124 size_t myIndexInParent;
125 FastVector<sharedFactor> childFactors;
126 boost::shared_ptr<BTNode> bayesTreeNode;
127#ifdef GTSAM_USE_TBB
128 boost::shared_ptr<std::mutex> writeLock;
129#endif
130
131 EliminationData(EliminationData* _parentData, size_t nChildren) :
132 parentData(_parentData), bayesTreeNode(boost::make_shared<BTNode>())
133#ifdef GTSAM_USE_TBB
134 , writeLock(boost::make_shared<std::mutex>())
135#endif
136 {
137 if (parentData) {
138#ifdef GTSAM_USE_TBB
139 parentData->writeLock->lock();
140#endif
141 myIndexInParent = parentData->childFactors.size();
142 parentData->childFactors.push_back(sharedFactor());
143#ifdef GTSAM_USE_TBB
144 parentData->writeLock->unlock();
145#endif
146 } else {
147 myIndexInParent = 0;
148 }
149 // Set up BayesTree parent and child pointers
150 if (parentData) {
151 if (parentData->parentData) // If our parent is not the dummy node
152 bayesTreeNode->parent_ = parentData->bayesTreeNode;
153 parentData->bayesTreeNode->children.push_back(bayesTreeNode);
154 }
155 }
156
157 // Elimination pre-order visitor - creates the EliminationData structure for the visited node.
158 static EliminationData EliminationPreOrderVisitor(
159 const typename CLUSTERTREE::sharedNode& node,
160 EliminationData& parentData) {
161 assert(node);
162 EliminationData myData(&parentData, node->nrChildren());
163 myData.bayesTreeNode->problemSize_ = node->problemSize();
164 return myData;
165 }
166
167 // Elimination post-order visitor - combine the child factors with our own factors, add the
168 // resulting conditional to the BayesTree, and add the remaining factor to the parent.
170 const typename CLUSTERTREE::Eliminate& eliminationFunction_;
171 typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex_;
172
173 public:
174 // Construct functor
175 EliminationPostOrderVisitor(
176 const typename CLUSTERTREE::Eliminate& eliminationFunction,
177 typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) :
178 eliminationFunction_(eliminationFunction), nodesIndex_(nodesIndex) {
179 }
180
181 // Function that does the HEAVY lifting
182 void operator()(const typename CLUSTERTREE::sharedNode& node, EliminationData& myData) {
183 assert(node);
184
185 // Gather factors
186 FactorGraphType gatheredFactors;
187 gatheredFactors.reserve(node->factors.size() + node->nrChildren());
188 gatheredFactors += node->factors;
189 gatheredFactors += myData.childFactors;
190
191 // Check for Bayes tree orphan subtrees, and add them to our children
192 // TODO(frank): should this really happen here?
193 for (const sharedFactor& factor: node->factors) {
194 auto asSubtree = dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(factor.get());
195 if (asSubtree) {
196 myData.bayesTreeNode->children.push_back(asSubtree->clique);
197 asSubtree->clique->parent_ = myData.bayesTreeNode;
198 }
199 }
200
201 // >>>>>>>>>>>>>> Do dense elimination step >>>>>>>>>>>>>>>>>>>>>>>>>>>>>
202 auto eliminationResult = eliminationFunction_(gatheredFactors, node->orderedFrontalKeys);
203 // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
204
205 // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the
206 // remaining factor
207 myData.bayesTreeNode->setEliminationResult(eliminationResult);
208
209 // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid
210 // putting orphan subtrees in the index - they'll already be in the index of the ISAM2
211 // object they're added to.
212 for (const Key& j: myData.bayesTreeNode->conditional()->frontals())
213 nodesIndex_.insert(std::make_pair(j, myData.bayesTreeNode));
214
215 // Store remaining factor in parent's gathered factors
216 if (!eliminationResult.second->empty()) {
217#ifdef GTSAM_USE_TBB
218 myData.parentData->writeLock->lock();
219#endif
220 myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second;
221#ifdef GTSAM_USE_TBB
222 myData.parentData->writeLock->unlock();
223#endif
224 }
225 }
226 };
227};
228
229/* ************************************************************************* */
230template<class BAYESTREE, class GRAPH>
232 const This& other) {
234
235 // Assign the remaining factors - these are pointers to factors in the original factor graph and
236 // we do not clone them.
237 remainingFactors_ = other.remainingFactors_;
238
239 return *this;
240}
241
242/* ************************************************************************* */
243template <class BAYESTREE, class GRAPH>
244std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> >
246 gttic(ClusterTree_eliminate);
247 // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node
248 // that contains all of the roots as its children. rootsContainer also stores the remaining
249 // un-eliminated factors passed up from the roots.
250 boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();
251
252 typedef EliminationData<This> Data;
253 Data rootsContainer(0, this->nrRoots());
254
255 typename Data::EliminationPostOrderVisitor visitorPost(function, result->nodes_);
256 {
257 TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP
258 treeTraversal::DepthFirstForestParallel(*this, rootsContainer, Data::EliminationPreOrderVisitor,
259 visitorPost, 10);
260 }
261
262 // Create BayesTree from roots stored in the dummy BayesTree node.
263 result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(),
264 rootsContainer.bayesTreeNode->children.end());
265
266 // Add remaining factors that were not involved with eliminated variables
267 boost::shared_ptr<FactorGraphType> remaining = boost::make_shared<FactorGraphType>();
268 remaining->reserve(remainingFactors_.size() + rootsContainer.childFactors.size());
269 remaining->push_back(remainingFactors_.begin(), remainingFactors_.end());
270 for (const sharedFactor& factor : rootsContainer.childFactors) {
271 if (factor)
272 remaining->push_back(factor);
273 }
274
275 // Return result
276 return std::make_pair(result, remaining);
277}
278
279} // namespace gtsam
Timing utilities.
Variable ordering for the elimination algorithm.
Collects factorgraph fragments defined on variable clusters, arranged in a tree.
Bayes Tree is a tree of cliques of a Bayes Chain.
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
Global functions in a separate testing namespace.
Definition chartTesting.h:28
void PrintKeyVector(const KeyVector &keys, const string &s, const KeyFormatter &keyFormatter)
Utility function to print sets of keys with optional prefix.
Definition Key.cpp:77
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
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:189
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:219
void DepthFirstForestParallel(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost, int problemSizeThreshold=10)
Traverse a forest depth-first with pre-order and post-order visits.
Definition treeTraversal-inst.h:154
An object whose scope defines a block where TBB and OpenMP parallelism are mixed.
Definition types.h:192
A cluster-tree that eliminates to a Bayes tree.
Definition ClusterTree.h:184
This & operator=(const This &other)
Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are copie...
Definition ClusterTree-inst.h:231
std::pair< boost::shared_ptr< BayesTreeType >, boost::shared_ptr< FactorGraphType > > eliminate(const Eliminate &function) const
Eliminate the factors to a Bayes tree and remaining factor graph.
Definition ClusterTree-inst.h:245
boost::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition ClusterTree.h:197
GRAPH::Eliminate Eliminate
Typedef for an eliminate subroutine.
Definition ClusterTree.h:195
Definition BayesTree.h:276
Definition ClusterTree-inst.h:115
A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: each node k re...
Definition ClusterTree.h:25
This & operator=(const This &other)
Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are copie...
Definition ClusterTree-inst.h:104
FastVector< sharedNode > roots_
concept check
Definition ClusterTree.h:116
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Print the cluster tree.
Definition ClusterTree-inst.h:98
boost::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition ClusterTree.h:32
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print this node
Definition ClusterTree-inst.h:26
Keys orderedFrontalKeys
Frontal keys of this node.
Definition ClusterTree.h:41
void mergeChildren(const std::vector< bool > &merge)
Merge all children for which bit is set into this node.
Definition ClusterTree-inst.h:56
void merge(const boost::shared_ptr< Cluster > &cluster)
Merge in given cluster.
Definition ClusterTree-inst.h:44
std::vector< size_t > nrFrontalsOfChildren() const
Return a vector with nrFrontal keys for each child.
Definition ClusterTree-inst.h:34