finished oriented simulated2D

cpp/Makefile.am

@@ -183,7 +183,7 @@ testTupleConfig_LDADD = libgtsam.la
 headers += simulated2D.h simulated2DOriented.h
 headers += Simulated2DConfig.h Simulated2DOrientedConfig.h
 headers += Simulated2DPosePrior.h Simulated2DPointPrior.h Simulated2DOrientedPosePrior.h
-headers += Simulated2DOdometry.h Simulated2DMeasurement.h
+headers += Simulated2DOdometry.h Simulated2DMeasurement.h Simulated2DOrientedOdometry.h
 sources += simulated2D.cpp simulated2DOriented.cpp 
 testSimulated2D_SOURCES = testSimulated2D.cpp
 testSimulated2D_LDADD = libgtsam.la

cpp/Simulated2DOrientedOdometry.h

@@ -0,0 +1,18 @@
+/*
+ * Simulated2DOrientedOdometry.h
+ *
+ * Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * Author: Kai Ni
+ */
+
+#pragma once
+
+#include "simulated2DOriented.h"
+#include "Simulated2DOrientedConfig.h"
+
+namespace gtsam {
+
+	typedef simulated2DOriented::Odometry Simulated2DOrientedOdometry;
+
+}
+

cpp/Simulated2DOrientedPosePrior.h

@@ -1,8 +1,8 @@
 /*
- * Simulated2DPosePrior.h
+ * Simulated2DOrientedPosePrior.h
  *
  * Re-created on Feb 22, 2010 for compatibility with MATLAB
- * Author: Frank Dellaert
+ * Author: Kai Ni
  */
 
 #pragma once

cpp/gtsam.h

@@ -209,6 +209,13 @@ class Simulated2DOdometry {
   double error(const Simulated2DConfig& c) const;
 };
 
+class Simulated2DOrientedOdometry {
+	Simulated2DOrientedOdometry(Pose2& mu, const SharedDiagonal& model, int i1, int i2);
+  void print(string s) const;
+	GaussianFactor* linearize(const Simulated2DOrientedConfig& config) const;
+  double error(const Simulated2DOrientedConfig& c) const;
+};
+
 class Simulated2DMeasurement {
   Simulated2DMeasurement(Point2& mu, const SharedDiagonal& model, int i, int j);
   void print(string s) const;

cpp/simulated2DOriented.cpp

@@ -25,6 +25,14 @@ namespace gtsam {
 			return x;
 		}
 
+		/* ************************************************************************* */
+		Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1,
+				boost::optional<Matrix&> H2) {
+			if (H1) *H1 = -I;
+			if (H2) *H2 = I;
+			return between(x1, x2);
+		}
+
 	/* ************************************************************************* */
 
 	} // namespace simulated2DOriented

cpp/simulated2DOriented.h

@@ -33,16 +33,25 @@ namespace gtsam {
 		}
 		Pose2 prior(const Pose2& x, boost::optional<Matrix&> H = boost::none);
 
+		/**
+		 * odometry between two poses, and optional derivative version
+		 */
+		inline Pose2 odo(const Pose2& x1, const Pose2& x2) {
+			return between(x1, x2);
+		}
+		Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1 =
+				boost::none, boost::optional<Matrix&> H2 = 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> {
+		struct GenericPosePrior: public NonlinearFactor1<Cfg, Key, Pose2> {
 
 			Pose2 z_;
 
 			GenericPosePrior(const Pose2& z, const SharedGaussian& model, const Key& key) :
-				NonlinearFactor1<Cfg, Key, Point2> (model, key), z_(z) {
+				NonlinearFactor1<Cfg, Key, Pose2> (model, key), z_(z) {
 			}
 
 			Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
@@ -52,5 +61,27 @@ namespace gtsam {
 
 		};
 
+		/**
+		 * Binary factor simulating "odometry" between two Vectors
+		 */
+		template<class Cfg = Config, class Key = PoseKey>
+		struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Pose2, Key,
+				Pose2> {
+			Pose2 z_;
+
+			GenericOdometry(const Pose2& z, const SharedGaussian& model,
+					const Key& i1, const Key& i2) :
+				z_(z), NonlinearFactor2<Cfg, Key, Pose2, Key, Pose2> (model, i1, i2) {
+			}
+
+			Vector evaluateError(const Pose2& x1, const Pose2& x2, boost::optional<
+					Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+				return logmap(z_, odo(x1, x2, H1, H2));
+			}
+
+		};
+
+		typedef GenericOdometry<Config, PoseKey> Odometry;
+
 	} // namespace simulated2DOriented
 } // namespace gtsam