diff --git a/include/usgscsm/Distortion.h b/include/usgscsm/Distortion.h
index 10c56691840b3a856255ee7727ac756786802d92..d5c3c5165f28bb793414b8f22d84882605595d05 100644
--- 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
index 38f397ac7d45176f326e2587989ca3333b040abe..e69a5cd2217183bc8c6aa409e0c24fefd3c10ca0 100644
--- 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
index 222e4ca463506f1e0a997170978c3aaf12f9b6cb..d702253c62ff8e28c16a9dcc7760810dbe0bfdc8 100644
--- a/src/UsgsAstroFrameSensorModel.cpp
+++ b/src/UsgsAstroFrameSensorModel.cpp
@@ -172,12 +172,8 @@ csm::ImageCoord UsgsAstroFrameSensorModel::groundToImage(
// Apply the distortion to the line/sample location and then convert back to
// line/sample
double distortedX, distortedY;
- // Divide by focal length
- undistortedx /= f; undistortedy /= f;
applyDistortion(undistortedx, undistortedy, distortedX, distortedY,
- m_opticalDistCoeffs, m_distortionType);
- // Apply multiplication by focal length
- distortedX *= f; distortedY *= f;
+ m_opticalDistCoeffs, m_focalLength, m_distortionType);
MESSAGE_LOG(
spdlog::level::trace,
@@ -272,16 +268,10 @@ csm::EcefCoord UsgsAstroFrameSensorModel::imageToGround(
m_detectorLineSumming, m_startingDetectorSample, m_startingDetectorLine,
&m_iTransS[0], &m_iTransL[0], x_camera, y_camera);
- // divide x_camera and y_camera by focal length
- x_camera /= m_focalLength; y_camera /= m_focalLength;
-
// Apply the distortion model (remove distortion)
double undistortedX, undistortedY;
removeDistortion(x_camera, y_camera, undistortedX, undistortedY,
- m_opticalDistCoeffs, m_distortionType);
-
- // Multiply by focal length
- undistortedX *= m_focalLength; undistortedY *= m_focalLength;
+ m_opticalDistCoeffs, m_focalLength, m_distortionType);
MESSAGE_LOG(
spdlog::level::trace,
@@ -405,13 +395,9 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus(
// Distort
double undistortedFocalPlaneX, undistortedFocalPlaneY;
- // divide by focal length
- focalPlaneX /= m_focalLength; focalPlaneY /= m_focalLength;
removeDistortion(focalPlaneX, focalPlaneY, undistortedFocalPlaneX,
undistortedFocalPlaneY, m_opticalDistCoeffs,
- m_distortionType);
- // Multiply by focal length
- undistortedFocalPlaneX *= m_focalLength; undistortedFocalPlaneY *= m_focalLength;
+ m_focalLength, m_distortionType);
MESSAGE_LOG(
spdlog::level::trace,
diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp
index 8d1d9857a3a78732a8a4462f92112c63a9380beb..d59e78b3603ed95cd9b721a3a976c1a5705e9ef9 100644
--- a/src/UsgsAstroLsSensorModel.cpp
+++ b/src/UsgsAstroLsSensorModel.cpp
@@ -774,7 +774,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,
@@ -1876,7 +1877,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 {} {}",
@@ -2800,7 +2801,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
index 10542000c09c8a554d2511ff8c050e7c0a269a33..a91948711f9fa19426275ab48661030ba805b574 100644
--- 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)