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

Fixed bug in iSAM2 where variables are deleted inside of iSAM2 even though they appear in the set of new, incoming factors

release/4.3a0
Stephen Williams 2012-07-05 18:37:08 +00:00
parent 50b40b1b5b
commit f575cb4cc0
1 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/nonlinear/ISAM2.cpp
View File

@ -579,6 +579,15 @@ ISAM2Result ISAM2::update(
markedKeys.insert(index); markedKeys.insert(index);
} }
// Delete any keys from 'unusedKeys' that are actually used by the new, incoming factors
FastSet<Key> newNonlinearKeys = newFactors.keys();
BOOST_FOREACH(Key key, newNonlinearKeys) {
FastSet<Key>::iterator iter = unusedKeys.find(key);
if(iter != unusedKeys.end()) {
unusedKeys.erase(iter);
}
}
// Remove unused keys. We must hold on to the new nodes index for now // Remove unused keys. We must hold on to the new nodes index for now
// instead of placing it into the tree because removeTop will need to // instead of placing it into the tree because removeTop will need to
// update it. // update it.