2D Pose SLAM: created a new templated factor to accommodate GPS measurements, and as part of the refactor I consolidated all Pose2 SLAM classes in pose2SLAM.h. For backwards compatibility it contains

typedef pose2SLAM::Prior Pose2Prior;
	typedef pose2SLAM::Odometry Pose2Factor;
	typedef pose2SLAM::Constraint Pose2Constraint;
	typedef pose2SLAM::Config Pose2Config;
	typedef pose2SLAM::Graph Pose2Graph;

.cproject

@@ -468,7 +468,6 @@
 </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>
@@ -476,6 +475,7 @@
 </target>
 <target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testSymbolicBayesNet.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -483,7 +483,6 @@
 </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>
@@ -585,14 +584,6 @@
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
-<target name="testPose2Constraint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-<buildCommand>make</buildCommand>
-<buildArguments>-j2</buildArguments>
-<buildTarget>testPose2Constraint.run</buildTarget>
-<stopOnError>true</stopOnError>
-<useDefaultCommand>true</useDefaultCommand>
-<runAllBuilders>true</runAllBuilders>
-</target>
 <target name="testRot2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
 <buildArguments>-j2</buildArguments>
@@ -689,10 +680,10 @@
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
-<target name="testPose2Graph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<target name="testPose2SLAM.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
 <buildArguments>-j2</buildArguments>
-<buildTarget>testPose2Graph.run</buildTarget>
+<buildTarget>testPose2SLAM.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
@@ -723,7 +714,6 @@
 </target>
 <target name="testGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>

cpp/Makefile.am

@@ -188,20 +188,21 @@ testSimulated3D_LDADD = libgtsam.la
 check_PROGRAMS += testSimulated3D 
 
 # Pose SLAM headers
-headers += BetweenFactor.h LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h
+headers += BetweenFactor.h PriorFactor.h 
+headers += LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h
 
-# 2D Pose constraints
+# 2D Pose SLAM
-headers += Pose2Prior.h 
+headers +=  
-sources += Pose2Graph.cpp 
+sources += pose2SLAM.cpp 
-check_PROGRAMS += testPose2Factor testPose2Config testPose2Graph testPose2Prior
+check_PROGRAMS += testPose2Factor testPose2Config testPose2SLAM testPose2Prior
+testPose2Prior_SOURCES  = $(example) testPose2Prior.cpp
+testPose2Prior_LDADD    = libgtsam.la
 testPose2Factor_SOURCES = $(example) testPose2Factor.cpp
 testPose2Factor_LDADD   = libgtsam.la
 testPose2Config_SOURCES = $(example) testPose2Config.cpp
 testPose2Config_LDADD   = libgtsam.la
-testPose2Graph_SOURCES  = $(example) testPose2Graph.cpp
+testPose2SLAM_SOURCES  = $(example) testPose2SLAM.cpp
-testPose2Graph_LDADD    = libgtsam.la
+testPose2SLAM_LDADD    = libgtsam.la
-testPose2Prior_SOURCES  = $(example) testPose2Prior.cpp
-testPose2Prior_LDADD    = libgtsam.la
 
 # 2D SLAM using Bearing and Range
 headers +=   

cpp/Pose2Config.cpp

@@ -1,30 +0,0 @@
-/**
- * @file    Pose2Config.cpp
- * @brief   Configuration of 2D poses
- * @author  Frank Dellaert
- */
-
-#include "Pose2Config.h"
-#include "LieConfig-inl.h"
-
-using namespace std;
-
-namespace gtsam {
-
-  /** Explicit instantiation of templated methods and functions */
-  template class LieConfig<Symbol<Pose2,'x'>,Pose2>;
-  template size_t dim(const Pose2Config& c);
-  template Pose2Config expmap(const Pose2Config& c, const Vector& delta);
-  template Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta);
-
-	/* ************************************************************************* */
-	Pose2Config pose2Circle(size_t n, double R) {
-		Pose2Config x;
-		double theta = 0, dtheta = 2*M_PI/n;
-		for(size_t i=0;i<n;i++, theta+=dtheta)
-			x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
-		return x;
-	}
-
-	/* ************************************************************************* */
-} // namespace

cpp/Pose2Config.h

@@ -1,28 +0,0 @@
-/**
- * @file    Pose2Config.cpp
- * @brief   Configuration of 2D poses
- * @author  Frank Dellaert
- */
-
-#pragma once
-
-#include "Pose2.h"
-#include "LieConfig.h"
-
-namespace gtsam {
-
-  /**
-   * Pose2Config is now simply a typedef
-   */
-  typedef LieConfig<Symbol<Pose2,'x'>,Pose2> Pose2Config;
-
-  /**
-   * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
-   * @param n number of poses
-   * @param R radius of circle
-   * @param c character to use for keys
-   * @return circle of n 2D poses
-   */
-	Pose2Config pose2Circle(size_t n, double R);
-
-} // namespace

cpp/Pose2Factor.h

@@ -1,17 +0,0 @@
-/**
- *  @file  Pose2Factor.H
- *  @authors Frank Dellaert, Viorela Ila
- **/
-
-#pragma once
-
-#include "BetweenFactor.h"
-#include "Pose2.h"
-#include "Pose2Config.h"
-
-namespace gtsam {
-
-	/** This is just a typedef now */
-	typedef BetweenFactor<Pose2Config, Pose2Config::Key, Pose2> Pose2Factor;
-
-} /// namespace gtsam

cpp/Pose2Graph.cpp

@@ -1,52 +0,0 @@
-/**
- * @file    Pose2Graph.cpp
- * @brief   A factor graph for the 2D PoseSLAM problem
- * @authors Frank Dellaert, Viorela Ila
- */
-
-#include "Pose2Graph.h"
-#include "FactorGraph-inl.h"
-#include "NonlinearFactorGraph-inl.h"
-#include "NonlinearOptimizer-inl.h"
-#include "NonlinearEquality.h"
-#include "graph-inl.h"
-#include "LieConfig-inl.h"
-
-using namespace std;
-using namespace gtsam;
-
-namespace gtsam {
-
-  /** Explicit instantiation of templated methods and functions */
-  template class LieConfig<Symbol<Pose2,'x'>,Pose2>;
-  template size_t dim(const Pose2Config& c);
-  template Pose2Config expmap(const Pose2Config& c, const Vector& delta);
-  template Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta);
-
-	// explicit instantiation so all the code is there and we can link with it
-	template class FactorGraph<NonlinearFactor<Pose2Config> > ;
-	template class NonlinearFactorGraph<Pose2Config> ;
-	template class NonlinearEquality<Pose2Config,Pose2Config::Key,Pose2> ;
-	template class NonlinearOptimizer<Pose2Graph, Pose2Config>;
-
-  /* ************************************************************************* */
-  Pose2Config pose2Circle(size_t n, double R) {
-    Pose2Config x;
-    double theta = 0, dtheta = 2*M_PI/n;
-    for(size_t i=0;i<n;i++, theta+=dtheta)
-      x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
-    return x;
-  }
-
-  /* ************************************************************************* */
-	void Pose2Graph::addConstraint(const Pose2Config::Key& key, const Pose2& pose) {
-		push_back(sharedFactor(new NonlinearEquality<Pose2Config, Pose2Config::Key,
-				Pose2> (key, pose)));
-	}
-
-  /* ************************************************************************* */
-	bool Pose2Graph::equals(const Pose2Graph& p, double tol) const {
-		return false;
-	}
-
-} // namespace gtsam

cpp/Pose2Graph.h

