From 15bd617a0c12696ef5b013a16aeb7e1af6f193c5 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 12 Aug 2013 18:21:31 +0000
Subject: [PATCH] Added missing non-ordered eliminatePartialMultifrontal

---
 .../inference/EliminateableFactorGraph-inst.h | 20 ++++++++++++++++
 .../tests/testSymbolicFactorGraph.cpp         | 23 +++++++++++++++++++
 2 files changed, 43 insertions(+)

diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h
index d75017ac8..8ec7bea45 100644
--- a/gtsam/inference/EliminateableFactorGraph-inst.h
+++ b/gtsam/inference/EliminateableFactorGraph-inst.h
@@ -137,6 +137,26 @@ namespace gtsam {
     }
   }
 
+  /* ************************************************************************* */
+  template<class FACTORGRAPH>
+  std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
+    EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
+    const std::vector<Key>& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
+  {
+    if(variableIndex) {
+      gttic(eliminatePartialMultifrontal);
+      // Compute full ordering
+      Ordering fullOrdering = Ordering::COLAMDConstrainedFirst(*variableIndex, variables);
+
+      // Split off the part of the ordering for the variables being eliminated
+      Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
+      return eliminatePartialMultifrontal(ordering, function, variableIndex);
+    } else {
+      // If no variable index is provided, compute one and call this function again
+      return eliminatePartialMultifrontal(variables, function, VariableIndex(asDerived()));
+    }
+  }
+
   /* ************************************************************************* */
   template<class FACTORGRAPH>
   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
index 879ab6601..c2eabdd20 100644
--- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
+++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
@@ -63,6 +63,14 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential)
 
   EXPECT(assert_equal(expectedSfg, *actualSfg));
   EXPECT(assert_equal(expectedBayesNet, *actualBayesNet));
+
+  SymbolicBayesNet::shared_ptr actualBayesNet2;
+  SymbolicFactorGraph::shared_ptr actualSfg2;
+  boost::tie(actualBayesNet2, actualSfg2) =
+    simpleTestGraph2.eliminatePartialSequential(list_of(0)(1).convert_to_container<vector<Key> >());
+
+  EXPECT(assert_equal(expectedSfg, *actualSfg2));
+  EXPECT(assert_equal(expectedBayesNet, *actualBayesNet2));
 }
 
 /* ************************************************************************* */
@@ -100,6 +108,21 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal)
 
   EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph));
   EXPECT(assert_equal(expectedBayesTree, *actualBayesTree));
+
+  SymbolicBayesTree expectedBayesTree2;
+  SymbolicBayesTreeClique::shared_ptr root2 = boost::make_shared<SymbolicBayesTreeClique>(
+    boost::make_shared<SymbolicConditional>(4,1));
+  root2->children.push_back(boost::make_shared<SymbolicBayesTreeClique>(
+    boost::make_shared<SymbolicConditional>(5,4)));
+  expectedBayesTree2.insertRoot(root2);
+
+  SymbolicBayesTree::shared_ptr actualBayesTree2;
+  SymbolicFactorGraph::shared_ptr actualFactorGraph2;
+  boost::tie(actualBayesTree2, actualFactorGraph2) =
+    simpleTestGraph2.eliminatePartialMultifrontal(list_of<Key>(4)(5).convert_to_container<vector<Key> >());
+
+  EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2));
+  EXPECT(assert_equal(expectedBayesTree2, *actualBayesTree2));
 }
 
 /* ************************************************************************* */