transform_to and between, with derivatives, unit-tested

cpp/Pose2.cpp

@@ -40,6 +40,59 @@ namespace gtsam {
 		return Pose2(c * x_ - s * y_, s * x_ + c * y_, theta + theta_);
 	}
 
-/* ************************************************************************* */
+	/* ************************************************************************* */
+	Point2 transform_to(const Pose2& pose, const Point2& point) {
+		double dx = point.x()-pose.x(), dy = point.y()-pose.y();
+		double ct=cos(pose.theta()), st=sin(pose.theta());
+		return Point2(ct*dx + st*dy, -st*dx + ct*dy);
+	}
+
+	// TODO, have a combined function that returns both function value and derivative
+	Matrix Dtransform_to1(const Pose2& pose, const Point2& point) {
+		double dx = point.x()-pose.x(), dy = point.y()-pose.y();
+		double ct=cos(pose.theta()), st=sin(pose.theta());
+		double transformed_x = ct*dx + st*dy, transformed_y = -st*dx + ct*dy;
+		return Matrix_(2,3,
+				-ct, -st,  transformed_y,
+				 st, -ct, -transformed_x
+				);
+	}
+
+	Matrix Dtransform_to2(const Pose2& pose, const Point2& point) {
+		double ct=cos(pose.theta()), st=sin(pose.theta());
+		return Matrix_(2,2,
+				 ct,  st,
+				-st,  ct
+				);
+	}
+
+	/* ************************************************************************* */
+	Pose2 between(const Pose2& p1, const Pose2& p2) {
+		double dx = p2.x()-p1.x(), dy = p2.y()-p1.y();
+		double ct=cos(p1.theta()), st=sin(p1.theta());
+		return Pose2(ct*dx + st*dy, -st*dx + ct*dy, p2.theta()-p1.theta());
+	}
+
+	Matrix Dbetween1(const Pose2& p1, const Pose2& p2) {
+		double dx = p2.x()-p1.x(), dy = p2.y()-p1.y();
+		double ct=cos(p1.theta()), st=sin(p1.theta());
+		double transformed_x = ct*dx + st*dy, transformed_y = -st*dx + ct*dy;
+		return Matrix_(3,3,
+				-ct, -st,  transformed_y,
+				 st, -ct, -transformed_x,
+				0.0, 0.0, -1.0
+				);
+	}
+
+	Matrix Dbetween2(const Pose2& p1, const Pose2& p2) {
+		double ct=cos(p1.theta()), st=sin(p1.theta());
+		return Matrix_(3,3,
+				 ct,  st, 0.0,
+				-st,  ct, 0.0,
+				0.0, 0.0, 1.0
+				);
+	}
+
+	/* ************************************************************************* */
 
 } // namespace gtsam

cpp/Pose2.h

@@ -9,7 +9,7 @@
 #pragma once
 
 #include "Point2.h"
-#include "Vector.h"
+#include "Matrix.h"
 #include "Testable.h"
 
 namespace gtsam {
@@ -84,4 +84,18 @@ namespace gtsam {
     }
 	}; // Pose2
 
+	/**
+	 * Return point coordinates in pose coordinate frame
+	 */
+	Point2 transform_to(const Pose2& pose, const Point2& point);
+	Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
+	Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
+
+	/**
+	 * Return relative pose between p1 and p2, in p1 coordinate frame
+	 */
+	Pose2 between(const Pose2& p1, const Pose2& p2);
+	Matrix Dbetween1(const Pose2& p1, const Pose2& p2);
+	Matrix Dbetween2(const Pose2& p1, const Pose2& p2);
+
 } // namespace gtsam

cpp/testPose2.cpp

@@ -3,8 +3,11 @@
  * @brief  Unit tests for Pose2 class
  */
 
+#include <math.h>
 #include <CppUnitLite/TestHarness.h>
+#include "numericalDerivative.h"
 #include "Pose2.h"
+#include "Point2.h"
 
 using namespace gtsam;
 
@@ -30,6 +33,68 @@ TEST(Pose2, operators) {
 	CHECK(assert_equal(Pose2(0,0,0),Pose2(1,1,1)-Pose2(1,1,1)));
 }
 
+/* ************************************************************************* */
+TEST( Pose2, transform_to )
+{
+	Pose2 pose(1,2,M_PI_2); // robot at (1,2) looking towards y
+	Point2 point(-1,4);    // landmark at (-1,4)
+
+	// expected
+	Point2 expected(2,2);
+	Matrix expectedH1 = Matrix_(2,3, 0.0, -1.0, 2.0,  1.0, 0.0, -2.0);
+	Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0,  -1.0, 0.0);
+
+	// actual
+	Point2 actual = transform_to(pose,point);
+	Matrix actualH1 = Dtransform_to1(pose,point);
+	Matrix actualH2 = Dtransform_to2(pose,point);
+
+	CHECK(assert_equal(expected,actual));
+	CHECK(assert_equal(expectedH1,actualH1));
+	CHECK(assert_equal(expectedH2,actualH2));
+
+  Matrix numericalH1 = numericalDerivative21(transform_to, pose, point, 1e-5);
+	CHECK(assert_equal(numericalH1,actualH1));
+
+  Matrix numericalH2 = numericalDerivative22(transform_to, pose, point, 1e-5);
+	CHECK(assert_equal(numericalH2,actualH2));
+}
+
+/* ************************************************************************* */
+TEST( Pose2, between )
+{
+	Pose2 p1(1,2,M_PI_2); // robot at (1,2) looking towards y
+	Pose2 p2(-1,4,M_PI);  // robot at (-1,4) loooking at negative x
+
+	// expected
+	Pose2 expected(2,2,M_PI_2);
+	Matrix expectedH1 = Matrix_(3,3,
+			0.0,-1.0,2.0,
+			1.0,0.0,-2.0,
+			0.0,0.0,-1.0
+			);
+	Matrix expectedH2 = Matrix_(3,3,
+			 0.0,1.0,0.0,
+			-1.0,0.0,0.0,
+			 0.0,0.0,1.0
+			 );
+
+	// actual
+	Pose2 actual = between(p1,p2);
+	Matrix actualH1 = Dbetween1(p1,p2);
+	Matrix actualH2 = Dbetween2(p1,p2);
+
+	CHECK(assert_equal(expected,actual));
+	CHECK(assert_equal(expectedH1,actualH1));
+	CHECK(assert_equal(expectedH2,actualH2));
+
+  Matrix numericalH1 = numericalDerivative21(between, p1, p2, 1e-5);
+	CHECK(assert_equal(numericalH1,actualH1));
+
+  Matrix numericalH2 = numericalDerivative22(between, p1, p2, 1e-5);
+	CHECK(assert_equal(numericalH2,actualH2));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;