From d1ab2c7288dd036ee2ba98332872aea991cc91ff Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 8 Dec 2009 19:12:20 +0000 Subject: [PATCH] transform_to and between, with derivatives, unit-tested --- cpp/Pose2.cpp | 55 ++++++++++++++++++++++++++++++++++++++- cpp/Pose2.h | 16 +++++++++++- cpp/testPose2.cpp | 65 +++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 134 insertions(+), 2 deletions(-) diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index e1393f5b1..c4f4c82dd 100644 --- a/cpp/Pose2.cpp +++ b/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 diff --git a/cpp/Pose2.h b/cpp/Pose2.h index 7a3e028ea..f451f103b 100644 --- a/cpp/Pose2.h +++ b/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 diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp index 345f762f0..7460949e6 100644 --- a/cpp/testPose2.cpp +++ b/cpp/testPose2.cpp @@ -3,8 +3,11 @@ * @brief Unit tests for Pose2 class */ +#include #include +#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;