From 67f70c37b5d1cf94cb0c5af9c620e2f9b54df501 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 18 Nov 2013 19:23:13 +0000
Subject: [PATCH] Factored out symbolic elimination into a templated function

---
 gtsam/symbolic/SymbolicFactor-inst.h | 69 ++++++++++++++++++++++++++++
 gtsam/symbolic/SymbolicFactor.cpp    | 31 +------------
 2 files changed, 71 insertions(+), 29 deletions(-)
 create mode 100644 gtsam/symbolic/SymbolicFactor-inst.h

diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h
new file mode 100644
index 000000000..7b626f2fa
--- /dev/null
+++ b/gtsam/symbolic/SymbolicFactor-inst.h
@@ -0,0 +1,69 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    SymbolicFactor-inst.h
+ * @author  Richard Roberts
+ * @date    Oct 17, 2010
+ */
+
+#pragma once
+
+#include <utility>
+#include <boost/shared_ptr.hpp>
+#include <boost/foreach.hpp>
+#include <boost/make_shared.hpp>
+
+#include <gtsam/inference/Factor.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/symbolic/SymbolicFactor.h>
+#include <gtsam/symbolic/SymbolicConditional.h>
+
+namespace gtsam
+{
+  namespace internal
+  {
+    /** Implementation of dense elimination function for symbolic factors.  This is a templated
+     *  version for internally doing symbolic elimination on any factor. */
+    template<class FACTOR>
+    std::pair<boost::shared_ptr<SymbolicConditional>, boost::shared_ptr<SymbolicFactor> >
+      EliminateSymbolic(const FactorGraph<FACTOR>& factors, const Ordering& keys)
+    {
+      // Gather all keys
+      FastSet<Key> allKeys;
+      BOOST_FOREACH(const boost::shared_ptr<FACTOR>& factor, factors) {
+        allKeys.insert(factor->begin(), factor->end());
+      }
+
+      // Check keys
+      BOOST_FOREACH(Key key, keys) {
+        if(allKeys.find(key) == allKeys.end())
+          throw std::runtime_error("Requested to eliminate a key that is not in the factors");
+      }
+
+      // Sort frontal keys
+      FastSet<Key> frontals(keys);
+      const size_t nFrontals = keys.size();
+
+      // Build a key vector with the frontals followed by the separator
+      FastVector<Key> orderedKeys(allKeys.size());
+      std::copy(keys.begin(), keys.end(), orderedKeys.begin());
+      std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals);
+
+      // Return resulting conditional and factor
+      return make_pair(
+        boost::make_shared<SymbolicConditional>(
+        SymbolicConditional::FromKeys(orderedKeys, nFrontals)),
+        boost::make_shared<SymbolicFactor>(
+        SymbolicFactor::FromIterators(orderedKeys.begin() + nFrontals, orderedKeys.end())));
+    }
+  }
+}
diff --git a/gtsam/symbolic/SymbolicFactor.cpp b/gtsam/symbolic/SymbolicFactor.cpp
index f276c2e63..327de0c10 100644
--- a/gtsam/symbolic/SymbolicFactor.cpp
+++ b/gtsam/symbolic/SymbolicFactor.cpp
@@ -15,14 +15,12 @@
  * @date    Oct 17, 2010
  */
 
-#include <boost/foreach.hpp>
-#include <boost/make_shared.hpp>
-
 #include <gtsam/base/FastVector.h>
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/symbolic/SymbolicFactor.h>
 #include <gtsam/symbolic/SymbolicConditional.h>
 #include <gtsam/symbolic/SymbolicFactorGraph.h>
+#include <gtsam/symbolic/SymbolicFactor-inst.h>
 
 using namespace std;
 
@@ -32,32 +30,7 @@ namespace gtsam {
   std::pair<boost::shared_ptr<SymbolicConditional>, boost::shared_ptr<SymbolicFactor> >
     EliminateSymbolic(const SymbolicFactorGraph& factors, const Ordering& keys)
   {
-    // Gather all keys
-    FastSet<Key> allKeys;
-    BOOST_FOREACH(const SymbolicFactor::shared_ptr& factor, factors) {
-      allKeys.insert(factor->begin(), factor->end()); }
-
-    // Check keys
-    BOOST_FOREACH(Key key, keys) {
-      if(allKeys.find(key) == allKeys.end())
-        throw std::runtime_error("Requested to eliminate a key that is not in the factors");
-    }
-
-    // Sort frontal keys
-    FastSet<Key> frontals(keys);
-    const size_t nFrontals = keys.size();
-
-    // Build a key vector with the frontals followed by the separator
-    FastVector<Key> orderedKeys(allKeys.size());
-    std::copy(keys.begin(), keys.end(), orderedKeys.begin());
-    std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals);
-
-    // Return resulting conditional and factor
-    return make_pair(
-      boost::make_shared<SymbolicConditional>(
-      SymbolicConditional::FromKeys(orderedKeys, nFrontals)),
-      boost::make_shared<SymbolicFactor>(
-      SymbolicFactor::FromIterators(orderedKeys.begin() + nFrontals, orderedKeys.end())));
+    return internal::EliminateSymbolic(factors, keys);
   }
 
   /* ************************************************************************* */