diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index 76cae09d9..35b2c15b5 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -30,40 +30,55 @@ namespace gtsam {
 struct ExpmapImpl {
   const Vector3 omega;
   const double theta2;
-  Matrix3 W;
+  Matrix3 W, K, KK;
   bool nearZero;
-  double theta, sin_over_theta, one_minus_cos;
+  double theta, sin_theta, sin_over_theta, one_minus_cos, a, b;
 
-  void Initialize() {
-    const double wx = omega.x(), wy = omega.y(), wz = omega.z();
-    W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;  // Skew[omega]
+  void init() {
     nearZero = (theta2 <= std::numeric_limits<double>::epsilon());
     if (!nearZero) {
       theta = std::sqrt(theta2);  // rotation angle
-      sin_over_theta = std::sin(theta) / theta;
+      sin_theta = std::sin(theta);
+      sin_over_theta = sin_theta / theta;
       const double s2 = std::sin(theta / 2.0);
       one_minus_cos =
-          2.0 * s2 * s2;  // numerically better behaved than [1 - cos(theta)]
+          2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
+      a = one_minus_cos / theta;
+      b = 1.0 - sin_over_theta;
     }
   }
 
   // Constructor with element of Lie algebra so(3): W = omega^, normalized by
   // normx
   ExpmapImpl(const Vector3& omega) : omega(omega), theta2(omega.dot(omega)) {
-    Initialize();
+    const double wx = omega.x(), wy = omega.y(), wz = omega.z();
+    W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
+    init();
+    if (!nearZero) {
+      theta = std::sqrt(theta2);
+      K = W / theta;
+      KK = K * K;
+    }
   }
 
   // Constructor with axis-angle
-  ExpmapImpl(const Vector3& axis, double theta)
-      : omega(axis * theta), theta2(theta * theta) {
-    Initialize();
+  ExpmapImpl(const Vector3& axis, double angle)
+      : omega(axis * angle), theta2(angle * angle) {
+    const double ax = axis.x(), ay = axis.y(), az = axis.z();
+    K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
+    W = K * angle;
+    init();
+    if (!nearZero) {
+      theta = angle;
+      KK = K * K;
+    }
   }
 
   SO3 operator()() const {
     if (nearZero)
       return I_3x3 + W;
     else
-      return I_3x3 + sin_over_theta * W + one_minus_cos * W * W / theta2;
+      return I_3x3 + sin_theta * K + one_minus_cos * K * K;
   }
 
   // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
@@ -73,39 +88,32 @@ struct ExpmapImpl {
   // This maps a perturbation v in the tangent space to
   // a perturbation on the manifold Expmap(dexp * v) */
   SO3 dexp() const {
-    if (nearZero) {
+    if (nearZero)
       return I_3x3 - 0.5 * W;
-    } else {
-      const double a = one_minus_cos / theta2;
-      const double b = (1.0 - sin_over_theta) / theta2;
-      return I_3x3 - a * W + b * W * W;
-    }
+    else
+      return I_3x3 - a * K + b * KK;
   }
 
   // Just multiplies with dexp()
-  Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
-                    OptionalJacobian<3, 3> H2 = boost::none) const {
+  Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
+                    OptionalJacobian<3, 3> H2) const {
     if (nearZero) {
       if (H1) *H1 = 0.5 * skewSymmetric(v);
       if (H2) *H2 = I_3x3;
       return v;
-    } else {
-      const double a = one_minus_cos / theta2;
-      const double b = (1.0 - sin_over_theta) / theta2;
-      Matrix3 dexp = I_3x3 - a * W + b * W * W;
-      if (H1) {
-        const Vector3 Wv = omega.cross(v);
-        const Vector3 WWv = omega.cross(Wv);
-        const Matrix3 T = skewSymmetric(v);
-        const double Da = (sin_over_theta - 2.0 * a) / theta2;
-        const double Db =
-            (3.0 * sin_over_theta - std::cos(theta) - 2.0) / theta2 / theta2;
-        *H1 = (-Da * Wv + Db * WWv) * omega.transpose() + a * T -
-              b * skewSymmetric(Wv) - b * W * T;
-      }
-      if (H2) *H2 = dexp;
-      return dexp * v;
     }
+    Matrix3 dexp = I_3x3 - a * K + b * KK;
+    if (H1) {
+      const Vector3 Kv = omega.cross(v / theta);
+      const Vector3 KKv = omega.cross(Kv / theta);
+      const Matrix3 T = skewSymmetric(v / theta);
+      const double Da = (sin_over_theta - 2.0 * a / theta) / theta;
+      const double Db = (3.0 * sin_over_theta - std::cos(theta) - 2.0) / theta2;
+      *H1 = (-Da * Kv + Db * KKv) * omega.transpose() + a * T -
+            skewSymmetric(Kv * b / theta) - b * K * T;
+    }
+    if (H2) *H2 = dexp;
+    return dexp * v;
   }
 };