From 806d86513d205fc4a3972f07b18c9d4ce9d37996 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Thu, 5 Jul 2012 17:08:05 +0000
Subject: [PATCH] Fixed bug in iSAM2 variable removal where removed variables
 still became marked when marking relin-affected variables

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

diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index e47127edd..f1881ebd5 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -649,6 +649,15 @@ ISAM2Result ISAM2::update(
       // add other cliques that have the marked ones in the separator
       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
       if(params_.enableDetailedResults) {
         FastSet<Index> involvedRelinKeys;