From d9ef3814831e787c645dc582d7da4c4314cd41a4 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 18 Mar 2013 19:27:53 +0000
Subject: [PATCH] Fixed bug in FactorGraph::eliminate - was not unpermuting the
 removed factors - even though they're removed, since they're shared, they can
 be used still in other graphs

---
 gtsam/inference/FactorGraph-inl.h | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h
index 783ed1db3..f757f5e74 100644
--- a/gtsam/inference/FactorGraph-inl.h
+++ b/gtsam/inference/FactorGraph-inl.h
@@ -184,8 +184,7 @@ namespace gtsam {
         Permutation toFront = Permutation::PullToFront(variables, highestInvolvedVariable+1);
         Permutation toFrontInverse = *toFront.inverse();
         BOOST_FOREACH(const sharedFactor& factor, involvedFactors) {
-          factor->permuteWithInverse(toFrontInverse);
-        }
+          factor->permuteWithInverse(toFrontInverse); }
 
         // Eliminate into conditional and remaining factor
         EliminationResult eliminated = eliminateFcn(involvedFactors, variables.size());
@@ -193,6 +192,8 @@ namespace gtsam {
         remainingFactor = eliminated.second;
 
         // Undo the permutation
+        BOOST_FOREACH(const sharedFactor& factor, involvedFactors) {
+          factor->permuteWithInverse(toFront); }
         conditional->permuteWithInverse(toFront);
         remainingFactor->permuteWithInverse(toFront);
       } else {