diff --git a/include/usgscsm/Distortion.h b/include/usgscsm/Distortion.h index 574a97080777536fec72a7ff56f558b97f7b29cc..149e374c4dcf5ea83a6765227023a540ddfbd801 100644 --- a/include/usgscsm/Distortion.h +++ b/include/usgscsm/Distortion.h @@ -8,7 +8,8 @@ enum DistortionType { RADIAL, - TRANSVERSE + TRANSVERSE, + KAGUYATC }; // Transverse Distortion diff --git a/src/Distortion.cpp b/src/Distortion.cpp index 7a43cebc8037a38fb4eece6adce9a3b993c7d444..150320bad31d0f549b8829456c83ba6ee39834a3 100644 --- a/src/Distortion.cpp +++ b/src/Distortion.cpp @@ -1,19 +1,5 @@ #include "Distortion.h" - -/** - * @description Jacobian of the distortion function. The Jacobian was computed - * algebraically from the function described in the distortionFunction - * method. - * - * @param x - * @param y - * @param odtX opticalDistCoef In X - * @param odtY opticalDistCoef In Y - * - * @returns jacobian a jacobian vector of vectors as - [0][0]: xx, [0][1]: xy - [1][0]: yx, [1][1]: yy - */ +#include <string> void distortionJacobian(double x, double y, double *jacobian, const std::vector<double> opticalDistCoeffs) { @@ -173,6 +159,47 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, // number of iterations } break; + case KAGUYATC: { + // Apply distortion correction + // see: SEL_TC_V01.TI + // r2 = x^2 + y^2 + // Line-of-sight vector of pixel no. n can be expressed as below. + + // Distortion coefficients information: + // INS<INSTID>_DISTORTION_COEF_X = ( a0, a1, a2, a3) + // INS<INSTID>_DISTORTION_COEF_Y = ( b0, b1, b2, b3), + // + // Distance r from the center: + // r = - (n - INS<INSTID>_CENTER) * INS<INSTID>_PIXEL_SIZE. + + // Line-of-sight vector v is calculated as + // v[X] = INS<INSTID>BORESIGHT[X] + // +a0 +a1*r +a2*r^2 +a3*r^3 , + // v[Y] = INS<INSTID>BORESIGHT[Y] + // 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()); + } + + const double* odkx = opticalDistCoeffs.data(); + const double* odky = opticalDistCoeffs.data()+4; + + double r2 = dx*dx + dy*dy; + double r = sqrt(r2); + double r3 = r2 * r; + + int xPointer = 0; + int yPointer = 5; + + 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; + } } } @@ -233,5 +260,69 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, computeTransverseDistortion(ux, uy, dx, dy, opticalDistCoeffs); } break; + case KAGUYATC: { + if (opticalDistCoeffs.size() != 8) { + throw "Distortion coefficients for Kaguya TC must be of size 8, got: " + std::to_string(opticalDistCoeffs.size()); + } + + const double* odkx = opticalDistCoeffs.data(); + const double* odky = opticalDistCoeffs.data()+4; + + double xt = ux; + double yt = uy; + + double xx, yy, r, rr, rrr, dr_x, dr_y; + double xdistortion, ydistortion; + double xdistorted, ydistorted; + double xprevious, yprevious; + + xprevious = 1000000.0; + yprevious = 1000000.0; + + double tolerance = 0.000001; + bool bConverged = false; + + // Iterating to introduce distortion... + // We stop when the difference between distorted coordinates + // in successive iterations is below the given tolerance + for (int i = 0; i < 50; i++) { + xx = xt * xt; + yy = yt * yt; + rr = xx + yy; + r = sqrt(rr); + rrr = rr * r; + + // Radial distortion + // dr is the radial distortion contribution + dr_x = odkx[0] + odkx[1] * r + odkx[2] * rr + odkx[3] * rrr; + dr_y = odky[0] + odky[1] * r + odky[2] * rr + odky[3] * rrr; + + // Distortion at the current point location + xdistortion = dr_x; + ydistortion = dr_y; + + // updated image coordinates + xt = ux - xdistortion; + yt = uy - ydistortion; + + // distorted point corrected for principal point + xdistorted = xt; + ydistorted = yt; + + // check for convergence + if ((fabs(xt - xprevious) < tolerance) && (fabs(yt - yprevious) < tolerance)) { + bConverged = true; + break; + } + + xprevious = xt; + yprevious = yt; + } + + if (bConverged) { + dx = xdistorted; + dy = ydistorted; + } + } } } diff --git a/src/UsgsAstroFrameSensorModel.cpp b/src/UsgsAstroFrameSensorModel.cpp index 4fdef5212a7dc5247caa74c6f0a3ebef06423dd5..c5e07fd19f3f1dccc593161570f4c3b8dda7573c 100644 --- a/src/UsgsAstroFrameSensorModel.cpp +++ b/src/UsgsAstroFrameSensorModel.cpp @@ -743,6 +743,7 @@ std::string UsgsAstroFrameSensorModel::getModelState() const { {"m_currentParameterCovariance", m_currentParameterCovariance}, {"m_logFile", m_logFile} }; + return state.dump(); } diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp index ead2b43624defb6fc8409c87b5e1ce5f9f90b62f..282843f22717c78e701cd3a253d6c7a48cbd7548 100644 --- a/src/UsgsAstroLsSensorModel.cpp +++ b/src/UsgsAstroLsSensorModel.cpp @@ -1641,7 +1641,7 @@ void UsgsAstroLsSensorModel::losToEcf( removeDistortion(distortedFocalPlaneX, distortedFocalPlaneY, undistortedFocalPlaneX, undistortedFocalPlaneY, m_opticalDistCoeffs, - DistortionType::RADIAL); + m_distortionType); // Define imaging ray (look vector) in camera space double cameraLook[3]; diff --git a/src/Utilities.cpp b/src/Utilities.cpp index 59f26368a98fdcfd6276a48982ddba06d9f640c6..eb5a674a3dff2b8d1a853522003193e31c6a56cf 100644 --- a/src/Utilities.cpp +++ b/src/Utilities.cpp @@ -763,6 +763,9 @@ 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; + } } catch (...) { if (list) { @@ -827,6 +830,30 @@ std::vector<double> getDistortionCoeffs(json isd, csm::WarningList *list) { coefficients = std::vector<double>(3, 0.0); } } + break; + case DistortionType::KAGUYATC: { + try { + std::vector<double> coefficientsX(4,0); + std::vector<double> coefficientsY(4,0); + + coefficientsX = isd.at("optical_distortion").at("kaguyatc").at("x").get<std::vector<double>>(); + coefficientsY = isd.at("optical_distortion").at("kaguyatc").at("y").get<std::vector<double>>(); + coefficientsX.insert(coefficientsX.end(), coefficientsY.begin(), coefficientsY.end()); + + return coefficientsX; + } + catch (...) { + if (list) { + list->push_back( + csm::Warning( + csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse a set of transverse distortion model coefficients.", + "Utilities::getDistortion()")); + } + coefficients = std::vector<double>(8, 0.0); + } + } + } if (list) { list->push_back(