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

Fixed bug in iSAM2 variable removal where removed variables still became marked when marking relin-affected variables

release/4.3a0
Richard Roberts 2012-07-05 17:08:05 +00:00
parent ad66f9ee19
commit 806d86513d
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

@ -649,6 +649,15 @@ ISAM2Result ISAM2::update(
// add other cliques that have the marked ones in the separator // add other cliques that have the marked ones in the separator
Impl::FindAll(this->root(), markedKeys, markedRelinMask); Impl::FindAll(this->root(), markedKeys, markedRelinMask);
// FindAll might have marked some removed keys because the removed keys
// are still in the old BayesTree structure - we remove them from
// markedKeys here. Note that the limit for removing indices that we use
// (delta_.size()) is after all of the new variables - so we do not
// remove indices that were removed but then recreated by adding new
// keys.
FastSet<Index>::iterator firstRemovedIndex = markedKeys.lower_bound(delta_.size());
markedKeys.erase(firstRemovedIndex, markedKeys.end());
// Relin involved keys for detailed results // Relin involved keys for detailed results
if(params_.enableDetailedResults) { if(params_.enableDetailedResults) {
FastSet<Index> involvedRelinKeys; FastSet<Index> involvedRelinKeys;