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

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

release/4.3a0
Stephen Williams 2012-07-05 18:50:09 +00:00
parent f575cb4cc0
commit f55612d381
1 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/nonlinear/ISAM2.cpp
View File

@ -575,8 +575,6 @@ ISAM2Result ISAM2::update(
Index index = ordering_[key]; Index index = ordering_[key];
if(variableIndex_[index].empty()) if(variableIndex_[index].empty())
unusedKeys.insert(key); unusedKeys.insert(key);
else
markedKeys.insert(index);
} }
// Delete any keys from 'unusedKeys' that are actually used by the new, incoming factors // Delete any keys from 'unusedKeys' that are actually used by the new, incoming factors
@ -593,6 +591,16 @@ ISAM2Result ISAM2::update(
// update it. // update it.
Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_); 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"); toc(1,"push_back factors");