diff --git a/gtsam.h b/gtsam.h index 521495bdf..8382c06a7 100644 --- a/gtsam.h +++ b/gtsam.h @@ -642,7 +642,7 @@ 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 std::pair ToAxisAngle(const gtsam::Rot3& R) const; + static pair ToAxisAngle(const gtsam::Rot3& R); // Testable void print(string s) const; @@ -675,6 +675,7 @@ class Rot3 { double roll() const; double pitch() const; double yaw() const; + pair axisAngle() const; // Vector toQuaternion() const; // FIXME: Can't cast to Vector properly Vector quaternion() const; gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; @@ -1309,7 +1310,7 @@ class SymbolicBayesTree { // class SymbolicBayesTreeClique { // BayesTreeClique(); // BayesTreeClique(CONDITIONAL* conditional); -// // BayesTreeClique(const std::pair& result) : Base(result) {} +// // BayesTreeClique(const pair& result) : Base(result) {} // // bool equals(const This& other, double tol) const; // void print(string s) const; diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index b1e8dd14b..9fc7ee6b1 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -16,6 +16,7 @@ * @author Christian Potthast * @author Frank Dellaert * @author Richard Roberts + * @author Varun Agrawal */ #include @@ -185,6 +186,11 @@ Vector Rot3::quaternion() const { return v; } +/* ************************************************************************* */ +pair Rot3::axisAngle() { + return Rot3::ToAxisAngle(*this); +} + /* ************************************************************************* */ Matrix3 Rot3::ExpmapDerivative(const Vector3& x) { return SO3::ExpmapDerivative(x); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 974e9b195..3f2a962d5 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -17,6 +17,7 @@ * @author Frank Dellaert * @author Richard Roberts * @author Luca Carlone + * @author Varun Agrawal */ // \callgraph @@ -217,13 +218,19 @@ namespace gtsam { } /** - * Compute to the Euler axis and angle (in radians) representation + * 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); - return std::pair(Unit3(omega), omega.norm()); + 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()); } /** @@ -439,7 +446,7 @@ namespace gtsam { /** * Use RQ to calculate roll-pitch-yaw angle representation - * @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r) + * @return a vector containing rpy s.t. R = Rot3::Ypr(y,p,r) */ Vector3 rpy() const; @@ -471,6 +478,13 @@ namespace gtsam { /// @name Advanced Interface /// @{ + /** + * Compute the Euler axis and angle (in radians) representation + * of this rotation. + * @return pair consisting of Unit3 axis and angle in radians + */ + std::pair axisAngle(); + /** Compute the quaternion representation of this rotation. * @return The quaternion */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index dbd4e84a1..59397d199 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -663,16 +663,75 @@ TEST(Rot3, ClosestTo) { } /* ************************************************************************* */ -TEST(Rot3, angle) { - Rot3 R1 = Rot3::Ypr(M_PI/4, 0, 0); +TEST(Rot3, ToAxisAngle) { + /// Test rotations along each axis + Rot3 R1; - Unit3 expectedAxis(-0.350067, -0.472463, 0.808846); - double expectedAngle = 0.709876; - - pair actual = Rot3::ToAxisAngle(R.between(R1)); - - EXPECT(assert_equal(expectedAxis, actual.first, 1e-6)); - EXPECT(assert_equal(expectedAngle, actual.second, 1e-6)); + // 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 + 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), yaw.between(R1).axisAngle().first, 1e-6)); + EXPECT_DOUBLES_EQUAL(M_PI/4, yaw.between(R1).axisAngle().second, 1e-9); + + EXPECT(assert_equal(Unit3(0, 1, 0), pitch.between(R1).axisAngle().first, 1e-6)); + EXPECT_DOUBLES_EQUAL(M_PI/4, pitch.between(R1).axisAngle().second, 1e-9); + + EXPECT(assert_equal(Unit3(1, 0, 0), roll.between(R1).axisAngle().first, 1e-6)); + EXPECT_DOUBLES_EQUAL(M_PI/4, roll.between(R1).axisAngle().second, 1e-9); + + Unit3 axis; double angle; + std::tie(axis,angle) = R.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); + + EXPECT_DOUBLES_EQUAL(theta - 2*M_PI, R2.axisAngle().second, 1e-9); + + theta = -M_PI/2; // 270 degrees + R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); + + EXPECT_DOUBLES_EQUAL(theta, R2.axisAngle().second, 1e-9); } /* ************************************************************************* */