diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 50e487e75..334660ca8 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -285,11 +285,13 @@ Matrix4 Pose3::matrix() const { } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 Pose3 Pose3::transform_to(const Pose3& pose) const { Rot3 cRv = R_ * Rot3(pose.R_.inverse()); Point3 t = pose.transform_to(t_); return Pose3(cRv, t); } +#endif /* ************************************************************************* */ Pose3 Pose3::transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1, diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 757c4fdd4..0a27e5f17 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -253,11 +253,6 @@ public: /** convert to 4*4 matrix */ Matrix4 matrix() const; - /** receives a pose in local coordinates and transforms it to world coordinates - * @deprecated: This is actually equivalent to transform_from, so it is WRONG! Use - * transform_pose_to instead. */ - Pose3 transform_to(const Pose3& pose) const; - /** receives a pose in world coordinates and transforms it to local coordinates */ Pose3 transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1 = boost::none, OptionalJacobian<6, 6> H2 = boost::none) const; @@ -321,6 +316,14 @@ public: GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + /// This function is neither here not there. Do not use. + Pose3 transform_to(const Pose3& pose) const; + /// @} +#endif + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index d516ddc8b..9a6cce753 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -484,58 +484,6 @@ TEST( Pose3, transform_roundtrip) EXPECT(assert_equal(expected, actual)); } -/* ************************************************************************* */ -TEST( Pose3, transformPose_to_origin) -{ - // transform to origin - Pose3 actual = T3.transform_to(Pose3()); - EXPECT(assert_equal(T3, actual, 1e-8)); -} - -/* ************************************************************************* */ -TEST( Pose3, transformPose_to_itself) -{ - // transform to itself - Pose3 actual = T3.transform_to(T3); - EXPECT(assert_equal(Pose3(), actual, 1e-8)); -} - -/* ************************************************************************* */ -TEST( Pose3, transformPose_to_translation) -{ - // transform translation only - Rot3 r = Rot3::Rodrigues(-1.570796,0,0); - Pose3 pose2(r, Point3(21.,32.,13.)); - Pose3 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3))); - Pose3 expected(r, Point3(20.,30.,10.)); - EXPECT(assert_equal(expected, actual, 1e-8)); -} - -/* ************************************************************************* */ -TEST( Pose3, transformPose_to_simple_rotate) -{ - // transform translation only - Rot3 r = Rot3::Rodrigues(0,0,-1.570796); - Pose3 pose2(r, Point3(21.,32.,13.)); - Pose3 transform(r, Point3(1,2,3)); - Pose3 actual = pose2.transform_to(transform); - Pose3 expected(Rot3(), Point3(-30.,20.,10.)); - EXPECT(assert_equal(expected, actual, 0.001)); -} - -/* ************************************************************************* */ -TEST( Pose3, transformPose_to) -{ - // transform to - Rot3 r = Rot3::Rodrigues(0,0,-1.570796); //-90 degree yaw - Rot3 r2 = Rot3::Rodrigues(0,0,0.698131701); //40 degree yaw - Pose3 pose2(r2, Point3(21.,32.,13.)); - Pose3 transform(r, Point3(1,2,3)); - Pose3 actual = pose2.transform_to(transform); - Pose3 expected(Rot3::Rodrigues(0,0,2.26892803), Point3(-30.,20.,10.)); - EXPECT(assert_equal(expected, actual, 0.001)); -} - /* ************************************************************************* */ TEST(Pose3, Retract_LocalCoordinates) {