Implement radtan distortion and undistortion

include/usgscsm/Distortion.h

@@ -6,10 +6,10 @@
 #include <tuple>
 #include <vector>
 
-enum DistortionType { RADIAL, TRANSVERSE, KAGUYALISM, DAWNFC, LROLROCNAC, CAHVOR };
+enum DistortionType { RADIAL, TRANSVERSE, KAGUYALISM, DAWNFC, LROLROCNAC, CAHVOR, RADTAN };
 
-// Transverse Distortion
-void distortionJacobian(double x, double y, double *jacobian,
+// Transverse distortion Jacobian
+void transverseDistortionJacobian(double x, double y, double *jacobian,
                                   const std::vector<double> opticalDistCoeffs);
 
 void computeTransverseDistortion(double ux, double uy, double &dx, double &dy,

src/Distortion.cpp

@@ -3,7 +3,8 @@
 #include <Error.h>
 #include <string>
 
-void distortionJacobian(double x, double y, double *jacobian,
+// Jacobian for Transverse distortion
+void transverseDistortionJacobian(double x, double y, double *jacobian,
                                   const std::vector<double> opticalDistCoeffs) {
   double d_dx[10];
   d_dx[0] = 0;
@@ -82,6 +83,138 @@ void computeTransverseDistortion(double ux, double uy, double &dx, double &dy,
   }
 }
 
+// Compute distorted focal plane coordinates given undistorted coordinates. Use
+// the radial-tangential distortion model with 5 coefficients (k1, k2, k3 for
+// radial distortion, and p1, p2 for tangential distortion). This was tested to
+// give the same results as the OpenCV distortion model, by invoking the
+// function cv::projectPoints() (with zero rotation, zero translation, and
+// identity camera matrix). The parameters are stored in opticalDistCoeffs 
+// in the order: [k1, k2, p1, p2, k3]. 
+void computeRadTanDistortion(double ux, double uy, double &dx, double &dy,
+                             std::vector<double> const& opticalDistCoeffs) {
+
+  if (opticalDistCoeffs.size() != 5) {
+    csm::Error::ErrorType errorType = csm::Error::INDEX_OUT_OF_RANGE;
+    std::string message =
+        "Distortion coefficients for the radtan distortion model must be of size 5. "
+        "Got: " + std::to_string(opticalDistCoeffs.size());
+    std::string function = "computeRadTanDistortion";
+    throw csm::Error(errorType, message, function);
+  }
+  
+  // Shorten notation
+  double x = ux, y = uy; 
+  double k1 = opticalDistCoeffs[0];
+  double k2 = opticalDistCoeffs[1];
+  double p1 = opticalDistCoeffs[2];
+  double p2 = opticalDistCoeffs[3];
+  double k3 = opticalDistCoeffs[4];
+  
+  double r2 = (x * x) + (y * y);
+  
+  dx = x * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)
+     + (2.0 * p1 * x * y + p2 * (r2 + 2.0 * x * x));
+       
+  dy = y * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)
+     + (p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * x * y);
+}
+
+// Compute the jacobian for radtan distortion
+void radTanDistortionJacobian(double x, double y, double *jacobian,
+                              std::vector<double> const& opticalDistCoeffs) {
+
+  double k1 = opticalDistCoeffs[0];
+  double k2 = opticalDistCoeffs[1];
+  double p1 = opticalDistCoeffs[2];
+  double p2 = opticalDistCoeffs[3];
+  double k3 = opticalDistCoeffs[4];
+  
+  double r2 = x * x + y * y;
+  double dr2dx = 2.0 * x;
+  double dr2dy = 2.0 * y;
+
+  // dfx / dx 
+  jacobian[0] = (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)    
+              + x * (k1 * dr2dx + k2 * dr2dx * 2.0 * r2 + k3 * dr2dx * 3.0 * r2 * r2)
+              + 2.0 * p1 * y + p2 * (dr2dx + 4.0 * x);
+  
+  // dfx / dy
+  jacobian[1] = x * (k1 * dr2dy + k2 * dr2dy * 2.0 * r2 + k3 * dr2dy * 3.0 * r2 * r2)
+              + 2.0 * p1 * x  + p2 * dr2dy;
+              
+  // dfy / dx
+  jacobian[2] = y * (k1 * dr2dx + k2 * dr2dx * 2.0 * r2 + k3 * dr2dx * 3.0 * r2 * r2)
+              + (p1 * dr2dx + 2.0 * p2 * y);
+  
+  // dfy / dy
+  jacobian[3] = (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)
+              + y * (k1 * dr2dy + k2 * dr2dy * 2.0 * r2 + k3 * dr2dy * 3.0 * r2 * r2)
+              + p1 * (dr2dy + 4.0 * y) + 2.0 * p2 * x;
+
+#if 0
+// Check the partial derivatives with numerical differentiation              
+  {
+    double eps = 1e-4;
+    double y0 = y;
+    y = y0 + eps;
+    r2 = (x * x) + (y * y);
+      
+    double dx1 = x * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)
+     + (2.0 * p1 * x * y + p2 * (r2 + 2.0 * x * x));
+
+    double dy1 = y * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)
+     + (p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * x * y);
+
+    y = y0 - eps;
+    r2 = (x * x) + (y * y);
+
+    double dx2 = x * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)
+     + (2.0 * p1 * x * y + p2 * (r2 + 2.0 * x * x));
+              
+    double dy2 = y * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)
+     + (p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * x * y);
+
+    std::cout << "--numerical1 is " << (dx1-dx2)/(2*eps) << " " << jacobian[1] << std::endl;
+    std::cout << "diff1 is " << (dx1-dx2)/(2*eps) - jacobian[1] << std::endl;
+                  
+    std::cout << "--numerical3 is " << (dy1-dy2)/(2*eps) << " " << jacobian[3] << std::endl;
+    std::cout << "diff3 is " << (dy1-dy2)/(2*eps) - jacobian[3] << std::endl;
+    
+    y = y0;
+  }
+
+  {
+    double eps = 1e-4;
+    double x0 = x;
+    x = x0 + eps;
+    r2 = (x * x) + (y * y);
+      
+    double dx1 = x * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)
+     + (2.0 * p1 * x * y + p2 * (r2 + 2.0 * x * x));
+
+    double dy1 = y * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)
+     + (p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * x * y);
+
+    x = x0 - eps;
+    r2 = (x * x) + (y * y);
+
+    double dx2 = x * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)
+     + (2.0 * p1 * x * y + p2 * (r2 + 2.0 * x * x));
+              
+    double dy2 = y * (1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2)
+     + (p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * x * y);
+
+    std::cout << "--numerical0 is " << (dx1-dx2)/(2*eps) << " " << jacobian[0] << std::endl;
+    std::cout << "diff0 is " << (dx1-dx2)/(2*eps) - jacobian[0] << std::endl;
+                  
+    std::cout << "--numerical2 is " << (dy1-dy2)/(2*eps) << " " << jacobian[2] << std::endl;
+    std::cout << "diff2 is " << (dy1-dy2)/(2*eps) - jacobian[2] << std::endl;
+    
+    x = x0;
+  }
+#endif  
+}
+
 void removeDistortion(double dx, double dy, double &ux, double &uy,
                       const std::vector<double> opticalDistCoeffs,
                       DistortionType distortionType, const double tolerance) {
@@ -132,7 +265,7 @@ void removeDistortion(double dx, double dy, double &ux, double &uy,
         fx = dx - fx;
         fy = dy - fy;
 
-        distortionJacobian(x, y, jacobian, opticalDistCoeffs);
+        transverseDistortionJacobian(x, y, jacobian, opticalDistCoeffs);
 
         // Jxx * Jyy - Jxy * Jyx
         double determinant =
@@ -316,6 +449,66 @@ void removeDistortion(double dx, double dy, double &ux, double &uy,
       }
     }
     break;
+    
+    // Compute undistorted focal plane coordinate given distorted coordinates
+    // with the RADTAN model. See computeRadTanDistortion() for more details.
+    case RADTAN:
+    {
+      // Solve the distortion equation using the Newton-Raphson method.
+      // Set the error tolerance to about one millionth of a NAC pixel.
+      // The maximum number of iterations of the Newton-Raphson method.
+      const int maxTries = 20;
+
+      double x;
+      double y;
+      double fx;
+      double fy;
+      double jacobian[4];
+
+      // Initial guess at the root
+      x = dx;
+      y = dy;
+
+      computeRadTanDistortion(x, y, fx, fy, opticalDistCoeffs);
+
+      for (int count = 1;
+           ((fabs(fx) + fabs(fy)) > tolerance) && (count < maxTries); count++) {
+        computeRadTanDistortion(x, y, fx, fy, opticalDistCoeffs);
+
+        fx = dx - fx;
+        fy = dy - fy;
+
+        radTanDistortionJacobian(x, y, jacobian, opticalDistCoeffs);
+
+        // Jxx * Jyy - Jxy * Jyx
+        double determinant =
+            jacobian[0] * jacobian[3] - jacobian[1] * jacobian[2];
+        if (fabs(determinant) < 1e-6) {
+          ux = x;
+          uy = y;
+          //
+          // Near-zero determinant. Add error handling here.
+          //
+          //-- Just break out and return with no convergence
+          return;
+        }
+
+        x = x + (jacobian[3] * fx - jacobian[1] * fy) / determinant;
+        y = y + (jacobian[0] * fy - jacobian[2] * fx) / determinant;
+      }
+
+      if ((fabs(fx) + fabs(fy)) <= tolerance) {
+        // The method converged to a root.
+        ux = x;
+        uy = y;
+
+        return;
+      }
+      // Otherwise method did not converge to a root within the maximum
+      // number of iterations
+    }
+    break;
+    
   }
 }
 
