diff --git a/include/usgscsm/Distortion.h b/include/usgscsm/Distortion.h index 9cf8c3cd2196f44df62304b620f3e36b9d9b0c4a..fccc1ed6989fd69c67344da5734af40938eeeff1 100644 --- a/include/usgscsm/Distortion.h +++ b/include/usgscsm/Distortion.h @@ -9,7 +9,7 @@ enum DistortionType { RADIAL, TRANSVERSE, - KAGUYATC, + KAGUYALISM, DAWNFC, LROLROCNAC }; diff --git a/src/Distortion.cpp b/src/Distortion.cpp index 1d680898e91349c3ace129dfc9673a5dead27b79..8f4f313c1fc75f1857b5233afc0fe7d36ffa99c8 100644 --- a/src/Distortion.cpp +++ b/src/Distortion.cpp @@ -1,4 +1,6 @@ #include "Distortion.h" + +#include <Error.h> #include <string> void distortionJacobian(double x, double y, double *jacobian, @@ -99,7 +101,7 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, if (rr > tolerance) { double dr = opticalDistCoeffs[0] + (rr * (opticalDistCoeffs[1] + rr * opticalDistCoeffs[2])); - + ux = dx * (1.0 - dr); uy = dy * (1.0 - dr); } @@ -164,10 +166,9 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, } break; - // KaguyaTC - case KAGUYATC: { + case KAGUYALISM: { // Apply distortion correction - // see: SEL_TC_V01.TI + // see: SEL_TC_V01.TI and SEL_MI_V01.TI // r2 = x^2 + y^2 // Line-of-sight vector of pixel no. n can be expressed as below. @@ -185,13 +186,18 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, // b0 +b1*r +b2*r^2 +b3*r^3 // v[Z] = INS<INSTID>BORESIGHT[Z] . - // Coeffs should be [x0,x1,x2,x3,y0,y1,y2,y3] - if (opticalDistCoeffs.size() != 8) { - throw "Distortion coefficients for Kaguya TC must be of size 8, got: " + std::to_string(opticalDistCoeffs.size()); + // Coeffs should be [boresightX,x0,x1,x2,x3,boresightY,y0,y1,y2,y3] + if (opticalDistCoeffs.size() != 10) { + csm::Error::ErrorType errorType = csm::Error::INDEX_OUT_OF_RANGE; + std::string message = "Distortion coefficients for Kaguya LISM must be of size 10, got: " + std::to_string(opticalDistCoeffs.size()); + std::string function = "removeDistortion"; + throw csm::Error(errorType, message, function); } - const double* odkx = opticalDistCoeffs.data(); - const double* odky = opticalDistCoeffs.data()+4; + double boresightX = opticalDistCoeffs[0]; + std::vector<double> odkx(opticalDistCoeffs.begin()+1, opticalDistCoeffs.begin()+5); + double boresightY = opticalDistCoeffs[5]; + std::vector<double> odky(opticalDistCoeffs.begin()+6, opticalDistCoeffs.begin()+10); double r2 = dx*dx + dy*dy; double r = sqrt(r2); @@ -203,8 +209,8 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, double dr_x = odkx[0] + odkx[1] * r + odkx[2] * r2 + odkx[3] * r3; double dr_y = odky[0] + odky[1] * r + odky[2] * r2 + odky[3] * r3; - ux = dx + dr_x; - uy = dy + dr_y; + ux = dx + dr_x + boresightX; + uy = dy + dr_y + boresightY; } break; @@ -250,7 +256,7 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, return; } } - while(!done); + while(!done); /**************************************************************************** * Sucess ... @@ -265,14 +271,20 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, case LROLROCNAC: { if (opticalDistCoeffs.size() != 1) { - throw "Distortion coefficients for LRO LROC NAC must be of size 1, current size: " + std::to_string(opticalDistCoeffs.size()); + csm::Error::ErrorType errorType = csm::Error::INDEX_OUT_OF_RANGE; + std::string message = "Distortion coefficients for LRO LROC NAC must be of size 1, current size: " + std::to_string(opticalDistCoeffs.size()); + std::string function = "removeDistortion"; + throw csm::Error(errorType, message, function); } double dk1 = opticalDistCoeffs[0]; double den = 1 + dk1 * dy * dy; // r = dy*dy = distance from the focal plane center if (den == 0.0) { - throw "Unable to remove distortion for LRO LROC NAC. Focal plane position " + std::to_string(dy); + csm::Error::ErrorType errorType = csm::Error::ALGORITHM; + std::string message = "Unable to remove distortion for LRO LROC NAC. Focal plane position " + std::to_string(dy); + std::string function = "removeDistortion"; + throw csm::Error(errorType, message, function); } ux = dx; @@ -306,12 +318,12 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, // Compute first fractional distortion using rp double drOverR = opticalDistCoeffs[0] + (rp2 * (opticalDistCoeffs[1] + (rp2 * opticalDistCoeffs[2]))); - + // Compute first distorted point estimate, r double r = rp + (drOverR * rp); double r_prev, r2_prev; int iteration = 0; - + do { // Don't get in an end-less loop. This algorithm should // converge quickly. If not then we are probably way outside @@ -335,7 +347,7 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, iteration++; } while (fabs(r - r_prev) > desiredPrecision); - + dx = ux / (1.0 - drOverR); dy = uy / (1.0 - drOverR); } @@ -346,17 +358,21 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, } break; - // KaguyaTC - case KAGUYATC: { - if (opticalDistCoeffs.size() != 8) { - throw "Distortion coefficients for Kaguya TC must be of size 8, got: " + std::to_string(opticalDistCoeffs.size()); + case KAGUYALISM: { + if (opticalDistCoeffs.size() != 10) { + csm::Error::ErrorType errorType = csm::Error::INDEX_OUT_OF_RANGE; + std::string message = "Distortion coefficients for Kaguya LISM must be of size 10, got: " + std::to_string(opticalDistCoeffs.size()); + std::string function = "applyDistortion"; + throw csm::Error(errorType, message, function); } - const double* odkx = opticalDistCoeffs.data(); - const double* odky = opticalDistCoeffs.data()+4; + double boresightX = opticalDistCoeffs[0]; + std::vector<double> odkx(opticalDistCoeffs.begin()+1, opticalDistCoeffs.begin()+5); + double boresightY = opticalDistCoeffs[5]; + std::vector<double> odky(opticalDistCoeffs.begin()+6, opticalDistCoeffs.begin()+10); - double xt = ux; - double yt = uy; + double xt = ux - boresightX; + double yt = uy - boresightY; double xx, yy, r, rr, rrr, dr_x, dr_y; double xdistortion, ydistortion; @@ -389,8 +405,8 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, ydistortion = dr_y; // updated image coordinates - xt = ux - xdistortion; - yt = uy - ydistortion; + xt = ux - xdistortion - boresightX; + yt = uy - ydistortion - boresightY; // distorted point corrected for principal point xdistorted = xt; @@ -426,7 +442,7 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, } break; - // The LRO LROC NAC distortion model uses an iterative approach to go from + // 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 case LROLROCNAC: { @@ -441,7 +457,10 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, bool bConverged = false; if (opticalDistCoeffs.size() != 1) { - throw "Distortion coefficients for LRO LROC NAC must be of size 1, current size: " + std::to_string(opticalDistCoeffs.size()); + csm::Error::ErrorType errorType = csm::Error::INDEX_OUT_OF_RANGE; + std::string message = "Distortion coefficients for LRO LROC NAC must be of size 1, current size: " + std::to_string(opticalDistCoeffs.size()); + std::string function = "applyDistortion"; + throw csm::Error(errorType, message, function); } double dk1 = opticalDistCoeffs[0]; @@ -483,7 +502,7 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, yprevious = yt; } - + if(bConverged) { dx = ux; dy = ydistorted; diff --git a/src/Utilities.cpp b/src/Utilities.cpp index 8c8000a1b8014c3fcdc93923bb8a4350e1416780..dbcc87d0e6c73d66ca74bab02658a4f67287ff01 100644 --- a/src/Utilities.cpp +++ b/src/Utilities.cpp @@ -765,8 +765,8 @@ DistortionType getDistortionModel(json isd, csm::WarningList *list) { else if (distortion.compare("radial") == 0) { return DistortionType::RADIAL; } - else if (distortion.compare("kaguyatc") == 0) { - return DistortionType::KAGUYATC; + else if (distortion.compare("kaguyalism") == 0) { + return DistortionType::KAGUYALISM; } else if (distortion.compare("dawnfc") == 0) { return DistortionType::DAWNFC; @@ -841,13 +841,18 @@ std::vector<double> getDistortionCoeffs(json isd, csm::WarningList *list) { } } break; - case DistortionType::KAGUYATC: { + case DistortionType::KAGUYALISM: { try { - std::vector<double> coefficientsX = isd.at("optical_distortion").at("kaguyatc").at("x").get<std::vector<double>>(); + std::vector<double> coefficientsX = isd.at("optical_distortion").at("kaguyalism").at("x").get<std::vector<double>>(); + std::vector<double> coefficientsY = isd.at("optical_distortion").at("kaguyalism").at("y").get<std::vector<double>>(); + double boresightX = isd.at("optical_distortion").at("kaguyalism").at("boresight_x").get<double>(); + double boresightY = isd.at("optical_distortion").at("kaguyalism").at("boresight_y").get<double>(); + coefficientsX.resize(4, 0.0); - std::vector<double> coefficientsY = isd.at("optical_distortion").at("kaguyatc").at("y").get<std::vector<double>>(); coefficientsY.resize(4, 0.0); + coefficientsX.insert(coefficientsX.begin(), boresightX); + coefficientsY.insert(coefficientsY.begin(), boresightY); coefficientsX.insert(coefficientsX.end(), coefficientsY.begin(), coefficientsY.end()); return coefficientsX; @@ -857,7 +862,7 @@ std::vector<double> getDistortionCoeffs(json isd, csm::WarningList *list) { list->push_back( csm::Warning( csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse a set of transverse distortion model coefficients.", + "Could not parse a set of Kaguya LISM distortion model coefficients.", "Utilities::getDistortion()")); } coefficients = std::vector<double>(8, 0.0); diff --git a/tests/DistortionTests.cpp b/tests/DistortionTests.cpp index 3aa73c50dfc90747f6414bdbaf1c7b3adde45b77..4cd47381628fc6dd0f78acce6a47d7f9ff3a6722 100644 --- a/tests/DistortionTests.cpp +++ b/tests/DistortionTests.cpp @@ -199,57 +199,58 @@ TEST(DawnFc, testZeroCoeffs) { ASSERT_DOUBLE_EQ(uy, 10.0); } -TEST(KaguyaTc, testRemoveCoeffs) { +TEST(KaguyaLism, testRemoveCoeffs) { csm::ImageCoord imagePt(1.0, 1.0); double ux, uy; double desiredPrecision = 0.0000001; - std::vector<double> distortionCoeffs = {1, 2, 3, 4, - 1, 2, 3, 4}; + std::vector<double> distortionCoeffs = {0.5, 1, 2, 3, 4, + 0.5, 1, 2, 3, 4}; removeDistortion(imagePt.samp, imagePt.line, ux, uy, distortionCoeffs, - DistortionType::KAGUYATC, desiredPrecision); + DistortionType::KAGUYALISM, desiredPrecision); - EXPECT_NEAR(ux, 1 + 1 + 2.828427125 + 6 + 11.313708499, 1e-8); - EXPECT_NEAR(uy, 1 + 1 + 2.828427125 + 6 + 11.313708499, 1e-8); + EXPECT_NEAR(ux, 1 + 1 + 2.828427125 + 6 + 11.313708499 + 0.5, 1e-8); + EXPECT_NEAR(uy, 1 + 1 + 2.828427125 + 6 + 11.313708499 + 0.5, 1e-8); } -TEST(KaguyaTc, testCoeffs) { +TEST(KaguyaLism, testCoeffs) { csm::ImageCoord imagePt(1.0, 1.0); double ux, uy, dx, dy; double desiredPrecision = 0.0000001; // Coeffs obtained from file TC1W2B0_01_05211N095E3380.img - std::vector<double> coeffs = {-0.0009649900000000001, 0.00098441, 8.5773e-06, -3.7438e-06, - -0.0013796, 1.3502e-05, 2.7251e-06, -6.193800000000001e-06}; + std::vector<double> coeffs = {-0.0725, -0.0009649900000000001, 0.00098441, 8.5773e-06, -3.7438e-06, + 0.0214, -0.0013796, 1.3502e-05, 2.7251e-06, -6.193800000000001e-06}; + + removeDistortion(imagePt.samp, imagePt.line, ux, uy, coeffs, + DistortionType::KAGUYALISM, desiredPrecision); + applyDistortion(ux, uy, dx, dy, coeffs, + DistortionType::KAGUYALISM, desiredPrecision); - applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs, - DistortionType::KAGUYATC, desiredPrecision); - removeDistortion(dx, dy, ux, uy, coeffs, - DistortionType::KAGUYATC, desiredPrecision); - EXPECT_NEAR(dx, 0.999566, 1e-6); - EXPECT_NEAR(dy, 1.00137, 1e-5); - EXPECT_NEAR(ux, 1.0, 1e-8); - EXPECT_NEAR(uy, 1.0, 1e-8); + EXPECT_NEAR(ux, 0.9279337415074662, 1e-6); + EXPECT_NEAR(uy, 1.0200274261995939, 1e-5); + EXPECT_NEAR(dx, 1.0, 1e-8); + EXPECT_NEAR(dy, 1.0, 1e-8); } -TEST(KaguyaTc, testZeroCoeffs) { +TEST(KaguyaLism, testZeroCoeffs) { csm::ImageCoord imagePt(1.0, 1.0); double ux, uy, dx, dy; double desiredPrecision = 0.0000001; - std::vector<double> coeffs = {0, 0, 0, 0, - 0, 0, 0, 0}; + std::vector<double> coeffs = {0, 0, 0, 0, 0, + 0, 0, 0, 0, 0}; applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs, - DistortionType::KAGUYATC, desiredPrecision); + DistortionType::KAGUYALISM, desiredPrecision); removeDistortion(dx, dy, ux, uy, coeffs, - DistortionType::KAGUYATC, desiredPrecision); + DistortionType::KAGUYALISM, desiredPrecision); ASSERT_DOUBLE_EQ(dx, 1.0); ASSERT_DOUBLE_EQ(dy, 1.0); @@ -314,4 +315,3 @@ TEST(LroLrocNac, testZeroCoeffs) { ASSERT_DOUBLE_EQ(ux, 0.0); ASSERT_DOUBLE_EQ(uy, 0.0); } -