diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index f2d60f1f7..9b305640b 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -189,17 +189,7 @@ Vector Rot3::quaternion() const { /* ************************************************************************* */ pair Rot3::axisAngle() { Vector3 omega = Rot3::Logmap(*this); - // Get the rotation angle. - double theta = omega.norm(); - - // Check if angle `theta` belongs to (-pi, pi]. - // If yes, rotate in opposite direction to maintain range. - // Since omega = theta * u, if all coefficients are negative, - // then theta is outside the expected range. - if ((omega.array() < 0).all()) { - theta = -theta; - } - return std::pair(Unit3(omega), theta); + return std::pair(Unit3(omega), omega.norm()); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index b1cfc4926..80d363d35 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -465,6 +465,9 @@ namespace gtsam { /** * Compute the Euler axis and angle (in radians) representation * of this rotation. + * The angle is in the range [0, π]. If the angle is not in the range, + * the axis is flipped around accordingly so that the returned angle is + * within the specified range. * @return pair consisting of Unit3 axis and angle in radians */ std::pair axisAngle(); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index f7d2609d4..6b0daa368 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -691,7 +691,8 @@ TEST(Rot3, axisAngle) { /// Test for sign ambiguity 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, R2.axisAngle().second, 1e-9); + EXPECT(assert_equal(Unit3(-0.1, -0.3, -0.4), R2.axisAngle().first, 1e-9)); + EXPECT_DOUBLES_EQUAL(M_PI/2, R2.axisAngle().second, 1e-9); theta = -(M_PI + M_PI/2); // 90 (or -270) degrees R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); @@ -700,7 +701,8 @@ TEST(Rot3, axisAngle) { /// Non-trivial angles theta = 195 * M_PI / 180; // 195 degrees Rot3 R3 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); - EXPECT_DOUBLES_EQUAL(theta - 2*M_PI, R3.axisAngle().second, 1e-9); + EXPECT(assert_equal(Unit3(-0.1, -0.3, -0.4), R3.axisAngle().first, 1e-9)); + EXPECT_DOUBLES_EQUAL(165*M_PI/180, R3.axisAngle().second, 1e-9); theta = -195 * M_PI / 180; // 165 degrees R3 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta);