From 4fb723531092f8b7e75d885d5329c84a07e036a7 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 20 Aug 2013 16:53:44 +0000 Subject: [PATCH] Ported a fix from Steve for keeping factor order consistent (I don't remember why this is needed) --- gtsam/nonlinear/ISAM2.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index b3d976acc..612fe669c 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -762,7 +762,8 @@ void ISAM2::marginalizeLeaves(const FastList& 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 marginalFactors; +// multimap marginalFactors; + map > 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& 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