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
// 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
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
// marginal factor. So, now associate this marginal factor with the
// 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
// information has been stored in marginalFactors.
const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques