From 3a1bb8aa674f76ca53c9dcdc0b0544587e2154a5 Mon Sep 17 00:00:00 2001
From: Oleg Alexandrov <oleg.alexandrov@gmail.com>
Date: Mon, 29 Apr 2024 13:50:19 -0700
Subject: [PATCH] Hide normalizing of distorted coordiantes in the lens
distortion model
---
include/usgscsm/Distortion.h | 2 ++
src/Distortion.cpp | 23 +++++++++++++++--------
src/UsgsAstroFrameSensorModel.cpp | 20 +++-----------------
src/UsgsAstroLsSensorModel.cpp | 7 ++++---
src/UsgsAstroPushFrameSensorModel.cpp | 6 +++---
5 files changed, 27 insertions(+), 31 deletions(-)
diff --git a/include/usgscsm/Distortion.h b/include/usgscsm/Distortion.h
index 10c5669..d5c3c51 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 38f397a..e69a5cd 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 222e4ca..d702253 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 8d1d985..d59e78b 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 1054200..a919487 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)
--
GitLab