@@ -1,74 +0,0 @@
-/**
- * @file    Pose2Graph.h
- * @brief   A factor graph for the 2D PoseSLAM problem
- * @author  Frank Dellaert
- * @author  Viorela Ila
- */
-
-#pragma once
-
-#include "NonlinearFactorGraph.h"
-#include "Pose2.h"
-#include "LieConfig.h"
-#include "BetweenFactor.h"
-#include "Key.h"
-
-namespace gtsam {
-
-  /**
-   * Pose2Config is now simply a typedef
-   */
-  typedef LieConfig<Symbol<Pose2,'x'>,Pose2> Pose2Config;
-
-  typedef BetweenFactor<Pose2Config, Pose2Config::Key, Pose2> Pose2Factor;
-
-  /**
-   * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
-   * @param n number of poses
-   * @param R radius of circle
-   * @param c character to use for keys
-   * @return circle of n 2D poses
-   */
-  Pose2Config pose2Circle(size_t n, double R);
-
-
-	/**
-	 * Non-linear factor graph for visual SLAM
-	 */
-	class Pose2Graph: public gtsam::NonlinearFactorGraph<Pose2Config> {
-
-	public:
-
-		/** default constructor is empty graph */
-		Pose2Graph() {
-		}
-
-		/**
-		 * equals
-		 */
-		bool equals(const Pose2Graph& p, double tol = 1e-9) const;
-
-		/**
-		 * Add a factor without having to do shared factor dance
-		 */
-		inline void add(const Pose2Config::Key& key1, const Pose2Config::Key& key2,
-				const Pose2& measured, const Matrix& covariance) {
-			push_back(sharedFactor(new Pose2Factor(key1, key2, measured, covariance)));
-		}
-
-		/**
-		 *  Add an equality constraint on a pose
-		 *  @param key of pose
-		 *  @param pose which pose to constrain it to
-		 */
-		void addConstraint(const Pose2Config::Key& key, const Pose2& pose =	Pose2());
-
-	private:
-		/** Serialization function */
-		friend class boost::serialization::access;
-		template<class Archive>
-		void serialize(Archive & ar, const unsigned int version) {
-		}
-	};
-
-} // namespace gtsam

cpp/PriorFactor.h

@@ -1,17 +1,12 @@
 /**
- *  @file  Pose2Prior.h
+ *  @file  PriorFactor.h
- *  @authors Michael Kaess
+ *  @authors Frank Dellaert
  **/
 #pragma once
 
-#include <map>
+#include <ostream>
 #include "NonlinearFactor.h"
-#include "GaussianFactor.h"
 #include "Pose2.h"
-#include "Matrix.h"
-#include "Key.h"
-#include "Pose2Graph.h"
-#include "ostream"
 
 namespace gtsam {
 
@@ -68,7 +63,4 @@ namespace gtsam {
 		}
 	};
 
-	/** This is just a typedef now */
-	typedef PriorFactor<Pose2Config, Pose2Config::Key, Pose2> Pose2Prior;
-
 } /// namespace gtsam

cpp/pose2SLAM.cpp

@@ -0,0 +1,50 @@
+/**
+ *  @file  pose2SLAM.cpp
+ *  @brief: bearing/range measurements in 2D plane
+ *  @authors Frank Dellaert
+ **/
+
+#include "pose2SLAM.h"
+#include "LieConfig-inl.h"
+#include "NonlinearFactorGraph-inl.h"
+#include "NonlinearOptimizer-inl.h"
+
+// Use pose2SLAM namespace for specific SLAM instance
+namespace gtsam {
+
+	using namespace pose2SLAM;
+	INSTANTIATE_LIE_CONFIG(Key, Pose2)
+	INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
+	INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)
+
+	namespace pose2SLAM {
+
+		/* ************************************************************************* */
+		Config circle(size_t n, double R) {
+			Config x;
+			double theta = 0, dtheta = 2 * M_PI / n;
+			for (size_t i = 0; i < n; i++, theta += dtheta)
+				x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
+			return x;
+		}
+
+		/* ************************************************************************* */
+		void Graph::addConstraint(const Key& i, const Pose2& p) {
+			sharedFactor factor(new Constraint(i, p));
+			push_back(factor);
+		}
+
+		void Graph::addPrior(const Key& i, const Pose2& p, const Matrix& cov) {
+			sharedFactor factor(new Prior(i, p, cov));
+			push_back(factor);
+		}
+
+		void Graph::add(const Key& i, const Key& j, const Pose2& z, const Matrix& cov) {
+			sharedFactor factor(new Odometry(i, j, z, cov));
+			push_back(factor);
+		}
+	/* ************************************************************************* */
+
+	} // pose2SLAM
+
+} // gtsam

