updated distortion to closer match ISIS (#243)

* updated distortion to closer match ISIS

* updated tolerance

* put back half removed comment

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<double> 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

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);
       }