Changed names and moved two LinearFactorGraph functions to FactorGraph

Added SymbolicFactorGraph compiulation unit and unit tests
Added symbolic combine constructor

.cproject

@@ -492,6 +492,14 @@
 <useDefaultCommand>false</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </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>
+<runAllBuilders>true</runAllBuilders>
+</target>
 <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
 <buildArguments/>

cpp/ConstrainedLinearFactorGraph.cpp

@@ -107,7 +107,7 @@ ConstrainedConditionalGaussian::shared_ptr ConstrainedLinearFactorGraph::elimina
 	ConstrainedConditionalGaussian::shared_ptr ccg = constraint->eliminate(key);
 
 	// perform a change of variables on the linear factors in the separator
-	vector<LinearFactor::shared_ptr> separator = find_factors_and_remove(key);
+	vector<LinearFactor::shared_ptr> separator = findAndRemoveFactors(key);
 	BOOST_FOREACH(LinearFactor::shared_ptr factor, separator) {
 		// store the block matrices
 		map<string, Matrix> blocks;

cpp/FactorGraph-inl.h

@@ -171,13 +171,13 @@ list<int> FactorGraph<Factor>::factors(const string& key) const {
 /* ************************************************************************* */
 template<class Factor>
 vector<boost::shared_ptr<Factor> >
-FactorGraph<Factor>::find_factors_and_remove(const string& key) {
+FactorGraph<Factor>::findAndRemoveFactors(const string& key) {
 	vector<boost::shared_ptr<Factor> > found;
 
 	Indices::iterator it = indices_.find(key);
 	if (it == indices_.end())
 		throw(std::invalid_argument
-				("FactorGraph::find_factors_and_remove invalid key: " + key));
+				("FactorGraph::findAndRemoveFactors invalid key: " + key));
 
 	list<int> *indices_ptr; // pointer to indices list in indices_ map
 	indices_ptr = &(it->second);
@@ -190,5 +190,18 @@ FactorGraph<Factor>::find_factors_and_remove(const string& key) {
 	return found;
 }
 
+/* ************************************************************************* */
+/* find factors and remove them from the factor graph: O(n)                  */
+/* ************************************************************************* */
+template<class Factor>
+boost::shared_ptr<Factor>
+FactorGraph<Factor>::removeAndCombineFactors(const string& key)
+{
+	typedef typename boost::shared_ptr<Factor> shared_factor;
+	vector<shared_factor> found = findAndRemoveFactors(key);
+	shared_factor new_factor(new Factor(found));
+	return new_factor;
+}
+
 /* ************************************************************************* */
 }

cpp/FactorGraph.h

@@ -93,7 +93,14 @@ namespace gtsam {
      * from the factor graph
      * @param key the key for the given node
      */
-		std::vector<shared_factor> find_factors_and_remove(const std::string& key);
+		std::vector<shared_factor> findAndRemoveFactors(const std::string& key);
+
+    /**
+     * extract and combine all the factors that involve a given node
+     * @param key the key for the given node
+     * @return the combined linear factor
+     */
+    shared_factor removeAndCombineFactors(const std::string& key);
 
 	private:
 

cpp/LinearFactorGraph.cpp

@@ -13,6 +13,7 @@
 #include <colamd/colamd.h>
 
 #include "ChordalBayesNet.h"
+#include "FactorGraph-inl.h"
 #include "LinearFactorGraph.h"
 
 using namespace std;
@@ -47,17 +48,6 @@ set<string> LinearFactorGraph::find_separator(const string& key) const
 	return separator;
 }
 
-/* ************************************************************************* */
-/* find factors and remove them from the factor graph: O(n)                  */
-/* ************************************************************************* */ 
-boost::shared_ptr<LinearFactor>
-LinearFactorGraph::combine_factors(const string& key)
-{
-	vector<LinearFactor::shared_ptr> found = find_factors_and_remove(key);
-	boost::shared_ptr<LinearFactor> lf(new LinearFactor(found));
-	return lf;
-}
-
 /* ************************************************************************* */
 /* eliminate one node from the linear factor graph                           */ 
 /* ************************************************************************* */
@@ -65,7 +55,7 @@ ConditionalGaussian::shared_ptr LinearFactorGraph::eliminate_one(const string& k
 {
 	// combine the factors of all nodes connected to the variable to be eliminated
 	// if no factors are connected to key, returns an empty factor
-	boost::shared_ptr<LinearFactor> joint_factor = combine_factors(key);
+	boost::shared_ptr<LinearFactor> joint_factor = removeAndCombineFactors(key);
 
 	// eliminate that joint factor
 	try {

cpp/LinearFactorGraph.h

@@ -67,17 +67,6 @@ namespace gtsam {
      */
     std::set<std::string> find_separator(const std::string& key) const;
 
-    /**
-     * extract and combine all the factors that involve a given node
-     * NOTE: the combined factor will be depends on a system-dependent
-	 * ordering of the input set of factors.  Do not rely on this order
-	 * when using the function.
-     * @param key the key for the given node
-     * @return the combined linear factor
-     */
-    boost::shared_ptr<LinearFactor>
-      combine_factors(const std::string& key);
-
     /**
      * eliminate one node yielding a ConditionalGaussian
      * Eliminates the factors from the factor graph through find_factors_and_remove

cpp/Makefile.am

@@ -81,15 +81,16 @@ timeLinearFactor: CXXFLAGS += -I /opt/local/include
 timeLinearFactor: LDFLAGS += -L.libs -lgtsam
 
 # graphs 
-sources += LinearFactorGraph.cpp 
+sources += SymbolicFactorGraph.cpp LinearFactorGraph.cpp 
-sources += SymbolicBayesChain.cpp 
+sources += SymbolicBayesChain.cpp ChordalBayesNet.cpp 
-sources += ChordalBayesNet.cpp 
 sources += ConstrainedNonlinearFactorGraph.cpp ConstrainedLinearFactorGraph.cpp 
-check_PROGRAMS += testFactorgraph testLinearFactorGraph testNonlinearFactorGraph 
+check_PROGRAMS += testFactorgraph testSymbolicFactorGraph
+check_PROGRAMS += testLinearFactorGraph testNonlinearFactorGraph 
 check_PROGRAMS += testChordalBayesNet testNonlinearOptimizer
 check_PROGRAMS += testSymbolicBayesChain testBayesTree  
 check_PROGRAMS += testConstrainedNonlinearFactorGraph testConstrainedLinearFactorGraph
 testFactorgraph_SOURCES          = testFactorgraph.cpp
+testSymbolicFactorGraph_SOURCES  = $(example) testSymbolicFactorGraph.cpp
 testLinearFactorGraph_SOURCES    = $(example) testLinearFactorGraph.cpp
 testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp
 testNonlinearOptimizer_SOURCES   = $(example) testNonlinearOptimizer.cpp
@@ -100,6 +101,7 @@ testConstrainedNonlinearFactorGraph_SOURCES = $(example) testConstrainedNonlinea
 testConstrainedLinearFactorGraph_SOURCES    = $(example) testConstrainedLinearFactorGraph.cpp
 
 testFactorgraph_LDADD          = libgtsam.la
+testSymbolicFactorGraph_LDADD  = libgtsam.la
 testLinearFactorGraph_LDADD    = libgtsam.la
 testNonlinearFactorGraph_LDADD = libgtsam.la
 testNonlinearOptimizer_LDADD   = libgtsam.la

cpp/SymbolicFactorGraph.cpp

@@ -0,0 +1,47 @@
+/*
+ * SymbolicFactorGraph.cpp
+ *
+ *  Created on: Oct 29, 2009
+ *      Author: Frank Dellaert
+ */
+
+#include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
+#include "SymbolicFactorGraph.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_;
+	}
+	/* ************************************************************************* */
+
+}

cpp/SymbolicFactorGraph.h

@@ -0,0 +1,76 @@
+/*
+ * SymbolicFactorGraph.h
+ *
+ *  Created on: Oct 29, 2009
+ *      Author: Frank Dellaert
+ */
+
+#ifndef SYMBOLICFACTORGRAPH_H_
+#define SYMBOLICFACTORGRAPH_H_
+
+#include <string>
+#include <list>
+#include "FactorGraph.h"
+
+namespace gtsam {
+
+	/** 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_;
+		}
+	};
+
+	/** Symbolic Factor Graph */
+	class SymbolicFactorGraph: public FactorGraph<SymbolicFactor> {
+	public:
+
+		SymbolicFactorGraph() {
+		}
+
+		template<class Factor>
+		SymbolicFactorGraph(const FactorGraph<Factor>& fg) {
+			for (size_t i = 0; i < fg.size(); i++) {
+				boost::shared_ptr<Factor> f = fg[i];
+				std::list<std::string> keys = f->keys();
+				SymbolicFactor::shared_ptr factor(new SymbolicFactor(keys));
+				push_back(factor);
+			}
+		}
+
+	};
+
+}
+
+#endif /* SYMBOLICFACTORGRAPH_H_ */

cpp/testConstrainedNonlinearFactorGraph.cpp

@@ -7,6 +7,7 @@
 
 
 #include <CppUnitLite/TestHarness.h>
+#include "FactorGraph-inl.h"
 #include "ConstrainedNonlinearFactorGraph.h"
 #include "smallExample.h"
 

cpp/testLinearFactorGraph.cpp

@@ -63,20 +63,13 @@ TEST( LinearFactorGraph, find_separator )
 }
 
 /* ************************************************************************* */
-// Note: This test fails on different systems due to a dependency on the ordering
-//       of LinearFactor::shared_ptr in STL sets.  Because the ordering is based
-//       on pointer values instead of factor values, the ordering is different
-//       between separate systems.
 TEST( LinearFactorGraph, combine_factors_x1 )
 {	
   // create a small example for a linear factor graph
   LinearFactorGraph fg = createLinearFactorGraph();
 
   // combine all factors
-  LinearFactor::shared_ptr actual = fg.combine_factors("x1");
+  LinearFactor::shared_ptr actual = fg.removeAndCombineFactors("x1");
-
-  //FIXME: this expected value must be constructed in the order that
-  //       the factors are stored in a set - which differs between systems
 
   // the expected linear factor
   Matrix Al1 = Matrix_(6,2,
@@ -128,10 +121,7 @@ TEST( LinearFactorGraph, combine_factors_x2 )
   LinearFactorGraph fg = createLinearFactorGraph();
 
   // combine all factors
-  LinearFactor::shared_ptr actual = fg.combine_factors("x2");
+  LinearFactor::shared_ptr actual = fg.removeAndCombineFactors("x2");
-
-  // FIXME: change the ordering of the expected to match whatever the real
-  //        ordering is and constructed the expected with the correct order
 
   // the expected linear factor
   Matrix Al1 = Matrix_(4,2,
@@ -476,7 +466,7 @@ TEST( LinearFactorGraph, factor_lookup)
 }
 
 /* ************************************************************************* */
-TEST( LinearFactorGraph, find_factors_and_remove )
+TEST( LinearFactorGraph, findAndRemoveFactors )
 {
 	// create the graph
 	LinearFactorGraph fg = createLinearFactorGraph();
@@ -487,7 +477,7 @@ TEST( LinearFactorGraph, find_factors_and_remove )
   LinearFactor::shared_ptr f2 = fg[2];
 
   // call the function
-  vector<LinearFactor::shared_ptr> factors = fg.find_factors_and_remove("x1");
+  vector<LinearFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
 
   // Check the factors
   CHECK(f0==factors[0]);
@@ -499,7 +489,7 @@ TEST( LinearFactorGraph, find_factors_and_remove )
   }
 
 /* ************************************************************************* */
-TEST( LinearFactorGraph, find_factors_and_remove__twice )
+TEST( LinearFactorGraph, findAndRemoveFactors_twice )
 {
 	// create the graph
 	LinearFactorGraph fg = createLinearFactorGraph();
@@ -510,14 +500,14 @@ TEST( LinearFactorGraph, find_factors_and_remove__twice )
   LinearFactor::shared_ptr f2 = fg[2];
 
   // call the function
-  vector<LinearFactor::shared_ptr> factors = fg.find_factors_and_remove("x1");
+  vector<LinearFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
 
   // Check the factors
   CHECK(f0==factors[0]);
   CHECK(f1==factors[1]);
   CHECK(f2==factors[2]);
 
-  factors = fg.find_factors_and_remove("x1");
+  factors = fg.findAndRemoveFactors("x1");
   CHECK(factors.size() == 0);
 
   // CHECK if the factors are deleted from the factor graph

cpp/testSymbolicBayesChain.cpp

@@ -11,135 +11,9 @@
 #include "BayesChain-inl.h"
 #include "SymbolicBayesChain-inl.h"
 
-namespace gtsam {
-
-	/** Symbolic Factor */
-	class SymbolicFactor: public Testable<SymbolicFactor> {
-	private:
-
-		std::list<std::string> keys_;
-
-	public:
-
-		SymbolicFactor(std::list<std::string> keys) :
-			keys_(keys) {
-		}
-
-		typedef boost::shared_ptr<SymbolicFactor> shared_ptr;
-
-		/** print */
-		void print(const std::string& s = "SymbolicFactor") const {
-			cout << s << " ";
-			BOOST_FOREACH(string key, keys_) cout << key << " ";
-			cout << endl;
-		}
-
-		/** check equality */
-		bool equals(const SymbolicFactor& other, double tol = 1e-9) const {
-			return keys_ == other.keys_;
-		}
-
-		/**
-		 * Find all variables
-		 * @return The set of all variable keys
-		 */
-		std::list<std::string> keys() const {
-			return keys_;
-		}
-	};
-
-	/** Symbolic Factor Graph */
-	class SymbolicFactorGraph: public FactorGraph<SymbolicFactor> {
-	public:
-
-		SymbolicFactorGraph() {}
-
-		template<class Factor>
-		SymbolicFactorGraph(const FactorGraph<Factor>& fg) {
-			for (size_t i = 0; i < fg.size(); i++) {
-				boost::shared_ptr<Factor> f = fg[i];
-				std::list<std::string> keys = f->keys();
-				SymbolicFactor::shared_ptr factor(new SymbolicFactor(keys));
-				push_back(factor);
-			}
-		}
-
-	};
-
-}
-
 using namespace std;
 using namespace gtsam;
 
-/* ************************************************************************* */
-TEST( SymbolicFactorGraph, symbolicFactorGraph )
-{
-	// construct expected symbolic graph
-	SymbolicFactorGraph expected;
-
-	list<string> f1_keys; f1_keys.push_back("x1");
-	SymbolicFactor::shared_ptr f1(new SymbolicFactor(f1_keys));
-	expected.push_back(f1);
-
-	list<string> f2_keys; f2_keys.push_back("x1"); f2_keys.push_back("x2");
-	SymbolicFactor::shared_ptr f2(new SymbolicFactor(f2_keys));
-	expected.push_back(f2);
-
-	list<string> f3_keys; f3_keys.push_back("l1"); f3_keys.push_back("x1");
-	SymbolicFactor::shared_ptr f3(new SymbolicFactor(f3_keys));
-	expected.push_back(f3);
-
-	list<string> f4_keys; f4_keys.push_back("l1"); f4_keys.push_back("x2");
-	SymbolicFactor::shared_ptr f4(new SymbolicFactor(f4_keys));
-	expected.push_back(f4);
-
-	// construct it from the factor graph graph
-	LinearFactorGraph factorGraph = createLinearFactorGraph();
-	SymbolicFactorGraph actual(factorGraph);
-
-	CHECK(assert_equal(expected, actual));
-}
-
-/* ************************************************************************* */
-TEST( SymbolicFactorGraph, find_factors_and_remove )
-{
-	// construct it from the factor graph graph
-	LinearFactorGraph factorGraph = createLinearFactorGraph();
-	SymbolicFactorGraph actual(factorGraph);
-  SymbolicFactor::shared_ptr f1 = actual[0];
-  SymbolicFactor::shared_ptr f3 = actual[2];
-	actual.find_factors_and_remove("x2");
-
-	// construct expected graph after find_factors_and_remove
-	SymbolicFactorGraph expected;
-	SymbolicFactor::shared_ptr null;
-	expected.push_back(f1);
-	expected.push_back(null);
-	expected.push_back(f3);
-	expected.push_back(null);
-
-	CHECK(assert_equal(expected, actual));
-}
-/* ************************************************************************* */
-TEST( SymbolicFactorGraph, factor_lookup)
-{
-	// create a test graph
-	LinearFactorGraph factorGraph = createLinearFactorGraph();
-	SymbolicFactorGraph fg(factorGraph);
-
-	// ask for all factor indices connected to x1
-	list<int> x1_factors = fg.factors("x1");
-	int x1_indices[] = { 0, 1, 2 };
-	list<int> x1_expected(x1_indices, x1_indices + 3);
-	CHECK(x1_factors==x1_expected);
-
-	// ask for all factor indices connected to x2
-	list<int> x2_factors = fg.factors("x2");
-	int x2_indices[] = { 1, 3 };
-	list<int> x2_expected(x2_indices, x2_indices + 2);
-	CHECK(x2_factors==x2_expected);
-}
-
 /* ************************************************************************* */
 TEST( SymbolicBayesChain, constructor )
 {

cpp/testSymbolicFactorGraph.cpp

@@ -0,0 +1,107 @@
+/**
+ * @file    testSymbolicBayesChain.cpp
+ * @brief   Unit tests for a symbolic Bayes chain
+ * @author  Frank Dellaert
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include "smallExample.h"
+#include "FactorGraph-inl.h"
+#include "SymbolicFactorGraph.h"
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST( SymbolicFactorGraph, symbolicFactorGraph )
+{
+	// construct expected symbolic graph
+	SymbolicFactorGraph expected;
+
+	list<string> f1_keys; f1_keys.push_back("x1");
+	SymbolicFactor::shared_ptr f1(new SymbolicFactor(f1_keys));
+	expected.push_back(f1);
+
+	list<string> f2_keys; f2_keys.push_back("x1"); f2_keys.push_back("x2");
+	SymbolicFactor::shared_ptr f2(new SymbolicFactor(f2_keys));
+	expected.push_back(f2);
+
+	list<string> f3_keys; f3_keys.push_back("l1"); f3_keys.push_back("x1");
+	SymbolicFactor::shared_ptr f3(new SymbolicFactor(f3_keys));
+	expected.push_back(f3);
+
+	list<string> f4_keys; f4_keys.push_back("l1"); f4_keys.push_back("x2");
+	SymbolicFactor::shared_ptr f4(new SymbolicFactor(f4_keys));
+	expected.push_back(f4);
+
+	// construct it from the factor graph graph
+	LinearFactorGraph factorGraph = createLinearFactorGraph();
+	SymbolicFactorGraph actual(factorGraph);
+
+	CHECK(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST( SymbolicFactorGraph, findAndRemoveFactors )
+{
+	// construct it from the factor graph graph
+	LinearFactorGraph factorGraph = createLinearFactorGraph();
+	SymbolicFactorGraph actual(factorGraph);
+  SymbolicFactor::shared_ptr f1 = actual[0];
+  SymbolicFactor::shared_ptr f3 = actual[2];
+	actual.findAndRemoveFactors("x2");
+
+	// construct expected graph after find_factors_and_remove
+	SymbolicFactorGraph expected;
+	SymbolicFactor::shared_ptr null;
+	expected.push_back(f1);
+	expected.push_back(null);
+	expected.push_back(f3);
+	expected.push_back(null);
+
+	CHECK(assert_equal(expected, actual));
+}
+/* ************************************************************************* */
+TEST( SymbolicFactorGraph, factors)
+{
+	// create a test graph
+	LinearFactorGraph factorGraph = createLinearFactorGraph();
+	SymbolicFactorGraph fg(factorGraph);
+
+	// ask for all factor indices connected to x1
+	list<int> x1_factors = fg.factors("x1");
+	int x1_indices[] = { 0, 1, 2 };
+	list<int> x1_expected(x1_indices, x1_indices + 3);
+	CHECK(x1_factors==x1_expected);
+
+	// ask for all factor indices connected to x2
+	list<int> x2_factors = fg.factors("x2");
+	int x2_indices[] = { 1, 3 };
+	list<int> x2_expected(x2_indices, x2_indices + 2);
+	CHECK(x2_factors==x2_expected);
+}
+
+/* ************************************************************************* */
+TEST( LinearFactorGraph, removeAndCombineFactors )
+{
+	// create a test graph
+	LinearFactorGraph factorGraph = createLinearFactorGraph();
+	SymbolicFactorGraph fg(factorGraph);
+
+  // combine all factors connected to x1
+  SymbolicFactor::shared_ptr actual = fg.removeAndCombineFactors("x1");
+
+	list<string> keys; keys.push_back("l1"); keys.push_back("x1"); keys.push_back("x2");
+  SymbolicFactor expected(keys);
+
+  // check if the two factors are the same
+  CHECK(assert_equal(expected,*actual));
+}
+
+/* ************************************************************************* */
+int main() {
+	TestResult tr;
+	return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */