Improved accuracy for analytic undistortion

borglab · Jul 9, 2021 · 66af007 · dellaert · Jul 9, 2021 · 66af007
1 parent a411b66
commit 66af007
Showing 1 changed file with 3 additions and 1 deletion.
diff --git 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