cpp/pose2SLAM.h

@@ -0,0 +1,63 @@
+/**
+ *  @file  pose2SLAM.h
+ *  @brief: bearing/range measurements in 2D plane
+ *  @authors Frank Dellaert
+ **/
+
+#pragma once
+
+#include "Key.h"
+#include "Pose2.h"
+#include "LieConfig.h"
+#include "PriorFactor.h"
+#include "BetweenFactor.h"
+#include "NonlinearEquality.h"
+#include "NonlinearFactorGraph.h"
+#include "NonlinearOptimizer.h"
+
+namespace gtsam {
+
+	// Use pose2SLAM namespace for specific SLAM instance
+	namespace pose2SLAM {
+
+		// Keys and Config
+		typedef Symbol<Pose2, 'x'> Key;
+		typedef LieConfig<Key, Pose2> Config;
+
+		/**
+		 * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
+		 * @param n number of poses
+		 * @param R radius of circle
+		 * @param c character to use for keys
+		 * @return circle of n 2D poses
+		 */
+		Config circle(size_t n, double R);
+
+		// Factors
+		typedef PriorFactor<Config, Key, Pose2> Prior;
+		typedef BetweenFactor<Config, Key, Pose2> Odometry;
+		typedef NonlinearEquality<Config, Key, Pose2> Constraint;
+
+		// Graph
+		struct Graph: public NonlinearFactorGraph<Config> {
+			void addConstraint(const Key& i, const Pose2& p);
+			void addPrior(const Key& i, const Pose2& p, const Matrix& cov);
+			void add(const Key& i, const Key& j, const Pose2& z, const Matrix& cov);
+		};
+
+		// Optimizer
+		typedef NonlinearOptimizer<Graph, Config> Optimizer;
+
+	} // pose2SLAM
+
+	/**
+	 * Backwards compatibility
+	 */
+	typedef pose2SLAM::Prior Pose2Prior;
+	typedef pose2SLAM::Odometry Pose2Factor;
+	typedef pose2SLAM::Constraint Pose2Constraint;
+	typedef pose2SLAM::Config Pose2Config;
+	typedef pose2SLAM::Graph Pose2Graph;
+
+} // namespace gtsam
+

cpp/testGraph.cpp

@@ -12,7 +12,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include "Pose2Graph.h"
+#include "pose2SLAM.h"
 #include "LieConfig-inl.h"
 #include "graph-inl.h"
 
@@ -43,8 +43,8 @@ TEST( Graph, composePoses )
 {
 	Pose2Graph graph;
 	Matrix cov = eye(3);
-	graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor(1,2, Pose2(2.0, 0.0, 0.0), cov)));
+	graph.add(1,2, Pose2(2.0, 0.0, 0.0), cov);
-	graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor(2,3, Pose2(3.0, 0.0, 0.0), cov)));
+	graph.add(2,3, Pose2(3.0, 0.0, 0.0), cov);
 
 	PredecessorMap<Pose2Config::Key> tree;
 	tree.insert(1,2);

cpp/testIterative.cpp

@@ -12,8 +12,7 @@ using namespace boost::assign;
 #include "Ordering.h"
 #include "iterative.h"
 #include "smallExample.h"
-#include "Pose2Graph.h"
+#include "pose2SLAM.h"
-#include "Pose2Prior.h"
 
 using namespace std;
 using namespace gtsam;
