Factor Graph serialization, and renaming of factors -> factors_

.cproject

@@ -300,6 +300,7 @@
 <buildTargets>
 <target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -307,7 +308,6 @@
 </target>
 <target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments></buildArguments>
 <buildTarget>testSimpleCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -315,7 +315,6 @@
 </target>
 <target name="testCal3_S2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments></buildArguments>
 <buildTarget>testCal3_S2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -323,6 +322,7 @@
 </target>
 <target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testVSLAMFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -330,7 +330,6 @@
 </target>
 <target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments></buildArguments>
 <buildTarget>testCalibratedCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -338,6 +337,7 @@
 </target>
 <target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testConditionalGaussian.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -345,14 +345,22 @@
 </target>
 <target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments></buildArguments>
 <buildTarget>testPose2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
+<target name="testFGConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
+<buildTarget>testFGConfig.run</buildTarget>
+<stopOnError>true</stopOnError>
+<useDefaultCommand>true</useDefaultCommand>
+<runAllBuilders>true</runAllBuilders>
+</target>
 <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>install</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -360,6 +368,7 @@
 </target>
 <target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>clean</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -367,6 +376,7 @@
 </target>
 <target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>

cpp/ConstrainedLinearFactorGraph.cpp

@@ -91,14 +91,14 @@ DeltaFunction::shared_ptr ConstrainedLinearFactorGraph::eliminate_one_eq(const s
 
 	// remove all unary linear factors on this node
 	vector<LinearFactor::shared_ptr> newfactors;
-	BOOST_FOREACH(LinearFactor::shared_ptr f, factors)
+	BOOST_FOREACH(LinearFactor::shared_ptr f, factors_)
 	{
 		if (f->size() != 1 || !f->involves(key))
 		{
 			newfactors.push_back(f);
 		}
 	}
-	factors = newfactors;
+	factors_ = newfactors;
 
 	// combine the linear factors connected to equality node
 	boost::shared_ptr<MutableLinearFactor> joint_factor = combine_factors(key);
@@ -146,7 +146,7 @@ FGConfig ConstrainedLinearFactorGraph::optimize(const Ordering& ordering){
 void ConstrainedLinearFactorGraph::print(const std::string& s) const
 {
 	cout << "ConstrainedFactorGraph: " << s << endl;
-	BOOST_FOREACH(LinearFactor::shared_ptr f, factors)
+	BOOST_FOREACH(LinearFactor::shared_ptr f, factors_)
 	{
 		f->print();
 	}
@@ -208,7 +208,7 @@ Ordering ConstrainedLinearFactorGraph::getOrdering() const
 LinearFactorGraph ConstrainedLinearFactorGraph::convert() const
 {
 	LinearFactorGraph ret;
-	BOOST_FOREACH(LinearFactor::shared_ptr f, factors)
+	BOOST_FOREACH(LinearFactor::shared_ptr f, factors_)
 	{
 		ret.push_back(f);
 	}

cpp/ConstrainedLinearFactorGraph.h

@@ -56,15 +56,14 @@ public:
       return eq_factors.end();
     }
 
-    /** clear the factor graph - reimplemented to include equality factors */
+    /** clear the factor graph - re-implemented to include equality factors */
     void clear(){
-      factors.clear();
+      factors_.clear();
-      node_to_factors_.clear();
       eq_factors.clear();
     }
 
-    /** size - reimplemented to include the equality factors */
+    /** size - reimplemented to include the equality factors_ */
-    inline size_t size() const { return factors.size() + eq_factors.size(); }
+    inline size_t size() const { return factors_.size() + eq_factors.size(); }
 
     /** Check equality - checks equality constraints as well*/
     bool equals(const LinearFactorGraph& fg, double tol=1e-9) const;

cpp/ConstrainedNonlinearFactorGraph.cpp

@@ -28,7 +28,7 @@ ConstrainedLinearFactorGraph ConstrainedNonlinearFactorGraph::linearize(const FG
 	ConstrainedLinearFactorGraph ret;
 
 	// linearize all nonlinear factors
-	for(const_iterator factor=factors.begin(); factor<factors.end(); factor++){
+	for(const_iterator factor=factors_.begin(); factor<factors_.end(); factor++){
 		LinearFactor::shared_ptr lf = (*factor)->linearize(config);
 		ret.push_back(lf);
 	}
@@ -45,10 +45,9 @@ ConstrainedLinearFactorGraph ConstrainedNonlinearFactorGraph::linearize(const FG
 NonlinearFactorGraph ConstrainedNonlinearFactorGraph::convert() const
 {
 	NonlinearFactorGraph ret;
-	BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> f, factors)
+	BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> f, factors_)
-	{
 		ret.push_back(f);
-	}
+
 	return ret;
 }
 

cpp/FactorGraph.h

@@ -9,104 +9,106 @@
 
 #pragma once
 
-#include <list>
-#include <map>
-#include <vector>
 #include <boost/shared_ptr.hpp>
+#include <boost/serialization/vector.hpp>
+#include <boost/serialization/shared_ptr.hpp>
+
 #include "Factor.h"
 #include "FGConfig.h"
 
 namespace gtsam {
 
-  /**
+	/**
-   * A factor graph is a bipartite graph with factor nodes connected to variable nodes.
+	 * A factor graph is a bipartite graph with factor nodes connected to variable nodes.
-   * In this class, however, only factor nodes are kept around.
+	 * In this class, however, only factor nodes are kept around.
-   */
+	 */
-  template<class T> class FactorGraph
+	template<class T> class FactorGraph {
-  {
+	public:
-  public:
+		typedef typename boost::shared_ptr<T> shared_factor;
-    typedef typename boost::shared_ptr<T> shared_factor;
+		typedef typename std::vector<shared_factor>::iterator iterator;
-    typedef typename std::vector<shared_factor>::iterator iterator;
+		typedef typename std::vector<shared_factor>::const_iterator const_iterator;
-    typedef typename std::vector<shared_factor>::const_iterator const_iterator;
 
-  protected:
+	protected:
-    /** Collection of factors */
+		/** Collection of factors */
-    std::vector<shared_factor> factors;
+		std::vector<shared_factor> factors_;
-    std::map<std::string, std::list<int> > node_to_factors_;
 
-  public:
+		/** Serialization function */
+		friend class boost::serialization::access;
+		template<class Archive>
+		void serialize(Archive & ar, const unsigned int version) {
+			ar & BOOST_SERIALIZATION_NVP(factors_);
+		}
 
-    /** get the factors to a specific node */
+	public:
-      const std::list<int>& get_factors_to_node(const std::string& key) const {
-      return node_to_factors_[key];
-    }
 
-    /** STL like, return the iterator pointing to the first factor */
+		/** STL like, return the iterator pointing to the first factor */
-    const_iterator begin() const {
+		const_iterator begin() const {
-      return factors.begin();
+			return factors_.begin();
-    }
+		}
 
-    /** STL like, return the iterator pointing to the last factor */
+		/** STL like, return the iterator pointing to the last factor */
-    const_iterator end() const {
+		const_iterator end() const {
-      return factors.end();
+			return factors_.end();
-    }
+		}
 
-    /** clear the factor graph */
+		/** clear the factor graph */
-    void clear(){
+		void clear() {
-      factors.clear();
+			factors_.clear();
-      node_to_factors_.clear();
+		}
-    }
 
-    /** Get a specific factor by index */
+		/** Get a specific factor by index */
-    shared_factor operator[](size_t i) const {
+		shared_factor operator[](size_t i) const {
-      return factors[i];
+			return factors_[i];
-    }
+		}
 
-    /** return the numbers of the factors in the factor graph */
+		/** return the numbers of the factors_ in the factor graph */
-    inline size_t size() const { return factors.size(); }
+		inline size_t size() const {
+			return factors_.size();
+		}
 
-    /** Add a factor */
+		/** Add a factor */
-    void push_back(shared_factor ptr_f) {factors.push_back(ptr_f);}
+		void push_back(shared_factor ptr_f) {
+			factors_.push_back(ptr_f);
+		}
 
-    /** unnormalized error */
+		/** unnormalized error */
-    double error(const FGConfig& c) const {
+		double error(const FGConfig& c) const {
-      double total_error = 0.;
+			double total_error = 0.;
-      /** iterate over all the factors to accumulate the log probabilities */
+			/** iterate over all the factors_ to accumulate the log probabilities */
-      for(const_iterator factor=factors.begin(); factor!=factors.end(); factor++)
+			for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++)
-        total_error += (*factor)->error(c);
+				total_error += (*factor)->error(c);
 
-      return total_error;
+			return total_error;
-    }
+		}
 
-    /** Unnormalized probability. O(n) */
+		/** Unnormalized probability. O(n) */
-    double probPrime(const FGConfig& c) const {
+		double probPrime(const FGConfig& c) const {
-      return exp(-0.5*error(c));
+			return exp(-0.5 * error(c));
-    }  
+		}
 
-    /** print out graph */
+		/** print out graph */
-    void print(const std::string& s = "FactorGraph") const{
+		void print(const std::string& s = "FactorGraph") const {
-      std::cout << s << std::endl;
+			std::cout << s << std::endl;
-      printf("size: %d\n", (int)size());
+			printf("size: %d\n", (int) size());
-      for(const_iterator factor=factors.begin(); factor!=factors.end(); factor++)
+			for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++)
-        (*factor)->print();
+				(*factor)->print();
-    }
+		}
 
-    /** Check equality */
+		/** Check equality */
-    bool equals(const FactorGraph& fg, double tol=1e-9) const
+		bool equals(const FactorGraph& fg, double tol = 1e-9) const {
-    {
+			/** check whether the two factor graphs have the same number of factors_ */
-      /** check whether the two factor graphs have the same number of factors */
+			if (factors_.size() != fg.size()) goto fail;
-      if( factors.size() != fg.size() ) goto fail;
 
-      /** check whether the factors are the same */
+			/** check whether the factors_ are the same */
-      for(size_t i=0;i<factors.size();i++)
+			for (size_t i = 0; i < factors_.size(); i++)
-        if ( ! factors[i]->equals(*fg.factors[i], tol) ) goto fail; //TODO: Doesn't this force order of factor insertion?
+				// TODO: Doesn't this force order of factor insertion?
-      return true;
+				if (!factors_[i]->equals(*fg.factors_[i], tol)) goto fail;
+			return true;
 
-    fail:
+			fail: print();
-      print();
+			fg.print();
-      fg.print();
+			return false;
-      return false;
+		}
-    }
+	};
-  };
 
 } // namespace gtsam

cpp/LinearFactorGraph.cpp

@@ -40,8 +40,8 @@ void LinearFactorGraph::setCBN(const ChordalBayesNet& CBN)
 set<string> LinearFactorGraph::find_separator(const string& key) const
 {
 	set<string> separator;
-	BOOST_FOREACH(shared_factor factor,factors)
+	BOOST_FOREACH(shared_factor factor,factors_)
-	factor->tally_separator(key,separator);
+		factor->tally_separator(key,separator);
 
 	return separator;
 }
@@ -54,10 +54,10 @@ LinearFactorGraph::find_factors_and_remove(const string& key)
 {
 	LinearFactorSet found;
 
-	for(iterator factor=factors.begin(); factor!=factors.end(); )
+	for(iterator factor=factors_.begin(); factor!=factors_.end(); )
 		if ((*factor)->involves(key)) {
 			found.insert(*factor);
-			factor = factors.erase(factor);
+			factor = factors_.erase(factor);
 		} else {
 			factor++; // important, erase will have effect of ++
 		}
@@ -130,7 +130,7 @@ LinearFactorGraph::eliminate(const Ordering& ordering)
 	// after eliminate, only one zero indegree factor should remain
 	// TODO: this check needs to exist - verify that unit tests work when this check is in place
 	/*
-  if (factors.size() != 1) {
+  if (factors_.size() != 1) {
     print();
     throw(invalid_argument("LinearFactorGraph::eliminate: graph not empty after eliminate, ordering incomplete?"));
   }
@@ -156,7 +156,7 @@ FGConfig LinearFactorGraph::optimize(const Ordering& ordering)
 /** combine two factor graphs                                                 */ 
 /* ************************************************************************* */
 void LinearFactorGraph::combine(const LinearFactorGraph &lfg){
-	for(const_iterator factor=lfg.factors.begin(); factor!=lfg.factors.end(); factor++){
+	for(const_iterator factor=lfg.factors_.begin(); factor!=lfg.factors_.end(); factor++){
 		push_back(*factor);
 	}
 }
@@ -170,9 +170,9 @@ LinearFactorGraph LinearFactorGraph::combine2(const LinearFactorGraph& lfg1,
 	// create new linear factor graph equal to the first one
 	LinearFactorGraph fg = lfg1;
 
-	// add the second factors in the graph
+	// add the second factors_ in the graph
-	for (const_iterator factor = lfg2.factors.begin(); factor
+	for (const_iterator factor = lfg2.factors_.begin(); factor
-			!= lfg2.factors.end(); factor++) {
+			!= lfg2.factors_.end(); factor++) {
 		fg.push_back(*factor);
 	}
 
@@ -184,7 +184,7 @@ LinearFactorGraph LinearFactorGraph::combine2(const LinearFactorGraph& lfg1,
 // find all variables and their dimensions
 VariableSet LinearFactorGraph::variables() const {
 	VariableSet result;
-	BOOST_FOREACH(shared_factor factor,factors) {
+	BOOST_FOREACH(shared_factor factor,factors_) {
 		VariableSet vs = factor->variables();
 		BOOST_FOREACH(Variable v,vs) result.insert(v);
 	}
@@ -217,7 +217,7 @@ pair<Matrix,Vector> LinearFactorGraph::matrix(const Ordering& ordering) const {
 
 	// get all factors
 	LinearFactorSet found;
-	BOOST_FOREACH(shared_factor factor,factors)
+	BOOST_FOREACH(shared_factor factor,factors_)
 	found.insert(factor);
 
 	// combine them

cpp/NonlinearFactor.h

@@ -15,6 +15,7 @@
 #include <list>
 
 #include <boost/shared_ptr.hpp>
+#include <boost/serialization/list.hpp>
 
 #include "Factor.h"
 #include "Matrix.h"
@@ -37,6 +38,18 @@ namespace gtsam {
    */
   class NonlinearFactor : public Factor
   {
+  private:
+
+  	/** Serialization function */
+  	friend class boost::serialization::access;
+  	template<class Archive>
+  	void serialize(Archive & ar, const unsigned int version) {
+//  		ar & boost::serialization::base_object<Factor>(*this); // TODO: needed ?
+  		ar & BOOST_SERIALIZATION_NVP(z_);
+  		ar & BOOST_SERIALIZATION_NVP(sigma_);
+  		ar & BOOST_SERIALIZATION_NVP(keys_);
+  	}
+
   protected:
 
     Vector z_;     // measurement
@@ -45,9 +58,12 @@ namespace gtsam {
 		
   public:
 
+    /** Default constructor, with easily identifiable bogus values */
+    NonlinearFactor():z_(Vector_(2,888.0,999.0)),sigma_(0.1234567) {}
+
     /** Constructor */
-    NonlinearFactor(const Vector& z,	     // the measurement
+    NonlinearFactor(const Vector& z, // the measurement
-		    const double sigma);     // the variance
+		    const double sigma);         // the variance
 
     /** Vector of errors */
     virtual Vector error_vector(const FGConfig& c) const = 0;

cpp/NonlinearFactorGraph.cpp

@@ -27,7 +27,7 @@ LinearFactorGraph NonlinearFactorGraph::linearize(const FGConfig& config) const
 	LinearFactorGraph linearFG;
 
 	// linearize all factors
-	for(const_iterator factor=factors.begin(); factor<factors.end(); factor++){
+	for(const_iterator factor=factors_.begin(); factor<factors_.end(); factor++){
 		LinearFactor::shared_ptr lf = (*factor)->linearize(config);
 		linearFG.push_back(lf);
 	}
@@ -70,7 +70,7 @@ bool check_convergence (double relativeErrorTreshold,
 FGConfig NonlinearFactorGraph::iterate
 (const FGConfig& config, const Ordering& ordering) const
 {
-	LinearFactorGraph linear = linearize(config);   // linearize all factors
+	LinearFactorGraph linear = linearize(config);   // linearize all factors_
 	return linear.optimize(ordering);
 }
 

cpp/NonlinearFactorGraph.h

@@ -10,17 +10,27 @@
 
 #pragma once
 
-#include "LinearFactorGraph.h"
+#include <boost/serialization/base_object.hpp>
+#include <colamd/colamd.h>
 #include "FactorGraph.h"
 #include "NonlinearFactor.h"
+#include "LinearFactorGraph.h"
 #include "ChordalBayesNet.h"
-#include <colamd/colamd.h>
 
 namespace gtsam {
 
 /** Factor Graph Constsiting of non-linear factors */
 class NonlinearFactorGraph : public FactorGraph<NonlinearFactor>
 {
+private:
+
+	/** Serialization function */
+	friend class boost::serialization::access;
+	template<class Archive>
+	void serialize(Archive & ar, const unsigned int version) {
+		ar & boost::serialization::base_object< FactorGraph<NonlinearFactor> >(*this);
+	}
+
 public: // internal, exposed for testing only, doc in .cpp file
 
 	FGConfig iterate(const FGConfig& config, const Ordering& ordering) const;