diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index 0fcf28dfb..a01be867d 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -19,52 +19,69 @@
 #include <gtsam/geometry/SO3.h>
 #include <gtsam/base/concepts.h>
 #include <cmath>
-
-using namespace std;
+#include <limits>
 
 namespace gtsam {
 
 /* ************************************************************************* */
-SO3 SO3::Rodrigues(const Vector3& axis, double theta) {
-  using std::cos;
-  using std::sin;
-
-  // get components of axis \omega
-  double wx = axis(0), wy = axis(1), wz = axis(2);
-
-  double costheta = cos(theta), sintheta = sin(theta), c_1 = 1 - costheta;
-  double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta, wz_sintheta = wz * sintheta;
-
-  double C00 = c_1 * wx * wx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz;
-  double C11 = c_1 * wy * wy, C12 = c_1 * wy * wz;
-  double C22 = c_1 * wz * wz;
-
+// Near zero, we just have I + skew(omega)
+static SO3 FirstOrder(const Vector3& omega) {
   Matrix3 R;
-  R(0, 0) =     costheta + C00;
-  R(1, 0) =  wz_sintheta + C01;
-  R(2, 0) = -wy_sintheta + C02;
-  R(0, 1) = -wz_sintheta + C01;
-  R(1, 1) =     costheta + C11;
-  R(2, 1) =  wx_sintheta + C12;
-  R(0, 2) =  wy_sintheta + C02;
-  R(1, 2) = -wx_sintheta + C12;
-  R(2, 2) =     costheta + C22;
-
+  R(0, 0) =  1.;
+  R(1, 0) =  omega.z();
+  R(2, 0) = -omega.y();
+  R(0, 1) = -omega.z();
+  R(1, 1) =  1.;
+  R(2, 1) =  omega.x();
+  R(0, 2) =  omega.y();
+  R(1, 2) = -omega.x();
+  R(2, 2) =  1.;
   return R;
 }
 
+SO3 SO3::Rodrigues(const Vector3& axis, double theta) {
+  if (theta*theta > std::numeric_limits<double>::epsilon()) {
+    using std::cos;
+    using std::sin;
+
+    // get components of axis \omega, where is a unit vector
+    const double& wx = axis.x(), wy = axis.y(), wz = axis.z();
+
+    const double costheta = cos(theta), sintheta = sin(theta), c_1 = 1 - costheta;
+    const double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta,
+                 wz_sintheta = wz * sintheta;
+
+    const double C00 = c_1 * wx * wx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz;
+    const double C11 = c_1 * wy * wy, C12 = c_1 * wy * wz;
+    const double C22 = c_1 * wz * wz;
+
+    Matrix3 R;
+    R(0, 0) =     costheta + C00;
+    R(1, 0) =  wz_sintheta + C01;
+    R(2, 0) = -wy_sintheta + C02;
+    R(0, 1) = -wz_sintheta + C01;
+    R(1, 1) =     costheta + C11;
+    R(2, 1) =  wx_sintheta + C12;
+    R(0, 2) =  wy_sintheta + C02;
+    R(1, 2) = -wx_sintheta + C12;
+    R(2, 2) =     costheta + C22;
+    return R;
+  } else {
+    return FirstOrder(axis*theta);
+  }
+
+}
+
 /// simply convert omega to axis/angle representation
-SO3 SO3::Expmap(const Vector3& omega,
-    ChartJacobian H) {
+SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
+  if (H) *H = ExpmapDerivative(omega);
 
-  if (H)
-    *H = ExpmapDerivative(omega);
-
-  if (omega.isZero())
-    return Identity();
-  else {
-    double angle = omega.norm();
-    return Rodrigues(omega / angle, angle);
+  double theta2 = omega.dot(omega);
+  if (theta2 > std::numeric_limits<double>::epsilon()) {
+    double theta = std::sqrt(theta2);
+    return Rodrigues(omega / theta, theta);
+  } else {
+    return FirstOrder(omega);
   }
 }
 
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index a08168ed8..9bcdd5f3d 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -93,7 +93,7 @@ public:
 
   /**
    * Exponential map at identity - create a rotation from canonical coordinates
-   * \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula
+   * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
    */
   static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);