From f55612d38139a31077729ffeb7e8ac6cad714f3d Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Thu, 5 Jul 2012 18:50:09 +0000 Subject: [PATCH] Moved the 'markKeys' generation in iSAM2 until after the call to RemoveVariables. Since the 'RemoveVariables' function permutes the ordering, this ensures markedKeys uses the correct indices --- gtsam/nonlinear/ISAM2.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index bb579195f..7fb23884b 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -575,8 +575,6 @@ ISAM2Result ISAM2::update( Index index = ordering_[key]; if(variableIndex_[index].empty()) unusedKeys.insert(key); - else - markedKeys.insert(index); } // Delete any keys from 'unusedKeys' that are actually used by the new, incoming factors @@ -593,6 +591,16 @@ ISAM2Result ISAM2::update( // update it. Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_); + + // Mark keys that are still in the use and are also included in the removed factors + // Note: The ordering has been modified during the RemoveVariables() function call. + // Hence, we do not create this list until after that call + BOOST_FOREACH(Key key, removedFactorSymbKeys) { + Index index; + if(ordering_.tryAt(key, index)) + markedKeys.insert(index); + } + } toc(1,"push_back factors");