Fixed bug in JunctionTree construction with merging the wrong children - cleaned up some unit tests and added a sequential elimination ASIA test.

gtsam/inference/BayesNetUnordered.h

@@ -33,21 +33,27 @@ namespace gtsam {
   template<class CONDITIONAL>
   class BayesNetUnordered : public FactorGraphUnordered<CONDITIONAL> {
 
-  public:
+  private:
 
     typedef FactorGraphUnordered<CONDITIONAL> Base;
-    typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional; ///< A shared pointer to a conditional
 
   public:
+    typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional; ///< A shared pointer to a conditional
 
+  protected:
     /// @name Standard Constructors
     /// @{
 
     /** Default constructor as an empty BayesNet */
     BayesNetUnordered() {};
 
+    /** Construct from iterator over conditionals */
+    template<typename ITERATOR>
+    BayesNetUnordered(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {}
+
     /// @}
 
+  public:
     /// @name Testable
     /// @{
 

gtsam/inference/JunctionTreeUnordered-inst.h

@@ -62,7 +62,7 @@ namespace gtsam {
     // Post-order visitor function
     template<class BAYESTREE, class GRAPH, class ETREE_NODE>
     void ConstructorTraversalVisitorPost(
-      const boost::shared_ptr<ETREE_NODE>& node,
+      const boost::shared_ptr<ETREE_NODE>& ETreeNode,
       const ConstructorTraversalData<BAYESTREE,GRAPH>& myData)
     {
       // In this post-order visitor, we combine the symbolic elimination results from the
@@ -74,21 +74,20 @@ namespace gtsam {
 
       // Do symbolic elimination for this node
       std::vector<SymbolicFactorUnordered::shared_ptr> symbolicFactors;
-      symbolicFactors.reserve(node->factors.size() + myData.childSymbolicFactors.size());
+      symbolicFactors.reserve(ETreeNode->factors.size() + myData.childSymbolicFactors.size());
       // Add symbolic versions of the ETree node factors
-      BOOST_FOREACH(const typename GRAPH::sharedFactor& factor, node->factors) {
+      BOOST_FOREACH(const typename GRAPH::sharedFactor& factor, ETreeNode->factors) {
         symbolicFactors.push_back(boost::make_shared<SymbolicFactorUnordered>(
           SymbolicFactorUnordered::FromKeys(*factor))); }
       // Add symbolic factors passed up from children
       symbolicFactors.insert(symbolicFactors.end(), myData.childSymbolicFactors.begin(), myData.childSymbolicFactors.end());
-      std::vector<Key> keyAsVector(1); keyAsVector[0] = node->key;
+      std::vector<Key> keyAsVector(1); keyAsVector[0] = ETreeNode->key;
       std::pair<SymbolicConditionalUnordered::shared_ptr, SymbolicFactorUnordered::shared_ptr> symbolicElimResult =
         EliminateSymbolicUnordered(symbolicFactors, keyAsVector);
 
       // Store symbolic elimination results in the parent
       myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first);
-      if(!symbolicElimResult.second->empty())
-        myData.parentData->childSymbolicFactors.push_back(symbolicElimResult.second);
+      myData.parentData->childSymbolicFactors.push_back(symbolicElimResult.second);
 
       // Merge our children if they are in our clique - if our conditional has exactly one fewer
       // parent than our child's conditional.
@@ -109,6 +108,8 @@ namespace gtsam {
           myData.myJTNode->children.insert(myData.myJTNode->children.end(), childToMerge.children.begin(), childToMerge.children.end());
           // Remove child from list.
           myData.myJTNode->children.erase(myData.myJTNode->children.begin() + child - nrMergedChildren);
+          // Increment number of merged children
+          ++ nrMergedChildren;
         }
       }
     }

gtsam/inference/OrderingUnordered.h

@@ -25,10 +25,17 @@
 
 namespace gtsam {
   class OrderingUnordered : public std::vector<Key> {
+  protected:
+    typedef std::vector<Key> Base;
+
   public:
     /// Create an empty ordering
     GTSAM_EXPORT OrderingUnordered() {}
 
+    /// Create an ordering using iterators over keys
+    template<typename ITERATOR>
+    OrderingUnordered(ITERATOR firstKey, ITERATOR lastKey) : Base(firstKey, lastKey) {}
+
     /// Compute an ordering using COLAMD directly from a factor graph - this internally builds a
     /// VariableIndex so if you already have a VariableIndex, it is faster to use COLAMD(const
     /// VariableIndexUnordered&)

gtsam/inference/inference-inst.h

@@ -54,7 +54,9 @@ namespace gtsam {
         RESULT& result, const typename TREE::Eliminate& eliminationFunction)
       {
         // Call eliminate on the node and add the result to the parent's gathered factors
-        myData.parentData->childFactors.push_back(node->eliminate(result, eliminationFunction, myData.childFactors));
+        typename TREE::sharedFactor childFactor = node->eliminate(result, eliminationFunction, myData.childFactors);
+        if(!childFactor->empty())
+          myData.parentData->childFactors.push_back(childFactor);
       }
     }
 

gtsam/symbolic/SymbolicBayesNetUnordered.h

@@ -41,6 +41,10 @@ namespace gtsam {
 
     /** Construct empty factor graph */
     SymbolicBayesNetUnordered() {}
+
+    /** Construct from iterator over conditionals */
+    template<typename ITERATOR>
+    SymbolicBayesNetUnordered(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {}
     
     /// @}
     /// @name Standard Interface

gtsam/symbolic/tests/testSymbolicBayesTree.cpp

@@ -18,6 +18,7 @@
  */
 
 #include <gtsam/symbolic/SymbolicBayesTreeUnordered.h>
+#include <gtsam/nonlinear/Symbol.h>
 
 #include <boost/assign/list_of.hpp>
 #include <boost/assign/std/vector.hpp>
@@ -27,12 +28,13 @@ using namespace boost::assign;
 
 using namespace std;
 using namespace gtsam;
+using namespace gtsam::symbol_shorthand;
 
 static bool debug = false;
 
 /* ************************************************************************* */
 // Conditionals for ASIA example from the tutorial with A and D evidence
-static const Key _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5;
+static const Key _X_=X(0), _T_=T(0), _S_=S(0), _E_=E(0), _L_=L(0), _B_=B(0);
 static SymbolicConditionalUnordered::shared_ptr
   B(new SymbolicConditionalUnordered(_B_)),
   L(new SymbolicConditionalUnordered(_L_, _B_)),

gtsam/symbolic/tests/testSymbolicEliminationTree.cpp

@@ -20,13 +20,16 @@
 
 #include <vector>
 #include <boost/assign/std/vector.hpp>
+#include <boost/assign/list_of.hpp>
 using namespace boost::assign;
 #include <boost/make_shared.hpp>
 
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/symbolic/SymbolicEliminationTreeUnordered.h>
+#include <gtsam/nonlinear/Symbol.h>
 
 using namespace gtsam;
+using namespace gtsam::symbol_shorthand;
 using namespace std;
 
 class EliminationTreeUnorderedTester {
@@ -64,6 +67,32 @@ public:
   }
 };
 
+/* ************************************************************************* */
+namespace {
+
+  /* ************************************************************************* */
+  // Conditionals for ASIA example from the tutorial with A and D evidence
+  const Key _X_=X(0), _T_=T(0), _S_=S(0), _E_=E(0), _L_=L(0), _B_=B(0);
+
+  // Bayes Tree for Asia example
+  SymbolicFactorGraphUnordered createAsiaGraph() {
+    SymbolicFactorGraphUnordered asiaGraph;
+    asiaGraph.push_factor(_T_);
+    asiaGraph.push_factor(_S_);
+    asiaGraph.push_factor(_T_, _E_, _L_);
+    asiaGraph.push_factor(_L_, _S_);
+    asiaGraph.push_factor(_S_, _B_);
+    asiaGraph.push_factor(_E_, _B_);
+    asiaGraph.push_factor(_E_, _X_);
+    return asiaGraph;
+  }
+  
+  /* ************************************************************************* */
+  OrderingUnordered asiaOrdering = list_of(_X_)(_T_)(_S_)(_E_)(_L_)(_B_);
+
+}
+
+/* ************************************************************************* */
 TEST(EliminationTree, Create)
 {
   // create example factor graph
@@ -115,6 +144,23 @@ TEST_UNSAFE(EliminationTree, eliminate )
   EXPECT(assert_equal(expected,actual));
 }
 
+/* ************************************************************************* */
+TEST(EliminationTree, eliminateAsiaExample)
+{
+  SymbolicBayesNetUnordered expected = list_of
+    (boost::make_shared<SymbolicConditionalUnordered>(_T_, _E_, _L_))
+    (boost::make_shared<SymbolicConditionalUnordered>(_X_, _E_))
+    (boost::make_shared<SymbolicConditionalUnordered>(_E_, _B_, _L_))
+    (boost::make_shared<SymbolicConditionalUnordered>(_S_, _B_, _L_))
+    (boost::make_shared<SymbolicConditionalUnordered>(_L_, _B_))
+    (boost::make_shared<SymbolicConditionalUnordered>(_B_));
+
+  SymbolicBayesNetUnordered actual = *createAsiaGraph().eliminateSequential(
+    EliminateSymbolicUnordered, asiaOrdering);
+
+  EXPECT(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 TEST(EliminationTree, disconnected_graph) {
   SymbolicFactorGraphUnordered fg;