new Pose2 class

.cproject

@@ -300,7 +300,6 @@
 <buildTargets>
 <target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments></buildArguments>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -308,6 +307,7 @@
 </target>
 <target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testSimpleCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -315,6 +315,7 @@
 </target>
 <target name="testCal3_S2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testCal3_S2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -322,7 +323,6 @@
 </target>
 <target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments></buildArguments>
 <buildTarget>testVSLAMFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -330,6 +330,7 @@
 </target>
 <target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testCalibratedCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -337,15 +338,21 @@
 </target>
 <target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments></buildArguments>
 <buildTarget>testConditionalGaussian.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
+<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
+<buildTarget>testPose2.run</buildTarget>
+<stopOnError>true</stopOnError>
+<useDefaultCommand>true</useDefaultCommand>
+<runAllBuilders>true</runAllBuilders>
+</target>
 <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments></buildArguments>
 <buildTarget>install</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -353,7 +360,6 @@
 </target>
 <target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments></buildArguments>
 <buildTarget>clean</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -361,7 +367,6 @@
 </target>
 <target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments></buildArguments>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>

cpp/Makefile.am

@@ -100,15 +100,17 @@ testConstrainedLinearFactorGraph_LDADD    = libgtsam.la
 testConstrainedChordalBayesNet_LDADD      = libgtsam.la
 
 # geometry
-sources += Point2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
+sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
-check_PROGRAMS += testPoint2 testPoint3 testRot3 testPose3 testCal3_S2
+check_PROGRAMS += testPoint2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2
 testPoint2_SOURCES  = testPoint2.cpp
+testPose2_SOURCES   = testPose2.cpp
 testPoint3_SOURCES  = testPoint3.cpp
 testRot3_SOURCES    = testRot3.cpp
 testPose3_SOURCES   = testPose3.cpp
 testCal3_S2_SOURCES = testCal3_S2.cpp               
 
 testPoint2_LDADD    = libgtsam.la
+testPose2_LDADD     = libgtsam.la
 testPoint3_LDADD    = libgtsam.la
 testRot3_LDADD      = libgtsam.la
 testPose3_LDADD     = libgtsam.la

cpp/Pose2.cpp

@@ -0,0 +1,39 @@
+/**
+ * @file  Pose2.cpp
+ * @brief 2D Pose
+ */
+
+#include "Pose2.h"
+
+using namespace std;
+
+namespace gtsam {
+
+	/* ************************************************************************* */
+	void Pose2::print(const string& s) const {
+		cout << s << "(" << x_ << ", " << y_ << ", " << theta_ << ")" << endl;
+	}
+
+	/* ************************************************************************* */
+	Pose2 Pose2::exmap(const Vector& v) const {
+		return Pose2(x_ + v(0), y_ + v(1), theta_ + v(2));
+	}
+
+	/* ************************************************************************* */
+	bool Pose2::equals(const Pose2& q, double tol) const {
+		return (fabs(x_ - q.x_) < tol && fabs(y_ - q.y_) < tol && fabs(theta_
+				- q.theta_) < tol);
+	}
+
+	/* ************************************************************************* */
+	bool assert_equal(const Pose2& A, const Pose2& B, double tol) {
+		if (A.equals(B, tol)) return true;
+		printf("not equal:\n");
+		A.print("A");
+		B.print("B");
+		return false;
+	}
+
+/* ************************************************************************* */
+
+} // namespace gtsam

cpp/Pose2.h

@@ -0,0 +1,57 @@
+/**
+ * @file  Pose2.h
+ * @brief 3D Pose
+ * @author: Frank Dellaert
+ */
+
+// \callgraph
+
+#pragma once
+
+#include "Point2.h"
+
+namespace gtsam {
+
+	/**
+	 * A 2D pose (x,y,theta)
+	 */
+	class Pose2 {
+
+	private:
+		double x_, y_, theta_;
+
+	public:
+
+		/** default constructor = origin */
+		Pose2() :
+			x_(0), y_(0), theta_(0) {
+		} // default is origin
+
+		/** copy constructor */
+		Pose2(const Pose2& pose) :
+			x_(pose.x_), y_(pose.y_), theta_(pose.theta_) {
+		}
+
+		/** construct from (x,y,theta) */
+		Pose2(double x, double y, double theta) :
+			x_(x), y_(y), theta_(theta) {
+		}
+
+		/** construct from rotation and translation */
+		Pose2(const Point2& t, double theta) :
+			x_(t.x()), y_(t.y()), theta_(theta) {
+		}
+
+		/** print with optional string */
+		void print(const std::string& s = "") const;
+
+		Pose2 exmap(const Vector& v) const;
+
+		/** assert equality up to a tolerance */
+		bool equals(const Pose2& pose, double tol = 1e-9) const;
+	};
+
+	/** assert equality up to a tolerance */
+	bool assert_equal(const Pose2& A, const Pose2& B, double tol = 1e-9);
+
+} // namespace gtsam

cpp/testPose2.cpp

@@ -0,0 +1,25 @@
+/**
+ * @file   testPose2.cpp
+ * @brief  Unit tests for Pose2 class
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include "Pose2.h"
+
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST(Pose2, constructors) {
+	Point2 p;
+	Pose2 pose(p,0);
+	Pose2 origin;
+	assert_equal(pose,origin);
+}
+
+/* ************************************************************************* */
+int main() {
+	TestResult tr;
+	return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+