From 8a9f5c74946b872cdcb05d8754a7f59e25bb4fb4 Mon Sep 17 00:00:00 2001
From: Kai Ni <nikai97@gmail.com>
Date: Wed, 7 Apr 2010 21:27:16 +0000
Subject: [PATCH] finished oriented simulated2D

---
 cpp/Makefile.am                    |  2 +-
 cpp/Simulated2DOrientedOdometry.h  | 18 +++++++++++++++
 cpp/Simulated2DOrientedPosePrior.h |  4 ++--
 cpp/gtsam.h                        |  7 ++++++
 cpp/simulated2DOriented.cpp        |  8 +++++++
 cpp/simulated2DOriented.h          | 35 ++++++++++++++++++++++++++++--
 6 files changed, 69 insertions(+), 5 deletions(-)
 create mode 100644 cpp/Simulated2DOrientedOdometry.h

diff --git a/cpp/Makefile.am b/cpp/Makefile.am
index f175b6baf..aa6d0d172 100644
--- a/cpp/Makefile.am
+++ b/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
diff --git a/cpp/Simulated2DOrientedOdometry.h b/cpp/Simulated2DOrientedOdometry.h
new file mode 100644
index 000000000..d1cb7d66c
--- /dev/null
+++ b/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;
+
+}
+
diff --git a/cpp/Simulated2DOrientedPosePrior.h b/cpp/Simulated2DOrientedPosePrior.h
index c9a4a8681..ff25741a7 100644
--- a/cpp/Simulated2DOrientedPosePrior.h
+++ b/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
diff --git a/cpp/gtsam.h b/cpp/gtsam.h
index 026592995..e6fb5f997 100644
--- a/cpp/gtsam.h
+++ b/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;
diff --git a/cpp/simulated2DOriented.cpp b/cpp/simulated2DOriented.cpp
index 744b1f7e9..602870315 100644
--- a/cpp/simulated2DOriented.cpp
+++ b/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
diff --git a/cpp/simulated2DOriented.h b/cpp/simulated2DOriented.h
index 7706dbc80..75b6d2609 100644
--- a/cpp/simulated2DOriented.h
+++ b/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