From 99b1ab475408adaac3c7a88593c8f198db288e7b Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 24 Jun 2013 19:30:18 +0000
Subject: [PATCH] Added FactorGraph constructor from BayesTree in unordered
 code.  Made FactorGraph += operator templated so can += any type that works
 with push_back.

---
 gtsam/inference/FactorGraphUnordered-inst.h   | 28 +++++++++++----
 gtsam/inference/FactorGraphUnordered.h        | 35 +++++++++++--------
 .../symbolic/SymbolicFactorGraphUnordered.cpp |  5 +++
 gtsam/symbolic/SymbolicFactorGraphUnordered.h |  7 ++++
 4 files changed, 54 insertions(+), 21 deletions(-)

diff --git a/gtsam/inference/FactorGraphUnordered-inst.h b/gtsam/inference/FactorGraphUnordered-inst.h
index bc07760d6..e7b4e62f6 100644
--- a/gtsam/inference/FactorGraphUnordered-inst.h
+++ b/gtsam/inference/FactorGraphUnordered-inst.h
@@ -16,24 +16,21 @@
  * @author Frank Dellaert
  * @author Alireza Fathi
  * @author Michael Kaess
+ * @author Richard Roberts
  */
 
 #pragma once
 
 #include <gtsam/base/FastSet.h>
-#include <gtsam/inference/BayesTree.h>
+#include <gtsam/base/treeTraversal-inst.h>
 #include <gtsam/inference/VariableIndex.h>
 #include <gtsam/inference/FactorGraphUnordered.h>
 
 #include <boost/foreach.hpp>
-#include <boost/tuple/tuple.hpp>
-#include <boost/format.hpp>
-#include <boost/iterator/transform_iterator.hpp>
+#include <boost/bind.hpp>
 
 #include <stdio.h>
-#include <list>
 #include <sstream>
