From d5c49d98b07b285246c42de6a4bc895d46b19129 Mon Sep 17 00:00:00 2001
From: Oleg Alexandrov <oleg.alexandrov@gmail.com>
Date: Mon, 6 Nov 2023 18:21:37 -0800
Subject: [PATCH] Implement radtan distortion and undistortion
---
include/usgscsm/Distortion.h | 8 +-
src/Distortion.cpp | 222 +++++++++++++++++++++++++++++++++--
tests/DistortionTests.cpp | 33 +++++-
tests/Fixtures.h | 3 +
4 files changed, 248 insertions(+), 18 deletions(-)
diff --git a/include/usgscsm/Distortion.h b/include/usgscsm/Distortion.h
index 990f513..7561816 100644
--- a/include/usgscsm/Distortion.h
+++ b/include/usgscsm/Distortion.h
@@ -6,11 +6,11 @@
#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,
- const std::vector<double> opticalDistCoeffs);
+// 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,
const std::vector<double> opticalDistCoeffs);
diff --git a/src/Distortion.cpp b/src/Distortion.cpp
index a40ae12..5971501 100644
--- a/src/Distortion.cpp
+++ b/src/Distortion.cpp
@@ -3,8 +3,9 @@
#include <Error.h>
#include <string>
-void distortionJacobian(double x, double y, double *jacobian,
- const std::vector<double> opticalDistCoeffs) {
+// 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;
d_dx[1] = 1;
@@ -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;
+
}
}
diff --git a/tests/DistortionTests.cpp b/tests/DistortionTests.cpp
index 1c44432..3cbec40 100644
--- a/tests/DistortionTests.cpp
+++ b/tests/DistortionTests.cpp
@@ -19,8 +19,8 @@ TEST_P(ImageCoordParameterizedTest, JacobianTest) {
csm::ImageCoord imagePt = GetParam();
double jacobian[4];
- distortionJacobian(imagePt.samp, imagePt.line, jacobian,
- transverseDistortionCoeffs);
+ transverseDistortionJacobian(imagePt.samp, imagePt.line, jacobian,
+ transverseDistortionCoeffs);
// Jxx * Jyy - Jxy * Jyx
double determinant =
@@ -36,8 +36,8 @@ 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,
- transverseDistortionCoeffs);
+ transverseDistortionJacobian(imagePt.samp, imagePt.line, jacobian,
+ transverseDistortionCoeffs);
EXPECT_NEAR(jacobian[0], 56.25, 1e-8);
EXPECT_NEAR(jacobian[1], 112.5, 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);
+}
diff --git a/tests/Fixtures.h b/tests/Fixtures.h
index 2b5b053..36c5e0a 100644
--- a/tests/Fixtures.h
+++ b/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:
--
GitLab