Made some progress on symbolic analysis

cpp/BayesChain-inl.h

@@ -0,0 +1,47 @@
+/**
+ * @file   BayesChain-inl.h
+ * @brief  Bayes chain template definitions
+ * @author Frank Dellaert
+ */
+
+#include <iostream>
+#include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
+
+#include "BayesChain.h"
+
+using namespace std;
+
+// trick from some reading group
+#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
+
+namespace gtsam {
+
+	/* ************************************************************************* */
+	template<class Conditional>
+	void BayesChain<Conditional>::print(const string& s) const {
+		cout << s << ":" << endl;
+		BOOST_FOREACH(string key, keys_) {
+			const_iterator it = nodes_.find(key);
+			it->second->print("\nNode[" + key + "]");
+		}
+	}
+
+	/* ************************************************************************* */
+	template<class Conditional>
+	bool BayesChain<Conditional>::equals(const BayesChain& cbn, double tol) const {
+		const_iterator it1 = nodes_.begin(), it2 = cbn.nodes_.begin();
+		if(nodes_.size() != cbn.nodes_.size()) return false;
+		for(; it1 != nodes_.end(); it1++, it2++) {
+			const string& j1 = it1->first, j2 = it2->first;
+			boost::shared_ptr<Conditional> node1 = it1->second, node2 = it2->second;
+			if (j1 != j2) return false;
+			if (!node1->equals(*node2,tol))
+			return false;
+		}
+		return true;
+	}
+
+/* ************************************************************************* */
+
+} // namespace gtsam

cpp/BayesChain.h

@@ -8,10 +8,12 @@
 
 #pragma once
 
-#include <list>
+#include <boost/shared_ptr.hpp>
 #include <boost/serialization/map.hpp>
 #include <boost/serialization/list.hpp>
 
