From 3245a2304eeecaa0de08a789658829bfd0cddf49 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Sun, 12 Jul 2015 10:29:55 -0700
Subject: [PATCH] Improved small angle behavior of retract

---
 gtsam/geometry/Unit3.cpp | 43 +++++++++++++++++++++-------------------
 1 file changed, 23 insertions(+), 20 deletions(-)

diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp
index 922b1e940..5142faeb5 100644
--- a/gtsam/geometry/Unit3.cpp
+++ b/gtsam/geometry/Unit3.cpp
@@ -38,6 +38,7 @@
 
 #include <boost/random/variate_generator.hpp>
 #include <iostream>
+#include <limits>
 
 using namespace std;
 
@@ -135,37 +136,39 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
 
 /* ************************************************************************* */
 Unit3 Unit3::retract(const Vector2& v) const {
-
   // Compute the 3D xi_hat vector
   Vector3 xi_hat = basis() * v;
-  double xi_hat_norm = xi_hat.norm();
+  double theta = xi_hat.norm();
 
-  // When v is the so small and approximate as a direction
-  if (xi_hat_norm < 1e-8) {
-    return Unit3(cos(xi_hat_norm) * p_ + xi_hat);
+  // Treat case of very small v differently
+  if (theta < std::numeric_limits<double>::epsilon()) {
+    return Unit3(cos(theta) * p_ + xi_hat);
   }
 
-  Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p_
-      + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
+  Vector3 exp_p_xi_hat =
+      cos(theta) * p_ + xi_hat * (sin(theta) / theta);
   return Unit3(exp_p_xi_hat);
 }
 
 /* ************************************************************************* */
-Vector2 Unit3::localCoordinates(const Unit3& y) const {
-
-  double dot = p_.dot(y.p_);
-
-  // Check for special cases
-  if (std::abs(dot - 1.0) < 1e-16)
-    return Vector2(0.0, 0.0);
-  else if (std::abs(dot + 1.0) < 1e-16)
-    return Vector2(M_PI, 0.0);
-  else {
+Vector2 Unit3::localCoordinates(const Unit3& other) const {
+  const double x = p_.dot(other.p_);
+  // Crucial quantitity here is y = theta/sin(theta) with theta=acos(x)
+  // Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2)
+  // We treat the special caes 1 and -1 below
+  const double x2 = x * x;
+  const double z = 1 - x2;
+  double y;
+  if (z < std::numeric_limits<double>::epsilon()) {
+    if (x > 0)  // expand at x=1
+      y = 1.0 - (x - 1.0) / 3.0;
+    else  // cop out
+      return Vector2(M_PI, 0.0);
+  } else {
     // no special case
-    const double theta = acos(dot);
-    Vector3 result_hat = (theta / sin(theta)) * (y.p_ - p_ * dot);
-    return basis().transpose() * result_hat;
+    y = acos(x) / sqrt(z);
   }
+  return basis().transpose() * y * (other.p_ - x * p_);
 }
 /* ************************************************************************* */