Fixed christian01 example, polluting gtsam with extra header files for MATLAB. Need to look for a better solution.

cpp/Makefile.am

@@ -178,6 +178,9 @@ testLieConfig_LDADD = libgtsam.la
 testTupleConfig_LDADD = libgtsam.la
 
 # simulated2D example
+headers += Simulated2DConfig.h 
+headers += Simulated2DPosePrior.h Simulated2DPointPrior.h 
+headers += Simulated2DOdometry.h Simulated2DMeasurement.h
 sources += simulated2D.cpp 
 testSimulated2D_SOURCES = testSimulated2D.cpp
 testSimulated2D_LDADD = libgtsam.la

cpp/Simulated2DConfig.h

@@ -0,0 +1,48 @@
+/*
+ * Simulated2DConfig.h
+ *
+ * Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * Author: Frank Dellaert
+ */
+
+#pragma once
+
+#include "simulated2D.h"
+
+namespace gtsam {
+
+	class Simulated2DConfig: public simulated2D::Config {
+	public:
+		typedef boost::shared_ptr<Point2> sharedPoint;
+
+		Simulated2DConfig() {
+		}
+
+		void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
+			insert(i, p);
+		}
+
+		void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
+			insert(j, p);
+		}
+
+		int nrPoses() const {
+			return this->first_.size();
+		}
+
+		int nrPoints() const {
+			return this->second_.size();
+		}
+
+		sharedPoint pose(const simulated2D::PoseKey& i) {
+			return sharedPoint(new Point2((*this)[i]));
+		}
+
+		sharedPoint point(const simulated2D::PointKey& j) {
+			return sharedPoint(new Point2((*this)[j]));
+		}
+
+	};
+
+} // namespace gtsam
+

cpp/Simulated2DMeasurement.h

@@ -0,0 +1,18 @@
+/*
+ * Simulated2DMeasurement.h
+ *
+ * Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * Author: Frank Dellaert
+ */
+
+#pragma once
+
+#include "simulated2D.h"
+#include "Simulated2DConfig.h"
+
+namespace gtsam {
+
+	typedef simulated2D::Measurement Simulated2DMeasurement;
+
+}
+

cpp/Simulated2DOdometry.h

@@ -0,0 +1,18 @@
+/*
+ * Simulated2DOdometry.h
+ *
+ * Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * Author: Frank Dellaert
+ */
+
+#pragma once
+
+#include "simulated2D.h"
+#include "Simulated2DConfig.h"
+
+namespace gtsam {
+
+	typedef simulated2D::Odometry Simulated2DOdometry;
+
+}
+

cpp/Simulated2DPointPrior.h

@@ -0,0 +1,19 @@
+/*
+ * Simulated2DPointPrior.h
+ *
+ * Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * Author: Frank Dellaert
+ */
+
+#pragma once
+
+#include "simulated2D.h"
+#include "Simulated2DConfig.h"
+
+namespace gtsam {
+
+	/** Create a prior on a landmark Point2 with key 'l1' etc... */
+	typedef simulated2D::GenericPrior<Simulated2DConfig, simulated2D::PointKey> Simulated2DPointPrior;
+
+}
+

cpp/Simulated2DPosePrior.h

@@ -0,0 +1,19 @@
+/*
+ * Simulated2DPosePrior.h
+ *
+ * Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * Author: Frank Dellaert
+ */
+
+#pragma once
+
+#include "simulated2D.h"
+#include "Simulated2DConfig.h"
+
+namespace gtsam {
+
+	/** Create a prior on a pose Point2 with key 'x1' etc... */
+	typedef simulated2D::GenericPrior<Simulated2DConfig, simulated2D::PoseKey> Simulated2DPosePrior;
+
+}
+

cpp/gtsam-broken.h

@@ -2,36 +2,6 @@
 // Solve by parsing a namespace pose2SLAM::Config and making a Pose2SLAMConfig class
 // We also have to solve the shared pointer mess to avoid duplicate methods
 
