diff --git a/include/usgscsm/Distortion.h b/include/usgscsm/Distortion.h index 10c566918..d5c3c5165 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 const& opticalDistCoeffs, + double focalLength, DistortionType distortionType, const double tolerance = 1.0E-6); void applyDistortion(double ux, double uy, double &dx, double &dy, std::vector 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 38f397ac7..e69a5cd22 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 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 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 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 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 6eecd62e1..d702253c6 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 9e27bea59..89c9a1103 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 0c9332647..418df839e 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 bda373bd2..1302532f5 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 offsets = GetParam(); std::vector 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 offsets = GetParam(); std::vector 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 offsets = GetParam(); std::vector 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 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); }