wrapping PlanarSLAMExample_easy

.cproject

@@ -769,6 +769,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="tests/testSimulated2D.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testSimulated2D.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1720,10 +1728,10 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="PlanarSLAMExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="PlanarSLAMExample_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>PlanarSLAMExample.run</buildTarget>
+				<buildTarget>PlanarSLAMExample_easy.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>

examples/PlanarSLAMExample_easy.cpp

@@ -86,8 +86,10 @@ int main(int argc, char** argv) {
 	initialEstimate.print("initial estimate");
 
 	// optimize using Levenberg-Marquardt optimization with an ordering from colamd
-	Values result = optimize<Graph, Values>(graph, initialEstimate);
+//	Values result = optimize<Graph, Values>(graph, initialEstimate);
-	result.print("final result");
+//	result.print("final result");
+	boost::shared_ptr<Values> result = graph.optimize(initialEstimate);
+	result->print("final result");
 
 	return 0;
 }

examples/matlab/PlanarSLAMExample_easy.m

@@ -0,0 +1,75 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+% Atlanta, Georgia 30332-0415
+% All Rights Reserved
+% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+% 
+% See LICENSE for the license information
+%
+% @brief Simple robotics example using the pre-built planar SLAM domain
+% @author Alex Cunningham
+% @author Frank Dellaert
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% Assumptions
+%  - All values are axis aligned
+%  - Robot poses are facing along the X axis (horizontal, to the right in images)
+%  - We have bearing and range information for measurements
+%  - We have full odometry for measurements
+%  - The robot and landmarks are on a grid, moving 2 meters each step
+%  - Landmarks are 2 meters away from the robot trajectory
+
+%% Create keys for variables
+x1 = 1; x2 = 2; x3 = 3;
+l1 = 1; l2 = 2;
+
+%% Create graph container and add factors to it
+graph = PlanarSLAMGraph;
+
+%% Add prior
+% gaussian for prior
+prior_model = SharedDiagonal([0.3; 0.3; 0.1]);
+prior_measurement = Pose2(0.0, 0.0, 0.0); % prior at origin
+graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph
+
+%% Add odometry
+% general noisemodel for odometry
+odom_model = SharedDiagonal([0.2; 0.2; 0.1]);
+odom_measurement = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
+graph.addOdometry(x1, x2, odom_measurement, odom_model);
+graph.addOdometry(x2, x3, odom_measurement, odom_model);
+
+%% Add measurements
+% general noisemodel for measurements
+meas_model = SharedDiagonal([0.1; 0.2]);
+
+% create the measurement values - indices are (pose id, landmark id)
+degrees = pi/180;
+bearing11 = Rot2(45*degrees);
+bearing21 = Rot2(90*degrees);
+bearing32 = Rot2(90*degrees);
+range11 = sqrt(4+4);
+range21 = 2.0;
+range32 = 2.0;
+
+% create bearing/range factors and add them
+graph.addBearingRange(x1, l1, bearing11, range11, meas_model);
+graph.addBearingRange(x2, l1, bearing21, range21, meas_model);
+graph.addBearingRange(x3, l2, bearing32, range32, meas_model);
+
+% print
+graph.print('full graph');
+
+%% Initialize to noisy points
+initialEstimate = PlanarSLAMValues;
+initialEstimate.insertPose(x1, Pose2(0.5, 0.0, 0.2));
+initialEstimate.insertPose(x2, Pose2(2.3, 0.1,-0.2));
+initialEstimate.insertPose(x3, Pose2(4.1, 0.1, 0.1));
+initialEstimate.insertPoint(l1, Landmark2(1.8, 2.1));
+initialEstimate.insertPoint(l2, Landmark2(4.1, 1.8));
+
+initialEstimate.print('initial estimate');
+
+%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
+result = graph.optimize(initialEstimate);
+result.print('final result');

gtsam.h

