From 550548311163beec36aac4c458f55eb44cbd5d13 Mon Sep 17 00:00:00 2001
From: Stephen Williams <stephen.vincent.williams@gmail.com>
Date: Sun, 21 Oct 2012 02:09:58 +0000
Subject: [PATCH] Fixed bug in iSAM2 when removing factors/variables. The set
 of ordering constraints should not contain unaffected variables.

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

diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index d1d172cde..be82fadc3 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -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);