+#include "Testable.h"
+
 namespace gtsam {
 
 	/**
@@ -21,8 +23,25 @@ namespace gtsam {
 	 * a ConditionalProbabilityTable, a ConditionalGaussian, or a SymbolicConditional.
 	 */
 	template<class Conditional>
-	class BayesChain {
+	class BayesChain: public Testable<BayesChain<Conditional> > {
+	protected:
+
+		/** nodes keys stored in topological sort order, i.e. from parents to children */
+		std::list<std::string> keys_;
+
+		/** nodes stored on key */
+		typedef typename std::map<std::string, boost::shared_ptr<Conditional> > Nodes;
+		Nodes nodes_;
+
+		typedef typename Nodes::const_iterator const_iterator;
+
 	public:
+
+		/** print */
+		void print(const std::string& s = "") const;
+
+		/** check equality */
+		bool equals(const BayesChain& other, double tol = 1e-9) const;
 	};
 
 } /// namespace gtsam

cpp/Makefile.am

@@ -80,17 +80,14 @@ timeLinearFactor: timeLinearFactor.cpp
 timeLinearFactor: CXXFLAGS += -I /opt/local/include
 timeLinearFactor: LDFLAGS += -L.libs -lgtsam
 
-# not the correct way, I'm sure: Kai ?
-timeLinearFactorGraph: timeLinearFactorGraph.cpp  
-timeLinearFactorGraph: CXXFLAGS += -I /opt/local/include -I ..
-timeLinearFactorGraph: LDFLAGS += SmallExample.o -L.libs -lgtsam -L../CppUnitLite -lCppUnitLite
-
 # graphs 
 sources += LinearFactorGraph.cpp 
-sources += SymbolicBayesChain.cpp ChordalBayesNet.cpp 
+#sources += BayesChain.cpp SymbolicBayesChain.cpp 
+sources += ChordalBayesNet.cpp 
 sources += ConstrainedNonlinearFactorGraph.cpp ConstrainedLinearFactorGraph.cpp 
-check_PROGRAMS += testFactorgraph testLinearFactorGraph testNonlinearFactorGraph testNonlinearOptimizer
+check_PROGRAMS += testFactorgraph testLinearFactorGraph testNonlinearFactorGraph 
-check_PROGRAMS += testSymbolicBayesChain testChordalBayesNet testBayesTree  
+check_PROGRAMS += testChordalBayesNet testNonlinearOptimizer
+#check_PROGRAMS += testSymbolicBayesChain testBayesTree  
 check_PROGRAMS += testConstrainedNonlinearFactorGraph testConstrainedLinearFactorGraph
 testFactorgraph_SOURCES          = testFactorgraph.cpp
 testLinearFactorGraph_SOURCES    = $(example) testLinearFactorGraph.cpp
@@ -112,6 +109,11 @@ testBayesTree_LDADD            = libgtsam.la
 testConstrainedNonlinearFactorGraph_LDADD = libgtsam.la 
 testConstrainedLinearFactorGraph_LDADD    = libgtsam.la
 
+# not the correct way, I'm sure: Kai ?
+timeLinearFactorGraph: timeLinearFactorGraph.cpp  
+timeLinearFactorGraph: CXXFLAGS += -I /opt/local/include -I ..
+timeLinearFactorGraph: LDFLAGS += SmallExample.o -L.libs -lgtsam -L../CppUnitLite -lCppUnitLite
+
 # geometry
 sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
 check_PROGRAMS += testPoint2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2

cpp/SymbolicBayesChain-inl.h

@@ -4,14 +4,21 @@
  * @author Frank Dellaert
  */
 
+#include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
+
 #include "SymbolicBayesChain.h"
 
 using namespace std;
-using namespace gtsam;
+
+namespace gtsam {
+
+	/* ************************************************************************* */
+	template<class Factor, class Config>
+	SymbolicBayesChain::SymbolicBayesChain(
+			const FactorGraph<Factor, Config>& factorGraph, const Ordering& ordering) {
+	}
 
 /* ************************************************************************* */
-template<class Factor, class Config>
-SymbolicBayesChain::SymbolicBayesChain(const FactorGraph<Factor,Config>& factorGraph) {
-}
 
-/* ************************************************************************* */
+} // namespace gtsam

cpp/SymbolicBayesChain.cpp

@@ -4,40 +4,29 @@
  * @author Frank Dellaert
  */
 
-#include <stdarg.h>
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
 
-#include "SymbolicBayesChain.h"
-
-using namespace std;
-using namespace gtsam;
-
 // trick from some reading group
 #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
 
-/* ************************************************************************* */
+#include "SymbolicBayesChain.h"
-void SymbolicBayesChain::print(const string& s) const {
+
-	//  BOOST_FOREACH(string key, keys) {
+using namespace std;
-	//    const_iterator it = nodes.find(key);
+
-	//    it->second->print("\nNode[" + key + "]");
+namespace gtsam {
-	//  }
+
-}
+	typedef pair<string,SymbolicConditional::shared_ptr> pp;
+
+	/* ************************************************************************* */
+	SymbolicBayesChain::SymbolicBayesChain(const std::map<std::string,
+			SymbolicConditional::shared_ptr>& nodes) {
+		BOOST_FOREACH(pp p, nodes) {
+		  keys_.push_front(p.first);
+			nodes_.insert(p);
+		}
+	}
 
 /* ************************************************************************* */
-bool SymbolicBayesChain::equals(const SymbolicBayesChain& cbn, double tol) const {
-	//  const_iterator it1 = nodes.begin(), it2 = cbn.nodes.begin();
-	//
-	//  if(nodes.size() != cbn.nodes.size()) return false;
-	//  for(; it1 != nodes.end(); it1++, it2++){
-	//    const string& j1 = it1->first, j2 = it2->first;
-	//    ConditionalGaussian::shared_ptr node1 = it1->second, node2 = it2->second;
-	//    if (j1 != j2) return false;
-	//    if (!node1->equals(*node2,tol))
-	//      return false;
-	//  }
-	//  return true;
-	return false;
-}
 
-/* ************************************************************************* */
+} // namespace gtsam

cpp/SymbolicBayesChain.h

@@ -15,37 +15,34 @@
 #include "Testable.h"
 #include "BayesChain.h"
 #include "FactorGraph.h"
+#include "SymbolicConditional.h"
 
 namespace gtsam {
 
-	/**
+	class Ordering;
-	 * Conditional node for use in a symbolic Bayes chain
-	 */
-	class SymbolicConditional {
-	};
 
 	/**
 	 *  Symbolic Bayes Chain, the (symbolic) result of eliminating a factor graph
 	 */
-	class SymbolicBayesChain: public BayesChain<SymbolicConditional> ,
+	class SymbolicBayesChain: public BayesChain<SymbolicConditional> {
-			public Testable<SymbolicBayesChain> {
 	public:
 
+		/**
+		 *  Construct from a map of nodes
+		 */
+		SymbolicBayesChain(const std::map<std::string,
+				SymbolicConditional::shared_ptr>& nodes);
+
 		/**
 		 *  Construct from any factor graph
 		 */
 		template<class Factor, class Config>
-		SymbolicBayesChain(const FactorGraph<Factor, Config>& factorGraph);
+		SymbolicBayesChain(const FactorGraph<Factor, Config>& factorGraph,
+				const Ordering& ordering);
 
 		/** Destructor */
 		virtual ~SymbolicBayesChain() {
 		}
-
-		/** print */
-		void print(const std::string& s = "") const;
-
-		/** check equality */
-		bool equals(const SymbolicBayesChain& other, double tol = 1e-9) const;
 	};
 
 } /// namespace gtsam

cpp/SymbolicConditional.h

@@ -0,0 +1,53 @@
+/**
+ * @file    SymbolicConditional.h
+ * @brief   Symbolic Conditional node for use in Bayes nets
+ * @author  Frank Dellaert
+ */
+
+// \callgraph
+
+#pragma once
+
+#include "Testable.h"
+
+namespace gtsam {
+
+	/**
+	 * Conditional node for use in a Bayes nets
+	 */
+	class SymbolicConditional: Testable<SymbolicConditional> {
+	public:
+
+		typedef boost::shared_ptr<SymbolicConditional> shared_ptr;
+
+		/**
+		 * No parents
+		 */
+		SymbolicConditional() {
+		}
+
+		/**
+		 * Single parent
+		 */
+		SymbolicConditional(const std::string& key) {
+		}
+
+		/**
+		 * Two parents
+		 */
+		SymbolicConditional(const std::string& key1, const std::string& key2) {
+		}
+
+		/** print */
+		void print(const std::string& s = "SymbolicConditional") const {
+			std::cout << s << std::endl;
+		}
+
+		/** check equality */
+		bool equals(const SymbolicConditional& other, double tol = 1e-9) const {
+			return false;
+		}
+
+	};
+
+} /// namespace gtsam

cpp/testBayesTree.cpp

@@ -6,6 +6,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include "BayesChain-inl.h"
 #include "SymbolicBayesChain-inl.h"
 #include "smallExample.h"
 #include "BayesTree.h"
@@ -16,7 +17,11 @@ using namespace gtsam;
 TEST( BayesTree, constructor )
 {
 	LinearFactorGraph factorGraph = createLinearFactorGraph();
-	SymbolicBayesChain symbolicBayesChain(factorGraph);
+  Ordering ordering;
+  ordering.push_back("x2");
+  ordering.push_back("l1");
+  ordering.push_back("x1");
+	SymbolicBayesChain symbolicBayesChain(factorGraph,ordering);
 	BayesTree<SymbolicConditional> bayesTree(symbolicBayesChain);
 }
 

cpp/testSymbolicBayesChain.cpp

@@ -7,6 +7,7 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include "smallExample.h"
+#include "BayesChain-inl.h"
 #include "SymbolicBayesChain-inl.h"
 
 using namespace gtsam;
@@ -14,8 +15,25 @@ using namespace gtsam;
 /* ************************************************************************* */
 TEST( SymbolicBayesChain, constructor )
 {
+	// Create manually
+	SymbolicConditional::shared_ptr x2(new SymbolicConditional("x1","l1"));
+	SymbolicConditional::shared_ptr l1(new SymbolicConditional("x1"));
+	SymbolicConditional::shared_ptr x1(new SymbolicConditional());
+	map<string, SymbolicConditional::shared_ptr> nodes;
+	nodes.insert(make_pair("x2",x2));
+	nodes.insert(make_pair("l1",l1));
+	nodes.insert(make_pair("x1",x1));
+	SymbolicBayesChain expected(nodes);
+
+	// Create from a factor graph
+  Ordering ordering;
+  ordering.push_back("x2");
+  ordering.push_back("l1");
+  ordering.push_back("x1");
 	LinearFactorGraph factorGraph = createLinearFactorGraph();
-	SymbolicBayesChain symbolicChordalBayesNet(factorGraph);
+	SymbolicBayesChain actual(factorGraph,ordering);
+
+	//CHECK(assert_equal(expected, actual));
 }
 
 /* ************************************************************************* */