From 5c293ba2a73c4d8e38d0b935c107f15c20a08126 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 25 Jan 2011 22:28:29 +0000 Subject: [PATCH] Fixed key sorting bug in symbolic elimination (still have work to do on key sorting) --- gtsam/inference/EliminationTree-inl.h | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h index 275b8d26f..db2d61d86 100644 --- a/gtsam/inference/EliminationTree-inl.h +++ b/gtsam/inference/EliminationTree-inl.h @@ -49,6 +49,10 @@ EliminationTree::eliminate_(Conditionals& conditionals) const { FACTOR::CombineAndEliminate(factors, 1)); conditionals[this->key_] = eliminated.first->front(); + if(debug) cout << "Eliminated " << this->key_ << " to get:\n"; + if(debug) eliminated.first->front()->print("Conditional: "); + if(debug) eliminated.second->print("Factor: "); + return eliminated.second; } @@ -117,10 +121,10 @@ EliminationTree::Create(const FactorGraph& factorGraph, c // Hang factors in right places tic(3, "hang factors"); BOOST_FOREACH(const typename DERIVEDFACTOR::shared_ptr& derivedFactor, factorGraph) { - // Here we static_cast to the factor type of this EliminationTree. This + // Here we upwards-cast to the factor type of this EliminationTree. This // allows performing symbolic elimination on, for example, GaussianFactors. - sharedFactor factor(boost::shared_static_cast(derivedFactor)); - Index j = factor->front(); + sharedFactor factor(derivedFactor); + Index j = *std::min_element(factor->begin(), factor->end()); trees[j]->add(factor); } toc(3, "hang factors");