diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp
index 52d475d5d..fd2c7ab65 100644
--- a/gtsam/geometry/Cal3Fisheye.cpp
+++ b/gtsam/geometry/Cal3Fisheye.cpp
@@ -46,9 +46,9 @@ double Cal3Fisheye::Scaling(double r) {
 /* ************************************************************************* */
 Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
                                 OptionalJacobian<2, 2> H2) const {
-  const double xi = p.x(), yi = p.y();
+  const double xi = p.x(), yi = p.y(), zi = 1;
   const double r2 = xi * xi + yi * yi, r = sqrt(r2);
-  const double t = atan(r);
+  const double t = atan2(r, zi);
   const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4;
   Vector5 K, T;
   K << 1, k1_, k2_, k3_, k4_;
@@ -76,28 +76,32 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
 
   // Derivative for points in intrinsic coords (2 by 2)
   if (H2) {
-    const double dtd_dt =
-        1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8;
-    const double dt_dr = 1 / (1 + r2);
-    const double rinv = 1 / r;
-    const double dr_dxi = xi * rinv;
-    const double dr_dyi = yi * rinv;
-    const double dtd_dxi = dtd_dt * dt_dr * dr_dxi;
-    const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
+    if (r2==0) {
+      *H2 = DK;
+    } else {
+      const double dtd_dt =
+          1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8;
+      const double R2 = r2 + zi*zi;
+      const double dt_dr = zi / R2;
+      const double rinv = 1 / r;
+      const double dr_dxi = xi * rinv;
+      const double dr_dyi = yi * rinv;
+      const double dtd_dr = dtd_dt * dt_dr;
+  
+      const double c2 = dr_dxi * dr_dxi;
+      const double s2 = dr_dyi * dr_dyi;
+      const double cs = dr_dxi * dr_dyi;
 
-    const double td = t * K.dot(T);
-    const double rrinv = 1 / r2;
-    const double dxd_dxi =
-        dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi;
-    const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi;
-    const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi;
-    const double dyd_dyi =
-        dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi;
+      const double dxd_dxi = dtd_dr * c2 + s * (1 - c2);
+      const double dxd_dyi = (dtd_dr - s) * cs;
+      const double dyd_dxi = dxd_dyi;
+      const double dyd_dyi = dtd_dr * s2 + s * (1 - s2);
 
-    Matrix2 DR;
-    DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
+      Matrix2 DR;
+      DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
 
-    *H2 = DK * DR;
+      *H2 = DK * DR;
+    }
   }
 
   return uv;
diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py
index 298c6e57b..e54afc757 100644
--- a/python/gtsam/tests/test_Cal3Fisheye.py
+++ b/python/gtsam/tests/test_Cal3Fisheye.py
@@ -17,6 +17,15 @@ import gtsam
 from gtsam.utils.test_case import GtsamTestCase
 from gtsam.symbol_shorthand import K, L, P
 
+
+def ulp(ftype=np.float64):
+    """
+    Unit in the last place of floating point datatypes
+    """
+    f = np.finfo(ftype)
+    return f.tiny / ftype(1 << f.nmant)
+
+
 class TestCal3Fisheye(GtsamTestCase):
     
     @classmethod
@@ -105,6 +114,71 @@ class TestCal3Fisheye(GtsamTestCase):
         score = graph.error(state)
         self.assertAlmostEqual(score, 0)
 
+    def test_jacobian_on_axis(self):
+        """Check of jacobian at optical axis"""
+        obj_point_on_axis = np.array([0, 0, 1])
+        img_point = np.array([0, 0])
+        f, z, H = self.evaluate_jacobian(obj_point_on_axis, img_point)
+        self.assertAlmostEqual(f, 0)
+        self.gtsamAssertEquals(z, np.zeros(2))
+        self.gtsamAssertEquals(H @ H.T, 3*np.eye(2))
+
+    def test_jacobian_convergence(self):
+        """Test stability of jacobian close to optical axis"""
+        t = ulp(np.float64)
+        obj_point_close_to_axis = np.array([t, 0, 1])
+        img_point = np.array([np.sqrt(t), 0])
+        f, z, H = self.evaluate_jacobian(obj_point_close_to_axis, img_point)
+        self.assertAlmostEqual(f, 0)
+        self.gtsamAssertEquals(z, np.zeros(2))
+        self.gtsamAssertEquals(H @ H.T, 3*np.eye(2))
+
+        # With a height of sqrt(ulp), this may cause an overflow
+        t = ulp(np.float64)
+        obj_point_close_to_axis = np.array([np.sqrt(t), 0, 1])
+        img_point = np.array([np.sqrt(t), 0])
+        f, z, H = self.evaluate_jacobian(obj_point_close_to_axis, img_point)
+        self.assertAlmostEqual(f, 0)
+        self.gtsamAssertEquals(z, np.zeros(2))
+        self.gtsamAssertEquals(H @ H.T, 3*np.eye(2))
+
+    def test_scaling_factor(self):
+        """Check convergence of atan2(r, z)/r ~ 1/z for small r"""
+        r = ulp(np.float64)
+        s = np.arctan(r) / r
+        self.assertEqual(s, 1.0)
+        z = 1
+        s = self.scaling_factor(r, z)
+        self.assertEqual(s, 1.0/z)
+        z = 2
+        s = self.scaling_factor(r, z)
+        self.assertEqual(s, 1.0/z)
+        s = self.scaling_factor(2*r, z)
+        self.assertEqual(s, 1.0/z)
+
+    @staticmethod
+    def scaling_factor(r, z):
+        """Projection factor theta/r for equidistant fisheye lens model"""
+        return np.arctan2(r, z) / r if r/z != 0 else 1.0/z
+
+    @staticmethod
+    def evaluate_jacobian(obj_point, img_point):
+        """Evaluate jacobian at given object point"""
+        pose = gtsam.Pose3()
+        camera = gtsam.Cal3Fisheye()
+        state = gtsam.Values()
+        camera_key, pose_key, landmark_key = K(0), P(0), L(0)
+        state.insert_point3(landmark_key, obj_point)
+        state.insert_pose3(pose_key, pose)
+        g = gtsam.NonlinearFactorGraph()
+        noise_model = gtsam.noiseModel.Unit.Create(2)
+        factor = gtsam.GenericProjectionFactorCal3Fisheye(img_point, noise_model, pose_key, landmark_key, camera)
+        g.add(factor)
+        f = g.error(state)
+        gaussian_factor_graph = g.linearize(state)
+        H, z = gaussian_factor_graph.jacobian()
+        return f, z, H
+
     @unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
     def test_triangulation_skipped(self):
         """Estimate spatial point from image measurements"""
diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py
index dab1ae446..bafbacfa4 100644
--- a/python/gtsam/tests/test_Cal3Unified.py
+++ b/python/gtsam/tests/test_Cal3Unified.py
@@ -117,6 +117,28 @@ class TestCal3Unified(GtsamTestCase):
         score = graph.error(state)
         self.assertAlmostEqual(score, 0)
 
+    def test_jacobian(self):
+        """Evaluate jacobian at optical axis"""
+        obj_point_on_axis = np.array([0, 0, 1])
+        img_point = np.array([0.0, 0.0])
+        pose = gtsam.Pose3()
+        camera = gtsam.Cal3Unified()
+        state = gtsam.Values()
+        camera_key, pose_key, landmark_key = K(0), P(0), L(0)
+        state.insert_cal3unified(camera_key, camera)
+        state.insert_point3(landmark_key, obj_point_on_axis)
+        state.insert_pose3(pose_key, pose)
+        g = gtsam.NonlinearFactorGraph()
+        noise_model = gtsam.noiseModel.Unit.Create(2)
+        factor = gtsam.GeneralSFMFactor2Cal3Unified(img_point, noise_model, pose_key, landmark_key, camera_key)
+        g.add(factor)
+        f = g.error(state)
+        gaussian_factor_graph = g.linearize(state)
+        H, z = gaussian_factor_graph.jacobian()
+        self.assertAlmostEqual(f, 0)
+        self.gtsamAssertEquals(z, np.zeros(2))
+        self.gtsamAssertEquals(H @ H.T, 4*np.eye(2))
+
     @unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
     def test_triangulation(self):
         """Estimate spatial point from image measurements"""