diff --git a/gtsam.h b/gtsam.h index 8382c06a7..de1a3887b 100644 --- a/gtsam.h +++ b/gtsam.h @@ -642,7 +642,6 @@ class Rot3 { static gtsam::Rot3 Rodrigues(Vector v); static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); static gtsam::Rot3 ClosestTo(const Matrix M); - static pair ToAxisAngle(const gtsam::Rot3& R); // Testable void print(string s) const; diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 9fc7ee6b1..14646659d 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -188,7 +188,14 @@ Vector Rot3::quaternion() const { /* ************************************************************************* */ pair Rot3::axisAngle() { - return Rot3::ToAxisAngle(*this); + const Vector3 omega = Rot3::Logmap(*this); + int direction = 1; + // Check if any element in axis is negative. + // This implies that the rotation is clockwise and not counterclockwise. + if (omega.minCoeff() < 0.0) { + direction = -1; + } + return std::pair(Unit3(omega), direction * omega.norm()); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 3f2a962d5..b1cfc4926 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -217,22 +217,6 @@ namespace gtsam { return AxisAngle(axis.unitVector(),angle); } - /** - * Compute the Euler axis and angle (in radians) representation - * @param R is the rotation matrix - * @return pair consisting of Unit3 axis and angle in radians - */ - static std::pair ToAxisAngle(const Rot3& R) { - const Vector3 omega = Rot3::Logmap(R); - int direction = 1; - // Check if any element in axis is negative. - // This implies that the rotation is clockwise and not counterclockwise. - if (omega.minCoeff() < 0.0) { - direction = -1; - } - return std::pair(Unit3(omega), direction * omega.norm()); - } - /** * Rodrigues' formula to compute an incremental rotation * @param w a vector of incremental roll,pitch,yaw diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 59397d199..1b01b92c2 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -662,42 +662,6 @@ TEST(Rot3, ClosestTo) { EXPECT(assert_equal(expected, actual.matrix(), 1e-6)); } -/* ************************************************************************* */ -TEST(Rot3, ToAxisAngle) { - /// Test rotations along each axis - Rot3 R1; - - // Negative because rotation is counterclockwise when looking at unchanging axis - Rot3 yaw = Rot3::Ypr(-M_PI/4, 0, 0); - Rot3 pitch = Rot3::Ypr(0, -M_PI/4, 0); - Rot3 roll = Rot3::Ypr(0, 0, -M_PI/4); - - EXPECT(assert_equal(Unit3(0, 0, 1), Rot3::ToAxisAngle(yaw.between(R1)).first, 1e-6)); - EXPECT_DOUBLES_EQUAL(M_PI/4, Rot3::ToAxisAngle(yaw.between(R1)).second, 1e-9); - - EXPECT(assert_equal(Unit3(0, 1, 0), Rot3::ToAxisAngle(pitch.between(R1)).first, 1e-6)); - EXPECT_DOUBLES_EQUAL(M_PI/4, Rot3::ToAxisAngle(pitch.between(R1)).second, 1e-9); - - EXPECT(assert_equal(Unit3(1, 0, 0), Rot3::ToAxisAngle(roll.between(R1)).first, 1e-6)); - EXPECT_DOUBLES_EQUAL(M_PI/4, Rot3::ToAxisAngle(roll.between(R1)).second, 1e-9); - - Unit3 axis; double angle; - std::tie(axis,angle) = Rot3::ToAxisAngle(R); - EXPECT(assert_equal(Unit3(0.1, 0.4, 0.2), axis)); - EXPECT_DOUBLES_EQUAL(Vector3(0.1, 0.4, 0.2).norm(), angle, 1e-9); - - /// Test for sign ambiguity - double theta = M_PI + M_PI/2; // 270 degrees - Rot3 R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); - - EXPECT_DOUBLES_EQUAL(theta - 2*M_PI, Rot3::ToAxisAngle(R2).second, 1e-9); - - theta = -M_PI/2; // -90 degrees - R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); - - EXPECT_DOUBLES_EQUAL(theta, Rot3::ToAxisAngle(R2).second, 1e-9); -} - /* ************************************************************************* */ TEST(Rot3, axisAngle) { /// Test rotations along each axis