diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 1256271bc..f2d60f1f7 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -192,14 +192,12 @@ pair Rot3::axisAngle() { // Get the rotation angle. double theta = omega.norm(); - // Check if angle belongs to (-pi, pi]. + // Check if angle `theta` belongs to (-pi, pi]. // If yes, rotate in opposite direction to maintain range. - if (theta > M_PI) { - theta = theta - 2*M_PI; - omega = -omega; - } else if (theta <= -M_PI) { - theta = theta + 2*M_PI; - omega = -omega; + // 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); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 1b01b92c2..f7d2609d4 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -686,16 +686,25 @@ TEST(Rot3, axisAngle) { 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); + double theta; + /// 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); - theta = -M_PI/2; // 270 degrees + theta = -(M_PI + M_PI/2); // 90 (or -270) degrees R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); + EXPECT_DOUBLES_EQUAL(2*M_PI + theta, R2.axisAngle().second, 1e-9); - EXPECT_DOUBLES_EQUAL(theta, R2.axisAngle().second, 1e-9); + /// 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); + + theta = -195 * M_PI / 180; // 165 degrees + R3 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); + EXPECT_DOUBLES_EQUAL(2*M_PI + theta, R3.axisAngle().second, 1e-9); } /* ************************************************************************* */