From 7484b31bc3170dac6c7e18ef95e53d5f9fecc677 Mon Sep 17 00:00:00 2001
From: Alex Trevor <atrevor@gmail.com>
Date: Wed, 8 Jan 2014 00:18:58 -0500
Subject: [PATCH] Fixed bug on retraction of zero in Sphere2 expmap method.

---
 gtsam/geometry/Sphere2.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp
index 93a00eb2e..4049432e4 100644
--- a/gtsam/geometry/Sphere2.cpp
+++ b/gtsam/geometry/Sphere2.cpp
@@ -110,6 +110,11 @@ Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
   if (mode == Sphere2::EXPMAP) {
     double xi_hat_norm = xi_hat.norm();
     Vector exp_p_xi_hat = cos (xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
+
+    // Avoid nan
+    if (xi_hat_norm == 0.0)
+      return Sphere2 (point3 ());
+
     return Sphere2(exp_p_xi_hat);
   } else if (mode == Sphere2::RENORM) {
     // Project onto the manifold, i.e. the closest point on the circle to the new location;