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

Ported a fix from Steve for keeping factor order consistent (I don't remember why this is needed)

release/4.3a0
Richard Roberts 2013-08-20 16:53:44 +00:00
parent 9f7bc2399e
commit 4fb7235310
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/nonlinear/ISAM2.cpp
View File

@ -762,7 +762,8 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
// Keep track of marginal factors - map from clique to the marginal factors // Keep track of marginal factors - map from clique to the marginal factors
// that should be incorporated into it, passed up from it's children. // that should be incorporated into it, passed up from it's children.
multimap<sharedClique, GaussianFactor::shared_ptr> marginalFactors; // multimap<sharedClique, GaussianFactor::shared_ptr> marginalFactors;
map<Key, vector<GaussianFactor::shared_ptr> > marginalFactors; // Steve's fix for making factor order consistent
// Remove each variable and its subtrees // Remove each variable and its subtrees
BOOST_REVERSE_FOREACH(Key j, leafKeys) { BOOST_REVERSE_FOREACH(Key j, leafKeys) {
@ -784,7 +785,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
// because their information is already incorporated in the new // because their information is already incorporated in the new
// marginal factor. So, now associate this marginal factor with the // marginal factor. So, now associate this marginal factor with the
// parent of this clique. // parent of this clique.
marginalFactors.insert(make_pair(clique->parent(), marginalFactor)); marginalFactors[clique->parent()->conditional()->front()].push_back(marginalFactor);
// Now remove this clique and its subtree - all of its marginal // Now remove this clique and its subtree - all of its marginal
// information has been stored in marginalFactors. // information has been stored in marginalFactors.
const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques