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

Fixed two ISAM2 bugs

release/4.3a0
Richard Roberts 2013-08-19 22:07:31 +00:00
parent 568335d21f
commit 7ce91c7b27
2 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -195,9 +195,15 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta,
assert(values.size() == delta.size()); assert(values.size() == delta.size());
Values::iterator key_value; Values::iterator key_value;
VectorValues::const_iterator key_delta; VectorValues::const_iterator key_delta;
#ifdef GTSAM_USE_TBB
for(key_value = values.begin(); key_value != values.end(); ++key_value)
{
key_delta = delta.find(key_value->key);
#else
for(key_value = values.begin(), key_delta = delta.begin(); key_value != values.end(); ++key_value, ++key_delta) for(key_value = values.begin(), key_delta = delta.begin(); key_value != values.end(); ++key_value, ++key_delta)
{ {
assert(key_value->key == key_delta->first); assert(key_value->key == key_delta->first);
#endif
Key var = key_value->key; Key var = key_value->key;
assert(delta[var].size() == (int)key_value->value.dim()); assert(delta[var].size() == (int)key_value->value.dim());
assert(delta[var].allFinite()); assert(delta[var].allFinite());

6
gtsam/nonlinear/ISAM2.cpp
View File

@ -473,9 +473,11 @@ boost::shared_ptr<FastSet<Key> > ISAM2::recalculate(const FastSet<Key>& markedKe
} }
// Remove unaffected keys from the constraints // Remove unaffected keys from the constraints
for(FastMap<Key,int>::iterator iter = constraintGroups.begin(); iter != constraintGroups.end(); ++iter) { for(FastMap<Key,int>::iterator iter = constraintGroups.begin(); iter != constraintGroups.end(); /*Incremented in loop ++iter*/) {
if(unusedIndices.exists(iter->first) || !affectedKeysSet->exists(iter->first)) if(unusedIndices.exists(iter->first) || !affectedKeysSet->exists(iter->first))
constraintGroups.erase(iter); constraintGroups.erase(iter ++);
else
++ iter;
} }
gttoc(ordering_constraints); gttoc(ordering_constraints);