12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed key sorting bug in symbolic elimination (still have work to do on key sorting)

release/4.3a0
Richard Roberts 2011-01-25 22:28:29 +00:00
parent 4880257e69
commit 5c293ba2a7
1 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/inference/EliminationTree-inl.h
View File

@ -49,6 +49,10 @@ EliminationTree<FACTOR>::eliminate_(Conditionals& conditionals) const {
FACTOR::CombineAndEliminate(factors, 1)); FACTOR::CombineAndEliminate(factors, 1));
conditionals[this->key_] = eliminated.first->front(); 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; return eliminated.second;
} }
@ -117,10 +121,10 @@ EliminationTree<FACTOR>::Create(const FactorGraph<DERIVEDFACTOR>& factorGraph, c
// Hang factors in right places // Hang factors in right places
tic(3, "hang factors"); tic(3, "hang factors");
BOOST_FOREACH(const typename DERIVEDFACTOR::shared_ptr& derivedFactor, factorGraph) { 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. // allows performing symbolic elimination on, for example, GaussianFactors.
sharedFactor factor(boost::shared_static_cast<FACTOR>(derivedFactor)); sharedFactor factor(derivedFactor);
Index j = factor->front(); Index j = *std::min_element(factor->begin(), factor->end());
trees[j]->add(factor); trees[j]->add(factor);
} }
toc(3, "hang factors"); toc(3, "hang factors");