diff --git a/include/usgscsm/Distortion.h b/include/usgscsm/Distortion.h index c27eb39d3..9cf8c3cd2 100644 --- a/include/usgscsm/Distortion.h +++ b/include/usgscsm/Distortion.h @@ -30,5 +30,5 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, void applyDistortion(double ux, double uy, double &dx, double &dy, const std::vector opticalDistCoeffs, DistortionType distortionType, - const double desiredPrecision = 0, const double tolerance = 1.0E-6); + const double desiredPrecision = 1.0E-6, const double tolerance = 1.0E-6); #endif diff --git a/src/Distortion.cpp b/src/Distortion.cpp index d39fe07c1..1d680898e 100644 --- a/src/Distortion.cpp +++ b/src/Distortion.cpp @@ -97,9 +97,9 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, case RADIAL: { double rr = dx * dx + dy * dy; - if (rr > tolerance) - { + if (rr > tolerance) { double dr = opticalDistCoeffs[0] + (rr * (opticalDistCoeffs[1] + rr * opticalDistCoeffs[2])); + ux = dx * (1.0 - dr); uy = dy * (1.0 - dr); } @@ -306,17 +306,19 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, // Compute first fractional distortion using rp double drOverR = opticalDistCoeffs[0] + (rp2 * (opticalDistCoeffs[1] + (rp2 * opticalDistCoeffs[2]))); + // Compute first distorted point estimate, r double r = rp + (drOverR * rp); double r_prev, r2_prev; int iteration = 0; + do { // Don't get in an end-less loop. This algorithm should // converge quickly. If not then we are probably way outside // of the focal plane. Just set the distorted position to the // undistorted position. Also, make sure the focal plane is less // than 1km, it is unreasonable for it to grow larger than that. - if (iteration >= 15 || r > 1E9) { + if (iteration >= 20 || r > 1E9) { drOverR = 0.0; break; } @@ -332,7 +334,8 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, r = rp + (drOverR * r_prev); iteration++; } - while (fabs(r * (1 - drOverR) - rp) > desiredPrecision); + while (fabs(r - r_prev) > desiredPrecision); + dx = ux / (1.0 - drOverR); dy = uy / (1.0 - drOverR); }