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

Fixed remaining problems in elimination

release/4.3a0
Richard Roberts 2013-06-06 15:36:12 +00:00
parent d5e721a1d0
commit 9bd7af848c
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/inference/EliminationTreeUnordered-inl.h
View File

@ -180,8 +180,9 @@ namespace gtsam {
// about the vector growing to be very large once and not being deallocated until this // about the vector growing to be very large once and not being deallocated until this
// function exits, because in the worst case we only store one pointer in this stack for each // function exits, because in the worst case we only store one pointer in this stack for each
// variable in the system. // variable in the system.
// TODO: Check whether this is faster as a vector (then use indices instead of parent pointers).
typedef EliminationNode<This> EliminationNode; typedef EliminationNode<This> EliminationNode;
std::stack<EliminationNode, std::vector<EliminationNode> > eliminationStack; std::stack<EliminationNode, FastList<EliminationNode> > eliminationStack;
// Create empty Bayes net and factor graph to hold result // Create empty Bayes net and factor graph to hold result
boost::shared_ptr<BayesNetType> bayesNet = boost::make_shared<BayesNetType>(); boost::shared_ptr<BayesNetType> bayesNet = boost::make_shared<BayesNetType>();
@ -203,9 +204,6 @@ namespace gtsam {
// expanded - we'll come back and eliminate it later after the children have been processed. // expanded - we'll come back and eliminate it later after the children have been processed.
EliminationNode& node = eliminationStack.top(); EliminationNode& node = eliminationStack.top();
if(node.expanded) { if(node.expanded) {
// Remove from stack
eliminationStack.pop();
// Do a dense elimination step // Do a dense elimination step
std::vector<Key> keyAsVector(1); keyAsVector[0] = node.treeNode->key; std::vector<Key> keyAsVector(1); keyAsVector[0] = node.treeNode->key;
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult = std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
@ -219,6 +217,9 @@ namespace gtsam {
node.parent->factors.push_back(eliminationResult.second); node.parent->factors.push_back(eliminationResult.second);
else else
remainingFactors->push_back(eliminationResult.second); remainingFactors->push_back(eliminationResult.second);
// Remove from stack
eliminationStack.pop();
} else { } else {
// Expand children and mark as expanded // Expand children and mark as expanded
node.expanded = true; node.expanded = true;