diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index 19b4db924..4d0241147 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -317,26 +317,46 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
 
   // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
   // we do something special
-  if (tr + 1.0 < so3::kNearPiThresholdSq) {
-    // Used below
-    auto omegaFromQ = [](double W, double r, double w1, double w2, double w3) {
+  if (tr + 1.0 < 1e-3) {
+    if (R33 > R22 && R33 > R11) {
+      // R33 is the largest diagonal, a=3, b=1, c=2
+      const double W = R21 - R12;
+      const double Q1 = 2.0 + 2.0 * R33;
+      const double Q2 = R31 + R13;
+      const double Q3 = R23 + R32;
+      const double r = sqrt(Q1);
       const double one_over_r = 1 / r;
-      const double norm = sqrt(w1 * w1 + w2 * w2 + w3 * w3 + W * W);
+      const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
       const double sgn_w = W < 0 ? -1.0 : 1.0;
       const double mag = M_PI - (2 * sgn_w * W) / norm;
       const double scale = 0.5 * one_over_r * mag;
-      return sgn_w * scale * Vector3(w1, w2, w3);
-      };
-
-    if (R33 > R22 && R33 > R11) { // R33 is the largest diagonal, a=3, b=1, c=2
-      const double Q1 = 2.0 + 2.0 * R33;
-      omega = omegaFromQ(R21 - R12, sqrt(Q1), R31 + R13, R23 + R32, Q1);
-    } else if (R22 > R11) { // R22 is the largest diagonal, a=2, b=3, c=1
+      omega = sgn_w * scale * Vector3(Q2, Q3, Q1);
+    } else if (R22 > R11) {
+      // R22 is the largest diagonal, a=2, b=3, c=1
+      const double W = R13 - R31;
       const double Q1 = 2.0 + 2.0 * R22;
-      omega = omegaFromQ(R13 - R31, sqrt(Q1), R12 + R21, Q1, R23 + R32);
-    } else { // R11 is the largest diagonal, a=1, b=2, c=3
+      const double Q2 = R23 + R32;
+      const double Q3 = R12 + R21;
+      const double r = sqrt(Q1);
+      const double one_over_r = 1 / r;
+      const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
+      const double sgn_w = W < 0 ? -1.0 : 1.0;
+      const double mag = M_PI - (2 * sgn_w * W) / norm;
+      const double scale = 0.5 * one_over_r * mag;
+      omega = sgn_w * scale * Vector3(Q3, Q1, Q2);
+    } else {
+      // R11 is the largest diagonal, a=1, b=2, c=3
+      const double W = R32 - R23;
       const double Q1 = 2.0 + 2.0 * R11;
-      omega = omegaFromQ(R32 - R23, sqrt(Q1), Q1, R12 + R21, R31 + R13);
+      const double Q2 = R12 + R21;
+      const double Q3 = R31 + R13;
+      const double r = sqrt(Q1);
+      const double one_over_r = 1 / r;
+      const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
+      const double sgn_w = W < 0 ? -1.0 : 1.0;
+      const double mag = M_PI - (2 * sgn_w * W) / norm;
+      const double scale = 0.5 * one_over_r * mag;
+      omega = sgn_w * scale * Vector3(Q1, Q2, Q3);
     }
   } else {
     double magnitude;