From 9dc3d28bf2a15b44b794699bdda3a9be7d5eb5ad Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Mon, 8 Dec 2014 12:37:00 +0100
Subject: [PATCH] Now Rot3 uses QuaternionChart Expmap/Logmap in quaternion
 mode

---
 gtsam/geometry/Rot3.h    |  9 ++++-----
 gtsam/geometry/Rot3Q.cpp | 28 +++-------------------------
 2 files changed, 7 insertions(+), 30 deletions(-)

diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index edd39619f..bfa42c980 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -23,6 +23,10 @@
 
 #include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
 
+#ifdef GTSAM_USE_QUATERNIONS
+#include <gtsam/geometry/Quaternion.h>
+#endif
+
 // You can override the default coordinate mode using this flag
 #ifndef ROT3_DEFAULT_COORDINATES_MODE
   #ifdef GTSAM_USE_QUATERNIONS
@@ -46,11 +50,6 @@
 
 namespace gtsam {
 
-  /// Typedef to an Eigen Quaternion<double>, we disable alignment because
-  /// geometry objects are stored in boost pool allocators, in Values
-  /// containers, and and these pool allocators do not support alignment.
-  typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
-
   /**
    * @brief A 3D rotation represented as a rotation matrix if the preprocessor
    * symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it
diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp
index 26ca25bf2..cb7236823 100644
--- a/gtsam/geometry/Rot3Q.cpp
+++ b/gtsam/geometry/Rot3Q.cpp
@@ -85,7 +85,8 @@ namespace gtsam {
 
   /* ************************************************************************* */
   Rot3 Rot3::rodriguez(const Vector& w, double theta) {
-    return Quaternion(Eigen::AngleAxisd(theta, w)); }
+    return QuaternionChart::Expmap(theta,w);
+  }
 
   /* ************************************************************************* */
   Rot3 Rot3::compose(const Rot3& R2,
@@ -134,30 +135,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   Vector3 Rot3::Logmap(const Rot3& R) {
-    using std::acos;
-    using std::sqrt;
-    static const double twoPi = 2.0 * M_PI,
-    // define these compile time constants to avoid std::abs:
-        NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10;
-
-    const Quaternion& q = R.quaternion_;
-    const double qw = q.w();
-    if (qw > NearlyOne) {
-      // Taylor expansion of (angle / s) at 1
-      return (2 - 2 * (qw - 1) / 3) * q.vec();
-    } else if (qw < NearlyNegativeOne) {
-      // Angle is zero, return zero vector
-      return Vector3::Zero();
-    } else {
-      // Normal, away from zero case
-      double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
-      // Important:  convert to [-pi,pi] to keep error continuous
-      if (angle > M_PI)
-        angle -= twoPi;
-      else if (angle < -M_PI)
-        angle += twoPi;
-      return (angle / s) * q.vec();
-    }
+    return QuaternionChart::Logmap(R.quaternion_);
   }
 
   /* ************************************************************************* */