From caa031f7337328675e13b23ad447178f5f4c60ec Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Wed, 31 Oct 2012 22:27:33 +0000
Subject: [PATCH] Reducing variable indices in computing joint marginals

---
 gtsam/inference/BayesTree-inl.h | 24 +++++++++++++++++++++---
 1 file changed, 21 insertions(+), 3 deletions(-)

diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h
index 788770812..131a3813f 100644
--- a/gtsam/inference/BayesTree-inl.h
+++ b/gtsam/inference/BayesTree-inl.h
@@ -641,10 +641,28 @@ namespace gtsam {
     if(C2 != B)
       p_BC1C2.push_back(C2->conditional()->toFactor());
 
-    // Compute final marginal by eliminating other variables
+    // Reduce the variable indices to start at zero
+    gttic(Reduce);
+    const Permutation reduction = internal::createReducingPermutation(p_BC1C2.keys());
+    internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
+    BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_BC1C2) {
+      if(factor) factor->reduceWithInverse(inverseReduction); }
+    std::vector<Index> js; js.push_back(inverseReduction[j1]); js.push_back(inverseReduction[j2]);
+    gttoc(Reduce);
+
+    // now, marginalize out everything that is not variable j
     GenericSequentialSolver<FactorType> solver(p_BC1C2);
-    std::vector<Index> js; js.push_back(j1); js.push_back(j2);
-    return solver.jointFactorGraph(js, function);
+    boost::shared_ptr<FactorGraph<FactorType> > result = solver.jointFactorGraph(js, function);
+
+    // Undo the reduction
+    gttic(Undo_Reduce);
+    BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, *result) {
+      if(factor) factor->permuteWithInverse(reduction); }
+    BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_BC1C2) {
+      if(factor) factor->permuteWithInverse(reduction); }
+    gttoc(Undo_Reduce);
+    return result;
+
   }
 
   /* ************************************************************************* */