added oriented version of simulate2d

cpp/Makefile.am

@@ -180,10 +180,11 @@ testLieConfig_LDADD = libgtsam.la
 testTupleConfig_LDADD = libgtsam.la
 
 # simulated2D example
-headers += Simulated2DConfig.h 
+headers += simulated2D.h simulated2DOriented.h
-headers += Simulated2DPosePrior.h Simulated2DPointPrior.h 
+headers += Simulated2DConfig.h Simulated2DOrientedConfig.h
+headers += Simulated2DPosePrior.h Simulated2DPointPrior.h Simulated2DOrientedPosePrior.h
 headers += Simulated2DOdometry.h Simulated2DMeasurement.h
-sources += simulated2D.cpp 
+sources += simulated2D.cpp simulated2DOriented.cpp 
 testSimulated2D_SOURCES = testSimulated2D.cpp
 testSimulated2D_LDADD = libgtsam.la
 check_PROGRAMS += testSimulated2D 

cpp/Simulated2DOrientedConfig.h

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

cpp/Simulated2DOrientedPosePrior.h

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

cpp/gtsam.h

@@ -170,6 +170,17 @@ class Simulated2DConfig {
 	Point2* point(int j);
 };
 
+class Simulated2DOrientedConfig {
+	Simulated2DOrientedConfig();
+  void print(string s) const;
+	void insertPose(int i, const Pose2& p);
+	void insertPoint(int j, const Point2& p);
+	int nrPoses() const;
+	int nrPoints() const;
+	Pose2* pose(int i);
+	Point2* point(int j);
+};
+
 class Simulated2DPosePrior {
 	Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i);
   void print(string s) const;
@@ -177,6 +188,13 @@ class Simulated2DPosePrior {
   double error(const Simulated2DConfig& c) const;
 };
 
+class Simulated2DOrientedPosePrior {
+	Simulated2DOrientedPosePrior(Pose2& mu, const SharedDiagonal& model, int i);
+  void print(string s) const;
+	GaussianFactor* linearize(const Simulated2DOrientedConfig& config) const;
+  double error(const Simulated2DOrientedConfig& c) const;
+};
+
 class Simulated2DPointPrior {
 	Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i);
   void print(string s) const;

cpp/simulated2DOriented.cpp

@@ -0,0 +1,31 @@
+/**
+ * @file    simulated2DOriented
+ * @brief   measurement functions and derivatives for simulated 2D robot
+ * @author  Frank Dellaert
+ */
+
+#include "simulated2DOriented.h"
+#include "TupleConfig-inl.h"
+
+namespace gtsam {
+
+	using namespace simulated2DOriented;
+//	INSTANTIATE_LIE_CONFIG(PointKey, Point2)
+//	INSTANTIATE_PAIR_CONFIG(PoseKey, Pose2, PointKey, Point2)
+//	INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
+//	INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)
+
+	namespace simulated2DOriented {
+
+		static Matrix I = gtsam::eye(3);
+
+		/* ************************************************************************* */
+		Pose2 prior(const Pose2& x, boost::optional<Matrix&> H) {
+			if (H) *H = I;
+			return x;
+		}
+
+	/* ************************************************************************* */
+
+	} // namespace simulated2DOriented
+} // namespace gtsam

cpp/simulated2DOriented.h

@@ -0,0 +1,56 @@
+/**
+ * @file    simulated2D.h
+ * @brief   measurement functions and derivatives for simulated 2D robot
+ * @author  Frank Dellaert
+ */
+
+// \callgraph
+
+#pragma once
+
+#include "Pose2.h"
+#include "TupleConfig.h"
+#include "NonlinearFactor.h"
+
+// \namespace
+
+namespace gtsam {
+
+	namespace simulated2DOriented {
+
+		// The types that take an oriented pose2 rather than point2
+		typedef TypedSymbol<Point2, 'l'> PointKey;
+		typedef TypedSymbol<Pose2,  'x'> PoseKey;
+		typedef PairConfig<PoseKey, Pose2,  PointKey, Point2> Config;
+
+		//TODO:: point prior is not implemented right now
+
+		/**
+		 * Prior on a single pose, and optional derivative version
+		 */
+		inline Pose2 prior(const Pose2& x) {
+			return x;
+		}
+		Pose2 prior(const Pose2& x, boost::optional<Matrix&> H = boost::none);
+
+		/**
+		 * Unary factor encoding a soft prior on a vector
+		 */
+		template<class Cfg = Config, class Key = PoseKey>
+		struct GenericPosePrior: public NonlinearFactor1<Cfg, Key, Point2> {
+
+			Pose2 z_;
+
+			GenericPosePrior(const Pose2& z, const SharedGaussian& model, const Key& key) :
+				NonlinearFactor1<Cfg, Key, Point2> (model, key), z_(z) {
+			}
+
+			Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
+					boost::none) const {
+				return logmap(z_, prior(x, H));
+			}
+
+		};
+
+	} // namespace simulated2DOriented
+} // namespace gtsam