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

Fixed bug in iSAM2 when removing factors/variables. The set of ordering constraints should not contain unaffected variables.

release/4.3a0
Stephen Williams 2012-10-21 02:09:58 +00:00
parent 71bd64c693
commit 5505483111
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

@ -463,6 +463,15 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
BOOST_FOREACH(Index unused, unusedIndices) {
affectedUsedKeys.erase(unused);
}
// Remove unaffected keys from the constraints
FastMap<Index,int>::iterator iter = reorderingMode.constrainedKeys->begin();
while(iter != reorderingMode.constrainedKeys->end()) {
if(affectedUsedKeys.find(iter->first) == affectedUsedKeys.end()) {
reorderingMode.constrainedKeys->erase(iter++);
} else {
++iter;
}
}
Impl::PartialSolveResult partialSolveResult =
Impl::PartialSolve(factors, affectedUsedKeys, reorderingMode, (params_.factorization == ISAM2Params::QR));
gttoc(PartialSolve);