From 31fc894a4a31dafba27e52ff608a7ef06e28e334 Mon Sep 17 00:00:00 2001
From: Kai Ni <nikai97@gmail.com>
Date: Tue, 9 Feb 2010 21:32:14 +0000
Subject: [PATCH] added 4-way symbolic factor fixed a bug in the bayes tree to
 graphviz routine

---
 cpp/BayesTree-inl.h       | 10 ++++++++--
 cpp/BayesTree.h           |  2 +-
 cpp/SymbolicFactor.h      |  8 ++++++++
 cpp/SymbolicFactorGraph.h |  6 ++++++
 cpp/testBayesTree.cpp     |  3 +++
 5 files changed, 26 insertions(+), 3 deletions(-)

diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h
index db4ed8e3d..5d6f4d3e6 100644
--- a/cpp/BayesTree-inl.h
+++ b/cpp/BayesTree-inl.h
@@ -101,20 +101,24 @@ namespace gtsam {
 	template<class Conditional>
 	void BayesTree<Conditional>::saveGraph(ostream &s,
 			BayesTree<Conditional>::sharedClique clique,
-			int parentnum, int num) const {
+			int parentnum) const {
+		static int num = 0;
 		bool first = true;
 		std::stringstream out;
 		out << num;
 		string parent = out.str();
 		parent += "[label=\"";
+
 		BOOST_FOREACH(boost::shared_ptr<Conditional> c, clique->conditionals_) {
 			if(!first) parent += ","; first = false;
 			parent += (string(c->key())).c_str();
 		}
+
 		if( clique != root_){
 			parent += " : ";
 			s << parentnum << "->" << num << "\n";
 		}
+
 		first = true;
 		BOOST_FOREACH(const Symbol& sep, clique->separator_) {
 			if(!first) parent += ","; first = false;
@@ -123,8 +127,10 @@ namespace gtsam {
 		parent += "\"];\n";
 		s << parent;
 		parentnum = num;
+
 		BOOST_FOREACH(sharedClique c, clique->children_) {
-			saveGraph(s, c, parentnum, ++num);
+			num++;
+			saveGraph(s, c, parentnum);
 		}
 	}
 
diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h
index ba0f2adc2..64ea69c8b 100644
--- a/cpp/BayesTree.h
+++ b/cpp/BayesTree.h
@@ -131,7 +131,7 @@ namespace gtsam {
 		void saveGraph(const std::string& s) const;
 	private:
 		void saveGraph(std::ostream &s, sharedClique clique,
-				int parentnum = 0, int num = 0) const;
+				int parentnum = 0) const;
 	public:
 
 		/** check equality */
diff --git a/cpp/SymbolicFactor.h b/cpp/SymbolicFactor.h
index ac3cfebbf..53c01b2e0 100644
--- a/cpp/SymbolicFactor.h
+++ b/cpp/SymbolicFactor.h
@@ -56,6 +56,14 @@ namespace gtsam {
 			keys_.push_back(key3);
 		}
 
+		/** Construct 4-way factor */
+		SymbolicFactor(const Symbol& key1, const Symbol& key2, const Symbol& key3, const Symbol& key4) {
+			keys_.push_back(key1);
+			keys_.push_back(key2);
+			keys_.push_back(key3);
+			keys_.push_back(key4);
+		}
+
 		/**
 		 * Constructor that combines a set of factors
 		 * @param factors Set of factors to combine
diff --git a/cpp/SymbolicFactorGraph.h b/cpp/SymbolicFactorGraph.h
index 76f3f0e3e..1b77806a8 100644
--- a/cpp/SymbolicFactorGraph.h
+++ b/cpp/SymbolicFactorGraph.h
@@ -44,6 +44,12 @@ namespace gtsam {
 			push_back(factor);
 		}
 
+		/** Push back 4-way factor */
+		void push_factor(const Symbol& key1, const Symbol& key2, const Symbol& key3, const Symbol& key4) {
+			boost::shared_ptr<SymbolicFactor> factor(new SymbolicFactor(key1,key2,key3,key4));
+			push_back(factor);
+		}
+
 		/**
 		 * Construct from a factor graph of any type
 		 */
diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp
index 1b02dac5b..600a53173 100644
--- a/cpp/testBayesTree.cpp
+++ b/cpp/testBayesTree.cpp
@@ -121,6 +121,9 @@ TEST( BayesTree, constructor )
 
 	list<Symbol> parents2; parents2 += _L_, _E_;
 	CHECK(assert_equal(_E_,bayesTree.findParentClique(parents2, index)));
+
+	list<Symbol> parents3; parents3 += _L_, _B_;
+	CHECK(assert_equal(_L_,bayesTree.findParentClique(parents3, index)));
 }
 
 /* ************************************************************************* */