diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 4eee045f9..9b68bb1f1 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -229,8 +229,9 @@ Rot3 Rot3::slerp(double t, const Rot3& other) const { } /* ************************************************************************* */ -double Rot3::angle(const Rot3& other) const { - return Rot3::Logmap(this->between(other)).norm() * 180.0 / M_PI; +pair Rot3::axisAngle(const Rot3& other) const { + Vector3 rot = Rot3::Logmap(this->between(other)); + return pair(Unit3(rot), rot.norm()); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index d810285fb..41b5a6ca1 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -480,10 +480,10 @@ namespace gtsam { Rot3 slerp(double t, const Rot3& other) const; /** - * @brief Compute the angle (in degrees) between *this and other + * @brief Compute the Euler axis and angle (in radians) between *this and other * @param other Rot3 element */ - double angle(const Rot3& other) const; + std::pair axisAngle(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 84c5bd094..c37dfda35 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -663,17 +663,17 @@ TEST(Rot3, ClosestTo) { /* ************************************************************************* */ TEST(Rot3, angle) { - Rot3 R1(0.996136, -0.0564186, -0.0672982, // - 0.0419472, 0.978941, -0.199787, // - 0.0771527, 0.196192, 0.977525); + Rot3 R1 = Rot3::Ypr(0, 0, 0); - Rot3 R2(0.99755, -0.0377748, -0.0588831, // - 0.0238455, 0.974883, -0.221437, // - 0.0657689, 0.21949, 0.973395); + Rot3 R2 = Rot3::Ypr(M_PI/4, 0, 0); - double expected = 1.7765686464446904; - - EXPECT(assert_equal(expected, R1.angle(R2), 1e-6)); + Unit3 expectedAxis(0, 0, 1); + double expectedAngle = M_PI/4; + + pair actual = R1.axisAngle(R2); + + EXPECT(assert_equal(expectedAxis, actual.first, 1e-6)); + EXPECT(assert_equal(expectedAngle, actual.second, 1e-6)); } /* ************************************************************************* */