diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 990ffdfe25..4c8973996a 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -112,6 +112,25 @@ class GTSAM_EXPORT Pose3: public LieGroup { return Pose3(R_ * T.R_, t_ + R_ * T.t_); } + /** + * Interpolate between two poses via individual rotation and translation + * interpolation. + * + * The default "interpolate" method defined in Lie.h minimizes the geodesic + * distance on the manifold, leading to a screw motion interpolation in + * Cartesian space, which might not be what is expected. + * In contrast, this method executes a straight line interpolation for the + * translation, while still using interpolate (aka "slerp") for the rotational + * component. This might be more intuitive in many applications. + * + * @param T End point of interpolation. + * @param t A value in [0, 1]. + */ + Pose3 interpolateRt(const Pose3& T, double t) const { + return Pose3(interpolate(R_, T.R_, t), + interpolate(t_, T.t_, t)); + } + /// @} /// @name Lie Group /// @{ diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 6de2c0a332..594d15c91b 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1016,6 +1016,33 @@ TEST(Pose3, TransformCovariance6) { TEST(Pose3, interpolate) { EXPECT(assert_equal(T2, interpolate(T2,T3, 0.0))); EXPECT(assert_equal(T3, interpolate(T2,T3, 1.0))); + + // Trivial example: start at origin and move to (1, 0, 0) while rotating pi/2 + // about z-axis. + Pose3 start; + Pose3 end(Rot3::Rz(M_PI_2), Point3(1, 0, 0)); + // This interpolation is easy to calculate by hand. + double t = 0.5; + Pose3 expected0(Rot3::Rz(M_PI_4), Point3(0.5, 0, 0)); + EXPECT(assert_equal(expected0, start.interpolateRt(end, t))); + + // Example from Peter Corke + // https://robotacademy.net.au/lesson/interpolating-pose-in-3d/ + t = 0.0759; // corresponds to the 10th element when calling `ctraj` in + // the video + Pose3 O; + Pose3 F(Rot3::Roll(0.6).compose(Rot3::Pitch(0.8)).compose(Rot3::Yaw(1.4)), + Point3(1, 2, 3)); + + // The expected answer matches the result presented in the video. + Pose3 expected1(interpolate(O.rotation(), F.rotation(), t), + interpolate(O.translation(), F.translation(), t)); + EXPECT(assert_equal(expected1, O.interpolateRt(F, t))); + + // Non-trivial interpolation, translation value taken from output. + Pose3 expected2(interpolate(T2.rotation(), T3.rotation(), t), + interpolate(T2.translation(), T3.translation(), t)); + EXPECT(assert_equal(expected2, T2.interpolateRt(T3, t))); } /* ************************************************************************* */