diff --git a/gtsam.h b/gtsam.h index f943c2e9d..6e2eea686 100644 --- a/gtsam.h +++ b/gtsam.h @@ -677,6 +677,7 @@ class Rot3 { // Vector toQuaternion() const; // FIXME: Can't cast to Vector properly Vector quaternion() const; gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; + double angle(const gtsam::Rot3& other) const; // enabling serialization functionality void serialize() const; diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index b1e8dd14b..4eee045f9 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -228,6 +228,11 @@ Rot3 Rot3::slerp(double t, const Rot3& other) const { return interpolate(*this, other, t); } +/* ************************************************************************* */ +double Rot3::angle(const Rot3& other) const { + return Rot3::Logmap(this->between(other)).norm() * 180.0 / M_PI; +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index e4408c9f4..d810285fb 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -479,6 +479,12 @@ namespace gtsam { */ Rot3 slerp(double t, const Rot3& other) const; + /** + * @brief Compute the angle (in degrees) between *this and other + * @param other Rot3 element + */ + double angle(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 c95b85f21..84c5bd094 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -661,6 +661,21 @@ TEST(Rot3, ClosestTo) { EXPECT(assert_equal(expected, actual.matrix(), 1e-6)); } +/* ************************************************************************* */ +TEST(Rot3, angle) { + Rot3 R1(0.996136, -0.0564186, -0.0672982, // + 0.0419472, 0.978941, -0.199787, // + 0.0771527, 0.196192, 0.977525); + + Rot3 R2(0.99755, -0.0377748, -0.0588831, // + 0.0238455, 0.974883, -0.221437, // + 0.0657689, 0.21949, 0.973395); + + double expected = 1.7765686464446904; + + EXPECT(assert_equal(expected, R1.angle(R2), 1e-6)); +} + /* ************************************************************************* */ int main() { TestResult tr;