From 66af0079baa5de84618fdc1927dc55e8e9fcff81 Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Fri, 9 Jul 2021 12:39:31 +0200
Subject: [PATCH] Improved accuracy for analytic undistortion

---
 gtsam/geometry/Cal3Fisheye.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp
index b9e60acee..3ee036eff 100644
--- a/gtsam/geometry/Cal3Fisheye.cpp
+++ b/gtsam/geometry/Cal3Fisheye.cpp
@@ -110,7 +110,9 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
   const double u = uv.x(), v = uv.y();
   const double yd = (v - v0_) / fy_;
   const double xd = (u - s_ * yd - u0_) / fx_;
-  Point2 pi(xd, yd);
+  const double theta = sqrt(xd * xd + yd * yd);
+  const double scale = (theta > 0) ? tan(theta) / theta;
+  Point2 pi(scale * xd, scale * yd);
 
   // Perform newtons method, break when solution converges past tol_,
   // throw exception if max iterations are reached