Fix for radial-tangential distortion (#484)

DOI-USGS · Jun 3, 2024 · 2f1ba95 · 2f1ba95
1 parent de195de
commit 2f1ba95
Show file tree

Hide file tree

Showing 6 changed files with 90 additions and 60 deletions.
diff --git a/include/usgscsm/Distortion.h b/include/usgscsm/Distortion.h
@@ -19,11 +19,13 @@ void computeTransverseDistortion(double ux, double uy, double &dx, double &dy,
 
 void removeDistortion(double dx, double dy, double &ux, double &uy,
                       std::vector<double> const& opticalDistCoeffs,
+                      double focalLength,
                       DistortionType distortionType,
                       const double tolerance = 1.0E-6);
 
 void applyDistortion(double ux, double uy, double &dx, double &dy,
                      std::vector<double> const& opticalDistCoeffs,
+                     double focalLength,
                      DistortionType distortionType,
                      const double desiredPrecision = 1.0E-6,
                      const double tolerance = 1.0E-6);

diff --git a/src/Distortion.cpp b/src/Distortion.cpp
@@ -84,13 +84,14 @@ 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]. 
+// Compute distorted normalized focal plane coordinates given undistorted
+// normalized 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]. 
+// Reference: https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html
 void computeRadTanDistortion(double ux, double uy, double &dx, double &dy,
                              std::vector<double> const& opticalDistCoeffs) {
 
@@ -120,7 +121,7 @@ void computeRadTanDistortion(double ux, double uy, double &dx, double &dy,
      + (p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * x * y);
 }
 
-// Compute the jacobian for radtan distortion
+// Compute the jacobian for radtan distortion at given normalized coordinates
 void radTanDistortionJacobian(double x, double y, double *jacobian,
                               std::vector<double> const& opticalDistCoeffs) {
 
@@ -155,6 +156,7 @@ void radTanDistortionJacobian(double x, double y, double *jacobian,
 
 void removeDistortion(double dx, double dy, double &ux, double &uy,
                       std::vector<double> const& opticalDistCoeffs,
+                      double focalLength,
                       DistortionType distortionType, const double tolerance) {
   ux = dx;
   uy = dy;
@@ -343,8 +345,10 @@ void removeDistortion(double dx, double dy, double &ux, double &uy,
     // with the radtan model. See computeRadTanDistortion() for more details.
     case RADTAN:
     {
+      dx /= focalLength; dy /= focalLength; // Find normalized coordinates
       newtonRaphson(dx, dy, ux, uy, opticalDistCoeffs, distortionType, tolerance, 
                     computeRadTanDistortion, radTanDistortionJacobian);
+      ux *= focalLength; uy *= focalLength; // Convert back to pixel coordinates
 
     }
     break;
@@ -354,6 +358,7 @@ void removeDistortion(double dx, double dy, double &ux, double &uy,
 
 void applyDistortion(double ux, double uy, double &dx, double &dy,
                      std::vector<double> const& opticalDistCoeffs,
+                     double focalLength,
                      DistortionType distortionType,
                      const double desiredPrecision, const double tolerance) {
   dx = ux;
@@ -634,7 +639,9 @@ void applyDistortion(double ux, double uy, double &dx, double &dy,
     // with the RADTAN model. See computeRadTanDistortion() for more details.
     case RADTAN:
     {
+      ux /= focalLength; uy /= focalLength; // Find normalized coordinates
       computeRadTanDistortion(ux, uy, dx, dy, opticalDistCoeffs);  
+      dx *= focalLength; dy *= focalLength; // Convert back to pixel coordinates
     }  
     break;
 

diff --git a/src/UsgsAstroFrameSensorModel.cpp b/src/UsgsAstroFrameSensorModel.cpp
@@ -173,8 +173,8 @@ csm::ImageCoord UsgsAstroFrameSensorModel::groundToImage(
   // line/sample
   double distortedX, distortedY;
   applyDistortion(undistortedx, undistortedy, distortedX, distortedY,
-                  m_opticalDistCoeffs, m_distortionType);
-
+                  m_opticalDistCoeffs, m_focalLength, m_distortionType);
+  
   MESSAGE_LOG(
       spdlog::level::trace,
       "Found distortedX: {}, and distortedY: {}",
@@ -271,7 +271,8 @@ csm::EcefCoord UsgsAstroFrameSensorModel::imageToGround(
   // Apply the distortion model (remove distortion)
   double undistortedX, undistortedY;
   removeDistortion(x_camera, y_camera, undistortedX, undistortedY,
-                   m_opticalDistCoeffs, m_distortionType);
+                   m_opticalDistCoeffs, m_focalLength, m_distortionType);
+
   MESSAGE_LOG(
       spdlog::level::trace,
       "Found undistortedX: {}, and undistortedY: {}",
@@ -396,7 +397,7 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus(
   double undistortedFocalPlaneX, undistortedFocalPlaneY;
   removeDistortion(focalPlaneX, focalPlaneY, undistortedFocalPlaneX,
                    undistortedFocalPlaneY, m_opticalDistCoeffs,
-                   m_distortionType);
+                   m_focalLength, m_distortionType);
 
   MESSAGE_LOG(
       spdlog::level::trace,

diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp
@@ -776,7 +776,8 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage(
   // Invert distortion
   double distortedFocalX, distortedFocalY;
   applyDistortion(detectorView[0], detectorView[1], distortedFocalX,
-                  distortedFocalY, m_opticalDistCoeffs, m_distortionType,
+                  distortedFocalY, m_opticalDistCoeffs, m_focalLength,
+                  m_distortionType,
                   desiredPrecision);
   MESSAGE_LOG(
       spdlog::level::trace,
@@ -1878,7 +1879,7 @@ void UsgsAstroLsSensorModel::losToEcf(
   double undistortedFocalPlaneX, undistortedFocalPlaneY;
   removeDistortion(distortedFocalPlaneX, distortedFocalPlaneY,
                    undistortedFocalPlaneX, undistortedFocalPlaneY,
-                   m_opticalDistCoeffs, m_distortionType);
+                   m_opticalDistCoeffs, m_focalLength, m_distortionType);
   MESSAGE_LOG(
       spdlog::level::trace,
       "losToEcf: undistorted focal plane coordinate {} {}",
@@ -2802,7 +2803,7 @@ double UsgsAstroLsSensorModel::calcDetectorLineErr(double t, csm::ImageCoord con
   // Invert distortion
   double distortedFocalX, distortedFocalY;
   applyDistortion(detectorView[0], detectorView[1], distortedFocalX,
-                  distortedFocalY, m_opticalDistCoeffs, m_distortionType);
+                  distortedFocalY, m_opticalDistCoeffs, m_focalLength, m_distortionType);
 
   // Convert to detector line
   double detectorLine = m_iTransL[0] + m_iTransL[1] * distortedFocalX +

diff --git a/src/UsgsAstroPushFrameSensorModel.cpp b/src/UsgsAstroPushFrameSensorModel.cpp
@@ -762,8 +762,8 @@ csm::ImageCoord UsgsAstroPushFrameSensorModel::groundToImage(
   // Invert distortion
   double distortedFocalX, distortedFocalY;
   applyDistortion(focalCoord[0], focalCoord[1], distortedFocalX,
-                  distortedFocalY, m_opticalDistCoeffs, m_distortionType,
-                  desiredPrecision);
+                  distortedFocalY, m_opticalDistCoeffs, m_focalLength, 
+                  m_distortionType, desiredPrecision);
 
   // Convert to detector line and sample
   double detectorLine = m_iTransL[0] + m_iTransL[1] * distortedFocalX +
@@ -1793,7 +1793,7 @@ void UsgsAstroPushFrameSensorModel::losToEcf(
   double undistortedFocalPlaneX, undistortedFocalPlaneY;
   removeDistortion(distortedFocalPlaneX, distortedFocalPlaneY,
                    undistortedFocalPlaneX, undistortedFocalPlaneY,
-                   m_opticalDistCoeffs, m_distortionType);
+                   m_opticalDistCoeffs, m_focalLength, m_distortionType);
   MESSAGE_LOG("losToEcf: undistorted focal plane coordinate {} {}",
               undistortedFocalPlaneX, undistortedFocalPlaneY)