From 8df2c7086641892c88124f3b0a7af9ee6fabefaf Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Fri, 22 Oct 2021 19:39:09 +0200
Subject: [PATCH] Avoid division by zero in jacobian calculation

---
 gtsam/geometry/Cal3Fisheye.cpp | 42 +++++++++++++++++++---------------
 1 file changed, 23 insertions(+), 19 deletions(-)

diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp
index 52d475d5d..f889342b6 100644
--- a/gtsam/geometry/Cal3Fisheye.cpp
+++ b/gtsam/geometry/Cal3Fisheye.cpp
@@ -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 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;
 
-    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 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;
 
-    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;