diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp
index 8ddef1d7e..f661f819d 100755
--- a/gtsam/geometry/Unit3.cpp
+++ b/gtsam/geometry/Unit3.cpp
@@ -254,27 +254,27 @@ Unit3 Unit3::retract(const Vector2& v, OptionalJacobian<2,2> H) const {
   // Compute the 3D xi_hat vector
   const Vector3 xi_hat = basis() * v;
   const double theta = xi_hat.norm();
+  const double c = std::cos(theta);
 
   // Treat case of very small v differently.
   Matrix23 H_from_point;
   if (theta < std::numeric_limits<double>::epsilon()) {
-    const Unit3 exp_p_xi_hat = Unit3::FromPoint3(std::cos(theta) * p_ + xi_hat,
+    const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat,
                                                  H? &H_from_point : nullptr);
     if (H) { // Jacobian
-      *H = H_from_point * (-p_ * xi_hat.transpose() * basis() + basis());
+      *H = H_from_point *
+          (-p_ * xi_hat.transpose() + Matrix33::Identity()) * basis();
     }
     return exp_p_xi_hat;
   }
 
-  const Unit3 exp_p_xi_hat = Unit3::FromPoint3(
-        std::cos(theta) * p_ + xi_hat * (sin(theta) / theta),
-                        H? &H_from_point : nullptr);
+  const double st = std::sin(theta) / theta;
+  const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat * st,
+                                               H? &H_from_point : nullptr);
   if (H) { // Jacobian
     *H = H_from_point *
-        (p_ * (-std::sin(theta)) * xi_hat.transpose() / theta * basis() +
-        std::sin(theta) / theta * basis() +
-        xi_hat * ((theta * std::cos(theta) - std::sin(theta)) / std::pow(theta, 2))
-        * xi_hat.transpose() / theta * basis());
+        (p_ * -st * xi_hat.transpose() + st * Matrix33::Identity() +
+        xi_hat * ((c - st) / std::pow(theta, 2)) * xi_hat.transpose()) * basis();
   }
   return exp_p_xi_hat;
 }