diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index 3ee036eff..52d475d5d 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -106,12 +106,20 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, /* ************************************************************************* */ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal, OptionalJacobian<2, 2> Dp) const { - // initial gues just inverts the pinhole model + // Apply inverse camera matrix to map the pixel coordinate (u, v) + // of the equidistant fisheye image to angular coordinate space (xd, yd) + // with radius theta given in radians. const double u = uv.x(), v = uv.y(); const double yd = (v - v0_) / fy_; const double xd = (u - s_ * yd - u0_) / fx_; const double theta = sqrt(xd * xd + yd * yd); - const double scale = (theta > 0) ? tan(theta) / theta; + + // Provide initial guess for the Gauss-Newton search. + // The angular coordinates given by (xd, yd) are mapped back to + // the focal plane of the perspective undistorted projection pi. + // See Cal3Unified.calibrate() using the same pattern for the + // undistortion of omnidirectional fisheye projection. + const double scale = (theta > 0) ? tan(theta) / theta : 1.0; Point2 pi(scale * xd, scale * yd); // Perform newtons method, break when solution converges past tol_,