From 0e6efb98d7f3e8b7eb5b38dfc8830d8b19c2f670 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 6 Mar 2020 16:28:31 -0500
Subject: [PATCH 01/11] compute angle in degrees between 2 Rot3 matrices

---
 gtsam.h                           |  1 +
 gtsam/geometry/Rot3.cpp           |  5 +++++
 gtsam/geometry/Rot3.h             |  6 ++++++
 gtsam/geometry/tests/testRot3.cpp | 15 +++++++++++++++
 4 files changed, 27 insertions(+)

diff --git a/gtsam.h b/gtsam.h
index f943c2e9d..6e2eea686 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -677,6 +677,7 @@ class Rot3 {
 //  Vector toQuaternion() const;  // FIXME: Can't cast to Vector properly
   Vector quaternion() const;
   gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const;
+  double angle(const gtsam::Rot3& other) const;
 
   // enabling serialization functionality
   void serialize() const;
diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index b1e8dd14b..4eee045f9 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -228,6 +228,11 @@ Rot3 Rot3::slerp(double t, const Rot3& other) const {
   return interpolate(*this, other, t);
 }
 
+/* ************************************************************************* */
+double Rot3::angle(const Rot3& other) const {
+  return Rot3::Logmap(this->between(other)).norm() * 180.0 / M_PI;
+}
+
 /* ************************************************************************* */
 
 } // namespace gtsam
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index e4408c9f4..d810285fb 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -479,6 +479,12 @@ namespace gtsam {
      */
     Rot3 slerp(double t, const Rot3& other) const;
 
+    /**
+     * @brief Compute the angle (in degrees) between *this and other
+     * @param other Rot3 element
+     */
+    double angle(const Rot3& other) const;
+
     /// Output stream operator
     GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
 
diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index c95b85f21..84c5bd094 100644
--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@@ -661,6 +661,21 @@ TEST(Rot3, ClosestTo) {
   EXPECT(assert_equal(expected, actual.matrix(), 1e-6));
 }
 
+/* ************************************************************************* */
+TEST(Rot3, angle) {
+  Rot3 R1(0.996136,  -0.0564186, -0.0672982, //
+          0.0419472,  0.978941,  -0.199787, //
+          0.0771527,  0.196192,   0.977525);
+  
+  Rot3 R2(0.99755,  -0.0377748, -0.0588831, //
+          0.0238455, 0.974883,  -0.221437, //
+          0.0657689, 0.21949,    0.973395);
+
+  double expected = 1.7765686464446904;
+
+  EXPECT(assert_equal(expected, R1.angle(R2), 1e-6));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From 087247c6e03bbf2c6032d89c181370cdecf6ab46 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 6 Mar 2020 17:54:25 -0500
Subject: [PATCH 02/11] return axis-angle representation instead of just angle,
 updated test to be much simpler

---
 gtsam/geometry/Rot3.cpp           |  5 +++--
 gtsam/geometry/Rot3.h             |  4 ++--
 gtsam/geometry/tests/testRot3.cpp | 18 +++++++++---------
 3 files changed, 14 insertions(+), 13 deletions(-)

diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index 4eee045f9..9b68bb1f1 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -229,8 +229,9 @@ Rot3 Rot3::slerp(double t, const Rot3& other) const {
 }
 
 /* ************************************************************************* */
-double Rot3::angle(const Rot3& other) const {
-  return Rot3::Logmap(this->between(other)).norm() * 180.0 / M_PI;
+pair<Unit3, double> Rot3::axisAngle(const Rot3& other) const {
+  Vector3 rot = Rot3::Logmap(this->between(other));
+  return pair<Unit3, double>(Unit3(rot), rot.norm());
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index d810285fb..41b5a6ca1 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -480,10 +480,10 @@ namespace gtsam {
     Rot3 slerp(double t, const Rot3& other) const;
 
     /**
-     * @brief Compute the angle (in degrees) between *this and other
+     * @brief Compute the Euler axis and angle (in radians) between *this and other
      * @param other Rot3 element
      */
-    double angle(const Rot3& other) const;
+    std::pair<Unit3, double> axisAngle(const Rot3& other) const;
 
     /// Output stream operator
     GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index 84c5bd094..c37dfda35 100644
--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@@ -663,17 +663,17 @@ TEST(Rot3, ClosestTo) {
 
 /* ************************************************************************* */
 TEST(Rot3, angle) {
-  Rot3 R1(0.996136,  -0.0564186, -0.0672982, //
-          0.0419472,  0.978941,  -0.199787, //
-          0.0771527,  0.196192,   0.977525);
+  Rot3 R1 = Rot3::Ypr(0, 0, 0);
   
-  Rot3 R2(0.99755,  -0.0377748, -0.0588831, //
-          0.0238455, 0.974883,  -0.221437, //
-          0.0657689, 0.21949,    0.973395);
+  Rot3 R2 = Rot3::Ypr(M_PI/4, 0, 0);
 
-  double expected = 1.7765686464446904;
-
-  EXPECT(assert_equal(expected, R1.angle(R2), 1e-6));
+  Unit3 expectedAxis(0, 0, 1);
+  double expectedAngle = M_PI/4;
+  
+  pair<Unit3, double> actual = R1.axisAngle(R2);
+  
+  EXPECT(assert_equal(expectedAxis, actual.first, 1e-6));
+  EXPECT(assert_equal(expectedAngle, actual.second, 1e-6));
 }
 
 /* ************************************************************************* */

From 7019d6f4b8f9d45dae8f5f9cbe7b758bd6a5dac1 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Mar 2020 08:35:08 -0500
Subject: [PATCH 03/11] convert axisAngle to static ToAxisAngle, update tests

---
 gtsam.h                           |  2 +-
 gtsam/geometry/Rot3.cpp           |  6 ------
 gtsam/geometry/Rot3.h             | 18 +++++++++++-------
 gtsam/geometry/tests/testRot3.cpp | 11 +++++------
 4 files changed, 17 insertions(+), 20 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index 6e2eea686..521495bdf 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -642,6 +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;
 
   // Testable
   void print(string s) const;
@@ -677,7 +678,6 @@ class Rot3 {
 //  Vector toQuaternion() const;  // FIXME: Can't cast to Vector properly
   Vector quaternion() const;
   gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const;
-  double angle(const gtsam::Rot3& other) const;
 
   // enabling serialization functionality
   void serialize() const;
diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index 9b68bb1f1..b1e8dd14b 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -228,12 +228,6 @@ Rot3 Rot3::slerp(double t, const Rot3& other) const {
   return interpolate(*this, other, t);
 }
 
-/* ************************************************************************* */
-pair<Unit3, double> Rot3::axisAngle(const Rot3& other) const {
-  Vector3 rot = Rot3::Logmap(this->between(other));
-  return pair<Unit3, double>(Unit3(rot), rot.norm());
-}
-
 /* ************************************************************************* */
 
 } // namespace gtsam
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 41b5a6ca1..974e9b195 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -194,7 +194,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
      */
@@ -216,6 +216,16 @@ namespace gtsam {
       return AxisAngle(axis.unitVector(),angle);
     }
 
+     /**
+      * Compute to 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());
+    }
+
     /**
      * Rodrigues' formula to compute an incremental rotation
      * @param w a vector of incremental roll,pitch,yaw
@@ -479,12 +489,6 @@ namespace gtsam {
      */
     Rot3 slerp(double t, const Rot3& other) const;
 
-    /**
-     * @brief Compute the Euler axis and angle (in radians) between *this and other
-     * @param other Rot3 element
-     */
-    std::pair<Unit3, double> axisAngle(const Rot3& other) const;
-
     /// Output stream operator
     GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
 
diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index c37dfda35..dbd4e84a1 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 <gtsam/geometry/Point3.h>
@@ -663,14 +664,12 @@ TEST(Rot3, ClosestTo) {
 
 /* ************************************************************************* */
 TEST(Rot3, angle) {
-  Rot3 R1 = Rot3::Ypr(0, 0, 0);
-  
-  Rot3 R2 = Rot3::Ypr(M_PI/4, 0, 0);
+  Rot3 R1 = Rot3::Ypr(M_PI/4, 0, 0);
 
-  Unit3 expectedAxis(0, 0, 1);
-  double expectedAngle = M_PI/4;
+  Unit3 expectedAxis(-0.350067, -0.472463, 0.808846);
+  double expectedAngle = 0.709876;
   
-  pair<Unit3, double> actual = R1.axisAngle(R2);
+  pair<Unit3, double> actual = Rot3::ToAxisAngle(R.between(R1));
   
   EXPECT(assert_equal(expectedAxis, actual.first, 1e-6));
   EXPECT(assert_equal(expectedAngle, actual.second, 1e-6));

From 0fbe63aa1746fcddb02763a8e340df971c746cf7 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 8 Mar 2020 17:07:40 -0400
Subject: [PATCH 04/11] static function & method for axis-angle with improved
 tests and fix for sign ambiguity

---
 gtsam.h                           |  5 +-
 gtsam/geometry/Rot3.cpp           |  6 +++
 gtsam/geometry/Rot3.h             | 20 ++++++--
 gtsam/geometry/tests/testRot3.cpp | 77 +++++++++++++++++++++++++++----
 4 files changed, 94 insertions(+), 14 deletions(-)

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<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;
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 <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);
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<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
      */
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<Unit3, double> 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);
 }
 
 /* ************************************************************************* */

From ab46c7c3cee81cb69185509b5da3acb3a40f6aff Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 9 Mar 2020 16:01:24 -0400
Subject: [PATCH 05/11] removed static ToAxisAngle function

---
 gtsam.h                           |  1 -
 gtsam/geometry/Rot3.cpp           |  9 +++++++-
 gtsam/geometry/Rot3.h             | 16 --------------
 gtsam/geometry/tests/testRot3.cpp | 36 -------------------------------
 4 files changed, 8 insertions(+), 54 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index 8382c06a7..de1a3887b 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -642,7 +642,6 @@ 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 pair<gtsam::Unit3, double> ToAxisAngle(const gtsam::Rot3& R);
 
   // Testable
   void print(string s) const;
diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index 9fc7ee6b1..14646659d 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -188,7 +188,14 @@ Vector Rot3::quaternion() const {
 
 /* ************************************************************************* */
 pair<Unit3, double> Rot3::axisAngle() {
-  return Rot3::ToAxisAngle(*this);
+  const Vector3 omega = Rot3::Logmap(*this);
+  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());
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 3f2a962d5..b1cfc4926 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -217,22 +217,6 @@ namespace gtsam {
       return AxisAngle(axis.unitVector(),angle);
     }
 
-     /**
-      * 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);
-      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());
-    }
-
     /**
      * Rodrigues' formula to compute an incremental rotation
      * @param w a vector of incremental roll,pitch,yaw
diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index 59397d199..1b01b92c2 100644
--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@@ -662,42 +662,6 @@ TEST(Rot3, ClosestTo) {
   EXPECT(assert_equal(expected, actual.matrix(), 1e-6));
 }
 
-/* ************************************************************************* */
-TEST(Rot3, ToAxisAngle) {
-  /// 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), 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

From 1553d4321c479d60cecaea78e5dc537137cc5e67 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 13 Mar 2020 19:33:57 -0400
Subject: [PATCH 06/11] correct way of maintaining angle of axis-angle
 representation within (-pi,pi]

---
 gtsam/geometry/Rot3.cpp | 20 +++++++++++++-------
 1 file changed, 13 insertions(+), 7 deletions(-)

diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index 14646659d..1256271bc 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -188,14 +188,20 @@ Vector Rot3::quaternion() const {
 
 /* ************************************************************************* */
 pair<Unit3, double> Rot3::axisAngle() {
-  const Vector3 omega = Rot3::Logmap(*this);
-  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;
+  Vector3 omega = Rot3::Logmap(*this);
+  // Get the rotation angle.
+  double theta = omega.norm();
+
+  // Check if angle 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;    
   }
-  return std::pair<Unit3, double>(Unit3(omega), direction * omega.norm());
+  return std::pair<Unit3, double>(Unit3(omega), theta);
 }
 
 /* ************************************************************************* */

From 8f2a00e4bd2222b71e86da5473856875275e91f8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 14 Mar 2020 10:10:00 -0400
Subject: [PATCH 07/11] fix for angle outside the range (-pi,pi] and added more
 tests to verify

---
 gtsam/geometry/Rot3.cpp           | 12 +++++-------
 gtsam/geometry/tests/testRot3.cpp | 19 ++++++++++++++-----
 2 files changed, 19 insertions(+), 12 deletions(-)

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<Unit3, double> 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, double>(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);
 }
 
 /* ************************************************************************* */

From 170d1526b73a824c8d6b509a66d2b266065b01fc Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 16 Mar 2020 23:33:34 -0400
Subject: [PATCH 08/11] =?UTF-8?q?axisAngle=20maintains=20angle=20between?=
 =?UTF-8?q?=20[0,=20=CF=80]=20so=20updated=20docs=20and=20tests=20accordin?=
 =?UTF-8?q?gly?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 gtsam/geometry/Rot3.cpp           | 12 +-----------
 gtsam/geometry/Rot3.h             |  3 +++
 gtsam/geometry/tests/testRot3.cpp |  6 ++++--
 3 files changed, 8 insertions(+), 13 deletions(-)

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<Unit3, double> 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, double>(Unit3(omega), theta);
+  return std::pair<Unit3, double>(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<Unit3, double> 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);

From 81b52c674a4e3c1c6303003b8365f039ff06cbd7 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 16 Mar 2020 23:33:55 -0400
Subject: [PATCH 09/11] wrap AxisAngle constructor for rotations

---
 gtsam.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam.h b/gtsam.h
index de1a3887b..fbd70622f 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);

From 0d46932456667015ce8402767987aea40b5514d6 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 17 Mar 2020 13:15:35 -0400
Subject: [PATCH 10/11] Proper const

---
 gtsam/geometry/Rot3.cpp | 4 ++--
 gtsam/geometry/Rot3.h   | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index 9b305640b..3481ac146 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -187,8 +187,8 @@ Vector Rot3::quaternion() const {
 }
 
 /* ************************************************************************* */
-pair<Unit3, double> Rot3::axisAngle() {
-  Vector3 omega = Rot3::Logmap(*this);
+pair<Unit3, double> Rot3::axisAngle() const {
+  const Vector3 omega = Rot3::Logmap(*this);
   return std::pair<Unit3, double>(Unit3(omega), omega.norm());
 }
 
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 80d363d35..e5e65b77d 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -470,7 +470,7 @@ namespace gtsam {
       * within the specified range.
       * @return pair consisting of Unit3 axis and angle in radians
       */
-    std::pair<Unit3, double> axisAngle();
+    std::pair<Unit3, double> axisAngle() const;
 
     /** Compute the quaternion representation of this rotation.
      * @return The quaternion

From ca549630debb250112ed44d5bad91387f0daf96d Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 17 Mar 2020 13:15:48 -0400
Subject: [PATCH 11/11] More systematic tests

---
 gtsam/geometry/tests/testRot3.cpp | 80 ++++++++++++++++++-------------
 1 file changed, 46 insertions(+), 34 deletions(-)

diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index 6b0daa368..efb4bf70b 100644
--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@@ -664,49 +664,61 @@ TEST(Rot3, ClosestTo) {
 
 /* ************************************************************************* */
 TEST(Rot3, axisAngle) {
-  /// Test rotations along each axis
-  Rot3 R1;
+  Unit3 actualAxis;
+  double actualAngle;
 
-  // 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);
+// 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)))
 
-  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);
+  // 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);
 
-  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);
+  // 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))
 
-  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);
+  // 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))
 
-  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);
+  // 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))
 
-  double theta;
+  // 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))
 
-  /// 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(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);
+  const double theta195 = 195 * M_PI / 180;
+  const double theta165 = 165 * M_PI / 180;
 
-  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);
+  /// 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 angles
-  theta = 195 * M_PI / 180;  // 195 degrees
-  Rot3 R3 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta);
-  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);
-  EXPECT_DOUBLES_EQUAL(2*M_PI + theta, R3.axisAngle().second, 1e-9);
+  /// 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))
 }
 
 /* ************************************************************************* */