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 6eecd62e1c2675a0e1f0d4dbc88e2c8b19b4e12e..d702253c62ff8e28c16a9dcc7760810dbe0bfdc8 100644
--- 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
index 9e27bea59932323a06fa687388a7b2db3742d0c9..89c9a11038cce4af25ef651d0a61cb89845c3fa4 100644
--- 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
index 0c933264786dc6bdc688d6a772410bc5c5a5b3b9..418df839e1d2ad7da97aac1cced2e34c1e19fed2 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)
diff --git a/tests/DistortionTests.cpp b/tests/DistortionTests.cpp
index bda373bd24c1e89c64e4347e8c431a34c9b15a62..1302532f5caed73495e922fe72010b9a7a2a876c 100644
--- a/tests/DistortionTests.cpp
+++ b/tests/DistortionTests.cpp
@@ -77,14 +77,15 @@ TEST(Transverse, distortMe_AllCoefficientsOne) {
TEST(transverse, removeDistortion1) {
csm::ImageCoord imagePt(7.5, 7.5);
+ double focalLength = 1000.0;
double ux, uy;
std::vector<double> transverseDistortionCoeffs = {
0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
- removeDistortion(imagePt.samp, imagePt.line, ux, uy,
- transverseDistortionCoeffs, DistortionType::TRANSVERSE);
+ removeDistortion(imagePt.samp, imagePt.line, ux, uy, transverseDistortionCoeffs,
+ focalLength, DistortionType::TRANSVERSE);
EXPECT_NEAR(imagePt.samp, 7.5, 1e-8);
EXPECT_NEAR(imagePt.line, 7.5, 1e-8);
@@ -94,14 +95,15 @@ TEST(transverse, removeDistortion1) {
TEST(transverse, removeDistortion_AllCoefficientsOne) {
csm::ImageCoord imagePt(1872.25, 1872.25);
+ double focalLength = 1000.0;
double ux, uy;
std::vector<double> transverseDistortionCoeffs = {
1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
- removeDistortion(imagePt.samp, imagePt.line, ux, uy,
- transverseDistortionCoeffs, DistortionType::TRANSVERSE);
+ removeDistortion(imagePt.samp, imagePt.line, ux, uy, transverseDistortionCoeffs,
+ focalLength, DistortionType::TRANSVERSE);
// The Jacobian is singular, so the setFocalPlane should break out of it's
// iteration and returns the same distorted coordinates that were passed in.
@@ -111,14 +113,15 @@ TEST(transverse, removeDistortion_AllCoefficientsOne) {
TEST(transverse, removeDistortion_AlternatingOnes) {
csm::ImageCoord imagePt(963.75, 908.5);
+ double focalLength = 1000.0;
double ux, uy;
std::vector<double> transverseDistortionCoeffs = {
1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0,
0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0};
- removeDistortion(imagePt.samp, imagePt.line, ux, uy,
- transverseDistortionCoeffs, DistortionType::TRANSVERSE);
+ removeDistortion(imagePt.samp, imagePt.line, ux, uy, transverseDistortionCoeffs,
+ focalLength, DistortionType::TRANSVERSE);
EXPECT_NEAR(ux, 7.5, 1e-8);
EXPECT_NEAR(uy, 7.5, 1e-8);
@@ -128,18 +131,19 @@ TEST(Radial, testUndistortDistort) {
// Distorted pixel
csm::ImageCoord imagePt(0.0, 1e-1);
+ double focalLength = 1000.0;
// Undistort
double ux, uy;
std::vector<double> coeffs = {0.03, 0.00001, 0.000004};
double tolerance = 1e-2;
- removeDistortion(imagePt.samp, imagePt.line, ux, uy, coeffs,
+ removeDistortion(imagePt.samp, imagePt.line, ux, uy, coeffs, focalLength,
DistortionType::RADIAL, tolerance);
// Distort back
double desiredPrecision = 1e-6;
double dx, dy;
- applyDistortion(ux, uy, dx, dy, coeffs,
+ applyDistortion(ux, uy, dx, dy, coeffs, focalLength,
DistortionType::RADIAL, desiredPrecision, tolerance);
EXPECT_NEAR(dx, imagePt.samp, 1e-8);
@@ -152,10 +156,10 @@ TEST(Radial, testInverseDistortion) {
csm::ImageCoord imagePt(0.0, 4.0);
double dx, dy;
+ double focalLength = 1000.0;
double desiredPrecision = 0.01;
std::vector<double> coeffs = {0, 0, 0};
-
- applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs,
+ applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs, focalLength,
DistortionType::RADIAL, desiredPrecision);
EXPECT_NEAR(dx, 4, 1e-8);
@@ -166,10 +170,11 @@ TEST(Radial, testInverseOnesCoeffs) {
csm::ImageCoord imagePt(0.0, 4.0);
double dx, dy;
+ double focalLength = 1000.0;
double desiredPrecision = 0.01;
std::vector<double> coeffs = {1, 1, 1};
- applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs,
+ applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs, focalLength,
DistortionType::RADIAL, desiredPrecision);
EXPECT_NEAR(dx, 4, 1e-8);
@@ -180,10 +185,11 @@ TEST(DawnFc, testApply) {
csm::ImageCoord imagePt(10.0, 10.0);
double dx, dy;
+ double focalLength = 1000.0;
double desiredPrecision = 0.0000001;
std::vector<double> coeffs = {8.4e-06};
- applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs,
+ applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs, focalLength,
DistortionType::DAWNFC, desiredPrecision);
ASSERT_DOUBLE_EQ(dx, 10.0168);
@@ -194,11 +200,12 @@ TEST(DawnFc, testRemove) {
csm::ImageCoord imagePt(10.0168, 10.0168);
double ux, uy;
+ double focalLength = 1000.0;
double desiredPrecision = 0.0000001;
// Coeffs obtained from file FC21A0039691_15231053805F1E.IMG
std::vector<double> coeffs = {8.4e-06};
- removeDistortion(imagePt.samp, imagePt.line, ux, uy, coeffs,
+ removeDistortion(imagePt.samp, imagePt.line, ux, uy, coeffs, focalLength,
DistortionType::DAWNFC, desiredPrecision);
EXPECT_NEAR(ux, 10.0, 1e-8);
@@ -209,14 +216,15 @@ TEST(DawnFc, testZeroCoeffs) {
csm::ImageCoord imagePt(10.0, 10.0);
double ux, uy, dx, dy;
+ double focalLength = 1000.0;
double desiredPrecision = 0.0000001;
std::vector<double> coeffs = {0};
- applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs,
+ applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs, focalLength,
DistortionType::DAWNFC, desiredPrecision);
- removeDistortion(dx, dy, ux, uy, coeffs, DistortionType::DAWNFC,
- desiredPrecision);
+ removeDistortion(dx, dy, ux, uy, coeffs, focalLength,
+ DistortionType::DAWNFC, desiredPrecision);
ASSERT_DOUBLE_EQ(dx, 10.0);
ASSERT_DOUBLE_EQ(dy, 10.0);
@@ -228,10 +236,11 @@ TEST(KaguyaLism, testRemoveCoeffs) {
csm::ImageCoord imagePt(1.0, 1.0);
double ux, uy;
+ double focalLength = 1000.0;
double desiredPrecision = 0.0000001;
std::vector<double> distortionCoeffs = {0.5, 1, 2, 3, 4, 0.5, 1, 2, 3, 4};
- removeDistortion(imagePt.samp, imagePt.line, ux, uy, distortionCoeffs,
+ removeDistortion(imagePt.samp, imagePt.line, ux, uy, distortionCoeffs, focalLength,
DistortionType::KAGUYALISM, desiredPrecision);
EXPECT_NEAR(ux, 1 + 1 + 2.828427125 + 6 + 11.313708499 + 0.5, 1e-8);
@@ -242,6 +251,7 @@ TEST(KaguyaLism, testCoeffs) {
csm::ImageCoord imagePt(1.0, 1.0);
double ux, uy, dx, dy;
+ double focalLength = 1000.0;
double desiredPrecision = 0.0000001;
// Coeffs obtained from file TC1W2B0_01_05211N095E3380.img
std::vector<double> coeffs = {-0.0725, -0.0009649900000000001,
@@ -250,10 +260,10 @@ TEST(KaguyaLism, testCoeffs) {
-0.0013796, 1.3502e-05,
2.7251e-06, -6.193800000000001e-06};
- removeDistortion(imagePt.samp, imagePt.line, ux, uy, coeffs,
+ removeDistortion(imagePt.samp, imagePt.line, ux, uy, coeffs, focalLength,
DistortionType::KAGUYALISM, desiredPrecision);
- applyDistortion(ux, uy, dx, dy, coeffs, DistortionType::KAGUYALISM,
- desiredPrecision);
+ applyDistortion(ux, uy, dx, dy, coeffs, focalLength,
+ DistortionType::KAGUYALISM, desiredPrecision);
EXPECT_NEAR(ux, 0.9279337415074662, 1e-6);
EXPECT_NEAR(uy, 1.0200274261995939, 1e-5);
@@ -265,14 +275,15 @@ TEST(KaguyaLism, testZeroCoeffs) {
csm::ImageCoord imagePt(1.0, 1.0);
double ux, uy, dx, dy;
+ double focalLength = 1000.0;
double desiredPrecision = 0.0000001;
std::vector<double> coeffs = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
- applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs,
+ applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs, focalLength,
DistortionType::KAGUYALISM, desiredPrecision);
- removeDistortion(dx, dy, ux, uy, coeffs, DistortionType::KAGUYALISM,
- desiredPrecision);
+ removeDistortion(dx, dy, ux, uy, coeffs, focalLength,
+ DistortionType::KAGUYALISM, desiredPrecision);
ASSERT_DOUBLE_EQ(dx, 1.0);
ASSERT_DOUBLE_EQ(dy, 1.0);
@@ -283,15 +294,16 @@ TEST(KaguyaLism, testZeroCoeffs) {
// Test for LRO LROC NAC
TEST(LroLrocNac, testLastDetectorSample) {
double ux, uy, dx, dy;
+ double focalLength = 1000.0;
double desiredPrecision = 0.0000001;
// Coeffs obtained from file: lro_lroc_v18.ti
std::vector<double> coeffs = {1.81E-5};
- removeDistortion(0.0, 5064.0 / 2.0 * 0.007, ux, uy, coeffs,
+ removeDistortion(0.0, 5064.0 / 2.0 * 0.007, ux, uy, coeffs, focalLength,
DistortionType::LROLROCNAC, desiredPrecision);
- applyDistortion(ux, uy, dx, dy, coeffs, DistortionType::LROLROCNAC,
- desiredPrecision);
+ applyDistortion(ux, uy, dx, dy, coeffs, focalLength,
+ DistortionType::LROLROCNAC, desiredPrecision);
EXPECT_NEAR(dx, 0.0, 1e-8);
EXPECT_NEAR(dy, 17.724, 1e-8);
@@ -301,15 +313,16 @@ TEST(LroLrocNac, testLastDetectorSample) {
TEST(LroLrocNac, testCoeffs) {
double ux, uy, dx, dy;
+ double focalLength = 1000.0;
double desiredPrecision = 0.0000001;
// Coeff obtained from file: lro_lroc_v18.ti
std::vector<double> coeffs = {1.81E-5};
- applyDistortion(0.0, 0.0, dx, dy, coeffs, DistortionType::LROLROCNAC,
- desiredPrecision);
+ applyDistortion(0.0, 0.0, dx, dy, coeffs, focalLength,
+ DistortionType::LROLROCNAC, desiredPrecision);
- removeDistortion(dx, dy, ux, uy, coeffs, DistortionType::LROLROCNAC,
- desiredPrecision);
+ removeDistortion(dx, dy, ux, uy, coeffs, focalLength,
+ DistortionType::LROLROCNAC, desiredPrecision);
EXPECT_NEAR(dx, 0.0, 1e-8);
EXPECT_NEAR(dy, 0.0, 1e-8);
@@ -319,14 +332,15 @@ TEST(LroLrocNac, testCoeffs) {
TEST(LroLrocNac, testZeroCoeffs) {
double ux, uy, dx, dy;
+ double focalLength = 1000.0;
double desiredPrecision = 0.0000001;
std::vector<double> coeffs = {0};
- applyDistortion(0.0, 0.0, dx, dy, coeffs, DistortionType::LROLROCNAC,
- desiredPrecision);
+ applyDistortion(0.0, 0.0, dx, dy, coeffs, focalLength,
+ DistortionType::LROLROCNAC, desiredPrecision);
- removeDistortion(dx, dy, ux, uy, coeffs, DistortionType::LROLROCNAC,
- desiredPrecision);
+ removeDistortion(dx, dy, ux, uy, coeffs, focalLength,
+ DistortionType::LROLROCNAC, desiredPrecision);
ASSERT_DOUBLE_EQ(dx, 0.0);
ASSERT_DOUBLE_EQ(dy, 0.0);
@@ -343,11 +357,12 @@ TEST_P(CoeffOffsetParameterizedTest, RemoveDistortionCahvorTest)
csm::ImageCoord imagePt(0.0, 4.0);
double ux, uy;
+ double focalLength = 1000.0;
std::vector<double> offsets = GetParam();
std::vector<double> coeffs = {0, 0, 0};
coeffs.insert(coeffs.end(), offsets.begin(), offsets.end());
- removeDistortion(imagePt.samp, imagePt.line, ux, uy, coeffs,
+ removeDistortion(imagePt.samp, imagePt.line, ux, uy, coeffs, focalLength,
DistortionType::CAHVOR);
EXPECT_NEAR(ux, 4, 1e-8);
@@ -361,12 +376,13 @@ TEST_P(CoeffOffsetParameterizedTest, InverseDistortionCahvorTest)
csm::ImageCoord imagePt(0.0, 4.0);
double dx, dy;
+ double focalLength = 1000.0;
double desiredPrecision = 0.01;
std::vector<double> offsets = GetParam();
std::vector<double> coeffs = {0, 0, 0};
coeffs.insert(coeffs.end(), offsets.begin(), offsets.end());
- applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs,
+ applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs, focalLength,
DistortionType::CAHVOR, desiredPrecision);
EXPECT_NEAR(dx, 4, 1e-8);
@@ -378,12 +394,13 @@ TEST_P(CoeffOffsetParameterizedTest, InverseOnesCoeffsCahvorTest)
csm::ImageCoord imagePt(0.0, 4.0);
double dx, dy;
+ double focalLength = 1000.0;
double desiredPrecision = 0.01;
std::vector<double> offsets = GetParam();
std::vector<double> coeffs = {1, 1, 1};
coeffs.insert(coeffs.end(), offsets.begin(), offsets.end());
- applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs,
+ applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs, focalLength,
DistortionType::CAHVOR, desiredPrecision);
EXPECT_NEAR(dx, 4, 1e-8);
@@ -400,17 +417,19 @@ TEST_P(RadTanTest, RadTanInversionTest)
std::vector<double> distCoeffs= {0.000031, -0.000056, 1.3e-5, -1.7e-6, 2.9e-8};
double ux = 5.0, uy = 6.0;
-
+ double focalLength = 1000.0;
// Compute distortion
double dx, dy;
- applyDistortion(ux, uy, dx, dy, distCoeffs, DistortionType::RADTAN, 1e-8, 1e-8);
+ applyDistortion(ux, uy, dx, dy, distCoeffs, focalLength,
+ DistortionType::RADTAN, 1e-8, 1e-8);
// Remove distortion (undistort)
double ux2, uy2;
- removeDistortion(dx, dy, ux2, uy2, distCoeffs, DistortionType::RADTAN, 1e-8);
+ removeDistortion(dx, dy, ux2, uy2, distCoeffs, focalLength,
+ DistortionType::RADTAN, 1e-8);
- EXPECT_NEAR(dx, 4.0010785450000004, 1e-8);
- EXPECT_NEAR(dy, 4.8022116940000013, 1e-8);
+ EXPECT_NEAR(dx, 5.0000006007539586, 1e-8);
+ EXPECT_NEAR(dy, 6.0000016383447505, 1e-8);
EXPECT_NEAR(ux2, ux, 1e-8);
EXPECT_NEAR(uy2, uy, 1e-8);
}