-class Point2Prior {
-  Point2Prior(Vector mu, double sigma, string key);
-  Vector error_vector(const VectorConfig& c) const;
-  GaussianFactor* linearize(const VectorConfig& c) const;
-  double sigma();
-  Vector measurement();
-  double error(const VectorConfig& c) const;
-  void print(string s) const;
-};
-
-class Simulated2DOdometry {
-  Simulated2DOdometry(Vector odo, double sigma, string key, string key2);
-  Vector error_vector(const VectorConfig& c) const;
-  GaussianFactor* linearize(const VectorConfig& c) const;
-  double sigma();
-  Vector measurement();
-  double error(const VectorConfig& c) const;
-  void print(string s) const;
-};
-
-class Simulated2DMeasurement {
-  Simulated2DMeasurement(Vector odo, double sigma, string key, string key2);
-  Vector error_vector(const VectorConfig& c) const;
-  GaussianFactor* linearize(const VectorConfig& c) const;
-  double sigma();
-  Vector measurement();
-  double error(const VectorConfig& c) const;
-  void print(string s) const;
-};
-
 class Pose2Config{
 	Pose2Config();
 	Pose2 get(string key) const;

cpp/gtsam.h

@@ -1,18 +1,6 @@
-class Pose2SLAMOptimizer {
-	Pose2SLAMOptimizer(string dataset_name);
-	void print(string s) const;
-	void update(Vector x) const;
-	Vector optimize() const;
-	double error() const;
-	Matrix a1() const;
-	Matrix a2() const;
-	Vector b1() const;
-	Vector b2() const;
-};
-
 class SharedGaussian {
-		SharedGaussian(Matrix covariance);
+	SharedGaussian(Matrix covariance);
-		SharedGaussian(Vector sigmas);
+	SharedGaussian(Vector sigmas);
 };
 
 class SharedDiagonal {
@@ -23,9 +11,9 @@ class Ordering {
   Ordering();
   Ordering(string key);
   Ordering subtract(const Ordering& keys) const;
-  void push_back(string s);
   void print(string s) const;
   bool equals(const Ordering& ord, double tol) const;
+  void push_back(string s);
   void unique ();
   void reverse ();
 };
@@ -37,13 +25,12 @@ class SymbolicFactor{
 
 class VectorConfig {
   VectorConfig();
+  void print(string s) const;
+  bool equals(const VectorConfig& expected, double tol) const;
+  void insert(string name, Vector val);
   Vector get(string name) const;
   bool contains(string name) const;
   size_t size() const;
-  void insert(string name, Vector val);
-  void print(string s) const;
-  bool equals(const VectorConfig& expected, double tol) const;
-  void clear();
 };
 
 class GaussianFactor {
@@ -65,13 +52,13 @@ class GaussianFactor {
 	       Matrix A3,
 	       Vector b_in,
 	       const SharedDiagonal& model);
+  void print(string s) const;
+  bool equals(const GaussianFactor& lf, double tol) const;
   bool empty() const;
   Vector get_b() const;
   Matrix get_A(string key) const;
   double error(const VectorConfig& c) const;
   bool involves(string key) const;
-  void print(string s) const;
-  bool equals(const GaussianFactor& lf, double tol) const;
   pair<Matrix,Vector> matrix(const Ordering& ordering) const;
   pair<GaussianConditional*,GaussianFactor*> eliminate(string key) const;
 };
@@ -102,9 +89,9 @@ class GaussianConditional {
 		      Matrix T,
 		      Vector sigmas);
   void print(string s) const;
-  Vector solve(const VectorConfig& x);
-  void add(string key, Matrix S);
   bool equals(const GaussianConditional &cg, double tol) const;
+  void add(string key, Matrix S);
+  Vector solve(const VectorConfig& x);
 };
 
 class GaussianBayesNet {
@@ -117,13 +104,13 @@ class GaussianBayesNet {
 
 class GaussianFactorGraph {
   GaussianFactorGraph();
+  void print(string s) const;
+  bool equals(const GaussianFactorGraph& lfgraph, double tol) const;
 
   size_t size() const;
   void push_back(GaussianFactor* ptr_f);
   double error(const VectorConfig& c) const;
   double probPrime(const VectorConfig& c) const;
-  void print(string s) const;
-  bool equals(const GaussianFactorGraph& lfgraph, double tol) const;
   void combine(const GaussianFactorGraph& lfg);
 
   GaussianConditional* eliminateOne(string key);
@@ -138,20 +125,20 @@ class GaussianFactorGraph {
 class Point2 {
   Point2();
   Point2(double x, double y);
+  void print(string s) const;
   double x();
   double y();
-  void print(string s) const;
 };
 
 class Point3 {
   Point3();
   Point3(double x, double y, double z);
   Point3(Vector v);
+  void print(string s) const;
   Vector vector() const;
   double x();
   double y();
   double z();
-  void print(string s) const;
 }; 
 
 class Pose2 {
@@ -171,3 +158,55 @@ class Pose2 {
   Point2 t() const;
   Rot2 r() const;
 };
+
+class Simulated2DConfig {
+	Simulated2DConfig();
+  void print(string s) const;
+	void insertPose(int i, const Point2& p);
+	void insertPoint(int j, const Point2& p);
+	int nrPoses() const;
+	int nrPoints() const;
+	Point2* pose(int i);
+	Point2* point(int j);
+};
+
+class Simulated2DPosePrior {
+	Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i);
+  void print(string s) const;
+	GaussianFactor* linearize(const Simulated2DConfig& config) const;
+  double error(const Simulated2DConfig& c) const;
+};
+
+class Simulated2DPointPrior {
+	Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i);
+  void print(string s) const;
+	GaussianFactor* linearize(const Simulated2DConfig& config) const;
+  double error(const Simulated2DConfig& c) const;
+};
+
+class Simulated2DOdometry {
+  Simulated2DOdometry(Point2& mu, const SharedDiagonal& model, int i1, int i2);
+  void print(string s) const;
+	GaussianFactor* linearize(const Simulated2DConfig& config) const;
+  double error(const Simulated2DConfig& c) const;
+};
+
+class Simulated2DMeasurement {
+  Simulated2DMeasurement(Point2& mu, const SharedDiagonal& model, int i, int j);
+  void print(string s) const;
+	GaussianFactor* linearize(const Simulated2DConfig& config) const;
+  double error(const Simulated2DConfig& c) const;
+};
+
+class Pose2SLAMOptimizer {
+	Pose2SLAMOptimizer(string dataset_name);
+	void print(string s) const;
+	void update(Vector x) const;
+	Vector optimize() const;
+	double error() const;
+	Matrix a1() const;
+	Matrix a2() const;
+	Vector b1() const;
+	Vector b2() const;
+};
+

cpp/simulated2D.h

@@ -52,7 +52,7 @@ namespace gtsam {
 		/**
 		 * Unary factor encoding a soft prior on a vector
 		 */
-		template<class Cfg=Config, class Key=PoseKey>
+		template<class Cfg = Config, class Key = PoseKey>
 		struct GenericPrior: public NonlinearFactor1<Cfg, Key, Point2> {
 
 			Point2 z_;
@@ -71,15 +71,14 @@ namespace gtsam {
 		/**
 		 * Binary factor simulating "odometry" between two Vectors
 		 */
-		template<class Cfg=Config, class Key=PoseKey>
+		template<class Cfg = Config, class Key = PoseKey>
 		struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Point2, Key,
 				Point2> {
 			Point2 z_;
 
-			GenericOdometry(const Point2& z, const SharedGaussian& model, const Key& j1,
+			GenericOdometry(const Point2& z, const SharedGaussian& model,
-					const Key& j2) :
+					const Key& i1, const Key& i2) :
-				z_(z), NonlinearFactor2<Cfg, Key, Point2, Key, Point2> (
+				z_(z), NonlinearFactor2<Cfg, Key, Point2, Key, Point2> (model, i1, i2) {
-						model, j1, j2) {
 			}
 
 			Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional<
@@ -92,22 +91,22 @@ namespace gtsam {
 		/**
 		 * Binary factor simulating "measurement" between two Vectors
 		 */
-		template<class Cfg=Config, class XKey=PoseKey, class LKey=PointKey>
+		template<class Cfg = Config, class XKey = PoseKey, class LKey = PointKey>
-		class GenericMeasurement: public NonlinearFactor2<Cfg, XKey, Point2, LKey, Point2> {
+		class GenericMeasurement: public NonlinearFactor2<Cfg, XKey, Point2, LKey,
-			public:
+				Point2> {
+		public:
 
-				Point2 z_;
+			Point2 z_;
 
-				GenericMeasurement(const Point2& z, const SharedGaussian& model,
+			GenericMeasurement(const Point2& z, const SharedGaussian& model,
-						const XKey& j1, const LKey& j2) :
+					const XKey& i, const LKey& j) :
-					z_(z), NonlinearFactor2<Cfg, XKey, Point2, LKey, Point2> (
+				z_(z), NonlinearFactor2<Cfg, XKey, Point2, LKey, Point2> (model, i, j) {
-							model, j1, j2) {
+			}
-				}
 
-				Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional<
+			Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional<
-						Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+					Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
-					return (mea(x1, x2, H1, H2) - z_).vector();
+				return (mea(x1, x2, H1, H2) - z_).vector();
-				}
+			}
 
 		};
 

cpp/testSimulated2D.cpp

@@ -10,11 +10,21 @@
 
 #include "numericalDerivative.h"
 #include "simulated2D.h"
+#include "Simulated2DConfig.h"
 
 using namespace gtsam;
 using namespace std;
 using namespace simulated2D;
 
+/* ************************************************************************* */
+TEST( simulated2D, Simulated2DConfig )
+{
+	Simulated2DConfig actual;
+	actual.insertPose(1,Point2(1,1));
+	actual.insertPoint(2,Point2(2,2));
+  CHECK(assert_equal(actual,actual,1e-9));
+}
+
 /* ************************************************************************* */
 TEST( simulated2D, Dprior )
 {

matlab/example/christian01.m

@@ -0,0 +1,58 @@
+% Set up a small SLAM example
+% Christian Potthast, Frank Dellaert
+
+clear;
+close all;
+
+n = 100;
+m = 20;
+
+% have the robot move in this world
+trajectory = random_walk([0.1,0.1],5,m);
+plot(trajectory(1,:),trajectory(2,:),'b+');
+axis([0 100 0 100]);axis square;
+
+% Set up the map 
+mappingArea=max(trajectory,[],2);
+map = create_random_landmarks(n, mappingArea);
+figure(1);clf;
+plot(map(1,:), map(2,:),'g.'); hold on;
+axis([0 mappingArea(1) 0 mappingArea(2)]);axis square;
+
+% Check visibility and plot this on the problem figure
+visibility = create_visibility(map, trajectory,10);
+gplot(visibility,[map trajectory]');
+
+% simulate the measurements
+measurement_sigma = 1;
+odo_sigma = 0.1;
+[measurements, odometry] = simulate_measurements(map, trajectory, visibility, measurement_sigma, odo_sigma);
+
+% create a configuration of all zeroes
+config = create_config(n,m);
+
+% create the factor graph
+factorGraph = create_gaussian_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n);
+
+% create an ordering
+ord = create_ordering(n,m);
+
+% show the matrix
+figure(2); clf;
+A = factorGraph.matrix(ord);
+spy(A);
+
+% optimizing a BayesNet is not possible from MATLAB as
+% GaussianBayesNet is a typedef not a real class :-(
+% BayesNet = factorGraph.eliminate_(ord);
+% optimal = BayesNet.optimize;
+
+% However, we can call the optimize_ method of a GaussianFactorGraph
+optimal = factorGraph.optimize_(ord);
+
+% plot the solution
+figure(3);clf; 
+plot_config(optimal,n,m);hold on
+plot(trajectory(1,:),trajectory(2,:),'b+');
+plot(map(1,:), map(2,:),'g.');
+axis([0 mappingArea(1) 0 mappingArea(2)]);axis square;

matlab/example/create_config.m

@@ -3,12 +3,13 @@
 
 function config = create_config(n,m)
 
-config = VectorConfig();
+config = Simulated2DConfig();
-
+origin = Point2;
-for j = 1:n
-    config.insert(sprintf('l%d',j), [0;0]);
-end
 
 for i = 1:m
-    config.insert(sprintf('x%d',i), [0;0]); 
+    config.insertPose(i, origin); 
+end
+
+for j = 1:n
+    config.insertPoint(j, origin);
 end

matlab/example/create_gaussian_factor_graph.m

@@ -8,35 +8,39 @@ m = size(measurements,2);
 % create linear factor graph
 lfg = GaussianFactorGraph();
 
+% Point2 at origin
+origin = Point2;
+
 % create prior for initial robot pose
-prior = Point2Prior([0;0],0.2,'x1');
+model0_2 = SharedDiagonal([0.2;0.2]);
+prior = Simulated2DPosePrior(origin,model0_2,1);
 lf = prior.linearize(config);
 lfg.push_back(lf);
 
 % add prior for landmarks
+model1000 = SharedDiagonal([1000;1000]);
 for j = 1:n
-    key = sprintf('l%d',j);
+    prior = Simulated2DPointPrior(origin,model1000,j);
-    prior = Point2Prior([0;0],1000,key);
     lf = prior.linearize(config); 
     lfg.push_back(lf);
 end
 
-% add measurement factors
+% add odometry factors
-for k = 1 : size(measurements,2) 
+odo_model = SharedDiagonal([odo_sigma;odo_sigma]);
-    measurement = measurements{k};
+for i = 2 : size(odometry,2)
-    i = sprintf('x%d',measurement.i);
+    odo = Point2(odometry{i}(1),odometry{i}(2));
-    j = sprintf('l%d',measurement.j); 
+    nlf = Simulated2DOdometry(odo, odo_model, i-1, i);
-    nlf = Simulated2DMeasurement(measurement.z, measurement_sigma, i, j);
     lf = nlf.linearize(config);
     lfg.push_back(lf);
 end
 
-% add odometry factors
+% add measurement factors
-for i = 2 : size(odometry,2)
+measurement_model = SharedDiagonal([measurement_sigma;measurement_sigma]);
-    odo = odometry{i};
+for k = 1 : size(measurements,2) 
-    p = sprintf('x%d',i-1);
+    measurement = measurements{k};
-    c = sprintf('x%d',i);
+    point = Point2(measurement.z(1),measurement.z(2));
-    nlf = Simulated2DOdometry(odo, odo_sigma, p, c);
+    nlf = Simulated2DMeasurement(point, measurement_model, measurement.i, measurement.j);
     lf = nlf.linearize(config);
     lfg.push_back(lf);
-end
+end
+

matlab/example/create_linear_factor_graph.m

@@ -1,42 +0,0 @@
-% Christan Potthast
-% create linear factor graph
-
-function lfg = create_linear_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n)
-
-m = size(measurements,2);
-
-% create linear factor graph
-lfg = GaussianFactorGraph();
-
-% create prior for initial robot pose
-prior = Point2Prior([0;0],0.2,'x1');
-lf = prior.linearize(config);
-lfg.push_back(lf);
-
-% add prior for landmarks
-for j = 1:n
-    key = sprintf('l%d',j);
-    prior = Point2Prior([0;0],1000,key);
-    lf = prior.linearize(config); 
-    lfg.push_back(lf);
-end
-
-% add measurement factors
-for k = 1 : size(measurements,2) 
-    measurement = measurements{k};
-    i = sprintf('x%d',measurement.i);
-    j = sprintf('l%d',measurement.j); 
-    nlf = Simulated2DMeasurement(measurement.z, measurement_sigma, i, j);
-    lf = nlf.linearize(config);
-    lfg.push_back(lf);
-end
-
-% add odometry factors
-for i = 2 : size(odometry,2)
-    odo = odometry{i};
-    p = sprintf('x%d',i-1);
-    c = sprintf('x%d',i);
-    nlf = Simulated2DOdometry(odo, odo_sigma, p, c);
-    lf = nlf.linearize(config);
-    lfg.push_back(lf);
-end

matlab/example/plot_config.m

@@ -0,0 +1,19 @@
+% Christian Potthast
+% plot a configuration 
+
+function plot_config(config,n,m)
+
+hold on;
+
+for j = 1:n
+    key = sprintf('l%d',j);
+    mj = config.get(key);
+    plot(mj(1), mj(2),'r*');
+end
+
+for i = 1:m
+    key = sprintf('x%d',i);
+    xi = config.get(key);
+    plot(xi(1), xi(2),'rx');
+end
+