From 67431ba8ad29aba20cc63f309905a09ee11fa407 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Sat, 10 Aug 2013 15:51:52 +0000
Subject: [PATCH] Fix marginalizeLeaves bug

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

diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index 7047d7990..82bc67769 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -871,11 +871,11 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList)
         graph2.push_back(clique->conditional());
         GaussianFactorGraph::EliminationResult eliminationResult2 = 
           params_.getEliminationFunction()(graph2, Ordering(
-          clique->conditional()->beginFrontals(), jPosition));
+          clique->conditional()->beginFrontals(), jPosition + 1));
         GaussianFactorGraph graph3;
         graph3.push_back(eliminationResult2.second);
         GaussianFactorGraph::EliminationResult eliminationResult3 = 
-          params_.getEliminationFunction()(graph3, Ordering(jPosition, clique->conditional()->endFrontals()));
+          params_.getEliminationFunction()(graph3, Ordering(jPosition + 1, clique->conditional()->endFrontals()));
         sharedClique newClique = boost::make_shared<Clique>();
         newClique->setEliminationResult(make_pair(eliminationResult3.first, clique->cachedFactor()));