From 056254bf93b54b25c4e235618b1060be0f8f7388 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 23 Oct 2014 09:51:43 +0200
Subject: [PATCH] Optimized a bit more

---
 gtsam/geometry/Rot3Q.cpp | 23 ++++++++++++++---------
 1 file changed, 14 insertions(+), 9 deletions(-)

diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp
index d4545b886..6b7a4e0ce 100644
--- a/gtsam/geometry/Rot3Q.cpp
+++ b/gtsam/geometry/Rot3Q.cpp
@@ -121,24 +121,29 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-// Log map at identity - return the canonical coordinates of this rotation
   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_;
-    double qw = q.w();
-    if (std::abs(qw - 1.0) < 1e-10) {
+    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 (std::abs(qw + 1.0) < 1e-10) {
+    } 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);
-      // TODO C++ says: acos range is [0..pi], so below cannot occur !?
-      if (angle > M_PI) // Important:  use the smallest possible
-        angle -= 2.0 * M_PI; // angle, e.g. no more than PI, to keep
-      if (angle < -M_PI) // error continuous.
-        angle += 2.0 * M_PI;
+      // 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();
     }
   }