@@ -101,8 +100,6 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint )
 /* ************************************************************************* */
 TEST( Iterative, conjugateGradientDescent_soft_constraint )
 {
-	typedef Pose2Config::Key Key;
-
 	Pose2Config config;
 	config.insert(1, Pose2(0.,0.,0.));
 	config.insert(2, Pose2(1.5,0.,0.));
@@ -110,8 +107,8 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint )
 	Pose2Graph graph;
 	Matrix cov = eye(3);
 	Matrix cov2 = eye(3) * 1e-10;
-	graph.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(1.,0.,0.), cov)));
+	graph.add(1,2, Pose2(1.,0.,0.), cov);
-	graph.push_back(boost::shared_ptr<Pose2Prior>(new Pose2Prior(Key(1), Pose2(0.,0.,0.), cov2)));
+	graph.addPrior(1, Pose2(0.,0.,0.), cov2);
 
 	VectorConfig zeros;
 	zeros.insert("x1",zero(3));

cpp/testOrdering.cpp

@@ -6,7 +6,7 @@
 #include <boost/assign/std/list.hpp> // for operator +=
 #include <CppUnitLite/TestHarness.h>
 #include "Ordering-inl.h"
-#include "Pose2Graph.h"
+#include "pose2SLAM.h"
 
 
 using namespace std;

cpp/testPose2Config.cpp

@@ -6,7 +6,7 @@
 #include <iostream>
 
 #include <CppUnitLite/TestHarness.h>
-#include "Pose2Graph.h"
+#include "pose2SLAM.h"
 
 using namespace std;
 using namespace gtsam;
@@ -21,7 +21,7 @@ TEST( Pose2Config, pose2Circle )
 	expected.insert(2, Pose2(-1,  0, - M_PI_2));
 	expected.insert(3, Pose2( 0, -1,   0     ));
 
-	Pose2Config actual = pose2Circle(4,1.0);
+	Pose2Config actual = pose2SLAM::circle(4,1.0);
 	CHECK(assert_equal(expected,actual));
 }
 
@@ -37,7 +37,7 @@ TEST( Pose2Config, expmap )
 
 	// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
 	Vector delta = Vector_(12, 0.0,-0.1,0.0, -0.1,0.0,0.0, 0.0,0.1,0.0, 0.1,0.0,0.0);
-	Pose2Config actual = expmap(pose2Circle(4,1.0),delta);
+	Pose2Config actual = expmap(pose2SLAM::circle(4,1.0),delta);
 	CHECK(assert_equal(expected,actual));
 }
 

cpp/testPose2Factor.cpp

@@ -6,7 +6,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 #include "numericalDerivative.h"
-#include "Pose2Graph.h"
+#include "pose2SLAM.h"
 
 using namespace std;
 using namespace gtsam;

cpp/testPose2Prior.cpp

@@ -6,7 +6,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 #include "numericalDerivative.h"
-#include "Pose2Prior.h"
+#include "pose2SLAM.h"
 
 using namespace std;
 using namespace gtsam;

cpp/testPose2SLAM.cpp

@@ -14,7 +14,7 @@ using namespace boost::assign;
 #include "NonlinearOptimizer-inl.h"
 #include "FactorGraph-inl.h"
 #include "Ordering.h"
-#include "Pose2Graph.h"
+#include "pose2SLAM.h"
 
 using namespace std;
 using namespace gtsam;
@@ -115,7 +115,7 @@ TEST(Pose2Graph, optimize) {
 TEST(Pose2Graph, optimizeCircle) {
 
 	// Create a hexagon of poses
-	Pose2Config hexagon = pose2Circle(6,1.0);
+	Pose2Config hexagon = pose2SLAM::circle(6,1.0);
   Pose2 p0 = hexagon[0], p1 = hexagon[1];
 
 	// create a Pose graph with one equality constraint and one measurement

myconfigure

@@ -1,4 +1,4 @@
 ./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/
 
 # for maximum performance on Intel Core2 platform:
-#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS="-O3 -march=nocona -mtune=generic -DNDEBUG" --disable-static
+#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS=" -g -ftree-vectorize O3 -march=nocona -mtune=generic -DNDEBUG" --disable-static