added 4-way symbolic factor

fixed a bug in the bayes tree to graphviz routine

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);
 		}
 	}
 

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 */

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

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
 		 */

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)));
 }
 
 /* ************************************************************************* */