No need for tolerance check for radial distortion (#464)

* No need for tolerance check for radial distortion

* Indentation in src/Distortion.cpp

src/Distortion.cpp

@@ -94,13 +94,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]));
 
       ux = dx * (1.0 - dr);
       uy = dy * (1.0 - dr);
-      }
     } break;
 
     // Computes undistorted focal plane (x,y) coordinates given a distorted
@@ -221,7 +219,7 @@ void removeDistortion(double dx, double dy, double &ux, double &uy,
       bool done;
 
       /****************************************************************************
-       * Pre-loop intializations
+       * Pre-loop initializations
        ****************************************************************************/
 
       r2 = dy * dy + dx * dx;
@@ -255,7 +253,7 @@ void removeDistortion(double dx, double dy, double &ux, double &uy,
       } while (!done);
 
       /****************************************************************************
-       * Sucess ...
+       * Success ...
        ****************************************************************************/
 
       ux = guess_ux;
@@ -327,14 +325,13 @@ void applyDistortion(double ux, double uy, double &dx, double &dy,
   dy = uy;
 
   switch (distortionType) {
-    // Compute undistorted focal plane coordinate given a distorted
-    // focal plane coordinate. This case works by iteratively adding distortion
+    // Compute distorted focal plane coordinate given undistorted
+    // 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 =
@@ -372,7 +369,7 @@ void applyDistortion(double ux, double uy, double &dx, double &dy,
 
       dx = ux / (1.0 - drOverR);
       dy = uy / (1.0 - drOverR);
-      }
+      
     } break;
     case TRANSVERSE: {
       computeTransverseDistortion(ux, uy, dx, dy, opticalDistCoeffs);
@@ -468,7 +465,7 @@ void applyDistortion(double ux, double uy, double &dx, double &dy,
 
     // The LRO LROC NAC distortion model uses an iterative approach to go from
     // undistorted x,y to distorted x,y
-    // Algorithum adapted from ISIS3 LRONarrowAngleDistortionMap.cpp
+    // Algorithm adapted from ISIS3 LRONarrowAngleDistortionMap.cpp
     case LROLROCNAC: {
       double yt = uy;
 
@@ -491,9 +488,9 @@ void applyDistortion(double ux, double uy, double &dx, double &dy,
 
       double dk1 = opticalDistCoeffs[0];
 
-      // Owing to the odd distotion model employed in this senser if |y| is >
+      // Owing to the odd distortion model employed in this sensor if |y| is >
       // 116.881145553046 then there is no root to find.  Further, the greatest
-      // y that any measure on the sensor will acutally distort to is less
+      // y that any measure on the sensor will actually distort to is less
       // than 20.  Thus, if any distorted measure is greater that that skip the
       // iterations.  The points isn't in the cube, and exactly how far outside
       // the cube is irrelevant.  Just let the camera model know its not in the

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) {
   
-  double ux, uy;
-  std::vector<double> coeffs = {0, 0, 0};
+  // Distorted pixel
+  csm::ImageCoord imagePt(0.0, 1e-1);
 
+  // Undistort
+  double ux, uy;
+  std::vector<double> coeffs = {0.03, 0.00001, 0.000004};
+  double tolerance = 1e-2;
   removeDistortion(imagePt.samp, imagePt.line, ux, uy, coeffs,
-                   DistortionType::RADIAL);
+                   DistortionType::RADIAL, tolerance);
   
-  EXPECT_NEAR(ux, 4, 1e-8);
-  EXPECT_NEAR(uy, 0, 1e-8);
+  // 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