-#include <stdexcept>
 
 namespace gtsam {
 
@@ -67,6 +64,25 @@ namespace gtsam {
     return true;
   }
 
+  /* ************************************************************************* */
+  namespace {
+    template<class FACTOR, class CLIQUE>
+    int _pushClique(FactorGraphUnordered<FACTOR>* fg, const boost::shared_ptr<CLIQUE>& clique) {
+      fg->push_back(clique->conditional_);
+      return 0;
+    }
+  }
+
+  /* ************************************************************************* */
+  template<class FACTOR>
+  template<class CLIQUE>
+  void FactorGraphUnordered<FACTOR>::push_back_bayesTree(const BayesTreeUnordered<CLIQUE>& bayesTree)
+  {
+    // Traverse the BayesTree and add all conditionals to this graph
+    int data = 0; // Unused
+    treeTraversal::DepthFirstForest(bayesTree, data, boost::bind(&_pushClique<FACTOR,CLIQUE>, this, _1));
+  }
+
   /* ************************************************************************* */
   template<class FACTOR>
   size_t FactorGraphUnordered<FACTOR>::nrFactors() const {
diff --git a/gtsam/inference/FactorGraphUnordered.h b/gtsam/inference/FactorGraphUnordered.h
index 580b27fba..d5469c624 100644
--- a/gtsam/inference/FactorGraphUnordered.h
+++ b/gtsam/inference/FactorGraphUnordered.h
@@ -15,6 +15,7 @@
  * @author  Carlos Nieto
  * @author  Christian Potthast
  * @author  Michael Kaess
+ * @author  Richard Roberts
  */
 
 // \callgraph
@@ -27,6 +28,7 @@
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/inference/Key.h>
+#include <gtsam/inference/BayesTreeUnordered.h>
 
 namespace gtsam {
 
@@ -39,7 +41,6 @@ namespace gtsam {
   class FactorGraphUnordered {
 
   public:
-    typedef FactorGraphUnordered<FACTOR> This;
     typedef FACTOR FactorType;  ///< factor type
     typedef boost::shared_ptr<FACTOR> sharedFactor;  ///< Shared pointer to a factor
     typedef boost::shared_ptr<typename FACTOR::ConditionalType> sharedConditional;  ///< Shared pointer to a conditional
@@ -57,7 +58,6 @@ namespace gtsam {
     /** Collection of factors */
     std::vector<sharedFactor> factors_;
 
-  public:
     /// @name Standard Constructors
     /// @{
 
@@ -93,6 +93,8 @@ namespace gtsam {
     //}
 
     /// @}
+
+  public:
     /// @name Adding Factors
     /// @{
 
@@ -107,35 +109,38 @@ namespace gtsam {
     /** Add a factor directly using a shared_ptr */
     template<class DERIVEDFACTOR>
     void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) {
-      factors_.push_back(boost::shared_ptr<FACTOR>(factor));
-    }
+      factors_.push_back(boost::shared_ptr<FACTOR>(factor)); }
 
     /** Add a factor, will be copy-constructed into a shared_ptr (use push_back to avoid the copy). */
     template<class DERIVEDFACTOR>
     void add(const DERIVEDFACTOR& factor) {
-      factors_.push_back(boost::make_shared<DERIVEDFACTOR>(factor));
-    }
+      factors_.push_back(boost::make_shared<DERIVEDFACTOR>(factor)); }
 
     /** push back many factors */
     void push_back(const This& factors) {
-      factors_.insert(end(), factors.begin(), factors.end());
-    }
+      factors_.insert(end(), factors.begin(), factors.end()); }
 
     /** push back many factors with an iterator */
     template<typename ITERATOR>
     void push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
-      factors_.insert(end(), firstFactor, lastFactor);
-    }
-    
+      factors_.insert(end(), firstFactor, lastFactor); }
+
+  protected:
+    /** push back a BayesTree as a collection of factors.  NOTE: This should be hidden in derived
+     *  classes in favor of a type-specialized version that calls this templated function. */
+    template<class CLIQUE>
+    void push_back_bayesTree(const BayesTreeUnordered<CLIQUE>& bayesTree);
+
+  public:
     /** += syntax for push_back, e.g. graph += f1, f2, f3 */
-    boost::assign::list_inserter<boost::assign_detail::call_push_back<This>, sharedFactor>
-      operator+=(const sharedFactor& factor)
+    template<class T>
+    boost::assign::list_inserter<boost::assign_detail::call_push_back<This>, T>
+      operator+=(const T& factorOrContainer)
     {
       return boost::assign::make_list_inserter(
-        boost::assign_detail::call_push_back<This>(*this))(factor);
+        boost::assign_detail::call_push_back<This>(*this))(factorOrContainer);
     }
 
-
     /**
      * @brief Add a vector of derived factors
      * @param factors to add
diff --git a/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp b/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp
index 67ef4bd4c..9f6689713 100644
--- a/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp
+++ b/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp
@@ -47,6 +47,11 @@ namespace gtsam {
     push_back(boost::make_shared<SymbolicFactorUnordered>(key1,key2,key3,key4));
   }
 
+  /* ************************************************************************* */
+  void SymbolicFactorGraphUnordered::push_back_bayesTree(const SymbolicBayesTreeUnordered& bayesTree) {
+    Base::push_back_bayesTree(bayesTree);
+  }
+
 //  /* ************************************************************************* */
 //  std::pair<SymbolicFactorGraph::sharedConditional, SymbolicFactorGraph>
 //    SymbolicFactorGraph::eliminateFrontals(size_t nFrontals) const
diff --git a/gtsam/symbolic/SymbolicFactorGraphUnordered.h b/gtsam/symbolic/SymbolicFactorGraphUnordered.h
index fcd42f4ad..e382e74bf 100644
--- a/gtsam/symbolic/SymbolicFactorGraphUnordered.h
+++ b/gtsam/symbolic/SymbolicFactorGraphUnordered.h
@@ -65,6 +65,10 @@ namespace gtsam {
     template<typename ITERATOR>
     SymbolicFactorGraphUnordered(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
 
+    /** Constructor from a BayesTree */
+    SymbolicFactorGraphUnordered(const SymbolicBayesTreeUnordered& bayesTree) {
+      push_back_bayesTree(bayesTree); }
+
     /// @}
     /// @name Standard Interface
     /// @{
@@ -81,6 +85,9 @@ namespace gtsam {
     /** Push back 4-way factor */
     void push_factor(Key key1, Key key2, Key key3, Key key4);
 
+    /** push back a BayesTree as a collection of factors. */
+    void push_back_bayesTree(const SymbolicBayesTreeUnordered& bayesTree);
+
     /// @}
   };