Started implementing junction tree construction

gtsam/base/treeTraversal-inst.h

@@ -28,7 +28,7 @@ namespace gtsam {
       template<typename NODE, typename DATA>
       struct TraversalNode {
         bool expanded;
-        NODE* const treeNode;
+        NODE& const treeNode;
         DATA data;
         TraversalNode(NODE* _treeNode, const DATA& _data) :
           expanded(false), treeNode(_treeNode), data(_data) {}
@@ -38,16 +38,14 @@ namespace gtsam {
       template<typename NODE, typename VISITOR, typename DATA, typename STACK>
       struct PreOrderExpand {
         VISITOR& visitor_;
-        DATA* parentData_;
+        DATA& parentData_;
         STACK& stack_;
-        PreOrderExpand(VISITOR& visitor, DATA* parentData, STACK& stack) :
+        PreOrderExpand(VISITOR& visitor, DATA& parentData, STACK& stack) :
           visitor_(visitor), parentData_(parentData), stack_(stack) {}
         template<typename P>
         void operator()(const P& nodePointer) {
-          // Get raw pointer (in case nodePointer is a shared_ptr)
-          NODE* rawNodePointer = &(*nodePointer);
           // Add node
-          stack_.push(TraversalNode<NODE,DATA>(rawNodePointer, visitor_(nodePointer, *parentData_)));
+          stack_.push(TraversalNode<NODE,DATA>(*nodePointer, visitor_(nodePointer, parentData_)));
         }
       };
 
@@ -82,7 +80,7 @@ namespace gtsam {
       // Add roots to stack (use reverse iterators so children are processed in the order they
       // appear)
       (void) std::for_each(forest.roots().rbegin(), forest.roots().rend(),
-        Expander(visitorPre, &rootData, stack));
+        Expander(visitorPre, rootData, stack));
 
       // Traverse
       while(!stack.empty())
@@ -93,13 +91,13 @@ namespace gtsam {
         if(node.expanded) {
           // If already expanded, then the data stored in the node is no longer needed, so visit
           // then delete it.
-          (void) visitorPost(*node.treeNode, node.data);
+          (void) visitorPost(node.treeNode, node.data);
           stack.pop();
         } else {
           // If not already visited, visit the node and add its children (use reverse iterators so
           // children are processed in the order they appear)
           (void) std::for_each(node.treeNode->children.rbegin(), node.treeNode->children.rend(),
-            Expander(visitorPre, &node.data, stack));
+            Expander(visitorPre, node.data, stack));
           node.expanded = true;
         }
       }

gtsam/inference/EliminationTreeUnordered-inst.h

@@ -37,6 +37,8 @@ namespace gtsam {
     const boost::shared_ptr<BayesNetType>& output,
     const Eliminate& function, const std::vector<sharedFactor>& childrenResults) const
   {
+    // This function eliminates one node (Node::eliminate) - see below eliminate for the whole tree.
+
     assert(childrenResults.size() == children.size());
 
     // Gather factors
@@ -146,8 +148,7 @@ namespace gtsam {
     // Build variable index first
     const VariableIndexUnordered variableIndex(factorGraph);
     This temp(factorGraph, variableIndex, order);
-    roots_.swap(temp.roots_); // Swap in the tree, and temp will be deleted
+    this->swap(temp); // Swap in the tree, and temp will be deleted
-    remainingFactors_.swap(temp.remainingFactors_);
   }
 
   /* ************************************************************************* */
@@ -295,4 +296,12 @@ namespace gtsam {
     return true;
   }
 
+  /* ************************************************************************* */
+  template<class BAYESNET, class GRAPH>
+  void EliminationTreeUnordered<BAYESNET,GRAPH>::swap(This& other) {
+    roots_.swap(other.roots_);
+    remainingFactors_.swap(other.remainingFactors_);
+  }
+
+
 }

gtsam/inference/EliminationTreeUnordered.h

@@ -65,8 +65,8 @@ namespace gtsam {
       Eliminate; ///< Typedef for an eliminate subroutine
 
     struct Node {
-      typedef FastList<sharedFactor> Factors;
+      typedef std::vector<sharedFactor> Factors;
-      typedef FastList<boost::shared_ptr<Node> > Children;
+      typedef std::vector<boost::shared_ptr<Node> > Children;
 
       Key key; ///< key associated with root
       Factors factors; ///< factors associated with root
@@ -146,6 +146,9 @@ namespace gtsam {
     
     const FastList<sharedNode>& roots() const { return roots_; }
 
+    /** Swap the data of this tree with another one, this operation is very fast. */
+    void swap(This& other);
+
   protected:
     /// Protected default constructor
     EliminationTreeUnordered() {}

gtsam/inference/JunctionTreeUnordered-inst.h

@@ -28,6 +28,71 @@
 
 namespace gtsam {
 
+  namespace {
+  /* ************************************************************************* */
+  template<class BAYESTREE, class GRAPH>
+  struct ConstructorTraversalData {
+    const ConstructorTraversalData& parentData;
+    typename JunctionTreeUnordered<BAYESTREE,GRAPH>::sharedNode myJTNode;
+    ConstructorTraversalData(const ConstructorTraversalData& _parentData) : parentData(_parentData) {}
+  };
+
+  /* ************************************************************************* */
+  template<class BAYESTREE, class GRAPH, class ETREE_NODE>
+  ConstructorTraversalData<BAYESTREE,GRAPH> ConstructorTraversalVisitorPre(
+    const boost::shared_ptr<ETREE_NODE>& node,
+    const ConstructorTraversalData<BAYESTREE,GRAPH>& parentData)
+  {
+    // On the pre-order pass, before children have been visited, we just set up a traversal data
+    // structure with its own JT node, and create a child pointer in its parent.
+    ConstructorTraversalData<BAYESTREE,GRAPH> myData = ConstructorTraversalData<BAYESTREE,GRAPH>(parentData);
+    myData.myJTNode = boost::make_shared<typename JunctionTreeUnordered<BAYESREE,GRAPH>::Node>();
+    myData.myJTNode->keys.push_back(node->key);
+    myData.myJTNode->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end());
+    parentData.myJTNode->children.push_back(myData.myJTNode);
+    return myData;
+  }
+
+  /* ************************************************************************* */
+  template<class BAYESTREE, class GRAPH, class ETREE_NODE, class SYMBOLIC_CONDITIONAL>
+  struct ConstructorTraversalVisitorPost
+  {
+    // Store and increment an iterator into the Bayes net during the post-order step
+    FactorGraphUnordered<SYMBOLIC_CONDITIONAL>::const_iterator symbolicBayesNetIterator;
+
+    // Constructor that starts at the beginning of the Bayes net
+    ConstructorTraversalVisitorPost(const FactorGraphUnordered<SYMBOLIC_CONDITIONAL>& symbolicBayesNet) :
+      symbolicBayesNetIterator(symbolicBayesNet.begin()) {}
+
+    // Post-order visitor function
+    void operator()(
+      const boost::shared_ptr<ETREE_NODE>& node,
+      const ConstructorTraversalData<BAYESTREE,GRAPH>& myData)
+    {
+      // In the post-order step, we check if we are in the same clique with our parent.  If so, we
+      // merge our JT nodes.
+      if()
+    }
+  };
+
+  }
+
+  /* ************************************************************************* */
+  template<class BAYESTREE, class GRAPH>
+  template<class ETREE, class SYMBOLIC_CONDITIONAL>
+  JunctionTreeUnordered(const ETREE& eliminationTree,
+    const FactorGraphUnordered<SYMBOLIC_CONDITIONAL>& symbolicBayesNet)
+  {
+    // Here we rely on the BayesNet having been produced by this elimination tree, such that the
+    // conditionals are arranged in DFS post-order.  We traverse the elimination tree, and inspect
+    // the symbolic conditional corresponding to each node.  The elimination tree node is added to
+    // the same clique with its parent if it has exactly one more Bayes net conditional parent than
+    // does its elimination tree parent.
+
+
+  }
+
+
   /* ************************************************************************* */
   template <class FG, class BTCLIQUE>
   void JunctionTree<FG,BTCLIQUE>::construct(const FG& fg, const VariableIndex& variableIndex) {

gtsam/inference/JunctionTreeUnordered.h

@@ -21,51 +21,11 @@
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/inference/Key.h>
+#include <gtsam/inference/FactorGraphUnordered.h>
+#include <gtsam/inference/EliminationTreeUnordered.h>
 
 namespace gtsam {
 
-  template<class BAYESTREE, class GRAPH>
-  class JunctionTreeUnordered {
-
-  public:
-
-    typedef GRAPH FactorGraphType; ///< The factor graph type
-    typedef typename GRAPH::FactorType FactorType; ///< The type of factors
-    typedef JunctionTreeUnordered<BAYESTREE, GRAPH> This; ///< This class
-    typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
-    typedef typename boost::shared_ptr<FactorType> sharedFactor;  ///< Shared pointer to a factor
-    typedef BAYESTREE BayesTreeType; ///< The BayesNet corresponding to FACTOR
-    typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals
-    typedef typename boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
-    typedef boost::function<std::pair<sharedConditional,sharedFactor>(std::vector<sharedFactor>, std::vector<Key>)>
-      Eliminate; ///< Typedef for an eliminate subroutine
-
-    struct Node {
-      typedef FastList<sharedFactor> Factors;
-      typedef FastList<boost::shared_ptr<Node> > Children;
-
-      Key key; ///< key associated with root
-      Factors factors; ///< factors associated with root
-      Children children; ///< sub-trees
-
-      sharedFactor eliminate(const boost::shared_ptr<BayesTreeType>& output,
-        const Eliminate& function, const std::vector<sharedFactor>& childrenFactors) const;
-    };
-
-    typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
-
-  private:
-
-    /** concept check */
-    GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
-
-    FastList<sharedNode> roots_;
-    std::vector<sharedFactor> remainingFactors_;
-
-  public:
-
-  };
-
   /**
    * A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with
    * the additional property that it represents the clique tree associated with a Bayes net.
@@ -87,86 +47,58 @@ namespace gtsam {
    * \addtogroup Multifrontal
    * \nosubgrouping
    */
-  template<class FG, class BTCLIQUE=typename BayesTree<typename FG::FactorType::ConditionalType>::Clique>
+  template<class BAYESTREE, class GRAPH>
-  class JunctionTree: public ClusterTree<FG> {
+  class JunctionTreeUnordered {
 
   public:
 
-    /// In a junction tree each cluster is associated with a clique
+    typedef GRAPH FactorGraphType; ///< The factor graph type
-    typedef typename ClusterTree<FG>::Cluster Clique;
+    typedef typename GRAPH::FactorType FactorType; ///< The type of factors
-    typedef typename Clique::shared_ptr sharedClique; ///< Shared pointer to a clique
+    typedef JunctionTreeUnordered<BAYESTREE, GRAPH> This; ///< This class
+    typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
+    typedef typename boost::shared_ptr<FactorType> sharedFactor;  ///< Shared pointer to a factor
+    typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination
+    typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals
+    typedef typename boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
+    typedef boost::function<std::pair<sharedConditional,sharedFactor>(std::vector<sharedFactor>, std::vector<Key>)>
+      Eliminate; ///< Typedef for an eliminate subroutine
 
-    /// The BayesTree type produced by elimination
+    struct Node {
-    typedef BTCLIQUE BTClique;
+      typedef std::vector<Key> Keys;
+      typedef std::vector<sharedFactor> Factors;
+      typedef std::vector<boost::shared_ptr<Node> > Children;
 
-    /// Shared pointer to this class
+      Keys keys; ///< Frontal keys of this node
-    typedef boost::shared_ptr<JunctionTree<FG> > shared_ptr;
+      Factors factors; ///< Factors associated with this node
+      Children children; ///< sub-trees
 
-    /// We will frequently refer to a symbolic Bayes tree, used to find the clique structure
+      sharedFactor eliminate(const boost::shared_ptr<BayesTreeType>& output,
-    typedef gtsam::BayesTree<IndexConditional> SymbolicBayesTree;
+        const Eliminate& function, const std::vector<sharedFactor>& childrenFactors) const;
+    };
+
+    typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
 
   private:
 
-    /// @name Advanced Interface
+    /** concept check */
-    /// @{
+    GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
 
-    /// distribute the factors along the cluster tree
+    FastList<sharedNode> roots_;
-    sharedClique distributeFactors(const FG& fg,
+    std::vector<sharedFactor> remainingFactors_;
-        const SymbolicBayesTree::sharedClique& clique);
-
-    /// distribute the factors along the cluster tree
-    sharedClique distributeFactors(const FG& fg, const std::vector<FastList<size_t> >& targets,
-        const SymbolicBayesTree::sharedClique& clique);
-
-    /// recursive elimination function
-    std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor>
-    eliminateOneClique(typename FG::Eliminate function,
-        const boost::shared_ptr<const Clique>& clique) const;
-
-    /// internal constructor
-    void construct(const FG& fg, const VariableIndex& variableIndex);
-
-    /// @}
 
   public:
 
     /// @name Standard Constructors
     /// @{
 
-    /** Default constructor */
+    /** Build the junction tree from an elimination tree and a symbolic Bayes net. */
-    JunctionTree() {}
+    template<class ETREE, class SYMBOLIC_CONDITIONAL>
-
+    JunctionTreeUnordered(
-    /** Named constructor to build the junction tree of a factor graph.  Note
+      const ETREE& eliminationTree,
-     * that this has to compute the column structure as a VariableIndex, so if you
+      const FactorGraphUnordered<SYMBOLIC_CONDITIONAL>& symbolicBayesNet);
-     * already have this precomputed, use the JunctionTree(const FG&, const VariableIndex&)
-     * constructor instead.
-     * @param factorGraph The factor graph for which to build the elimination tree
-     */
-    JunctionTree(const FG& factorGraph);
-
-    /** Construct from a factor graph and pre-computed variable index.
-     * @param fg The factor graph for which to build the junction tree
-     * @param structure The set of factors involving each variable.  If this is not
-     * precomputed, you can call the JunctionTree(const FG&)
-     * constructor instead.
-     */
-    JunctionTree(const FG& fg, const VariableIndex& variableIndex);
-
-    /// @}
-    /// @name Standard Interface
-    /// @{
-
-    /** Eliminate the factors in the subgraphs to produce a BayesTree.
-     * @param function The function used to eliminate, see the namespace functions
-     * in GaussianFactorGraph.h
-     * @return The BayesTree resulting from elimination
-     */
-    typename BTClique::shared_ptr eliminate(typename FG::Eliminate function) const;
 
     /// @}
 
-  }; // JunctionTree
+  };
 
-} // namespace gtsam
+}
-
-#include <gtsam/inference/JunctionTree-inl.h>