diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index fd1a84acd..d16f91e46 100644 --- a/cpp/Pose2.cpp +++ b/cpp/Pose2.cpp @@ -72,6 +72,15 @@ namespace gtsam { return eye(3); } + /* ************************************************************************* */ + Point2 transform_from(const Pose2& pose, const Point2& point, + boost::optional H1, boost::optional H2) { + Point2 point_transformed = rotate(pose.r(), point, H1, H2) + pose.t(); + Matrix R = pose.r().matrix(); + if (H1) *H1 = collect(2, &R, &(*H1)); + return point_transformed; + } + /* ************************************************************************* */ Pose2 between(const Pose2& p1, const Pose2& p2, boost::optional H1, boost::optional H2) { diff --git a/cpp/Pose2.h b/cpp/Pose2.h index 53898cef7..c8688a723 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -102,8 +102,8 @@ namespace gtsam { Matrix Dtransform_to2(const Pose2& pose, const Point2& point); /** Return point coordinates in global frame */ - inline Point2 transform_from(const Pose2& pose, const Point2& point) { - return rotate(pose.r(), point)+pose.t(); } + Point2 transform_from(const Pose2& pose, const Point2& point, + boost::optional H1 = boost::none, boost::optional H2 = boost::none); /** Return relative pose between p1 and p2, in p1 coordinate frame */ Pose2 between(const Pose2& p1, const Pose2& p2, diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp index dfa81d47a..c0cf58930 100644 --- a/cpp/testPose2.cpp +++ b/cpp/testPose2.cpp @@ -113,6 +113,33 @@ TEST( Pose2, transform_to ) CHECK(assert_equal(numericalH2,actualH2)); } +/* ************************************************************************* */ +Point2 transform_from_proxy(const Pose2& pose, const Point2& point) { + return transform_from(pose, point); +} + +TEST (Pose2, transform_from) +{ + Pose2 pose(1., 0., M_PI_2); + Point2 pt(2., 1.); + Matrix H1, H2; + Point2 actual = transform_from(pose, pt, H1, H2); + + Point2 expected(0., 2.); + CHECK(assert_equal(expected, actual)); + + Matrix H1_expected = Matrix_(2, 3, 0., -1., -2., 1., 0., -1.); + Matrix H2_expected = Matrix_(2, 2, 0., -1., 1., 0.); + + Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt, 1e-5); + CHECK(assert_equal(H1_expected, H1)); + CHECK(assert_equal(H1_expected, numericalH1)); + + Matrix numericalH2 = numericalDerivative22(transform_from_proxy, pose, pt, 1e-5); + CHECK(assert_equal(H2_expected, H2)); + CHECK(assert_equal(H2_expected, numericalH2)); +} + /* ************************************************************************* */ TEST(Pose2, compose_a) {