static function & method for axis-angle with improved tests and fix for sign ambiguity

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<gtsam::Unit3, double> ToAxisAngle(const gtsam::Rot3& R) const;
+  static pair<gtsam::Unit3, double> 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<gtsam::Unit3, double> 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<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
+// //  BayesTreeClique(const pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
 //
 //   bool equals(const This& other, double tol) const;
 //   void print(string s) const;

gtsam/geometry/Rot3.cpp

@@ -16,6 +16,7 @@
  * @author  Christian Potthast
  * @author  Frank Dellaert
  * @author  Richard Roberts
+ * @author  Varun Agrawal
  */
 
 #include <gtsam/geometry/Rot3.h>
@@ -185,6 +186,11 @@ Vector Rot3::quaternion() const {
   return v;
 }
 
+/* ************************************************************************* */
+pair<Unit3, double> Rot3::axisAngle() {
+  return Rot3::ToAxisAngle(*this);
+}
+
 /* ************************************************************************* */
 Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
   return SO3::ExpmapDerivative(x);

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<Unit3, double> ToAxisAngle(const Rot3& R) {
       const Vector3 omega = Rot3::Logmap(R);
-      return std::pair<Unit3, double>(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, double>(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<Unit3, double> axisAngle();
+
     /** Compute the quaternion representation of this rotation.
      * @return The quaternion
      */

gtsam/geometry/tests/testRot3.cpp

@@ -663,16 +663,75 @@ TEST(Rot3, ClosestTo) {
 }
 
 /* ************************************************************************* */
-TEST(Rot3, angle) {
+TEST(Rot3, ToAxisAngle) {
-  Rot3 R1 = Rot3::Ypr(M_PI/4, 0, 0);
+  /// Test rotations along each axis
+  Rot3 R1;
 
-  Unit3 expectedAxis(-0.350067, -0.472463, 0.808846);
+  // Negative because rotation is counterclockwise when looking at unchanging axis
-  double expectedAngle = 0.709876;
+  Rot3 yaw = Rot3::Ypr(-M_PI/4, 0, 0);
-  
+  Rot3 pitch = Rot3::Ypr(0, -M_PI/4, 0);
-  pair<Unit3, double> actual = Rot3::ToAxisAngle(R.between(R1));
+  Rot3 roll = Rot3::Ypr(0, 0, -M_PI/4);
-  
+
-  EXPECT(assert_equal(expectedAxis, actual.first, 1e-6));
+  EXPECT(assert_equal(Unit3(0, 0, 1), Rot3::ToAxisAngle(yaw.between(R1)).first, 1e-6));
-  EXPECT(assert_equal(expectedAngle, actual.second, 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);
 }
 
 /* ************************************************************************* */