diff --git a/src/Distortion.cpp b/src/Distortion.cpp index 25c75fef9ea590a9bdef8e43163f3b59906db767..38f397ac7d45176f326e2587989ca3333b040abe 100644 --- a/src/Distortion.cpp +++ b/src/Distortion.cpp @@ -165,13 +165,11 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, case RADIAL: { double rr = dx * dx + dy * dy; - if (rr > tolerance) { - double dr = opticalDistCoeffs[0] + - (rr * (opticalDistCoeffs[1] + rr * opticalDistCoeffs[2])); + double dr = opticalDistCoeffs[0] + + (rr * (opticalDistCoeffs[1] + rr * opticalDistCoeffs[2])); - ux = dx * (1.0 - dr); - uy = dy * (1.0 - dr); - } + ux = dx * (1.0 - dr); + uy = dy * (1.0 - dr); } break; // Computes undistorted focal plane (x,y) coordinates given a distorted @@ -363,51 +361,50 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, switch (distortionType) { // Compute distorted focal plane coordinate given undistorted - // focal plane coordinate. This case works by iteratively adding distortion + // focal plane coordinates. This case works by iteratively adding distortion // until the new distorted point, r, undistorts to within a tolerance of the // original point, rp. case RADIAL: { double rp2 = (ux * ux) + (uy * uy); - if (rp2 > tolerance) { - double rp = sqrt(rp2); - // Compute first fractional distortion using rp - double drOverR = - opticalDistCoeffs[0] + - (rp2 * (opticalDistCoeffs[1] + (rp2 * opticalDistCoeffs[2]))); + double rp = sqrt(rp2); + // 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; + // 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 >= 20 || r > 1E9) { - drOverR = 0.0; - break; - } + 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 >= 20 || r > 1E9) { + drOverR = 0.0; + break; + } - r_prev = r; - r2_prev = r * r; + r_prev = r; + r2_prev = r * r; - // Compute new fractional distortion: - drOverR = opticalDistCoeffs[0] + - (r2_prev * - (opticalDistCoeffs[1] + (r2_prev * opticalDistCoeffs[2]))); + // Compute new fractional distortion: + drOverR = opticalDistCoeffs[0] + + (r2_prev * + (opticalDistCoeffs[1] + (r2_prev * opticalDistCoeffs[2]))); - // Compute new estimate of r - r = rp + (drOverR * r_prev); - iteration++; - } while (fabs(r - r_prev) > desiredPrecision); + // Compute new estimate of r + r = rp + (drOverR * r_prev); + iteration++; + } while (fabs(r - r_prev) > desiredPrecision); - dx = ux / (1.0 - drOverR); - dy = uy / (1.0 - drOverR); - } + dx = ux / (1.0 - drOverR); + dy = uy / (1.0 - drOverR); + } break; case TRANSVERSE: { computeTransverseDistortion(ux, uy, dx, dy, opticalDistCoeffs); diff --git a/tests/DistortionTests.cpp b/tests/DistortionTests.cpp index 3cbec40e4840a7b42dbbf0151c8b14869b3d2abf..bda373bd24c1e89c64e4347e8c431a34c9b15a62 100644 --- a/tests/DistortionTests.cpp +++ b/tests/DistortionTests.cpp @@ -124,17 +124,26 @@ TEST(transverse, removeDistortion_AlternatingOnes) { EXPECT_NEAR(uy, 7.5, 1e-8); } -TEST(Radial, testRemoveDistortion) { - csm::ImageCoord imagePt(0.0, 4.0); +TEST(Radial, testUndistortDistort) { + + // Distorted pixel + csm::ImageCoord imagePt(0.0, 1e-1); + // Undistort double ux, uy; - std::vector<double> coeffs = {0, 0, 0}; - + std::vector<double> coeffs = {0.03, 0.00001, 0.000004}; + double tolerance = 1e-2; removeDistortion(imagePt.samp, imagePt.line, ux, uy, coeffs, - DistortionType::RADIAL); - - EXPECT_NEAR(ux, 4, 1e-8); - EXPECT_NEAR(uy, 0, 1e-8); + DistortionType::RADIAL, tolerance); + + // Distort back + double desiredPrecision = 1e-6; + double dx, dy; + applyDistortion(ux, uy, dx, dy, coeffs, + DistortionType::RADIAL, desiredPrecision, tolerance); + + EXPECT_NEAR(dx, imagePt.samp, 1e-8); + EXPECT_NEAR(dy, imagePt.line, 1e-8); } // If coeffs are 0 then this will have the same result as removeDistortion