From f575cb4cc038ca8368f6b017c443c2765ca2467f Mon Sep 17 00:00:00 2001
From: Stephen Williams <stephen.vincent.williams@gmail.com>
Date: Thu, 5 Jul 2012 18:37:08 +0000
Subject: [PATCH] Fixed bug in iSAM2 where variables are deleted inside of
 iSAM2 even though they appear in the set of new, incoming factors

---
 gtsam/nonlinear/ISAM2.cpp | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index f1881ebd5..bb579195f 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -579,6 +579,15 @@ ISAM2Result ISAM2::update(
 				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
 		// instead of placing it into the tree because removeTop will need to
 		// update it.