Deprecated transform_to(Pose3)
							parent
							
								
									23f3f95ed2
								
							
						
					
					
						commit
						ae1a096239
					
				|  | @ -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, | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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) | ||||
| { | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue