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

reserve vector

release/4.3a0
dellaert 2015-06-21 01:46:11 -07:00
parent f9ddbb1345
commit e9d6feea5c
1 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/inference/EliminationTree-inst.h
View File

@ -101,10 +101,12 @@ namespace gtsam {
{ {
// Retrieve the factors involving this variable and create the current node // Retrieve the factors involving this variable and create the current node
const VariableIndex::Factors& factors = structure[order[j]]; const VariableIndex::Factors& factors = structure[order[j]];
nodes[j] = boost::make_shared<Node>(); const sharedNode node = boost::make_shared<Node>();
nodes[j]->key = order[j]; node->key = order[j];
// for row i \in Struct[A*j] do // for row i \in Struct[A*j] do
node->children.reserve(factors.size());
node->factors.reserve(factors.size());
BOOST_FOREACH(const size_t i, factors) { BOOST_FOREACH(const size_t i, factors) {
// If we already hit a variable in this factor, make the subtree containing the previous // If we already hit a variable in this factor, make the subtree containing the previous
// variable in this factor a child of the current node. This means that the variables // variable in this factor a child of the current node. This means that the variables
@ -123,16 +125,16 @@ namespace gtsam {
if (r != j) { if (r != j) {
// Now that we found the root, hook up parent and child pointers in the nodes. // Now that we found the root, hook up parent and child pointers in the nodes.
parents[r] = j; parents[r] = j;
nodes[j]->children.push_back(nodes[r]); node->children.push_back(nodes[r]);
} }
} else { } else {
// Add the current factor to the current node since we are at the first variable in this // Add the factor to the current node since we are at the first variable in this factor.
// factor. node->factors.push_back(graph[i]);
nodes[j]->factors.push_back(graph[i]);
factorUsed[i] = true; factorUsed[i] = true;
} }
prevCol[i] = j; prevCol[i] = j;
} }
nodes[j] = node;
} }
} catch(std::invalid_argument& e) { } catch(std::invalid_argument& e) {
// If this is thrown from structure[order[j]] above, it means that it was requested to // If this is thrown from structure[order[j]] above, it means that it was requested to