@@ -19,6 +19,11 @@ class Point3 {
 
 class Rot2 {
 	Rot2();
+	Rot2(double theta);
+	void print(string s) const;
+	bool equals(const Rot2& pose, double tol) const;
+	double c() const;
+	double s() const;
 };
 
 class Pose2 {
@@ -106,3 +111,40 @@ class GaussianFactorGraph {
 	void combine(const GaussianFactorGraph& lfg);
 };
 
+class Landmark2 {
+	Landmark2();
+	Landmark2(double x, double y);
+	void print(string s) const;
+	double x();
+	double y();
+};
+
+class Ordering{
+  void print(string s) const;
+  bool equals(const Ordering& ord, double tol) const;
+};
+
+class PlanarSLAMValues {
+	PlanarSLAMValues();
+	void print(string s) const;
+	void insertPose(int key, const Pose2& pose);
+	void insertPoint(int key, const Point2& point);
+};
+
+class PlanarSLAMGraph {
+	PlanarSLAMGraph();
+	double error(const PlanarSLAMValues& c) const;
+	Ordering* orderingCOLAMD(const PlanarSLAMValues& config) const;
+	void print(string s) const;
+	void addPrior(size_t i, const Pose2& p, const SharedNoiseModel& model);
+	void addPoseConstraint(size_t i, const Pose2& p);
+	void addOdometry(size_t i, size_t j, const Pose2& z,
+			const SharedNoiseModel& model);
+	void addBearing(size_t i, size_t j, const Rot2& z,
+			const SharedNoiseModel& model);
+	void addRange(size_t i, size_t j, double z, const SharedNoiseModel& model);
+	void addBearingRange(size_t i, size_t j, const Rot2& z1, double z2,
+			const SharedNoiseModel& model);
+	PlanarSLAMValues* optimize(const PlanarSLAMValues& initialEstimate);
+};
+

gtsam/geometry/Pose2.cpp

@@ -14,10 +14,12 @@
  * @brief 2D Pose
  */
 
-#include <boost/foreach.hpp>
+#include <gtsam/geometry/concepts.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/base/Lie-inl.h>
-#include <gtsam/geometry/concepts.h>
+#include <gtsam/base/Testable.h>
+#include <boost/foreach.hpp>
+#include <cmath>
 
 using namespace std;
 
@@ -55,7 +57,7 @@ namespace gtsam {
 	  assert(xi.size() == 3);
 		Point2 v(xi(0),xi(1));
 		double w = xi(2);
-		if (fabs(w) < 1e-10)
+		if (std::abs(w) < 1e-10)
 			return Pose2(xi[0], xi[1], xi[2]);
 		else {
 			Rot2 R(Rot2::fromAngle(w));
@@ -70,7 +72,7 @@ namespace gtsam {
   	const Rot2& R = p.r();
   	const Point2& t = p.t();
 		double w = R.theta();
-		if (fabs(w) < 1e-10)
+		if (std::abs(w) < 1e-10)
 			return Vector_(3, t.x(), t.y(), w);
 		else {
 			double c_1 = R.c()-1.0, s = R.s();
@@ -173,13 +175,13 @@ namespace gtsam {
 	  const Rot2& R1 = r_, R2 = p2.r();
 	  double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
 
+	  // Assert that R1 and R2 are normalized
+	  assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
+
 	  // Calculate delta rotation = between(R1,R2)
 	  double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
 	  Rot2 R(Rot2::atan2(s,c)); // normalizes
 	  
-	  // Assert that R1 and R2 are normalized
-	  assert(fabs(c1*c1 + s1*s1 - 1.0) < 1e-5 && fabs(c2*c2 + s2*s2 - 1.0) < 1e-5);
-
 	  // Calculate delta translation = unrotate(R1, dt);
 	  Point2 dt = p2.t() - t_;
 	  double x = dt.x(), y = dt.y();
@@ -228,16 +230,16 @@ namespace gtsam {
 		  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 	  if (!H1 && !H2)	return transform_to(point).norm();
 	  Point2 d = transform_to(point, H1, H2);
-	  double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
+	  double x = d.x(), y = d.y(), d2 = x * x + y * y, r = sqrt(d2);
 	  Matrix D_result_d;
-	  if(fabs(n) > 1e-10)
+	  if(std::abs(r) > 1e-10)
-	    D_result_d = Matrix_(1, 2, x / n, y / n);
+	    D_result_d = Matrix_(1, 2, x / r, y / r);
 	  else {
 	    D_result_d = Matrix_(1,2, 1.0, 1.0);
 	  }
 	  if (H1) *H1 = D_result_d * (*H1);
 	  if (H2) *H2 = D_result_d * (*H2);
-	  return n;
+	  return r;
   }
 
   /* ************************************************************************* */

gtsam/geometry/Rot2.h

@@ -23,170 +23,200 @@
 
 namespace gtsam {
 
-  /**
-   * Rotation matrix
-   * NOTE: the angle theta is in radians unless explicitly stated
-   * @ingroup geometry
-   */
-  class Rot2: public Lie<Rot2> {
-
-  public:
-	  /** get the dimension by the type */
-	  static const size_t dimension =  1;
-
-  private:
-
-    /** we store cos(theta) and sin(theta) */
-    double c_, s_;
-
-    /** private constructor from cos/sin */
-    inline Rot2(double c, double s) : c_(c), s_(s) {}
-
-    /** normalize to make sure cos and sin form unit vector */
-    Rot2& normalize();
-
-  public:
-
-    /** default constructor, zero rotation */
-    Rot2() : c_(1.0), s_(0.0) {}
-
-    /** "named constructors" */
-
-    /** Named constructor from angle == exponential map at identity  - theta is in radians*/
-    static Rot2 fromAngle(double theta) {
-		return Rot2(cos(theta), sin(theta));
-	}
-
-    /** Named constructor from angle in degrees */
-    static Rot2 fromDegrees(double theta) {
-    	const double degree = M_PI / 180;
-    	return fromAngle(theta * degree);
-    }
-
-    /** Named constructor from cos(theta),sin(theta) pair, will *not* normalize! */
-    static Rot2 fromCosSin(double c, double s);
-
 	/**
-	 * Named constructor with derivative
+	 * Rotation matrix
-	 * Calculate relative bearing to a landmark in local coordinate frame
+	 * NOTE: the angle theta is in radians unless explicitly stated
-	 * @param point 2D location of landmark
+	 * @ingroup geometry
-	 * @param H optional reference for Jacobian
-	 * @return 2D rotation \in SO(2)
 	 */
-	static Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H=boost::none);
+	class Rot2: public Lie<Rot2> {
 
-	/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
+	public:
-  	static Rot2 atan2(double y, double x);
+		/** get the dimension by the type */
+		static const size_t dimension = 1;
 
-  	/** return angle (RADIANS) */
+	private:
-    double theta() const { return ::atan2(s_,c_); }
 
-    /** return angle (DEGREES) */
+		/** we store cos(theta) and sin(theta) */
-    double degrees() const {
+		double c_, s_;
-    	const double degree = M_PI / 180;
-    	return theta() / degree;
-    }
 
-    /** return cos */
+		/** private constructor from cos/sin */
-    inline double c() const { return c_; }
+		inline Rot2(double c, double s) :
+				c_(c), s_(s) {
+		}
 
-    /** return sin */
+		/** normalize to make sure cos and sin form unit vector */
-    inline double s() const { return s_; }
+		Rot2& normalize();
 
-    /** print */
+	public:
-    void print(const std::string& s = "theta") const;
 
-    /** equals with an tolerance */
+		/** default constructor, zero rotation */
-    bool equals(const Rot2& R, double tol = 1e-9) const;
+		Rot2() :
+				c_(1.0), s_(0.0) {
+		}
 
-    /** dimension of the variable - used to autodetect sizes */
+		/// Constructor from angle in radians == exponential map at identity
-    inline static size_t Dim() { return dimension; }
+		Rot2(double theta) :
+				c_(cos(theta)), s_(sin(theta)) {
+		}
 
-    /** Lie requirements */
+		/** "named constructors" */
 
-    /** Dimensionality of the tangent space */
+		/// Named constructor from angle in radians
-    inline size_t dim() const { return dimension; }
+		static Rot2 fromAngle(double theta) {
+			return Rot2(theta);
+		}
 
-    /** Compose - make a new rotation by adding angles */
+		/// Named constructor from angle in degrees
-    inline Rot2 compose(const Rot2& R1,
+		static Rot2 fromDegrees(double theta) {
-        boost::optional<Matrix&> H1=boost::none,
+			const double degree = M_PI / 180;
-        boost::optional<Matrix&> H2=boost::none) const {
+			return fromAngle(theta * degree);
-      if(H1) *H1 = eye(1);
+		}
-      if(H2) *H2 = eye(1);
-      return *this * R1;
-    }
 
-    /** Expmap around identity - create a rotation from an angle */
+		/// Named constructor from cos(theta),sin(theta) pair, will *not* normalize!
-    static Rot2 Expmap(const Vector& v) {
+		static Rot2 fromCosSin(double c, double s);
-    	if (zero(v)) return (Rot2());
-    	else return Rot2::fromAngle(v(0));
-    }
 
-    /** Logmap around identity - return the angle of the rotation */
+		/**
-    static inline Vector Logmap(const Rot2& r) {
+		 * Named constructor with derivative
-      return Vector_(1, r.theta());
+		 * Calculate relative bearing to a landmark in local coordinate frame
-    }
+		 * @param point 2D location of landmark
+		 * @param H optional reference for Jacobian
+		 * @return 2D rotation \in SO(2)
+		 */
+		static Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H =
+				boost::none);
 
-    /** Binary expmap  */
+		/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
-    inline Rot2 expmap(const Vector& v) const { return *this * Expmap(v); }
+		static Rot2 atan2(double y, double x);
 
-    /** Binary Logmap  */
+		/** return angle (RADIANS) */
-    inline Vector logmap(const Rot2& p2) const { return Logmap(between(p2));}
+		double theta() const {
+			return ::atan2(s_, c_);
+		}
 
-    /** Between using the default implementation */
+		/** return angle (DEGREES) */
-    inline Rot2 between(const Rot2& p2,
+		double degrees() const {
-        boost::optional<Matrix&> H1=boost::none,
+			const double degree = M_PI / 180;
-        boost::optional<Matrix&> H2=boost::none) const {
+			return theta() / degree;
-      if(H1) *H1 = -eye(1);
+		}
-      if(H2) *H2 = eye(1);
-      return between_default(*this, p2);
-    }
 
-    /** return 2*2 rotation matrix */
+		/** return cos */
-    Matrix matrix() const;
+		inline double c() const {
+			return c_;
+		}
 
-    /** return 2*2 transpose (inverse) rotation matrix   */
+		/** return sin */
-    Matrix transpose() const;
+		inline double s() const {
+			return s_;
+		}
 
-    /** The inverse rotation - negative angle */
+		/** print */
-    Rot2 inverse() const { return Rot2(c_, -s_);}
+		void print(const std::string& s = "theta") const;
 
-    /** Compose - make a new rotation by adding angles */
+		/** equals with an tolerance */
-    Rot2 operator*(const Rot2& R) const {
+		bool equals(const Rot2& R, double tol = 1e-9) const;
+
+		/** dimension of the variable - used to autodetect sizes */
+		inline static size_t Dim() {
+			return dimension;
+		}
+
+		/** Lie requirements */
+
+		/** Dimensionality of the tangent space */
+		inline size_t dim() const {
+			return dimension;
+		}
+
+		/** Compose - make a new rotation by adding angles */
+		inline Rot2 compose(const Rot2& R1, boost::optional<Matrix&> H1 =
+				boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+			if (H1) *H1 = eye(1);
+			if (H2) *H2 = eye(1);
+			return *this * R1;
+		}
+
+		/** Expmap around identity - create a rotation from an angle */
+		static Rot2 Expmap(const Vector& v) {
+			if (zero(v))
+				return (Rot2());
+			else
+				return Rot2::fromAngle(v(0));
+		}
+
+		/** Logmap around identity - return the angle of the rotation */
+		static inline Vector Logmap(const Rot2& r) {
+			return Vector_(1, r.theta());
+		}
+
+		/** Binary expmap  */
+		inline Rot2 expmap(const Vector& v) const {
+			return *this * Expmap(v);
+		}
+
+		/** Binary Logmap  */
+		inline Vector logmap(const Rot2& p2) const {
+			return Logmap(between(p2));
+		}
+
+		/** Between using the default implementation */
+		inline Rot2 between(const Rot2& p2, boost::optional<Matrix&> H1 =
+				boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+			if (H1) *H1 = -eye(1);
+			if (H2) *H2 = eye(1);
+			return between_default(*this, p2);
+		}
+
+		/** return 2*2 rotation matrix */
+		Matrix matrix() const;
+
+		/** return 2*2 transpose (inverse) rotation matrix   */
+		Matrix transpose() const;
+
+		/** The inverse rotation - negative angle */
+		Rot2 inverse() const {
+			return Rot2(c_, -s_);
+		}
+
+		/** Compose - make a new rotation by adding angles */
+		Rot2 operator*(const Rot2& R) const {
 			return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
 		}
 
-    /**
+		/**
-     * rotate point from rotated coordinate frame to
+		 * rotate point from rotated coordinate frame to
-     * world = R*p
+		 * world = R*p
-     */
+		 */
-  	Point2 rotate(const Point2& p, boost::optional<Matrix&> H1 =
+		Point2 rotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
-  			boost::none, boost::optional<Matrix&> H2 = boost::none) const;
+				boost::optional<Matrix&> H2 = boost::none) const;
 
-    /** syntactic sugar for rotate */
+		/** syntactic sugar for rotate */
-    inline Point2 operator*(const Point2& p) const {return rotate(p);}
+		inline Point2 operator*(const Point2& p) const {
+			return rotate(p);
+		}
 
-    /**
+		/**
-     * rotate point from world to rotated
+		 * rotate point from world to rotated
-     * frame = R'*p
+		 * frame = R'*p
-     */
+		 */
-    Point2 unrotate(const Point2& p, boost::optional<Matrix&> H1 =
+		Point2 unrotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
-  			boost::none, boost::optional<Matrix&> H2 = boost::none) const;
+				boost::optional<Matrix&> H2 = boost::none) const;
 
-    /**
+		/**
-     * Creates a unit vector as a Point2
+		 * Creates a unit vector as a Point2
-     */
+		 */
-    inline Point2 unit() const { return Point2(c_, s_); }
+		inline Point2 unit() const {
+			return Point2(c_, s_);
+		}
 
-  private:
+	private:
-    /** Serialization function */
+		/** Serialization function */
-    friend class boost::serialization::access;
+		friend class boost::serialization::access;
-    template<class ARCHIVE>
+		template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
+		void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & BOOST_SERIALIZATION_NVP(c_);
+			ar & BOOST_SERIALIZATION_NVP(c_);
-      ar & BOOST_SERIALIZATION_NVP(s_);
+			ar & BOOST_SERIALIZATION_NVP(s_);
-    }
+		}
 
-  }; // Rot2
+	};
 
 } // gtsam

gtsam/geometry/concepts.h

@@ -13,6 +13,9 @@
 
 #pragma once
 
+#include <gtsam/base/Matrix.h>
+#include <boost/optional.hpp>
+
 namespace gtsam {
 
 /**

gtsam/geometry/tests/testRot2.cpp

@@ -30,6 +30,8 @@ TEST( Rot2, constructors_and_angle)
 {
 	double c=cos(0.1), s=sin(0.1);
 	DOUBLES_EQUAL(0.1,R.theta(),1e-9);
+	CHECK(assert_equal(R,Rot2(0.1)));
+	CHECK(assert_equal(R,Rot2::fromAngle(0.1)));
 	CHECK(assert_equal(R,Rot2::fromCosSin(c,s)));
 	CHECK(assert_equal(R,Rot2::atan2(s*5,c*5)));
 }

gtsam/slam/BearingFactor.h

@@ -16,9 +16,9 @@
 
 #pragma once
 
-#include <gtsam/base/Testable.h>
-#include <gtsam/geometry/concepts.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/geometry/concepts.h>
+#include <gtsam/base/Testable.h>
 
 namespace gtsam {
 
@@ -58,12 +58,12 @@ namespace gtsam {
 		/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
 		Vector evaluateError(const Pose& pose, const Point& point,
 				boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-			Rot2 hx = pose.bearing(point, H1, H2);
+			Rot hx = pose.bearing(point, H1, H2);
 			return Rot::Logmap(z_.between(hx));
 		}
 
 		/** return the measured */
-		inline const Rot2 measured() const {
+		inline const Rot measured() const {
 			return z_;
 		}
 

gtsam/slam/Landmark2.h

@@ -0,0 +1,27 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Landmark2.h
+ * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Point2.h>
+
+namespace gtsam {
+
+	typedef gtsam::Point2 Landmark2;
+
+}
+

gtsam/slam/Makefile.am

@@ -50,6 +50,11 @@ check_PROGRAMS += tests/testPose2SLAM
 sources += planarSLAM.cpp
 check_PROGRAMS += tests/testPlanarSLAM
 
+# MATLAB Wrap headers for planarSLAM
+headers += Landmark2.h
+headers += PlanarSLAMGraph.h
+headers += PlanarSLAMValues.h
+
 # 3D Pose constraints
 sources += pose3SLAM.cpp 
 check_PROGRAMS += tests/testPose3SLAM 

gtsam/slam/PlanarSLAMGraph.h

@@ -0,0 +1,29 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file PlanarSLAMGraph.h
+ * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/slam/PlanarSLAM.h>
+#include <gtsam/slam/PlanarSLAMValues.h>
+#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
+
+namespace gtsam {
+
+	typedef gtsam::planarSLAM::Graph PlanarSLAMGraph;
+
+}
+

gtsam/slam/PlanarSLAMValues.h

@@ -0,0 +1,27 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file PlanarSLAMValues.h
+ * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/slam/PlanarSLAM.h>
+
+namespace gtsam {
+
+	typedef gtsam::planarSLAM::Values PlanarSLAMValues;
+
+}
+

gtsam/slam/planarSLAM.h

@@ -17,16 +17,17 @@
 
 #pragma once
 
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/nonlinear/TupleValues.h>
-#include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/RangeFactor.h>
 #include <gtsam/slam/BearingFactor.h>
 #include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/nonlinear/TupleValues.h>
+#include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/NonlinearOptimization.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
+#include <gtsam/geometry/Pose2.h>
 
 // We use gtsam namespace for generally useful factors
 namespace gtsam {
@@ -46,10 +47,27 @@ namespace gtsam {
 		/// Typedef for LieValues structure with PointKey type
 		typedef LieValues<PointKey> PointValues;
 
-		/// Typedef for the TupleValues2 to use PoseKeys and PointKeys
+		/// Values class, inherited from TupleValues2, using PoseKeys and PointKeys
-		typedef TupleValues2<PoseValues, PointValues> Values;
+		struct Values: public TupleValues2<PoseValues, PointValues> {
 
-		/**
+			/// Default constructor
+			Values() {}
+
+			/// Copy constructor
+			Values(const TupleValues2<PoseValues, PointValues>& values) :
+				TupleValues2<PoseValues, PointValues>(values) {
+			}
+
+			// Convenience for MATLAB wrapper, which does not allow for identically named methods
+
+			/// insert a pose
+		  void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); }
+
+		  /// insert a point
+		  void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); }
+		};
+
+			/**
 		 * List of typedefs for factors
 		 */
 
@@ -96,6 +114,15 @@ namespace gtsam {
 			/// Creates a factor with a Rot2 between a PoseKey i and PointKey j for difference in rotation and location
 			void addBearingRange(const PoseKey& i, const PointKey& j,
 					const Rot2& z1, double z2, const SharedNoiseModel& model);
+
+			/// Optimize, mostly here for MATLAB
+			boost::shared_ptr<Values> optimize(const Values& initialEstimate) {
+				typedef NonlinearOptimizer<Graph, Values> Optimizer;
+//				NonlinearOptimizationParameters::LAMBDA
+				boost::shared_ptr<Values> result(
+						new Values(*Optimizer::optimizeGN(*this, initialEstimate)));
+				return result;
+			}
 		};
 
 		/// Optimizer

wrap/Module.cpp

@@ -210,6 +210,7 @@ void Module::matlab_code(const string& toolboxPath,
       cls.matlab_methods(classPath,nameSpace);
 
       // add lines to make m-file
+      ofs << "%% " << cls.name << endl;
       ofs << "cd(toolboxpath)" << endl;
       cls.matlab_make_fragment(ofs, toolboxPath, mexFlags);
     }  

wrap/README

@@ -24,4 +24,7 @@ METHOD (AND CONSTRUCTOR) ARGUMENTS
   generation of the correct conversion routines unwrap<Vector> and unwrap<Matrix>
 - passing classes as arguments works, provided they are passed by reference.
 	This triggers a call to unwrap_shared_ptr
+- GTSAM specific: keys will be cast automatically from strings via GTSAM_magic. This
+  allows us to not have to declare all key types in MATLAB. Just replace key arguments with
+  the (non-const, non-reference) string type