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] 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));