@@ -327,7 +520,7 @@ void applyDistortion(double ux, double uy, double &dx, double &dy,
   dy = uy;
 
   switch (distortionType) {
-    // Compute undistorted focal plane coordinate given a distorted
+    // Compute distorted focal plane coordinate given undistorted
     // focal plane coordinate. This case works by iteratively adding distortion
     // until the new distorted point, r, undistorts to within a tolerance of the
     // original point, rp.
@@ -454,9 +647,9 @@ void applyDistortion(double ux, double uy, double &dx, double &dy,
       }
     } break;
 
-    // The dawn distortion model is "reversed" from other distortion models so
-    // the apply function computes distorted coordinates as a
-    // fn(undistorted coordinates)
+    // The dawn distortion model is "reversed" from other distortion models. 
+    // The apply function computes distorted coordinates as a
+    // function of undistorted coordinates.
     case DAWNFC: {
       double r2;
 
@@ -468,7 +661,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 +684,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
@@ -597,5 +790,14 @@ void applyDistortion(double ux, double uy, double &dx, double &dy,
       }
     }
     break;
+    
+    // Compute distorted focal plane coordinate given undistorted coordinates
+    // with the RADTAN model. See computeRadTanDistortion() for more details.
+    case RADTAN:
+    {
+      computeRadTanDistortion(ux, uy, dx, dy, opticalDistCoeffs);  
+    }  
+    break;
+    
   }
 }

