Symbolic eliminate now works, new compilation unit SymbolicFactor

cpp/BayesChain-inl.h

@@ -20,10 +20,10 @@ namespace gtsam {
 	/* ************************************************************************* */
 	template<class Conditional>
 	void BayesChain<Conditional>::print(const string& s) const {
-		cout << s << ":" << endl;
+		cout << s << ":\n";
 		BOOST_FOREACH(string key, keys_) {
 			const_iterator it = nodes_.find(key);
-			it->second->print("\nNode[" + key + "]");
+			it->second->print("Node[" + key + "]");
 		}
 	}
 
@@ -42,6 +42,14 @@ namespace gtsam {
 		return true;
 	}
 
+	/* ************************************************************************* */
+	template<class Conditional>
+	void BayesChain<Conditional>::insert
+		(const string& key, boost::shared_ptr<Conditional> node) {
+		keys_.push_front(key);
+		nodes_.insert(make_pair(key,node));
+	}
+
 /* ************************************************************************* */
 
 } // namespace gtsam

cpp/BayesChain.h

@@ -42,6 +42,9 @@ namespace gtsam {
 
 		/** check equality */
 		bool equals(const BayesChain& other, double tol = 1e-9) const;
+
+		/** insert: use reverse topological sort (i.e. parents last) */
+		void insert(const std::string& key, boost::shared_ptr<Conditional> node);
 	};
 
 } /// namespace gtsam

cpp/Makefile.am

@@ -57,23 +57,28 @@ testVector_LDADD = libgtsam.la
 testMatrix_LDADD = libgtsam.la
 
 # nodes
-sources += VectorConfig.cpp Ordering.cpp LinearFactor.cpp NonlinearFactor.cpp 
+sources += VectorConfig.cpp Ordering.cpp 
+sources += SymbolicFactor.cpp LinearFactor.cpp NonlinearFactor.cpp 
 sources += ConditionalGaussian.cpp LinearConstraint.cpp ConstrainedConditionalGaussian.cpp  
-check_PROGRAMS += testVectorConfig testLinearFactor testConditionalGaussian testNonlinearFactor testLinearConstraint testConstrainedConditionalGaussian
+check_PROGRAMS += testVectorConfig testSymbolicFactor testLinearFactor 
+check_PROGRAMS += testConditionalGaussian testNonlinearFactor 
+check_PROGRAMS += testLinearConstraint testConstrainedConditionalGaussian
 example = smallExample.cpp 
-testVectorConfig_SOURCES            = testVectorConfig.cpp
+testVectorConfig_SOURCES        = testVectorConfig.cpp
+testSymbolicFactor_SOURCES      = $(example) testSymbolicFactor.cpp
 testLinearFactor_SOURCES        = $(example) testLinearFactor.cpp
 testConditionalGaussian_SOURCES = $(example) testConditionalGaussian.cpp
 testNonlinearFactor_SOURCES     = $(example) testNonlinearFactor.cpp
-testLinearConstraint_SOURCES      = $(example) testLinearConstraint.cpp
+testLinearConstraint_SOURCES    = $(example) testLinearConstraint.cpp
-testConstrainedConditionalGaussian_SOURCES       = testConstrainedConditionalGaussian.cpp
+testConstrainedConditionalGaussian_SOURCES = testConstrainedConditionalGaussian.cpp
 
-testVectorConfig_LDADD            = libgtsam.la
+testVectorConfig_LDADD        = libgtsam.la
+testSymbolicFactor_LDADD      = libgtsam.la
 testLinearFactor_LDADD        = libgtsam.la
 testConditionalGaussian_LDADD = libgtsam.la
 testNonlinearFactor_LDADD     = libgtsam.la
 testLinearConstraint_LDADD	  = libgtsam.la
-testConstrainedConditionalGaussian_LDADD       = libgtsam.la
+testConstrainedConditionalGaussian_LDADD = libgtsam.la
 
 # not the correct way, I'm sure: Kai ?
 timeLinearFactor: timeLinearFactor.cpp  

cpp/SymbolicBayesChain.h

@@ -27,6 +27,14 @@ namespace gtsam {
 	class SymbolicBayesChain: public BayesChain<SymbolicConditional> {
 	public:
 
+		/** convenience typename for a shared pointer to this class */
+		typedef boost::shared_ptr<SymbolicBayesChain> shared_ptr;
+
+		/**
+		 * Empty constructor
+		 */
+		SymbolicBayesChain() {}
+
 		/**
 		 *  Construct from a map of nodes
 		 */

cpp/SymbolicConditional.h

@@ -8,8 +8,11 @@
 
 #pragma once
 
-#include "Testable.h"
+#include <list>
+#include <string>
+#include <boost/shared_ptr.hpp>
 #include <boost/foreach.hpp> // TODO: make cpp file
+#include "Testable.h"
 
 namespace gtsam {
 
@@ -23,6 +26,7 @@ namespace gtsam {
 
 	public:
 
+		/** convenience typename for a shared pointer to this class */
 		typedef boost::shared_ptr<SymbolicConditional> shared_ptr;
 
 		/**
@@ -54,7 +58,7 @@ namespace gtsam {
 
 		/** print */
 		void print(const std::string& s = "SymbolicConditional") const {
-			std::cout << s << std::endl;
+			std::cout << s;
 			BOOST_FOREACH(std::string parent, parents_) std::cout << " " << parent;
 			std::cout << std::endl;
 		}

cpp/SymbolicFactor.cpp

@@ -0,0 +1,69 @@
+/*
+ * SymbolicFactor.cpp
+ *
+ *  Created on: Oct 29, 2009
+ *      Author: Frank Dellaert
+ */
+
+#include <map>
+#include <iostream>
+#include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
+#include "SymbolicConditional.h"
+#include "SymbolicFactor.h"
+
+using namespace std;
+
+// trick from some reading group
+#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
+
+namespace gtsam {
+
+	/* ************************************************************************* */
+	SymbolicFactor::SymbolicFactor(const vector<shared_ptr> & factors) {
+
+		// store keys in a map to make them unique (set is not portable)
+		map<string, string> map;
+		BOOST_FOREACH(shared_ptr factor, factors)
+			BOOST_FOREACH(string key, factor->keys())
+				map.insert(make_pair(key,key));
+
+		// create the unique keys
+		string key,val;
+		FOREACH_PAIR(key, val, map)
+			keys_.push_back(key);
+	}
+
+	/* ************************************************************************* */
+	void SymbolicFactor::print(const string& s) const {
+		cout << s << " ";
+		BOOST_FOREACH(string key, keys_) cout << " " << key;
+		cout << endl;
+	}
+
+	/* ************************************************************************* */
+	bool SymbolicFactor::equals(const SymbolicFactor& other, double tol) const {
+		return keys_ == other.keys_;
+	}
+
+	/* ************************************************************************* */
+	pair<SymbolicConditional::shared_ptr, SymbolicFactor::shared_ptr>
+	SymbolicFactor::eliminate(const string& key) const
+	{
+		// get keys from input factor
+		list<string> separator;
+		BOOST_FOREACH(string j,keys_)
+			if (j!=key) separator.push_back(j);
+
+		// start empty remaining factor to be returned
+	  boost::shared_ptr<SymbolicFactor> lf(new SymbolicFactor(separator));
+
+	  // create SymbolicConditional on separator
+	  SymbolicConditional::shared_ptr cg (new SymbolicConditional(separator));
+
+	  return make_pair(cg,lf);
+	}
+
+	/* ************************************************************************* */
+
+}

cpp/SymbolicFactor.h

@@ -0,0 +1,78 @@
+/*
+ * SymbolicFactor.h
+ *
+ *  Created on: Oct 29, 2009
+ *      Author: Frank Dellaert
+ */
+
+#ifndef SYMBOLICFACTOR_H_
+#define SYMBOLICFACTOR_H_
+
+#include <list>
+#include <string>
+#include <vector>
+#include <boost/shared_ptr.hpp>
+#include "Testable.h"
+
+namespace gtsam {
+
+	class SymbolicConditional;
+
+	/** Symbolic Factor */
+	class SymbolicFactor: public Testable<SymbolicFactor> {
+	private:
+
+		std::list<std::string> keys_;
+
+	public:
+
+		typedef boost::shared_ptr<SymbolicFactor> shared_ptr;
+
+		/**
+		 * Constructor from a list of keys
+		 */
+		SymbolicFactor(std::list<std::string> keys) :
+			keys_(keys) {
+		}
+
+		/**
+		 * Constructor that combines a set of factors
+		 * @param factors Set of factors to combine
+		 */
+		SymbolicFactor(const std::vector<shared_ptr> & factors);
+
+		/** print */
+		void print(const std::string& s = "SymbolicFactor") const;
+
+		/** check equality */
+		bool equals(const SymbolicFactor& other, double tol = 1e-9) const;
+
+		/**
+		 * Find all variables
+		 * @return The set of all variable keys
+		 */
+		std::list<std::string> keys() const {
+			return keys_;
+		}
+
+		/**
+		 * eliminate one of the variables connected to this factor
+		 * @param key the key of the node to be eliminated
+		 * @return a new factor and a symbolic conditional on the eliminated variable
+		 */
+		std::pair<boost::shared_ptr<SymbolicConditional>, shared_ptr>
+		eliminate(const std::string& key) const;
+
+		/**
+		 * Check if empty factor
+		 */
+		inline bool empty() const {
+			return keys_.empty();
+		}
+
+
+	};
+
+}
+
+#endif /* SYMBOLICFACTOR_H_ */

cpp/SymbolicFactorGraph.cpp

@@ -6,59 +6,27 @@
  */
 
 #include <boost/foreach.hpp>
-#include <boost/tuple/tuple.hpp>
+#include "Ordering.h"
 #include "SymbolicFactorGraph.h"
+#include "BayesChain-inl.h"
+#include "SymbolicBayesChain.h"
 
 using namespace std;
 
-// trick from some reading group
-#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
-
 namespace gtsam {
 
 	/* ************************************************************************* */
-	SymbolicFactor::SymbolicFactor(const vector<shared_ptr> & factors) {
+	SymbolicBayesChain::shared_ptr
-
+	SymbolicFactorGraph::eliminate(const Ordering& ordering)
-		// store keys in a map to make them unique (set is not portable)
-		map<string, string> map;
-		BOOST_FOREACH(shared_ptr factor, factors)
-			BOOST_FOREACH(string key, factor->keys())
-				map.insert(make_pair(key,key));
-
-		// create the unique keys
-		string key,val;
-		FOREACH_PAIR(key, val, map)
-			keys_.push_back(key);
-	}
-
-	/* ************************************************************************* */
-	void SymbolicFactor::print(const string& s) const {
-		cout << s << " ";
-		BOOST_FOREACH(string key, keys_) cout << " " << key;
-		cout << endl;
-	}
-
-	/* ************************************************************************* */
-	bool SymbolicFactor::equals(const SymbolicFactor& other, double tol) const {
-		return keys_ == other.keys_;
-	}
-
-	/* ************************************************************************* */
-	pair<SymbolicConditional::shared_ptr, SymbolicFactor::shared_ptr>
-	SymbolicFactor::eliminate(const string& key) const
 	{
-		// get keys from input factor
+		SymbolicBayesChain::shared_ptr bayesChain (new SymbolicBayesChain());
-		list<string> separator;
-		BOOST_FOREACH(string j,keys_)
-			if (j!=key) separator.push_back(j);
 
-		// start empty remaining factor to be returned
+		BOOST_FOREACH(string key, ordering) {
-	  boost::shared_ptr<SymbolicFactor> lf(new SymbolicFactor(separator));
+			SymbolicConditional::shared_ptr conditional = eliminateOne<SymbolicConditional>(key);
+			bayesChain->insert(key,conditional);
+		}
 
-	  // create SymbolicConditional on separator
+		return bayesChain;
-	  SymbolicConditional::shared_ptr cg (new SymbolicConditional(separator));
-
-	  return make_pair(cg,lf);
 	}
 
 	/* ************************************************************************* */

cpp/SymbolicFactorGraph.h

@@ -11,72 +11,25 @@
 #include <string>
 #include <list>
 #include "FactorGraph.h"
-#include "SymbolicConditional.h"
+#include "SymbolicFactor.h"
 
 namespace gtsam {
 
-	/** Symbolic Factor */
+	class SymbolicBayesChain;
-	class SymbolicFactor: public Testable<SymbolicFactor> {
-	private:
-
-		std::list<std::string> keys_;
-
-	public:
-
-		typedef boost::shared_ptr<SymbolicFactor> shared_ptr;
-
-		/**
-		 * Constructor from a list of keys
-		 */
-		SymbolicFactor(std::list<std::string> keys) :
-			keys_(keys) {
-		}
-
-		/**
-		 * Constructor that combines a set of factors
-		 * @param factors Set of factors to combine
-		 */
-		SymbolicFactor(const std::vector<shared_ptr> & factors);
-
-		/** print */
-		void print(const std::string& s = "SymbolicFactor") const;
-
-		/** check equality */
-		bool equals(const SymbolicFactor& other, double tol = 1e-9) const;
-
-		/**
-		 * Find all variables
-		 * @return The set of all variable keys
-		 */
-		std::list<std::string> keys() const {
-			return keys_;
-		}
-
-		/**
-		 * eliminate one of the variables connected to this factor
-		 * @param key the key of the node to be eliminated
-		 * @return a new factor and a symbolic conditional on the eliminated variable
-		 */
-		std::pair<SymbolicConditional::shared_ptr, SymbolicFactor::shared_ptr>
-		eliminate(const std::string& key) const;
-
-		/**
-		 * Check if empty factor
-		 */
-		inline bool empty() const {
-			return keys_.empty();
-		}
-
-
-	};
 
 	/** Symbolic Factor Graph */
 	class SymbolicFactorGraph: public FactorGraph<SymbolicFactor> {
 	public:
 
+		/**
+		 * Construct empty factor graph
+		 */
 		SymbolicFactorGraph() {
 		}
 
+		/**
+		 * Construct from a factor graph of any type
+		 */
 		template<class Factor>
 		SymbolicFactorGraph(const FactorGraph<Factor>& fg) {
 			for (size_t i = 0; i < fg.size(); i++) {
@@ -87,6 +40,12 @@ namespace gtsam {
 			}
 		}
 
+		/**
+		 * eliminate factor graph in place(!) in the given order, yielding
+		 * a chordal Bayes net
+		 */
+		boost::shared_ptr<SymbolicBayesChain> eliminate(const Ordering& ordering);
+
 	};
 
 }

cpp/testSymbolicFactor.cpp

@@ -0,0 +1,19 @@
+/**
+ * @file    testSymbolicFactor.cpp
+ * @brief   Unit tests for a symbolic Factor
+ * @author  Frank Dellaert
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include "SymbolicFactor.h"
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+int main() {
+	TestResult tr;
+	return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

cpp/testSymbolicFactorGraph.cpp

@@ -9,6 +9,9 @@
 #include "smallExample.h"
 #include "FactorGraph-inl.h"
 #include "SymbolicFactorGraph.h"
+#include "BayesChain-inl.h"
+#include "SymbolicConditional.h"
+#include "SymbolicBayesChain.h"
 
 using namespace std;
 using namespace gtsam;
@@ -100,7 +103,7 @@ TEST( LinearFactorGraph, removeAndCombineFactors )
 }
 
 /* ************************************************************************* */
-TEST( LinearFactorGraph, eliminateOne_x1 )
+TEST( LinearFactorGraph, eliminateOne )
 {
 	// create a test graph
 	LinearFactorGraph factorGraph = createLinearFactorGraph();
@@ -116,6 +119,33 @@ TEST( LinearFactorGraph, eliminateOne_x1 )
   CHECK(assert_equal(expected,*actual));
 }
 
+/* ************************************************************************* */
+TEST( LinearFactorGraph, eliminate )
+{
+  // create expected Chordal bayes Net
+  SymbolicConditional::shared_ptr c1(new SymbolicConditional());
+  SymbolicConditional::shared_ptr c2(new SymbolicConditional("x1"));
+  SymbolicConditional::shared_ptr c3(new SymbolicConditional("l1", "x1"));
+
+  SymbolicBayesChain expected;
+  expected.insert("x1", c1);
+  expected.insert("l1", c2);
+  expected.insert("x2", c3);
+
+  // create a test graph
+	LinearFactorGraph factorGraph = createLinearFactorGraph();
+	SymbolicFactorGraph fg(factorGraph);
+
+	// eliminate it
+	Ordering ordering;
+  ordering.push_back("x2");
+  ordering.push_back("l1");
+  ordering.push_back("x1");
+  SymbolicBayesChain::shared_ptr actual = fg.eliminate(ordering);
+
+  CHECK(assert_equal(expected,*actual));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;