diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h index e077bf8fe..506f870e3 100644 --- a/gtsam/inference/EliminationTree-inl.h +++ b/gtsam/inference/EliminationTree-inl.h @@ -42,8 +42,9 @@ typename EliminationTree::sharedFactor EliminationTree::eliminat factors.push_back(this->factors_.begin(), this->factors_.end()); // for all subtrees, eliminate into Bayes net and a separator factor, added to [factors] - BOOST_FOREACH(const shared_ptr& child, subTrees_) { - factors.push_back(child->eliminate_(function, conditionals)); } + BOOST_FOREACH(const shared_ptr& child, subTrees_) + factors.push_back(child->eliminate_(function, conditionals)); // TODO: spawn thread + // TODO: wait for completion of all threads // Combine all factors (from this node and from subtrees) into a joint factor pair @@ -190,8 +191,9 @@ EliminationTree::eliminate(Eliminate function) const { // call recursive routine tic(1, "ET recursive eliminate"); - Conditionals conditionals(this->key_ + 1); - (void)eliminate_(function, conditionals); + size_t nrConditionals = this->key_ + 1; // root key has highest index + Conditionals conditionals(nrConditionals); // reserve a vector of conditional shared pointers + (void)eliminate_(function, conditionals); // modify in place toc(1, "ET recursive eliminate"); // Add conditionals to BayesNet diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index b2f97c616..fc3cb3569 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -72,6 +72,7 @@ private: /** * Recursive routine that eliminates the factors arranged in an elimination tree + * @param Conditionals is a vector of shared pointers that will be modified in place */ sharedFactor eliminate_(Eliminate function, Conditionals& conditionals) const;