diff --git a/gtsam.h b/gtsam.h index 062bcb531..04a1b1ac4 100644 --- a/gtsam.h +++ b/gtsam.h @@ -639,6 +639,7 @@ class Rot3 { static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) static gtsam::Rot3 Ypr(double y, double p, double r); static gtsam::Rot3 Quaternion(double w, double x, double y, double z); + static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle); static gtsam::Rot3 Rodrigues(Vector v); static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); static gtsam::Rot3 ClosestTo(const Matrix M); @@ -674,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; @@ -1310,7 +1312,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..3481ac146 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,12 @@ Vector Rot3::quaternion() const { return v; } +/* ************************************************************************* */ +pair Rot3::axisAngle() const { + const Vector3 omega = Rot3::Logmap(*this); + return std::pair(Unit3(omega), omega.norm()); +} + /* ************************************************************************* */ Matrix3 Rot3::ExpmapDerivative(const Vector3& x) { return SO3::ExpmapDerivative(x); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index e4408c9f4..e5e65b77d 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 @@ -194,7 +195,7 @@ namespace gtsam { /** * Convert from axis/angle representation - * @param axisw is the rotation axis, unit length + * @param axis is the rotation axis, unit length * @param angle rotation angle * @return incremental rotation */ @@ -429,7 +430,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; @@ -461,6 +462,16 @@ namespace gtsam { /// @name Advanced Interface /// @{ + /** + * 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() const; + /** 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 c95b85f21..efb4bf70b 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -14,6 +14,7 @@ * @brief Unit tests for Rot3 class - common between Matrix and Quaternion * @author Alireza Fathi * @author Frank Dellaert + * @author Varun Agrawal */ #include @@ -661,6 +662,65 @@ TEST(Rot3, ClosestTo) { EXPECT(assert_equal(expected, actual.matrix(), 1e-6)); } +/* ************************************************************************* */ +TEST(Rot3, axisAngle) { + Unit3 actualAxis; + double actualAngle; + +// not a lambda as otherwise we can't trace error easily +#define CHECK_AXIS_ANGLE(expectedAxis, expectedAngle, rotation) \ + std::tie(actualAxis, actualAngle) = rotation.axisAngle(); \ + EXPECT(assert_equal(expectedAxis, actualAxis, 1e-9)); \ + EXPECT_DOUBLES_EQUAL(expectedAngle, actualAngle, 1e-9); \ + EXPECT(assert_equal(rotation, Rot3::AxisAngle(expectedAxis, expectedAngle))) + + // CHECK R defined at top = Rot3::Rodrigues(0.1, 0.4, 0.2) + Vector3 omega(0.1, 0.4, 0.2); + Unit3 axis(omega), _axis(-omega); + CHECK_AXIS_ANGLE(axis, omega.norm(), R); + + // rotate by 90 + CHECK_AXIS_ANGLE(Unit3(1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, M_PI_2)) + CHECK_AXIS_ANGLE(Unit3(0, 1, 0), M_PI_2, Rot3::Ypr(0, M_PI_2, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, 1), M_PI_2, Rot3::Ypr(M_PI_2, 0, 0)) + CHECK_AXIS_ANGLE(axis, M_PI_2, Rot3::AxisAngle(axis, M_PI_2)) + + // rotate by -90 + CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, -M_PI_2)) + CHECK_AXIS_ANGLE(Unit3(0, -1, 0), M_PI_2, Rot3::Ypr(0, -M_PI_2, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, -1), M_PI_2, Rot3::Ypr(-M_PI_2, 0, 0)) + CHECK_AXIS_ANGLE(_axis, M_PI_2, Rot3::AxisAngle(axis, -M_PI_2)) + + // rotate by 270 + const double theta270 = M_PI + M_PI / 2; + CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, theta270)) + CHECK_AXIS_ANGLE(Unit3(0, -1, 0), M_PI_2, Rot3::Ypr(0, theta270, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, -1), M_PI_2, Rot3::Ypr(theta270, 0, 0)) + CHECK_AXIS_ANGLE(_axis, M_PI_2, Rot3::AxisAngle(axis, theta270)) + + // rotate by -270 + const double theta_270 = -(M_PI + M_PI / 2); // 90 (or -270) degrees + CHECK_AXIS_ANGLE(Unit3(1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, theta_270)) + CHECK_AXIS_ANGLE(Unit3(0, 1, 0), M_PI_2, Rot3::Ypr(0, theta_270, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, 1), M_PI_2, Rot3::Ypr(theta_270, 0, 0)) + CHECK_AXIS_ANGLE(axis, M_PI_2, Rot3::AxisAngle(axis, theta_270)) + + const double theta195 = 195 * M_PI / 180; + const double theta165 = 165 * M_PI / 180; + + /// Non-trivial angle 165 + CHECK_AXIS_ANGLE(Unit3(1, 0, 0), theta165, Rot3::Ypr(0, 0, theta165)) + CHECK_AXIS_ANGLE(Unit3(0, 1, 0), theta165, Rot3::Ypr(0, theta165, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, 1), theta165, Rot3::Ypr(theta165, 0, 0)) + CHECK_AXIS_ANGLE(axis, theta165, Rot3::AxisAngle(axis, theta165)) + + /// Non-trivial angle 195 + CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), theta165, Rot3::Ypr(0, 0, theta195)) + CHECK_AXIS_ANGLE(Unit3(0, -1, 0), theta165, Rot3::Ypr(0, theta195, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, -1), theta165, Rot3::Ypr(theta195, 0, 0)) + CHECK_AXIS_ANGLE(_axis, theta165, Rot3::AxisAngle(axis, theta195)) +} + /* ************************************************************************* */ int main() { TestResult tr;