diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 50de5cdf5..13a041a5b 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -261,6 +261,15 @@ Point3 Rot3::unrotate(const Point3& p) const { return Point3(transpose_*p.vector()); // q = Rt*p } +/* ************************************************************************* */ +Rot3 Rot3::slerp(double t, const Rot3& other) const { + // amazingly simple in GTSAM :-) + assert(t>=0 && t<=1); + cout << "slerp" << endl; + Vector3 omega = localCoordinates(other, Rot3::EXPMAP); + return retract(t * omega, Rot3::EXPMAP); +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 62ac9f3f9..b5e065a03 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -445,6 +445,13 @@ namespace gtsam { */ Vector quaternion() const; + /** + * @brief Spherical Linear intERPolation between *this and other + * @param s a value between 0 and 1 + * @param other final point of iterpolation geodesic on manifold + */ + Rot3 slerp(double t, const Rot3& other) const; + /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 89475eba6..9761dfd74 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -574,6 +574,18 @@ TEST( Rot3, stream) EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n"); } +/* ************************************************************************* */ +TEST( Rot3, slerp) +{ + // A first simple test + Rot3 R1 = Rot3::Rz(1), R2 = Rot3::Rz(2), R3 = Rot3::Rz(1.5); + EXPECT(assert_equal(R1, R1.slerp(0.0,R2))); + EXPECT(assert_equal(R2, R1.slerp(1.0,R2))); + EXPECT(assert_equal(R3, R1.slerp(0.5,R2))); + // Make sure other can be *this + EXPECT(assert_equal(R1, R1.slerp(0.5,R1))); +} + /* ************************************************************************* */ int main() { TestResult tr;