Small change necessitating lots of edits: Conditionals now include key of random variable

This simplifies Bayes nets quite a bit. Also created a Conditional base class, derived classes ConditionalGaussian and SymbolicConditional
Finally, some changes were needed because I moved some headers to .cpp

.cproject

@@ -300,7 +300,6 @@
 <buildTargets>
 <target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>install</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -308,7 +307,6 @@
 </target>
 <target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -316,6 +314,7 @@
 </target>
 <target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -323,7 +322,6 @@
 </target>
 <target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testSimpleCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -339,6 +337,7 @@
 </target>
 <target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testVSLAMFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -346,7 +345,6 @@
 </target>
 <target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testCalibratedCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -354,6 +352,7 @@
 </target>
 <target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testConditionalGaussian.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -361,7 +360,6 @@
 </target>
 <target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testPose2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -377,7 +375,6 @@
 </target>
 <target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testRot3.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -385,6 +382,7 @@
 </target>
 <target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testNonlinearOptimizer.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -392,7 +390,6 @@
 </target>
 <target name="testLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testLinearFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -400,7 +397,6 @@
 </target>
 <target name="testConstrainedNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testConstrainedNonlinearFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -408,7 +404,6 @@
 </target>
 <target name="testLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testLinearFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -416,6 +411,7 @@
 </target>
 <target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testNonlinearFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -423,7 +419,6 @@
 </target>
 <target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testPose3.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -431,6 +426,7 @@
 </target>
 <target name="testConstrainedLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testConstrainedLinearFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -438,6 +434,7 @@
 </target>
 <target name="testVectorConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testVectorConfig.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -445,6 +442,7 @@
 </target>
 <target name="testPoint2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testPoint2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -452,7 +450,6 @@
 </target>
 <target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testNonlinearFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -460,7 +457,6 @@
 </target>
 <target name="timeLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>timeLinearFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -468,22 +464,21 @@
 </target>
 <target name="timeLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>timeLinearFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
-<target name="testChordalBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
+<buildTarget>testGaussianBayesNet.run</buildTarget>
-<buildTarget>testChordalBayesNet.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
 <target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testBayesTree.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -491,6 +486,7 @@
 </target>
 <target name="testSymbolicBayesChain.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testSymbolicBayesChain.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -498,6 +494,7 @@
 </target>
 <target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testSymbolicFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -505,7 +502,6 @@
 </target>
 <target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testVector.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -513,7 +509,6 @@
 </target>
 <target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testMatrix.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -521,6 +516,7 @@
 </target>
 <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>install</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -528,6 +524,7 @@
 </target>
 <target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>clean</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -535,6 +532,7 @@
 </target>
 <target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>

cpp/BayesNet-inl.h

@@ -8,49 +8,42 @@
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
 
+#include <boost/assign/std/vector.hpp> // for +=
+using namespace boost::assign;
+
+#include "Ordering.h"
 #include "BayesNet.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 BayesNet<Conditional>::print(const string& s) const {
 		cout << s << ":\n";
-		BOOST_FOREACH(string key, keys_) {
+		std::string key;
-			const_iterator it = nodes_.find(key);
+		BOOST_FOREACH(conditional_ptr conditional,conditionals_)
-			it->second->print("Node[" + key + "]");
+			conditional->print("Node[" + conditional->key() + "]");
-		}
 	}
 
 	/* ************************************************************************* */
 	template<class Conditional>
 	bool BayesNet<Conditional>::equals(const BayesNet& cbn, double tol) const {
+		if(indices_ != cbn.indices_) return false;
 		if(size() != cbn.size()) return false;
-		if(keys_ != cbn.keys_) return false;
+		return equal(conditionals_.begin(),conditionals_.begin(),conditionals_.begin(),equals_star<Conditional>);
-		string key;
-		boost::shared_ptr<Conditional> node;
-		FOREACH_PAIR( key, node, nodes_) {
-			const_iterator cg = cbn.nodes_.find(key);
-			if (cg == nodes_.end()) return false;
-			if (!equals_star(node,cg->second,tol)) return false;
-		}
-		return true;
 	}
 
 	/* ************************************************************************* */
 	template<class Conditional>
 	void BayesNet<Conditional>::insert
-		(const string& key, boost::shared_ptr<Conditional> node) {
+		(const boost::shared_ptr<Conditional>& conditional) {
-		keys_.push_front(key);
+		indices_.insert(make_pair(conditional->key(),conditionals_.size()));
-		nodes_.insert(make_pair(key,node));
+		conditionals_.push_back(conditional);
 	}
 
-	/* ************************************************************************* */
+	/* ************************************************************************* *
 	template<class Conditional>
 	void BayesNet<Conditional>::erase(const string& key) {
 		list<string>::iterator it;
@@ -59,7 +52,16 @@ namespace gtsam {
 				break;
 		}
 		keys_.erase(it);
-		nodes_.erase(key);
+		conditionals_.erase(key);
+	}
+
+	/* ************************************************************************* */
+	template<class Conditional>
+	Ordering BayesNet<Conditional>::ordering() const {
+		Ordering ord;
+		BOOST_FOREACH(conditional_ptr conditional,conditionals_)
+		   ord.push_back(conditional->key());
+		return ord;
 	}
 
 	/* ************************************************************************* */

cpp/BayesNet.h

@@ -8,14 +8,18 @@
 
 #pragma once
 
+#include <vector>
 #include <boost/shared_ptr.hpp>
 #include <boost/serialization/map.hpp>
-#include <boost/serialization/list.hpp>
+#include <boost/serialization/vector.hpp>
+#include <boost/serialization/shared_ptr.hpp>
 
 #include "Testable.h"
 
 namespace gtsam {
 
+	class Ordering;
+
 	/**
 	 * Bayes network
 	 * This is the base class for SymbolicBayesNet, DiscreteBayesNet, and GaussianBayesNet
@@ -24,14 +28,36 @@ namespace gtsam {
 	 */
 	template<class Conditional>
 	class BayesNet: public Testable<BayesNet<Conditional> > {
+
+	public:
+
+		/** We store shared pointers to Conditional densities */
+		typedef typename boost::shared_ptr<Conditional> conditional_ptr;
+		typedef typename std::vector<conditional_ptr> Conditionals;
+		typedef typename Conditionals::const_iterator const_iterator;
+		typedef typename Conditionals::const_reverse_iterator const_reverse_iterator;
+
 	protected:
 
-		/** nodes keys stored in topological sort order, i.e. from parents to children */
+		/**
-		std::list<std::string> keys_;
+		 *  Conditional densities are stored in reverse topological sort order (i.e., leaves first,
+		 *  parents last), which corresponds to the elimination ordering if so obtained,
+		 *  and is consistent with the column (block) ordering of an upper triangular matrix.
+		 */
+		Conditionals conditionals_;
 
-		/** nodes stored on key */
+		/**
-		typedef typename std::map<std::string, boost::shared_ptr<Conditional> > Nodes;
+		 *   O(log n) random access on keys will provided by a map from keys to vector index.
-		Nodes nodes_;
+		 */
+		typedef std::map<std::string, int> Indices;
+		Indices indices_;
+
+		/** O(log n) lookup from key to node index */
+		inline int index(const std::string& key) const {
+			Indices::const_iterator it = indices_.find(key); // get node index
+			assert( it != indices_.end() );
+			return it->second;
+		}
 
 	public:
 
@@ -41,37 +67,36 @@ namespace gtsam {
 		/** check equality */
 		bool equals(const BayesNet& other, double tol = 1e-9) const;
 
-		/** insert: use reverse topological sort (i.e. parents last) */
+		/** insert: use reverse topological sort (i.e. parents last / elimination order) */
-		void insert(const std::string& key, boost::shared_ptr<Conditional> node);
+		void insert(const boost::shared_ptr<Conditional>& conditional);
-
-		/** delete */
-		void erase(const std::string& key);
 
 		/** size is the number of nodes */
-		inline size_t size() const {return nodes_.size();}
+		inline size_t size() const {
-
+			return conditionals_.size();
-		/** return keys in topological sort order (parents first), i.e., reverse elimination order */
-		inline std::list<std::string> keys() const { return keys_;}
-
-		inline boost::shared_ptr<Conditional> operator[](const std::string& key) const {
-			const_iterator cg = nodes_.find(key); // get node
-			assert( cg != nodes_.end() );
-			return cg->second;
 		}
 
-		/** return begin and end of the nodes. FD: breaks encapsulation? */
+		/** return keys in reverse topological sort order, i.e., elimination order */
-		typedef typename Nodes::const_iterator const_iterator;
+		Ordering ordering() const;
-		const_iterator const begin() const {return nodes_.begin();}
+
-		const_iterator const end()   const {return nodes_.end();}
+		/** O(log n) random access to Conditional by key */
+		inline conditional_ptr operator[](const std::string& key) const {
+			int i = index(key);
+			return conditionals_[i];
+		}
+
+		/** return iterators. FD: breaks encapsulation? */
+		const_iterator const begin() const {return conditionals_.begin();}
+		const_iterator const end()   const {return conditionals_.end();}
+		const_reverse_iterator const rbegin() const {return conditionals_.rbegin();}
+		const_reverse_iterator const rend()   const {return conditionals_.rend();}
 
 	private:
 		/** Serialization function */
 		friend class boost::serialization::access;
 		template<class Archive>
-		void serialize(Archive & ar, const unsigned int version)
+		void serialize(Archive & ar, const unsigned int version) {
-		{
+			ar & BOOST_SERIALIZATION_NVP(conditionals_);
-			ar & BOOST_SERIALIZATION_NVP(keys_);
+			ar & BOOST_SERIALIZATION_NVP(indices_);
-			ar & BOOST_SERIALIZATION_NVP(nodes_);
 		}
 	};
 

cpp/BayesTree-inl.h

@@ -13,8 +13,8 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	template<class Conditional>
-	Front<Conditional>::Front(string key, cond_ptr conditional) {
+	Front<Conditional>::Front(const conditional_ptr& conditional) {
-		add(key, conditional);
+		add(conditional);
 		separator_ = conditional->parents();
 	}
 
@@ -22,7 +22,8 @@ namespace gtsam {
 	template<class Conditional>
 	void Front<Conditional>::print(const string& s) const {
 		cout << s;
-		BOOST_FOREACH(string key, keys_) cout << " " << key;
+		BOOST_FOREACH(const conditional_ptr& conditional, conditionals_)
+			cout << " " << conditional->key();
 		if (!separator_.empty()) {
 			cout << " :";
 			BOOST_FOREACH(string key, separator_)
@@ -34,14 +35,12 @@ namespace gtsam {
 	/* ************************************************************************* */
 	template<class Conditional>
 	bool Front<Conditional>::equals(const Front<Conditional>& other, double tol) const {
-		return (keys_ == other.keys_) &&
+		return equal(conditionals_.begin(),conditionals_.end(),other.conditionals_.begin(),equals_star<Conditional>);
-		equal(conditionals_.begin(),conditionals_.end(),other.conditionals_.begin(),equals_star<Conditional>);
 	}
 
 	/* ************************************************************************* */
 	template<class Conditional>
-	void Front<Conditional>::add(string key, cond_ptr conditional) {
+	void Front<Conditional>::add(const conditional_ptr& conditional) {
-		keys_.push_front(key);
 		conditionals_.push_front(conditional);
 	}
 
@@ -51,12 +50,13 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	// TODO: traversal is O(n*log(n)) but could be O(n) with better bayesChain
+	// TODO: traversal is O(n*log(n)) but could be O(n) with better bayesNet
 	template<class Conditional>
-	BayesTree<Conditional>::BayesTree(BayesNet<Conditional>& bayesChain, bool verbose) {
+	BayesTree<Conditional>::BayesTree(const BayesNet<Conditional>& bayesNet, bool verbose) {
-		list<string> reverseOrdering = bayesChain.keys();
+		typename BayesNet<Conditional>::const_reverse_iterator rit;
-		BOOST_FOREACH(string key, reverseOrdering)
+		for ( rit=bayesNet.rbegin(); rit < bayesNet.rend(); ++rit ) {
-			insert(key,bayesChain[key],verbose);
+			insert(*rit,verbose);
+		}
 	}
 
 	/* ************************************************************************* */
@@ -78,8 +78,9 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	template<class Conditional>
-	void BayesTree<Conditional>::insert(string key, conditional_ptr conditional, bool verbose) {
+	void BayesTree<Conditional>::insert(const boost::shared_ptr<Conditional>& conditional, bool verbose) {
 
+		string key =  conditional->key();
 		if (verbose) cout << "Inserting " << key << "| ";
 
 		// get parents
@@ -90,7 +91,7 @@ namespace gtsam {
 		// if no parents, start a new root clique
 		if (parents.empty()) {
 			if (verbose) cout << "Creating root clique" << endl;
-			node_ptr root(new Node(key, conditional));
+			node_ptr root(new Node(conditional));
 			nodes_.push_back(root);
 			nodeMap_.insert(make_pair(key, 0));
 			return;
@@ -108,13 +109,13 @@ namespace gtsam {
 		if (parent_clique->size() == parents.size()) {
 			if (verbose) cout << "Adding to clique " << index << endl;
 			nodeMap_.insert(make_pair(key, index));
-			parent_clique->add(key, conditional);
+			parent_clique->add(conditional);
 			return;
 		}
 
 		// otherwise, start a new clique and add it to the tree
 		if (verbose) cout << "Starting new clique" << endl;
-		node_ptr new_clique(new Node(key, conditional));
+		node_ptr new_clique(new Node(conditional));
 		new_clique->parent_ = parent_clique;
 		parent_clique->children_.push_back(new_clique);
 		nodeMap_.insert(make_pair(key, nodes_.size()));
@@ -123,4 +124,5 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 
-} /// namespace gtsam
+}
+/// namespace gtsam

cpp/BayesTree.h

@@ -22,14 +22,13 @@ namespace gtsam {
 	template<class Conditional>
 	class Front: Testable<Front<Conditional> > {
 	private:
-		typedef boost::shared_ptr<Conditional> cond_ptr;
+		typedef boost::shared_ptr<Conditional> conditional_ptr;
-		std::list<std::string> keys_; /** frontal keys */
+		std::list<conditional_ptr> conditionals_; /** conditionals */
-		std::list<cond_ptr> conditionals_; /** conditionals */
 		std::list<std::string> separator_; /** separator keys */
 	public:
 
 		/** constructor */
-		Front(std::string key, cond_ptr conditional);
+		Front(const conditional_ptr& conditional);
 
 		/** print */
 		void print(const std::string& s = "") const;
@@ -38,10 +37,10 @@ namespace gtsam {
 		bool equals(const Front<Conditional>& other, double tol = 1e-9) const;
 
 		/** add a frontal node */
-		void add(std::string key, cond_ptr conditional);
+		void add(const conditional_ptr& conditional);
 
 		/** return size of the clique */
-		inline size_t size() const {return keys_.size() + separator_.size();}
+		inline size_t size() const {return conditionals_.size() + separator_.size();}
 	};
 
 	/**
@@ -55,6 +54,7 @@ namespace gtsam {
 	public:
 
 		typedef boost::shared_ptr<Conditional> conditional_ptr;
+		typedef std::pair<std::string,conditional_ptr> NamedConditional;
 
 	private:
 
@@ -64,7 +64,7 @@ namespace gtsam {
 			shared_ptr parent_;
 			std::list<shared_ptr> children_;
 
-			Node(std::string key, conditional_ptr conditional):Front<Conditional>(key,conditional) {}
+			Node(const boost::shared_ptr<Conditional>& conditional):Front<Conditional>(conditional) {}
 
 			/** print this node and entire subtree below it*/
 			void printTree(const std::string& indent) const {
@@ -88,8 +88,8 @@ namespace gtsam {
 		/** Create an empty Bayes Tree */
 		BayesTree();
 
-		/** Create a Bayes Tree from a SymbolicBayesNet */
+		/** Create a Bayes Tree from a Bayes Net */
-		BayesTree(BayesNet<Conditional>& bayesChain, bool verbose=false);
+		BayesTree(const BayesNet<Conditional>& bayesNet, bool verbose=false);
 
 		/** Destructor */
 		virtual ~BayesTree() {}
@@ -101,7 +101,7 @@ namespace gtsam {
 		bool equals(const BayesTree<Conditional>& other, double tol = 1e-9) const;
 
 		/** insert a new conditional */
-		void insert(std::string key, conditional_ptr conditional, bool verbose=false);
+		void insert(const boost::shared_ptr<Conditional>& conditional, bool verbose=false);
 
 			/** number of cliques */
 		inline size_t size() const { return nodes_.size();}

cpp/Conditional.h

@@ -0,0 +1,52 @@
+/**
+ * @file    Conditional.h
+ * @brief   Base class for conditional densities
+ * @author  Frank Dellaert
+ */
+
+// \callgraph
+
+#pragma once
+
+#include <boost/utility.hpp> // for noncopyable
+#include "Testable.h"
+
+namespace gtsam {
+
+  /** 
+   * Base class for conditional densities
+   *
+   * We make it noncopyable so we enforce the fact that factors are
+   * kept in pointer containers. To be safe, you should make them
+   * immutable, i.e., practicing functional programming.
+   */ 
+  class Conditional : boost::noncopyable, public Testable<Conditional>
+  {
+  protected:
+
+  	/** key of random variable */
+  	std::string key_;
+
+  public:
+
+  	/** constructor */
+  	Conditional(const std::string& key):key_(key) {}
+
+  	/* destructor */
+    virtual ~Conditional() {};
+
+		/** check equality */
+		bool equals(const Conditional& c, double tol = 1e-9) const {
+			return key_ == c.key_;
+		}
+
+		/** return key */
+    inline const std::string& key() const { return key_;}
+		
+    /** return parent keys */
+    virtual std::list<std::string> parents() const = 0;
+		
+    /** return the number of parents */
+    virtual std::size_t nrParents() const = 0;
+  };
+}

cpp/ConditionalGaussian.cpp

@@ -4,48 +4,38 @@
  * @author Christian Potthast
  */
 
-
 #include <string.h>
 #include <boost/numeric/ublas/vector.hpp>
+#include "Ordering.h"
 #include "ConditionalGaussian.h"
 
 using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
-ConditionalGaussian::ConditionalGaussian(Vector d,Matrix R) : R_(R),d_(d)
+ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R) :
-{
+	Conditional (key), R_(R), d_(d) {
 }
 
 /* ************************************************************************* */
-ConditionalGaussian::ConditionalGaussian(Vector d,
+ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R,
-					 Matrix R,
+		const string& name1, Matrix S) :
-					 const string& name1,
+	Conditional (key), R_(R), d_(d) {
-					 Matrix S)
-  : R_(R),d_(d)
-{
 	parents_.insert(make_pair(name1, S));
 }
 
 /* ************************************************************************* */
-ConditionalGaussian::ConditionalGaussian(Vector d,
+ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R,
-					 Matrix R,
+		const string& name1, Matrix S, const string& name2, Matrix T) :
-					 const string& name1,
+	Conditional (key), R_(R), d_(d) {
-					 Matrix S,
-					 const string& name2,
-					 Matrix T)
-  : R_(R),d_(d)
-{
 	parents_.insert(make_pair(name1, S));
 	parents_.insert(make_pair(name2, T));
 }
 
 /* ************************************************************************* */
-ConditionalGaussian::ConditionalGaussian(const Vector& d,
+ConditionalGaussian::ConditionalGaussian(const string& key,
-    		const Matrix& R,
+		const Vector& d, const Matrix& R, const map<string, Matrix>& parents) :
-    		const map<string, Matrix>& parents)
+	Conditional (key), R_(R), d_(d), parents_(parents) {
- : R_(R), d_(d), parents_(parents)
- {
 }
 
 /* ************************************************************************* */
@@ -62,23 +52,26 @@ void ConditionalGaussian::print(const string &s) const
 }    
 
 /* ************************************************************************* */
-bool ConditionalGaussian::equals(const ConditionalGaussian &cg, double tol) const {
+bool ConditionalGaussian::equals(const Conditional &c, double tol) const {
+	if (!Conditional::equals(c)) return false;
+	const ConditionalGaussian* p = dynamic_cast<const ConditionalGaussian*> (&c);
+	if (p == NULL) return false;
 	Parents::const_iterator it = parents_.begin();
 
 	// check if the size of the parents_ map is the same
-	if (parents_.size() != cg.parents_.size()) return false;
+	if (parents_.size() != p->parents_.size()) return false;
 
 	// check if R_ is equal
-	if (!(equal_with_abs_tol(R_, cg.R_, tol))) return false;
+	if (!(equal_with_abs_tol(R_, p->R_, tol))) return false;
 
 	// check if d_ is equal
-	if (!(::equal_with_abs_tol(d_, cg.d_, tol))) return false;
+	if (!(::equal_with_abs_tol(d_, p->d_, tol))) return false;
 
 	// check if the matrices are the same
 	// iterate over the parents_ map
 	for (it = parents_.begin(); it != parents_.end(); it++) {
-		Parents::const_iterator it2 = cg.parents_.find(it->first.c_str());
+		Parents::const_iterator it2 = p->parents_.find(it->first.c_str());
-		if (it2 != cg.parents_.end()) {
+		if (it2 != p->parents_.end()) {
 			if (!(equal_with_abs_tol(it->second, it2->second, tol))) return false;
 		} else
 			return false;

cpp/ConditionalGaussian.h

@@ -4,7 +4,6 @@
  * @author  Christian Potthast
  */
 
-
 // \callgraph
 
 #pragma once
@@ -14,21 +13,23 @@
 #include <boost/utility.hpp>
 #include <boost/shared_ptr.hpp>
 #include <boost/serialization/map.hpp>
+#include <boost/serialization/string.hpp>
 #include <boost/serialization/shared_ptr.hpp>
-#include "Matrix.h"
+
+#include "Conditional.h"
 #include "VectorConfig.h"
-#include "Ordering.h"
+#include "Matrix.h"
-#include "Testable.h"
 
 namespace gtsam {
 
+	class Ordering;
+
 	/**
 	 * A conditional Gaussian functions as the node in a Bayes network
 	 * It has a set of parents y,z, etc. and implements a probability density on x.
 	 * The negative log-probability is given by || Rx - (d - Sy - Tz - ...)||^2
 	 */
-  class ConditionalGaussian : boost::noncopyable, public Testable<ConditionalGaussian>
+	class ConditionalGaussian: public Conditional {
-  {
 	public:
 		typedef std::map<std::string, Matrix> Parents;
 		typedef Parents::const_iterator const_iterator;
@@ -47,55 +48,45 @@ namespace gtsam {
 
 	public:
 
-    /** constructor */
+		/** default constructor needed for serialization */
-    ConditionalGaussian() {};
+		ConditionalGaussian():Conditional("__unitialized__") {}
 
-    /** Copy Constructor */
+		/** constructor */
-    ConditionalGaussian(const ConditionalGaussian &cg) :
+		ConditionalGaussian(const std::string& key) :
-      boost::noncopyable(), R_(cg.R_), parents_(cg.parents_), d_(cg.d_){}
+			Conditional (key) {}
 
 		/** constructor with no parents
 		 * |Rx-d|
 		 */
-    ConditionalGaussian(Vector d,
+		ConditionalGaussian(const std::string& key, Vector d, Matrix R);
-			Matrix R);
 
 		/** constructor with only one parent
 		 * |Rx+Sy-d|
 		 */
-    ConditionalGaussian(Vector d,
+		ConditionalGaussian(const std::string& key, Vector d, Matrix R,
-			Matrix R,
+				const std::string& name1, Matrix S);
-			const std::string& name1,
-			Matrix S
-      );
 
 		/** constructor with two parents
 		 * |Rx+Sy+Tz-d|
 		 */
-    ConditionalGaussian(Vector d,
+		ConditionalGaussian(const std::string& key, Vector d, Matrix R,
-			Matrix R,
+				const std::string& name1, Matrix S, const std::string& name2, Matrix T);
-			const std::string& name1,
-			Matrix S,
-			const std::string& name2,
-			Matrix T
-      );
 
 		/**
 		 * constructor with number of arbitrary parents
 		 * |Rx+sum(Ai*xi)-d|
 		 */
-    ConditionalGaussian(const Vector& d,
+		ConditionalGaussian(const std::string& key, const Vector& d,
-    		const Matrix& R,
+				const Matrix& R, const Parents& parents);
-    		const Parents& parents);
 
 		/** deconstructor */
-    virtual ~ConditionalGaussian() {};
+		virtual ~ConditionalGaussian() {}
 
 		/** print */
 		void print(const std::string& = "ConditionalGaussian") const;
 
 		/** equals function */
-    bool equals(const ConditionalGaussian &cg, double tol=1e-9) const;
+		bool equals(const Conditional &cg, double tol = 1e-9) const;
 
 		/** dimension of multivariate variable */
 		size_t dim() const { return R_.size2();}
@@ -108,16 +99,24 @@ namespace gtsam {
 		const Matrix& get_R() const {	return R_;}
 
 		/** STL like, return the iterator pointing to the first node */
-    const_iterator const parentsBegin() const { return parents_.begin(); }
+		const_iterator const parentsBegin() const {
+			return parents_.begin();
+		}
 
 		/** STL like, return the iterator pointing to the last node */
-    const_iterator const parentsEnd() const { return parents_.end(); }
+		const_iterator const parentsEnd() const {
+			return parents_.end();
+		}
 
 		/** find the number of parents */
-    size_t size() const {return parents_.size();}
+		size_t nrParents() const {
+			return parents_.size();
+		}
 
 		/** determine whether a key is among the parents */
-    size_t contains(const std::string& key) const {return parents_.count(key);}
+		size_t contains(const std::string& key) const {
+			return parents_.count(key);
+		}
 
 		/**
 		 * solve a conditional Gaussian
@@ -129,13 +128,16 @@ namespace gtsam {
 		/**
 		 * adds a parent
 		 */
-    void add(const std::string key, Matrix S){ parents_.insert(make_pair(key, S)); }
+		void add(const std::string key, Matrix S) {
+			parents_.insert(make_pair(key, S));
+		}
 
 	private:
 		/** Serialization function */
 		friend class boost::serialization::access;
 		template<class Archive>
 		void serialize(Archive & ar, const unsigned int version) {
+			ar & BOOST_SERIALIZATION_NVP(key_);
 			ar & BOOST_SERIALIZATION_NVP(R_);
 			ar & BOOST_SERIALIZATION_NVP(d_);
 			ar & BOOST_SERIALIZATION_NVP(parents_);

cpp/ConstrainedConditionalGaussian.cpp

@@ -12,40 +12,39 @@
 using namespace gtsam;
 using namespace std;
 
-ConstrainedConditionalGaussian::ConstrainedConditionalGaussian() {
+ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
+		const string& key) :
+	ConditionalGaussian(key) {
 
 }
 
-ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Vector& v) :
-	ConditionalGaussian(v, eye(v.size())) {
-}
-
-ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Vector& b,
-		const Matrix& A) :
-	ConditionalGaussian(b, A) {
-}
-
-ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Vector& b,
-		const Matrix& A1, const std::string& parent, const Matrix& A2) :
-	ConditionalGaussian(b, A1, parent, A2) {
-}
-
-ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Vector& b,
-		const Matrix& A1, const std::string& parentY, const Matrix& A2,
-		const std::string& parentZ, const Matrix& A3)
-: ConditionalGaussian(b, A1, parentY, A2, parentZ, A3)
-{
-}
-
-ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Matrix& A1,
-			const std::map<std::string, Matrix>& parents, const Vector& b)
-: ConditionalGaussian(b, A1, parents)
-{
-}
-
-
 ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
-		const ConstrainedConditionalGaussian& df) {
+		const string& key, const Vector& v) :
+	ConditionalGaussian(key, v, eye(v.size())) {
+}
+
+ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
+		const string& key, const Vector& b, const Matrix& A) :
+	ConditionalGaussian(key, b, A) {
+}
+
+ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
+		const string& key, const Vector& b, const Matrix& A1,
+		const std::string& parent, const Matrix& A2) :
+	ConditionalGaussian(key, b, A1, parent, A2) {
+}
+
+ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
+		const string& key, const Vector& b, const Matrix& A1,
+		const std::string& parentY, const Matrix& A2, const std::string& parentZ,
+		const Matrix& A3) :
+	ConditionalGaussian(key, b, A1, parentY, A2, parentZ, A3) {
+}
+
+ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
+		const string& key, const Matrix& A1,
+		const std::map<std::string, Matrix>& parents, const Vector& b) :
+	ConditionalGaussian(key, b, A1, parents) {
 }
 
 Vector ConstrainedConditionalGaussian::solve(const VectorConfig& x) const {

cpp/ConstrainedConditionalGaussian.h

@@ -33,14 +33,14 @@ public:
 	 * Default Constructor
 	 * Don't use this
 	 */
-	ConstrainedConditionalGaussian();
+	ConstrainedConditionalGaussian(const std::string& key);
 
 	/**
 	 * Used for unary factors that simply associate a name with a particular value
 	 * Can use backsubstitution to solve trivially
 	 * @param value is a fixed value for x in the form x = value
 	 */
-	ConstrainedConditionalGaussian(const Vector& value);
+	ConstrainedConditionalGaussian(const std::string& key, const Vector& value);
 
 	/**
 	 * Used for unary factors of the form Ax=b
@@ -48,7 +48,7 @@ public:
 	 * @param b is the RHS of the equation
 	 * @param A is the A matrix
 	 */
-	ConstrainedConditionalGaussian(const Vector& value, const Matrix& A);
+	ConstrainedConditionalGaussian(const std::string& key, const Vector& value, const Matrix& A);
 
 	/**
 	 * Binary constructor of the form A1*x = b - A2*y
@@ -58,7 +58,7 @@ public:
 	 * @param parent is the string identifier for the parent node
 	 * @param A2 is the A2 matrix
 	 */
-	ConstrainedConditionalGaussian(const Vector& b, const Matrix& A1,
+	ConstrainedConditionalGaussian(const std::string& key, const Vector& b, const Matrix& A1,
 			const std::string& parent, const Matrix& A2);
 
 	/**
@@ -70,7 +70,7 @@ public:
 	 * @param parentZ string id for z
 	 * @param A3 is the A3 matrix
 	 */
-	ConstrainedConditionalGaussian(const Vector& b, const Matrix& A1,
+	ConstrainedConditionalGaussian(const std::string& key, const Vector& b, const Matrix& A1,
 			const std::string& parentY, const Matrix& A2,
 			const std::string& parentZ, const Matrix& A3);
 
@@ -81,14 +81,9 @@ public:
 	 * @param parents is the map of parents (Ai and xi from above)
 	 * @param b is the rhs vector
 	 */
-	ConstrainedConditionalGaussian(const Matrix& A1,
+	ConstrainedConditionalGaussian(const std::string& key, const Matrix& A1,
 			const std::map<std::string, Matrix>& parents, const Vector& b);
 
-	/**
-	 * Copy constructor
-	 */
-	ConstrainedConditionalGaussian(const ConstrainedConditionalGaussian& df);
-
 	virtual ~ConstrainedConditionalGaussian() {
 	}
 

cpp/ConstrainedLinearFactorGraph.cpp

@@ -6,6 +6,7 @@
 #include <iostream>
 #include <boost/tuple/tuple.hpp>
 #include <boost/foreach.hpp>
+#include "Ordering.h"
 #include "ConstrainedLinearFactorGraph.h"
 using namespace std;
 
@@ -78,12 +79,12 @@ GaussianBayesNet::shared_ptr ConstrainedLinearFactorGraph::eliminate(const Order
 		if (is_constrained(key))
 		{
 			ConditionalGaussian::shared_ptr ccg = eliminate_constraint(key);
-			cbn->insert(key,ccg);
+			cbn->insert(ccg);
 		}
 		else
 		{
 			ConditionalGaussian::shared_ptr cg = eliminateOne<ConditionalGaussian>(key);
-			cbn->insert(key,cg);
+			cbn->insert(cg);
 		}
 	}
 

cpp/FactorGraph.h

@@ -11,6 +11,7 @@
 
 #include <boost/shared_ptr.hpp>
 #include <boost/serialization/map.hpp>
+#include <boost/serialization/list.hpp>
 #include <boost/serialization/vector.hpp>
 #include <boost/serialization/shared_ptr.hpp>
 

cpp/GaussianBayesNet.cpp

@@ -20,6 +20,7 @@ template class BayesNet<ConditionalGaussian>;
 
 // trick from some reading group
 #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) 
+#define REVERSE_FOREACH_PAIR( KEY, VAL, COL) BOOST_REVERSE_FOREACH (boost::tie(KEY,VAL),COL)
 
 /* ************************************************************************* */
 boost::shared_ptr<VectorConfig> GaussianBayesNet::optimize() const
@@ -27,11 +28,9 @@ boost::shared_ptr<VectorConfig> GaussianBayesNet::optimize() const
   boost::shared_ptr<VectorConfig> result(new VectorConfig);
 	
   /** solve each node in turn in topological sort order (parents first)*/
-  BOOST_FOREACH(string key, keys_) {
+	BOOST_REVERSE_FOREACH(ConditionalGaussian::shared_ptr cg,conditionals_) {
-    const_iterator cg = nodes_.find(key);		// get node
+    Vector x = cg->solve(*result); // Solve for that variable
-    assert( cg != nodes_.end() );						// make sure it exists
+    result->insert(cg->key(),x);   // store result in partial solution
-    Vector x = cg->second->solve(*result);  // Solve for that variable
-    result->insert(key,x);                  // store result in partial solution
   }
   return result;
 }
@@ -41,22 +40,19 @@ pair<Matrix,Vector> GaussianBayesNet::matrix() const {
 
   // add the dimensions of all variables to get matrix dimension
   // and at the same time create a mapping from keys to indices
-  size_t N=0; map<string,size_t> indices;
+  size_t N=0; map<string,size_t> mapping;
-  BOOST_REVERSE_FOREACH(string key, keys_) {
+  BOOST_FOREACH(ConditionalGaussian::shared_ptr cg,conditionals_) {
-    // find corresponding node
+    mapping.insert(make_pair(cg->key(),N));
-    const_iterator it = nodes_.find(key);
+    N += cg->dim();
-    indices.insert(make_pair(key,N));
-    N += it->second->dim();
   }
 
   // create matrix and copy in values
   Matrix R = zeros(N,N);
   Vector d(N);
 	string key; size_t I;
-  FOREACH_PAIR(key,I,indices) {
+  FOREACH_PAIR(key,I,mapping) {
-    // find corresponding node
+    // find corresponding conditional
-    const_iterator it = nodes_.find(key);
+    ConditionalGaussian::shared_ptr cg = (*this)[key];
-    ConditionalGaussian::shared_ptr cg = it->second;
     
     // get RHS and copy to d
     const Vector& d_ = cg->get_d();
@@ -75,7 +71,7 @@ pair<Matrix,Vector> GaussianBayesNet::matrix() const {
     for (; keyS!=cg->parentsEnd(); keyS++) {
       Matrix S = keyS->second;                   // get S matrix      
       const size_t m = S.size1(), n = S.size2(); // find S size
-      const size_t J = indices[keyS->first];     // find column index
+      const size_t J = mapping[keyS->first];     // find column index
       for (size_t i=0;i<m;i++)
       	for(size_t j=0;j<n;j++)
       		R(I+i,J+j) = S(i,j);

cpp/LinearConstraint.cpp

@@ -81,7 +81,7 @@ ConstrainedConditionalGaussian::shared_ptr LinearConstraint::eliminate(const std
 	parents.erase(key);
 
 	// construct resulting CCG with parts
-	ConstrainedConditionalGaussian::shared_ptr ccg(new ConstrainedConditionalGaussian(A1, parents, b));
+	ConstrainedConditionalGaussian::shared_ptr ccg(new ConstrainedConditionalGaussian(key, A1, parents, b));
 	return ccg;
 }
 

cpp/LinearFactor.cpp

@@ -7,6 +7,10 @@
 
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
+
+#include "Matrix.h"
+#include "Ordering.h"
+#include "ConditionalGaussian.h"
 #include "LinearFactor.h"
 
 using namespace std;
@@ -20,6 +24,18 @@ using namespace gtsam;
 
 typedef pair<const string, Matrix>& mypair;
 
+/* ************************************************************************* */
+LinearFactor::LinearFactor(const boost::shared_ptr<ConditionalGaussian> cg) :
+	b(cg->get_d()) {
+	As.insert(make_pair(cg->key(), cg->get_R()));
+	std::map<std::string, Matrix>::const_iterator it = cg->parentsBegin();
+	for (; it != cg->parentsEnd(); it++) {
+		const std::string& j = it->first;
+		const Matrix& Aj = it->second;
+		As.insert(make_pair(j, Aj));
+	}
+}
+
 /* ************************************************************************* */
 LinearFactor::LinearFactor(const vector<shared_ptr> & factors)
 {
@@ -186,7 +202,7 @@ LinearFactor::eliminate(const string& key)
   // if this factor does not involve key, we exit with empty CG and LF
   if (it==As.end()) {
     // Conditional Gaussian is just a parent-less node with P(x)=1
-    ConditionalGaussian::shared_ptr cg(new ConditionalGaussian);
+    ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key));
     return make_pair(cg,lf);
   }
 
@@ -227,7 +243,7 @@ LinearFactor::eliminate(const string& key)
   } // column j
   
   // create ConditionalGaussian with first n rows
-  ConditionalGaussian::shared_ptr cg (new ConditionalGaussian(::sub(b,0,n), sub(R,0,n,0,n)) );
+  ConditionalGaussian::shared_ptr cg (new ConditionalGaussian(key,::sub(b,0,n), sub(R,0,n,0,n)) );
   
   // create linear factor with remaining rows
   lf->set_b(::sub(b,n,m));

cpp/LinearFactor.h

@@ -9,23 +9,18 @@
 
 #pragma once
 
-#include <list>
-#include <map>
-#include <set>
-#include <ostream>
 #include <boost/shared_ptr.hpp>
-#include <vector>
+#include <boost/serialization/map.hpp>
 
-#include "Matrix.h"
 #include "Factor.h"
-#include "LinearFactorSet.h"
+#include "Matrix.h"
-#include "ConditionalGaussian.h"
+#include "VectorConfig.h"
-#include "Ordering.h"
-
-#define CONSTRUCTOR 
 
 namespace gtsam {
 
+	class ConditionalGaussian;
+	class Ordering;
+
 /**
  * Base Class for a linear factor.
  * LinearFactor is non-mutable (all methods const!).
@@ -50,20 +45,17 @@ public:
 	}
 
 	/** Construct Null factor */
-	CONSTRUCTOR
 	LinearFactor(const Vector& b_in) :
 		b(b_in) { //TODO: add a way to initializing base class meaningfully
 	}
 
 	/** Construct unary factor */
-	CONSTRUCTOR
 	LinearFactor(const std::string& key1, const Matrix& A1, const Vector& b_in) :
 		b(b_in) {
 		As.insert(make_pair(key1, A1));
 	}
 
 	/** Construct binary factor */
-	CONSTRUCTOR
 	LinearFactor(const std::string& key1, const Matrix& A1,
 			const std::string& key2, const Matrix& A2, const Vector& b_in) :
 		b(b_in) {
@@ -72,7 +64,6 @@ public:
 	}
 
 	/** Construct ternary factor */
-	CONSTRUCTOR
 	LinearFactor(const std::string& key1, const Matrix& A1,
 			const std::string& key2, const Matrix& A2, const std::string& key3,
 			const Matrix& A3, const Vector& b_in) :
@@ -83,7 +74,6 @@ public:
 	}
 
 	/** Construct an n-ary factor */
-	CONSTRUCTOR
 	LinearFactor(const std::vector<std::pair<std::string, Matrix> > &terms,
 	    const Vector &b_in) :
 	    b(b_in) {
@@ -92,24 +82,12 @@ public:
 	}
 
 	/** Construct from Conditional Gaussian */
-	CONSTRUCTOR
+	LinearFactor(const boost::shared_ptr<ConditionalGaussian> cg);
-	LinearFactor(const std::string& key, const boost::shared_ptr<
-			ConditionalGaussian> cg) :
-		b(cg->get_d()) {
-		As.insert(make_pair(key, cg->get_R()));
-		std::map<std::string, Matrix>::const_iterator it = cg->parentsBegin();
-		for (; it != cg->parentsEnd(); it++) {
-			const std::string& j = it->first;
-			const Matrix& Aj = it->second;
-			As.insert(make_pair(j, Aj));
-		}
-	}
 
 	/**
 	 * Constructor that combines a set of factors
 	 * @param factors Set of factors to combine
 	 */
-	CONSTRUCTOR
 	LinearFactor(const std::vector<shared_ptr> & factors);
 
 	// Implementing Testable virtual functions
@@ -212,7 +190,7 @@ public:
 	 * @param key the key of the node to be eliminated
 	 * @return a new factor and a conditional gaussian on the eliminated variable
 	 */
-	std::pair<ConditionalGaussian::shared_ptr, shared_ptr> eliminate(const std::string& key);
+	std::pair<boost::shared_ptr<ConditionalGaussian>, shared_ptr> eliminate(const std::string& key);
 
 	/**
 	 * Take the factor f, and append to current matrices. Not very general.

cpp/LinearFactorGraph.cpp

@@ -15,6 +15,7 @@
 #include "GaussianBayesNet.h"
 #include "FactorGraph-inl.h"
 #include "LinearFactorGraph.h"
+#include "LinearFactorSet.h"
 
 using namespace std;
 using namespace gtsam;
@@ -34,7 +35,7 @@ void LinearFactorGraph::setCBN(const GaussianBayesNet& CBN)
 	clear();
 	GaussianBayesNet::const_iterator it = CBN.begin();
 	for(; it != CBN.end(); it++) {
-		LinearFactor::shared_ptr lf(new LinearFactor(it->first, it->second));
+		LinearFactor::shared_ptr lf(new LinearFactor(*it));
 		push_back(lf);
 	}
 }
@@ -62,7 +63,7 @@ LinearFactorGraph::eliminate_partially(const Ordering& ordering)
 
 	BOOST_FOREACH(string key, ordering) {
 		ConditionalGaussian::shared_ptr cg = eliminateOne<ConditionalGaussian>(key);
-		chordalBayesNet->insert(key,cg);
+		chordalBayesNet->insert(cg);
 	}
 
 	return chordalBayesNet;

cpp/LinearFactorGraph.h

@@ -13,14 +13,14 @@
 #pragma once
 
 #include <boost/shared_ptr.hpp>
-
-#include "LinearFactor.h"
-#include "VectorConfig.h"
 #include "FactorGraph.h"
-#include "GaussianBayesNet.h"
+#include "LinearFactor.h"
 
 namespace gtsam {
 
+	class Ordering;
+	class GaussianBayesNet;
+
   /**
    * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    *   Factor == LinearFactor

cpp/Makefile.am

@@ -161,7 +161,7 @@ testVSLAMFactor_LDADD = libgtsam.la
 
 
 # The header files will be installed in ~/include/gtsam
-headers = gtsam.h Value.h Testable.h Factor.h LinearFactorSet.h 
+headers = gtsam.h Value.h Testable.h Factor.h Conditional.h LinearFactorSet.h 
 headers += Point2Prior.h Simulated2DOdometry.h Simulated2DMeasurement.h smallExample.h 
 headers += $(sources:.cpp=.h)
 # templates: 

cpp/SymbolicConditional.h

@@ -12,14 +12,14 @@
 #include <string>
 #include <boost/shared_ptr.hpp>
 #include <boost/foreach.hpp> // TODO: make cpp file
-#include "Testable.h"
+#include "Conditional.h"
 
 namespace gtsam {
 
 	/**
 	 * Conditional node for use in a Bayes net
 	 */
-	class SymbolicConditional: Testable<SymbolicConditional> {
+	class SymbolicConditional: public Conditional {
 
 	private:
 
@@ -33,20 +33,24 @@ namespace gtsam {
 		/**
 		 * No parents
 		 */
-		SymbolicConditional() {
+		SymbolicConditional(const std::string& key) :
+			Conditional(key) {
 		}
 
 		/**
 		 * Single parent
 		 */
-		SymbolicConditional(const std::string& parent) {
+		SymbolicConditional(const std::string& key, const std::string& parent) :
+			Conditional(key) {
 			parents_.push_back(parent);
 		}
 
 		/**
 		 * Two parents
 		 */
-		SymbolicConditional(const std::string& parent1, const std::string& parent2) {
+		SymbolicConditional(const std::string& key, const std::string& parent1,
+				const std::string& parent2) :
+			Conditional(key) {
 			parents_.push_back(parent1);
 			parents_.push_back(parent2);
 		}
@@ -54,24 +58,34 @@ namespace gtsam {
 		/**
 		 * A list
 		 */
-		SymbolicConditional(const std::list<std::string>& parents):parents_(parents) {
+		SymbolicConditional(const std::string& key,
+				const std::list<std::string>& parents) :
+			Conditional(key), parents_(parents) {
 		}
 
 		/** print */
 		void print(const std::string& s = "SymbolicConditional") const {
-			std::cout << s;
+			std::cout << s << " P(" << key_ << " |";
 			BOOST_FOREACH(std::string parent, parents_) std::cout << " " << parent;
-			std::cout << std::endl;
+			std::cout << ")" << std::endl;
 		}
 
 		/** check equality */
-		bool equals(const SymbolicConditional& other, double tol = 1e-9) const {
+		bool equals(const Conditional& c, double tol = 1e-9) const {
-			return parents_ == other.parents_;
+			if (!Conditional::equals(c)) return false;
+			const SymbolicConditional* p = dynamic_cast<const SymbolicConditional*> (&c);
+			if (p == NULL) return false;
+			return parents_ == p->parents_;
 		}
 
-		/** return any parent */
+		/** return parents */
 		std::list<std::string> parents() const { return parents_;}
 
+		/** find the number of parents */
+		size_t nrParents() const {
+			return parents_.size();
+		}
+
 	};
 
 } /// namespace gtsam

cpp/SymbolicFactor.cpp

@@ -59,7 +59,7 @@ namespace gtsam {
 	  boost::shared_ptr<SymbolicFactor> lf(new SymbolicFactor(separator));
 
 	  // create SymbolicConditional on separator
-	  SymbolicConditional::shared_ptr cg (new SymbolicConditional(separator));
+	  SymbolicConditional::shared_ptr cg (new SymbolicConditional(key,separator));
 
 	  return make_pair(cg,lf);
 	}

cpp/SymbolicFactorGraph.cpp

@@ -22,14 +22,14 @@ namespace gtsam {
 	SymbolicBayesNet::shared_ptr
 	SymbolicFactorGraph::eliminate(const Ordering& ordering)
 	{
-		SymbolicBayesNet::shared_ptr bayesChain (new SymbolicBayesNet());
+		SymbolicBayesNet::shared_ptr bayesNet (new SymbolicBayesNet());
 
 		BOOST_FOREACH(string key, ordering) {
 			SymbolicConditional::shared_ptr conditional = eliminateOne<SymbolicConditional>(key);
-			bayesChain->insert(key,conditional);
+			bayesNet->insert(conditional);
 		}
 
-		return bayesChain;
+		return bayesNet;
 	}
 
 	/* ************************************************************************* */

cpp/smallExample.cpp

@@ -200,11 +200,11 @@ GaussianBayesNet createSmallGaussianBayesNet()
   
   // define nodes and specify in reverse topological sort (i.e. parents last)
   ConditionalGaussian::shared_ptr
-    x(new ConditionalGaussian(d1,R11,"y",S12)),
+    Px_y(new ConditionalGaussian("x",d1,R11,"y",S12)),
-    y(new ConditionalGaussian(d2,R22));
+    Py(new ConditionalGaussian("y",d2,R22));
   GaussianBayesNet cbn;
-  cbn.insert("x",x);
+  cbn.insert(Px_y);
-  cbn.insert("y",y);
+  cbn.insert(Py);
 
   return cbn;
 }

cpp/testBayesTree.cpp

@@ -4,31 +4,33 @@
  * @author  Frank Dellaert
  */
 
-#include <boost/assign/list_inserter.hpp> // for 'insert()'
 #include <boost/assign/std/list.hpp> // for operator +=
 using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
 #include "SymbolicBayesNet.h"
+#include "GaussianBayesNet.h"
+#include "Ordering.h"
 #include "BayesTree-inl.h"
-#include "SmallExample.h"
+#include "smallExample.h"
 
 using namespace gtsam;
 
 // Conditionals for ASIA example from the tutorial with A and D evidence
-SymbolicConditional::shared_ptr B(new SymbolicConditional()), L(
+SymbolicConditional::shared_ptr B(new SymbolicConditional("B")), L(
-		new SymbolicConditional("B")), E(new SymbolicConditional("L", "B")), S(
+		new SymbolicConditional("L", "B")), E(
-		new SymbolicConditional("L", "B")), T(new SymbolicConditional("E", "L")),
+		new SymbolicConditional("E", "L", "B")), S(new SymbolicConditional("S",
-		X(new SymbolicConditional("E"));
+		"L", "B")), T(new SymbolicConditional("T", "E", "L")), X(
+		new SymbolicConditional("X", "E"));
 
 /* ************************************************************************* */
 TEST( BayesTree, Front )
 {
-	Front<SymbolicConditional> f1("B", B);
+	Front<SymbolicConditional> f1(B);
-	f1.add("L", L);
+	f1.add(L);
-	Front<SymbolicConditional> f2("L", L);
+	Front<SymbolicConditional> f2(L);
-	f2.add("B", B);
+	f2.add(B);
 	CHECK(f1.equals(f1));
 	CHECK(!f1.equals(f2));
 }
@@ -38,31 +40,31 @@ TEST( BayesTree, constructor )
 {
 	// Create using insert
 	BayesTree<SymbolicConditional> bayesTree;
-	bayesTree.insert("B", B);
+	bayesTree.insert(B);
-	bayesTree.insert("L", L);
+	bayesTree.insert(L);
-	bayesTree.insert("E", E);
+	bayesTree.insert(E);
-	bayesTree.insert("S", S);
+	bayesTree.insert(S);
-	bayesTree.insert("T", T);
+	bayesTree.insert(T);
-	bayesTree.insert("X", X);
+	bayesTree.insert(X);
 
 	// Check Size
 	LONGS_EQUAL(4,bayesTree.size());
 
 	// Check root
-	Front<SymbolicConditional> expected_root("B", B);
+	Front<SymbolicConditional> expected_root(B);
-	expected_root.add("L", L);
+	expected_root.add(L);
-	expected_root.add("E", E);
+	expected_root.add(E);
 	Front<SymbolicConditional> actual_root = bayesTree.root();
 	CHECK(assert_equal(expected_root,actual_root));
 
 	// Create from symbolic Bayes chain in which we want to discover cliques
 	SymbolicBayesNet ASIA;
-	ASIA.insert("X", X);
+	ASIA.insert(X);
-	ASIA.insert("T", T);
+	ASIA.insert(T);
-	ASIA.insert("S", S);
+	ASIA.insert(S);
-	ASIA.insert("E", E);
+	ASIA.insert(E);
-	ASIA.insert("L", L);
+	ASIA.insert(L);
-	ASIA.insert("B", B);
+	ASIA.insert(B);
 	BayesTree<SymbolicConditional> bayesTree2(ASIA);
 
 	// Check whether the same

cpp/testConditionalGaussian.cpp

@@ -17,7 +17,6 @@
 #include "Matrix.h"
 #include "ConditionalGaussian.h"
 
-
 using namespace gtsam;
 
 /* ************************************************************************* */
@@ -42,8 +41,8 @@ TEST( ConditionalGaussian, equals )
   d(0) = 0.2; d(1) = 0.5;
   
   ConditionalGaussian 
-    expected(d, R, "x1", A1, "l1", A2),
+    expected("x",d, R, "x1", A1, "l1", A2),
-    actual(d, R, "x1", A1, "l1", A2);
+    actual("x",d, R, "x1", A1, "l1", A2);
   
   CHECK( expected.equals(actual) );
   
@@ -72,7 +71,7 @@ TEST( ConditionalGaussian, solve )
   Vector d(2);
   d(0) = 0.2; d(1) = 0.5;
   
-  ConditionalGaussian cg(d, R, "x1", A1, "l1", A2);
+  ConditionalGaussian cg("x",d, R, "x1", A1, "l1", A2);
   
   Vector sx1(2);
   sx1(0) = 0.2; sx1(1) = 0.5;
@@ -96,39 +95,38 @@ TEST( ConditionalGaussian, solve )
 #ifdef HAVE_BOOST_SERIALIZATION
 TEST( ConditionalGaussian, serialize )
 {
-//     // create a conditional gaussion node
+	 // create a conditional gaussion node
-//     Matrix A1(2,2);
+	 Matrix A1(2,2);
-//     A1(0,0) = 1 ; A1(1,0) = 2;
+	 A1(0,0) = 1 ; A1(1,0) = 2;
-//     A1(0,1) = 3 ; A1(1,1) = 4;
+	 A1(0,1) = 3 ; A1(1,1) = 4;
-// 
-//     Matrix A2(2,2);
-//     A2(0,0) = 6 ; A2(1,0) = 0.2;
-//     A2(0,1) = 8 ; A2(1,1) = 0.4;
-// 
-//     Matrix R(2,2);
-//     R(0,0) = 0.1 ; R(1,0) = 0.3;
-//     R(0,1) = 0.0 ; R(1,1) = 0.34;
-// 
-//     Vector d(2);
-//     d(0) = 0.2; d(1) = 0.5;
-// 
-//     ConditionalGaussian cg(d, R, "x1", A1, "l1", A2);
-//     
-//     //serialize the CG
-//     std::ostringstream in_archive_stream;
-//     boost::archive::text_oarchive in_archive(in_archive_stream);
-//     in_archive << cg;
-//     std::string serialized = in_archive_stream.str();
-//     
-//     //deserialize the CGg
-//     std::istringstream out_archive_stream(serialized);
-//     boost::archive::text_iarchive out_archive(out_archive_stream);
-//     ConditionalGaussian output;
-//     out_archive >> output;
-//     
-//     //check for equality
-//     CHECK(cg.equals(output));
 
+	 Matrix A2(2,2);
+	 A2(0,0) = 6 ; A2(1,0) = 0.2;
+	 A2(0,1) = 8 ; A2(1,1) = 0.4;
+
+	 Matrix R(2,2);
+	 R(0,0) = 0.1 ; R(1,0) = 0.3;
+	 R(0,1) = 0.0 ; R(1,1) = 0.34;
+
+	 Vector d(2);
+	 d(0) = 0.2; d(1) = 0.5;
+
+	 ConditionalGaussian cg("x2", d, R, "x1", A1, "l1", A2);
+
+	 //serialize the CG
+	 std::ostringstream in_archive_stream;
+	 boost::archive::text_oarchive in_archive(in_archive_stream);
+	 in_archive << cg;
+	 std::string serialized = in_archive_stream.str();
+
+	 //deserialize the CGg
+	 std::istringstream out_archive_stream(serialized);
+	 boost::archive::text_iarchive out_archive(out_archive_stream);
+	 ConditionalGaussian output;
+	 out_archive >> output;
+
+	 //check for equality
+	 CHECK(cg.equals(output));
 }
 #endif //HAVE_BOOST_SERIALIZATION
 /* ************************************************************************* */

cpp/testConstrainedConditionalGaussian.cpp

@@ -17,7 +17,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary1 )
 
 	// check unary constructor that doesn't require an R matrix
 	// assumed identity matrix
-	ConstrainedConditionalGaussian unary(v);
+	ConstrainedConditionalGaussian unary("x1",v);
 	VectorConfig fg;
 	fg.insert("x1", v);
 
@@ -32,7 +32,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary2 )
 	// check unary constructor that makes use of a A matrix
 	Matrix A = eye(2) * 10;
 
-	ConstrainedConditionalGaussian unary(10*v, A);
+	ConstrainedConditionalGaussian unary("x1",10*v, A);
 	VectorConfig fg;
 	fg.insert("x1", v);
 
@@ -51,7 +51,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary3 )
 	A(1,0) = 2.0 ; A(1,1) = 1.0;
 
 	Vector rhs = A*v;
-	ConstrainedConditionalGaussian unary(rhs, A);
+	ConstrainedConditionalGaussian unary("x1",rhs, A);
 	VectorConfig fg;
 	fg.insert("x1", v);
 
@@ -84,7 +84,7 @@ TEST (ConstrainedConditionalGaussian, basic_binary1 )
 
 	Vector expected = Vector_(2, -3.3333, 0.6667);
 
-	ConstrainedConditionalGaussian binary(b, A1, "x1", A2);
+	ConstrainedConditionalGaussian binary("x2",b, A1, "x1", A2);
 
 	CHECK(assert_equal(expected, binary.solve(fg), 1e-4));
 }
@@ -119,7 +119,7 @@ TEST (ConstrainedConditionalGaussian, basic_ternary1 )
 
 	Vector expected = Vector_(2, 6.6667, -9.3333);
 
-	ConstrainedConditionalGaussian ternary(b, A1, "x1", A2, "x2", A3);
+	ConstrainedConditionalGaussian ternary("x3",b, A1, "x1", A2, "x2", A3);
 
 	CHECK(assert_equal(expected, ternary.solve(fg), 1e-4));
 }

cpp/testConstrainedLinearFactorGraph.cpp

@@ -7,6 +7,7 @@
 #include <CppUnitLite/TestHarness.h>
 #include "ConstrainedLinearFactorGraph.h"
 #include "LinearFactorGraph.h"
+#include "Ordering.h"
 #include "smallExample.h"
 
 using namespace gtsam;
@@ -40,7 +41,7 @@ TEST( ConstrainedLinearFactorGraph, elimination1 )
 	Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0;
 	Matrix Ay1 = eye(2) * 10;
 	Vector b2 = Vector_(2, 1.0, 2.0);
-	ConstrainedConditionalGaussian expectedCCG1(b2, Ax1, "y", Ay1);
+	ConstrainedConditionalGaussian expectedCCG1("x",b2, Ax1, "y", Ay1);
 	CHECK(expectedCCG1.equals(*((*cbn)["x"])));
 
 	// verify remaining factor on y
@@ -64,7 +65,7 @@ TEST( ConstrainedLinearFactorGraph, elimination1 )
 	R(0, 0) = 74.5356; R(0, 1) = -59.6285;
 	R(1, 0) = 0.0;     R(1, 1) = 44.7214;
 	Vector br = Vector_(2, 8.9443, 4.4721);
-	ConditionalGaussian expected2(br, R);
+	ConditionalGaussian expected2("y",br, R);
 	CHECK(expected2.equals(*((*cbn)["y"])));
 }
 
@@ -236,18 +237,18 @@ TEST( ConstrainedLinearFactorGraph, eliminate_multi_constraint )
 
 	// eliminate the constraint
 	ConstrainedConditionalGaussian::shared_ptr cg1 = fg.eliminate_constraint("x");
-	CHECK(cg1->size() == 1);
+	CHECK(cg1->nrParents() == 1);
 	CHECK(fg.nrFactors() == 1);
 
 	// eliminate the induced constraint
 	ConstrainedConditionalGaussian::shared_ptr cg2 = fg.eliminate_constraint("y");
-	CHECK(cg2->size() == 1);
+	CHECK(cg2->nrParents() == 1);
 	CHECK(fg.nrFactors() == 0);
 
 	// eliminate the linear factor
 	ConditionalGaussian::shared_ptr cg3 = fg.eliminateOne<ConditionalGaussian>("z");
+	CHECK(cg3->nrParents() == 0);
 	CHECK(fg.size() == 0);
-	CHECK(cg3->size() == 0);
 
 	// solve piecewise
 	VectorConfig actual;

cpp/testGaussianBayesNet.cpp

@@ -20,6 +20,7 @@
 #include "GaussianBayesNet.h"
 #include "smallExample.h"
 
+using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
@@ -35,7 +36,7 @@ TEST( GaussianBayesNet, constructor )
   d1(0) = 9; d2(0) = 5;
   
   // define nodes and specify in reverse topological sort (i.e. parents last)
-  ConditionalGaussian x(d1,R11,"y",S12), y(d2,R22);
+  ConditionalGaussian x("x",d1,R11,"y",S12), y("y",d2,R22);
 
   // check small example which uses constructor
   GaussianBayesNet cbn = createSmallGaussianBayesNet();
@@ -82,44 +83,40 @@ TEST( GaussianBayesNet, optimize )
 #ifdef HAVE_BOOST_SERIALIZATION
 TEST( GaussianBayesNet, serialize )
 {
-//     //create a starting CBN
+	//create a starting CBN
-//     GaussianBayesNet cbn = createSmallGaussianBayesNet();
+	GaussianBayesNet cbn = createSmallGaussianBayesNet();
-//     
+
-//     //serialize the CBN
+	//serialize the CBN
-//     std::ostringstream in_archive_stream;
+	ostringstream in_archive_stream;
-//     boost::archive::text_oarchive in_archive(in_archive_stream);
+	boost::archive::text_oarchive in_archive(in_archive_stream);
-//     in_archive << cbn;
+	in_archive << cbn;
-//     std::string serialized = in_archive_stream.str();
+	string serialized = in_archive_stream.str();
-//     
+
-//     //DEBUG
+	//DEBUG
-//     std::cout << "CBN Raw string: [" << serialized << "]" << std::endl;
+	cout << "CBN Raw string: [" << serialized << "]" << endl;
-//     
+
-//     //remove newlines/carriage returns
+	//remove newlines/carriage returns
-//     std::string clean;
+	string clean;
-//     BOOST_FOREACH(char s, serialized)
+	BOOST_FOREACH(char s, serialized) {
-//     {
+		if (s != '\n') {
-// 	if (s != '\n') 
+			//copy in character
-// 	{
+			clean.append(string(1,s));
-// 	    //copy in character
+			}
-// 	    clean.append(std::string(1,s));
+		else {
-// 	}
+			cout << "   Newline character found!" << endl;
-// 	else
+			//replace with an identifiable string
-// 	{
+			clean.append(string(1,' '));
-// 	    std::cout << "   Newline character found!" << std::endl;
+			}
-// 	    //replace with an identifiable string
+		}
-// 	    clean.append(std::string(1,' '));
+
-// 	}
+	cout << "Cleaned CBN String: [" << clean << "]" << endl;
-//     }
+
-// 
+	//deserialize the CBN
-//     
+	istringstream out_archive_stream(clean);
-//     std::cout << "Cleaned CBN String: [" << clean << "]" << std::endl;
+	boost::archive::text_iarchive out_archive(out_archive_stream);
-//     
+	GaussianBayesNet output;
-//     //deserialize the CBN
+	out_archive >> output;
-//     std::istringstream out_archive_stream(clean);
+	CHECK(cbn.equals(output));
-//     boost::archive::text_iarchive out_archive(out_archive_stream);
-//     GaussianBayesNet output;
-//     out_archive >> output;
-//     CHECK(cbn.equals(output));
 }
 #endif //HAVE_BOOST_SERIALIZATION
 

cpp/testLinearFactor.cpp

@@ -14,6 +14,8 @@ using namespace boost::assign;
 #include <CppUnitLite/TestHarness.h>
 
 #include "Matrix.h"
+#include "Ordering.h"
+#include "ConditionalGaussian.h"
 #include "smallExample.h"
 
 using namespace std;
@@ -314,7 +316,7 @@ TEST( LinearFactor, eliminate )
 			 +0.00,-8.94427
 			 );
   Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
-  ConditionalGaussian expectedCG(d,R11,"l1",S12,"x1",S13);
+  ConditionalGaussian expectedCG("x2",d,R11,"l1",S12,"x1",S13);
 
   // the expected linear factor
   Matrix Bl1 = Matrix_(2,2,
@@ -384,7 +386,7 @@ TEST( LinearFactor, eliminate2 )
 			 +0.00,-2.23607,+0.00,-8.94427
 			 );
   Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
-  ConditionalGaussian expectedCG(d,R11,"l1x1",S12);
+  ConditionalGaussian expectedCG("x2",d,R11,"l1x1",S12);
 
   // the expected linear factor
   Matrix Bl1x1 = Matrix_(2,4,
@@ -424,7 +426,7 @@ TEST( LinearFactor, eliminate_empty )
   boost::tie(actualCG,actualLF) = f.eliminate("x2");
 
   // expected Conditional Gaussian is just a parent-less node with P(x)=1
-  ConditionalGaussian expectedCG;
+  ConditionalGaussian expectedCG("x2");
 
   // expected remaining factor is still empty :-)
   LinearFactor expectedLF;
@@ -495,8 +497,8 @@ TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian )
 		       +0.00,-2.23607
 		       );
   Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
-  ConditionalGaussian::shared_ptr CG(new ConditionalGaussian(d,R11,"l1x1",S12) );
+  ConditionalGaussian::shared_ptr CG(new ConditionalGaussian("x2",d,R11,"l1x1",S12) );
-  LinearFactor actualLF("x2",CG);
+  LinearFactor actualLF(CG);
   //  actualLF.print();
   LinearFactor expectedLF("x2",R11,"l1x1",S12,d);
 			  

cpp/testLinearFactorGraph.cpp

@@ -13,8 +13,11 @@ using namespace std;
 using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
+
 #include "Matrix.h"
+#include "Ordering.h"
 #include "smallExample.h"
+#include "GaussianBayesNet.h"
 
 using namespace gtsam;
 
@@ -182,7 +185,7 @@ TEST( LinearFactorGraph, eliminateOne_x1 )
 			 +0.00,-6.66667
 			 );
   Vector d(2); d(0) = -2; d(1) = -1.0/3.0;
-  ConditionalGaussian expected(d,R11,"l1",S12,"x2",S13);
+  ConditionalGaussian expected("x1",d,R11,"l1",S12,"x2",S13);
 
   CHECK(assert_equal(expected,*actual,tol));
 }
@@ -209,7 +212,7 @@ TEST( LinearFactorGraph, eliminateOne_x2 )
 			 +0.00,-8.94427
 			 );
   Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
-  ConditionalGaussian expected(d,R11,"l1",S12,"x1",S13);
+  ConditionalGaussian expected("x2",d,R11,"l1",S12,"x1",S13);
 
   CHECK(assert_equal(expected,*actual,tol));
 }
@@ -235,7 +238,7 @@ TEST( LinearFactorGraph, eliminateOne_l1 )
 			 +0.00,-3.53553
 			 );
   Vector d(2); d(0) = -0.707107; d(1) = 1.76777;
-  ConditionalGaussian expected(d,R11,"x1",S12,"x2",S13);
+  ConditionalGaussian expected("l1",d,R11,"x1",S12,"x2",S13);
 
   CHECK(assert_equal(expected,*actual,tol));
 }
@@ -248,7 +251,7 @@ TEST( LinearFactorGraph, eliminateAll )
                     0.0, 10};
   Matrix R1 = Matrix_(2,2, data1);
   Vector d1(2); d1(0) = -1; d1(1) = -1;
-  ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian(d1, R1));
+  ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian("x1",d1, R1));
   
   double data21[] = { 6.7082, 0.0,
                      0.0, 6.7082};
@@ -257,7 +260,7 @@ TEST( LinearFactorGraph, eliminateAll )
                        0.0, -6.7082};
   Matrix A1 = Matrix_(2,2, data22);
   Vector d2(2); d2(0) = 0.0; d2(1) = 1.34164;
-  ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian(d2, R2, "x1", A1));
+  ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian("l1",d2, R2, "x1", A1));
   
   double data31[] = { 11.1803, 0.0,
                          0.0, 11.1803};
@@ -270,12 +273,12 @@ TEST( LinearFactorGraph, eliminateAll )
   Matrix A22 = Matrix_(2,2, data33);
   
   Vector d3(2); d3(0) = 2.23607; d3(1) = -1.56525;
-  ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian(d3, R3, "l1", A21, "x1", A22));
+  ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x2",d3, R3, "l1", A21, "x1", A22));
   
   GaussianBayesNet expected;
-  expected.insert("x2", cg3);
+  expected.insert(cg3);
-  expected.insert("l1", cg2);
+  expected.insert(cg2);
-  expected.insert("x1", cg1);
+  expected.insert(cg1);
   
   // Check one ordering
   LinearFactorGraph fg1 = createLinearFactorGraph();

cpp/testNonlinearOptimizer.cpp

@@ -13,6 +13,7 @@ using namespace boost::assign;
 #include <CppUnitLite/TestHarness.h>
 
 #include "Matrix.h"
+#include "Ordering.h"
 #include "smallExample.h"
 
 // template definitions

cpp/testSymbolicBayesNet.cpp

@@ -10,6 +10,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
+#include "Ordering.h"
 #include "smallExample.h"
 #include "SymbolicBayesNet.h"
 #include "SymbolicFactorGraph.h"
@@ -22,13 +23,13 @@ TEST( SymbolicBayesNet, constructor )
 {
 	// Create manually
 	SymbolicConditional::shared_ptr
-		x2(new SymbolicConditional("l1", "x1")),
+		x2(new SymbolicConditional("x2","l1", "x1")),
-		l1(new SymbolicConditional("x1")),
+		l1(new SymbolicConditional("l1","x1")),
-		x1(new SymbolicConditional());
+		x1(new SymbolicConditional("x1"));
 	SymbolicBayesNet expected;
-	expected.insert("x2",x2);
+	expected.insert(x2);
-	expected.insert("l1",l1);
+	expected.insert(l1);
-	expected.insert("x1",x1);
+	expected.insert(x1);
 
 	// Create from a factor graph
 	LinearFactorGraph factorGraph = createLinearFactorGraph();

cpp/testSymbolicFactorGraph.cpp

@@ -9,6 +9,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
+#include "Ordering.h"
 #include "smallExample.h"
 #include "SymbolicFactorGraph.h"
 #include "SymbolicBayesNet.h"
@@ -114,7 +115,7 @@ TEST( LinearFactorGraph, eliminateOne )
   		fg.eliminateOne<SymbolicConditional>("x1");
 
   // create expected symbolic Conditional
-  SymbolicConditional expected("l1","x2");
+  SymbolicConditional expected("x1","l1","x2");
 
   CHECK(assert_equal(expected,*actual));
 }
@@ -123,14 +124,14 @@ TEST( LinearFactorGraph, eliminateOne )
 TEST( LinearFactorGraph, eliminate )
 {
   // create expected Chordal bayes Net
-  SymbolicConditional::shared_ptr x2(new SymbolicConditional("l1", "x1"));
+  SymbolicConditional::shared_ptr x2(new SymbolicConditional("x2", "l1", "x1"));
-  SymbolicConditional::shared_ptr l1(new SymbolicConditional("x1"));
+  SymbolicConditional::shared_ptr l1(new SymbolicConditional("l1", "x1"));
-  SymbolicConditional::shared_ptr x1(new SymbolicConditional());
+  SymbolicConditional::shared_ptr x1(new SymbolicConditional("x1"));
 
   SymbolicBayesNet expected;
-  expected.insert("x2", x2);
+  expected.insert(x2);
-  expected.insert("l1", l1);
+  expected.insert(l1);
-  expected.insert("x1", x1);
+  expected.insert(x1);
 
   // create a test graph
 	LinearFactorGraph factorGraph = createLinearFactorGraph();