Modernized Pose2 constraint and added numerical derivative check

cpp/Pose2.cpp

@@ -25,6 +25,21 @@ namespace gtsam {
 		return Pose2(x_ + v(0), y_ + v(1), theta_ + v(2));
 	}
 
+	/* ************************************************************************* */
+	Vector Pose2::vector() const {
+		Vector v(3);
+		v(0) = x_;
+		v(1) = y_;
+		v(2) = theta_;
+		return v;
+	}
+
+	/* ************************************************************************* */
+	Pose2 Pose2::rotate(double theta) const {
+		double c = cos(theta), s = sin(theta);
+		return Pose2(c * x_ - s * y_, s * x_ + c * y_, theta + theta_);
+	}
+
 /* ************************************************************************* */
 
 } // namespace gtsam

cpp/Pose2.h

@@ -9,6 +9,7 @@
 #pragma once
 
 #include "Point2.h"
+#include "Vector.h"
 #include "Testable.h"
 
 namespace gtsam {
@@ -55,11 +56,32 @@ namespace gtsam {
 		bool equals(const Pose2& pose, double tol = 1e-9) const;
 
     /** get functions for x, y, theta */
-		double x()     const { return x_;}
-		double y()     const { return y_;}
-		double theta() const { return theta_;}
+		inline double x()     const { return x_;}
+		inline double y()     const { return y_;}
+		inline double theta() const { return theta_;}
 
+		/** return DOF, dimensionality of tangent space = 3 */
+		size_t dim() const {
+			return 3;
+		}
+
+		/* exponential map */
 		Pose2 exmap(const Vector& v) const;
-	};
+
+    /** return vectorized form (column-wise)*/
+    Vector vector() const;
+
+    /** rotate pose by theta */
+    Pose2 rotate(double theta) const;
+
+    // operators
+    Pose2 operator+(const Pose2& p2) const {
+  		return Pose2(x_ + p2.x_, y_ + p2.y_, theta_ + p2.theta_);
+    }
+
+    Pose2 operator-(const Pose2& p2) const {
+  		return Pose2(x_ - p2.x_, y_ - p2.y_, theta_ - p2.theta_);
+    }
+	}; // Pose2
 
 } // namespace gtsam

cpp/testNonlinearFactor.cpp

@@ -173,53 +173,60 @@ TEST( NonlinearFactor, size )
 }
 
 /* ************************************************************************* */
-Vector RotatePoseDisplacement(Vector d, double theta) {
-	double co=cos(theta);
-	double si=sin(theta);
-	return Matrix_(3,3, co, -si, 0.0, si, co, 0.0, 0.0, 0.0, 1.0)*d;
 
+#include "numericalDerivative.h"
 
-}
-Vector h(const Pose2& p1, const Pose2& p2) {
-	double dx= p2.x()-p1.x();
-	double dy= p2.y()-p1.y();
-	double dtheta= p2.theta()-p1.theta();
-	return RotatePoseDisplacement(Vector_(3,dx,dy,dtheta),-p1.theta());
+Pose2 h(const Pose2& p1, const Pose2& p2) {
+	Pose2 dpose = p2 - p1;
+	return dpose.rotate(-p1.theta());
 }
 
 Matrix H1(const Pose2& p1, const Pose2& p2) {
-
 	double dx= p2.x()-p1.x();
 	double dy= p2.y()-p1.y();
-	double co=cos(p1.theta());
-	double si=sin(p1.theta());
-	return Matrix_(3,3, -co, -si, -si*dx+co*dy, si, -co, -co*dx-si*dy, 0.0, 0.0, -1.0);
-
+	double ct=cos(p1.theta());
+	double st=sin(p1.theta());
+	return Matrix_(3,3,
+			-ct, -st, -st*dx+ct*dy,
+			 st, -ct, -ct*dx-st*dy,
+			0.0, 0.0, -1.0
+			);
 }
 
 Matrix H2(const Pose2& p1) {
-	double co=cos(p1.theta());
-	double si=sin(p1.theta());
-	return Matrix_(3,3, co, si, 0.0, -si, co, 0.0, 0.0, 0.0, 1.0);
+	double ct=cos(p1.theta());
+	double st=sin(p1.theta());
+	return Matrix_(3,3,
+			 ct,  st, 0.0,
+			-st,  ct, 0.0,
+			0.0, 0.0, 1.0
+			);
 }
 
 TEST( PoseConstraintFactor2, testFunctions )
 {
 	Pose2 p1(0.0, 6.0, 0.0);
 	Pose2 p2(0.101826, 6.111236, 0.011499);
+
 	//expected
-	Vector expectedh = Vector_(3, 0.101826, 0.111236, 0.011499);
+	Pose2 expectedh(0.101826, 0.111236, 0.011499);
 	Matrix expectedH1 = Matrix_(3,3,-1.0, 0.0, 0.111236, 0.0, -1.0, -0.101826, 0.0, 0.0, -1.0);
 	Matrix expectedH2 = Matrix_(3,3, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
 
 	// actual
-	Vector actualh = h(p1,p2);
+	Pose2 actualh = h(p1,p2);
 	Matrix actualH1 = H1(p1,p2);
 	Matrix actualH2 = H2(p1);
 
-	CHECK(assert_equal(actualh,expectedh));
-	CHECK(assert_equal(actualH1,expectedH1));
-	CHECK(assert_equal(actualH2,expectedH2));
+	CHECK(assert_equal(expectedh,actualh));
+	CHECK(assert_equal(expectedH1,actualH1));
+	CHECK(assert_equal(expectedH2,actualH2));
+
+  Matrix numericalH1 = numericalDerivative21(h, p1, p2, 1e-5);
+	//CHECK(assert_equal(numericalH1,actualH1));
+
+  Matrix numericalH2 = numericalDerivative22(h, p1, p2, 1e-5);
+	CHECK(assert_equal(numericalH2,actualH2));
 }
 
 /* ************************************************************************* */

cpp/testPose2.cpp

@@ -16,6 +16,20 @@ TEST(Pose2, constructors) {
 	assert_equal(pose,origin);
 }
 
+/* ************************************************************************* */
+TEST(Pose2, rotate) {
+	double theta = 0.1, c=cos(theta),s=sin(theta);
+	Pose2 p1(1,0,0.2), p2(0,1,0.4);
+	CHECK(assert_equal(Pose2( c,s,0.3),p1.rotate(theta)));
+	CHECK(assert_equal(Pose2(-s,c,0.5),p2.rotate(theta)));
+}
+
+/* ************************************************************************* */
+TEST(Pose2, operators) {
+	CHECK(assert_equal(Pose2(2,2,2),Pose2(1,1,1)+Pose2(1,1,1)));
+	CHECK(assert_equal(Pose2(0,0,0),Pose2(1,1,1)-Pose2(1,1,1)));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;