diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h
index 1557a09db..2ef47d58e 100644
--- a/gtsam/geometry/Quaternion.h
+++ b/gtsam/geometry/Quaternion.h
@@ -117,13 +117,23 @@ struct traits<QUATERNION_TYPE> {
       omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
     } else {
       // Normal, away from zero case
-      _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
-      // Important:  convert to [-pi,pi] to keep error continuous
-      if (angle > M_PI)
-      angle -= twoPi;
-      else if (angle < -M_PI)
-      angle += twoPi;
-      omega = (angle / s) * q.vec();
+      if (qw > 0) {
+        _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
+        // Important:  convert to [-pi,pi] to keep error continuous
+        if (angle > M_PI)
+          angle -= twoPi;
+        else if (angle < -M_PI)
+          angle += twoPi;
+        omega = (angle / s) * q.vec();
+      } else {
+        // Make sure that we are using a canonical quaternion with w > 0
+        _Scalar angle = 2 * acos(-qw), s = sqrt(1 - qw * qw);
+        if (angle > M_PI)
+          angle -= twoPi;
+        else if (angle < -M_PI)
+          angle += twoPi;
+        omega = (angle / s) * -q.vec();
+      }
     }
 
     if(H) *H = SO3::LogmapDerivative(omega.template cast<double>());