tests/DistortionTests.cpp

@@ -19,7 +19,7 @@ TEST_P(ImageCoordParameterizedTest, JacobianTest) {
 
   csm::ImageCoord imagePt = GetParam();
   double jacobian[4];
-  distortionJacobian(imagePt.samp, imagePt.line, jacobian,
+  transverseDistortionJacobian(imagePt.samp, imagePt.line, jacobian,
                                transverseDistortionCoeffs);
 
   // Jxx * Jyy - Jxy * Jyx
@@ -36,7 +36,7 @@ TEST(Transverse, Jacobian1) {
       0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0};
 
   double jacobian[4];
-  distortionJacobian(imagePt.samp, imagePt.line, jacobian,
+  transverseDistortionJacobian(imagePt.samp, imagePt.line, jacobian,
                                transverseDistortionCoeffs);
 
   EXPECT_NEAR(jacobian[0], 56.25, 1e-8);
@@ -380,3 +380,28 @@ TEST_P(CoeffOffsetParameterizedTest, InverseOnesCoeffsCahvorTest)
   EXPECT_NEAR(dx, 4, 1e-8);
   EXPECT_NEAR(dy, 0, 1e-8);
 }
+
+INSTANTIATE_TEST_SUITE_P(RadTanInversionTest, RadTanTest,
+                         ::testing::Values(std::vector<double>(0, 0)));
+
+TEST_P(RadTanTest, RadTanInversionTest)
+{
+
+  // Initialize radtan distortion coefficients (k1, k2, p1, p2, k3)  
+  std::vector<double> distCoeffs= {0.000031, -0.000056, 1.3e-5, -1.7e-6, 2.9e-8};
+  
+  double ux = 5.0, uy = 6.0; 
+  
+  // Compute distortion 
+  double dx, dy;
+  applyDistortion(ux, uy, dx, dy, distCoeffs, DistortionType::RADTAN, 1e-8, 1e-8);
+  
+  // Remove distortion (undistort)
+  double ux2, uy2;
+  removeDistortion(dx, dy, ux2, uy2, distCoeffs, DistortionType::RADTAN, 1e-8);
+  
+  EXPECT_NEAR(dx, 4.0010785450000004, 1e-8);
+  EXPECT_NEAR(dy, 4.8022116940000013, 1e-8);
+  EXPECT_NEAR(ux2, ux, 1e-8);
+  EXPECT_NEAR(uy2, uy, 1e-8);
+}

tests/Fixtures.h

@@ -180,6 +180,9 @@ class ImageCoordParameterizedTest
 class CoeffOffsetParameterizedTest
     : public ::testing::TestWithParam<std::vector<double>> {};
 
+class RadTanTest
+    : public ::testing::TestWithParam<std::vector<double>> {};
+
 class FramerParameterizedTest
     : public ::testing::TestWithParam<csm::ImageCoord> {
  protected: