From 7ce91c7b27136274bd13d59c0f79e8db68fd6dd4 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 19 Aug 2013 22:07:31 +0000
Subject: [PATCH] Fixed two ISAM2 bugs

---
 gtsam/nonlinear/ISAM2-impl.cpp | 6 ++++++
 gtsam/nonlinear/ISAM2.cpp      | 6 ++++--
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp
index 23be20faf..2a3ff3672 100644
--- a/gtsam/nonlinear/ISAM2-impl.cpp
+++ b/gtsam/nonlinear/ISAM2-impl.cpp
@@ -195,9 +195,15 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta,
   assert(values.size() == delta.size());
   Values::iterator key_value;
   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)
   {
     assert(key_value->key == key_delta->first);
+#endif
     Key var = key_value->key;
     assert(delta[var].size() == (int)key_value->value.dim());
     assert(delta[var].allFinite());
diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index 1b3849bb3..b3d976acc 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -473,9 +473,11 @@ boost::shared_ptr<FastSet<Key> > ISAM2::recalculate(const FastSet<Key>& markedKe
     }
 
     // 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))
-        constraintGroups.erase(iter);
+        constraintGroups.erase(iter ++);
+      else
+        ++ iter;
     }
     gttoc(ordering_constraints);