diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h index 9b9d5d2f0..3e797ee1e 100644 --- a/gtsam/inference/EliminationTree-inl.h +++ b/gtsam/inference/EliminationTree-inl.h @@ -5,6 +5,7 @@ * @created Oct 13, 2010 */ +#include #include #include #include @@ -84,8 +85,12 @@ template typename EliminationTree::shared_ptr EliminationTree::Create(const FactorGraph& factorGraph, const VariableIndex& structure) { + tic("ET 1: Create"); + + tic("ET 1.1: ComputeParents"); // Compute the tree structure vector parents(ComputeParents(structure)); + toc("ET 1.1: ComputeParents"); // Number of variables const size_t n = structure.size(); @@ -93,6 +98,7 @@ EliminationTree::Create(const FactorGraph& factorGraph, c static const Index none = numeric_limits::max(); // Create tree structure + tic("ET 1.2: assemble tree"); vector trees(n); for (Index k = 1; k <= n; k++) { Index j = n - k; @@ -100,8 +106,10 @@ EliminationTree::Create(const FactorGraph& factorGraph, c if (parents[j] != none) trees[parents[j]]->add(trees[j]); } + toc("ET 1.2: assemble tree"); // Hang factors in right places + tic("ET 1.3: hang factors"); BOOST_FOREACH(const typename DERIVEDFACTOR::shared_ptr& derivedFactor, factorGraph) { // Here we static_cast to the factor type of this EliminationTree. This // allows performing symbolic elimination on, for example, GaussianFactors. @@ -109,6 +117,9 @@ EliminationTree::Create(const FactorGraph& factorGraph, c Index j = factor->front(); trees[j]->add(factor); } + toc("ET 1.3: hang factors"); + + toc("ET 1: Create"); // Assert that all other nodes have parents, i.e. that this is not a forest. #ifndef NDEBUG @@ -124,7 +135,14 @@ template template typename EliminationTree::shared_ptr EliminationTree::Create(const FactorGraph& factorGraph) { - return Create(factorGraph, VariableIndex(factorGraph)); + + // Build variable index + tic("ET 0: variable index"); + const VariableIndex variableIndex(factorGraph); + toc("ET 0: variable index"); + + // Build elimination tree + return Create(factorGraph, variableIndex); } /* ************************************************************************* */ @@ -157,16 +175,25 @@ template typename EliminationTree::BayesNet::shared_ptr EliminationTree::eliminate() const { + tic("ET 2: eliminate"); + // call recursive routine + tic("ET 2.1: recursive eliminate"); Conditionals conditionals(this->key_ + 1); (void)eliminate_(conditionals); + toc("ET 2.1: recursive eliminate"); // Add conditionals to BayesNet + tic("ET 2.1: assemble BayesNet"); typename BayesNet::shared_ptr bayesNet(new BayesNet); BOOST_FOREACH(const typename BayesNet::sharedConditional& conditional, conditionals) { if(conditional) bayesNet->push_back(conditional); } + toc("ET 2.1: assemble BayesNet"); + + toc("ET 2: eliminate"); + return bayesNet; }