From 53a5edd6deeb06f7fda77ec1c92b3cf7b143138f Mon Sep 17 00:00:00 2001 From: Kelvin Rodriguez <kr788@nau.edu> Date: Tue, 16 Apr 2019 14:05:34 -0700 Subject: [PATCH] Adding distortion for Kaguya TC (#225) * Merging dev into master for release (#158) * Updated ISD and tests (#87) * updated tests + RIP TestyMcTestFace * keys updated * Probably should not hard code .cpps in the CMakeLists.txt for GTest * updated specs to mimic synthetic json * appveyor assumed broken from standup, disabling until fixed * eulers are now quats * merge from jay's changes * merge markers * renamed quats as per Jessie's reccomendation * only appends sensor_orientation to missing params * stupid markers * updated test ISD * changes as per some comments * updated as per comments * defaults are all 0 * #pragma once to C style header guard * stragler change * missed some params * missed another * moving things around * patched segfault error + added detector center as defined in json schema (#94) * patched segfault error + added detector center as defined in the swagger schema * verbose travi * Fixed optical distortion and pixel to focal transforms. (#115) * Update to use quaternions in test to match new ISD * Fixes #121 * Fix 1 remaining failing omega rotation test * Updates token to be in quotes * Still trying to get the builds working. * Updates to force upload * Adds newline * Update for formatting * Trying to force travis to deploy with labels I am editing this on the repo in order to get Travis to do its thing - I can not do this via a PR. * Update .travis.yml * Update meta.yaml to try and force a dev tag on the builds to get labels to work. * Update meta.yaml * Update .travis.yml * test x scaling for reset model state * test FL500 conversion * test FL500 conversion * Add tests to include * commit to force run of tests on mac with debug output * switch to using tolerances from dev * 0.5 pixel offset * try moving set of filename into setup * initialize differently test * more debug output * Add matrix to debug output * Even more debug output from matrix calculation * Add missing quaternion component to state string * cleanup debug output now that problem is fixed * fix added spacing * Adds in check for PR for upload * echo to check travis env * fixes equality check for PR * Clean up some spacing (non-functional) in UsgsAstroFrameModel * Fixed canModelBeConstructedFromISD throwing an exception when the metadata file doesn't exist. (#134) * fixed can be converted error * Added not constructible test for frame plugin * Removed old LS ISD header (#137) * Update some of the tests that set a new state value to use a function in the fixture, rather than repeating code * Windows Build (#139) * adds win build * Windows build * Updates submodules * Refactoring to move to one plugin and removed unused classes. (#138) * merge * changed varrs * reset test * First iteration * it compiles, although renaming this is still neccessary * added source * cleaned up, tests still need to pass * post merge clean up * validation updated, validation tests are now passing * Addressed comments from Jesse * last Jesse comment, convertISDToModelState doesn't check if pointer is invalid anymore as the sensor model should except * model_name -> m_modelName for frame getState * copy paste error in FrameSensorModel * Resolve merge conflicts with FrameIsdTest vs. SimpleFrameIsdTest names * Fix hopefully last merge problem * Conda build on win (#143) * Conda build on win * Trying a straight build first * Unneeded csmapi * Now trying to upload build * Trying a build and upload * Trying syntax change * trying ps * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update meta.yaml * Update bld.bat This is a test to see where appveyor is failing. * Update bld.bat * Update bld.bat * Update meta.yaml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Adds if/else into appveyor (#146) * Adds if/else into appveyor * Update .appveyor.yml * Updates to use gcc7 * Update .travis.yml * Update .appveyor.yml * Fixed Line Scan construction and condensed plugin tests (#145) * First pass at test LS ISD * Initial LS plugin test base * fixed a test in frame plugin tests * Initial LS plugin test suite * Moved to single plugin test file * Added some new plugin tests * Fixed LS construction * Re-updated submodules * Reverted gtest * removed debuf prints left in and made getModelName functional (#148) * Changed line scan sensor model to new ISD spec (#149) * Changed line scan sensor model to new ISD spec * Updated LS to the proper spec * Changed model_name to name_model * Updated tests to use new name_ format in ISD * Updated LS test data to new spec * Fixed typo in ls test ISD * Moved framer to new ISD format. Added bad debug statements. * Updated LS to new spec * Fixed focal length epsilon name * Added model state test for framer (#152) * fixed nan issue (#153) * fixed nan issue * left in for loop * Removed switch statement for ls distortion: (#150) * Updated build recipe to use release and sha hash (#160) * Merge dev into master for 1.1 release (#190) * Updated ISD and tests (#87) * updated tests + RIP TestyMcTestFace * keys updated * Probably should not hard code .cpps in the CMakeLists.txt for GTest * updated specs to mimic synthetic json * appveyor assumed broken from standup, disabling until fixed * eulers are now quats * merge from jay's changes * merge markers * renamed quats as per Jessie's reccomendation * only appends sensor_orientation to missing params * stupid markers * updated test ISD * changes as per some comments * updated as per comments * defaults are all 0 * #pragma once to C style header guard * stragler change * missed some params * missed another * moving things around * patched segfault error + added detector center as defined in json schema (#94) * patched segfault error + added detector center as defined in the swagger schema * verbose travi * Fixed optical distortion and pixel to focal transforms. (#115) * Update to use quaternions in test to match new ISD * Fixes #121 * Fix 1 remaining failing omega rotation test * Updates token to be in quotes * Still trying to get the builds working. * Updates to force upload * Adds newline * Update for formatting * Trying to force travis to deploy with labels I am editing this on the repo in order to get Travis to do its thing - I can not do this via a PR. * Update .travis.yml * Update meta.yaml to try and force a dev tag on the builds to get labels to work. * Update meta.yaml * Update .travis.yml * test x scaling for reset model state * test FL500 conversion * test FL500 conversion * Add tests to include * commit to force run of tests on mac with debug output * switch to using tolerances from dev * 0.5 pixel offset * try moving set of filename into setup * initialize differently test * more debug output * Add matrix to debug output * Even more debug output from matrix calculation * Add missing quaternion component to state string * cleanup debug output now that problem is fixed * fix added spacing * Adds in check for PR for upload * echo to check travis env * fixes equality check for PR * Clean up some spacing (non-functional) in UsgsAstroFrameModel * Fixed canModelBeConstructedFromISD throwing an exception when the metadata file doesn't exist. (#134) * fixed can be converted error * Added not constructible test for frame plugin * Removed old LS ISD header (#137) * Update some of the tests that set a new state value to use a function in the fixture, rather than repeating code * Windows Build (#139) * adds win build * Windows build * Updates submodules * Refactoring to move to one plugin and removed unused classes. (#138) * merge * changed varrs * reset test * First iteration * it compiles, although renaming this is still neccessary * added source * cleaned up, tests still need to pass * post merge clean up * validation updated, validation tests are now passing * Addressed comments from Jesse * last Jesse comment, convertISDToModelState doesn't check if pointer is invalid anymore as the sensor model should except * model_name -> m_modelName for frame getState * copy paste error in FrameSensorModel * Resolve merge conflicts with FrameIsdTest vs. SimpleFrameIsdTest names * Fix hopefully last merge problem * Conda build on win (#143) * Conda build on win * Trying a straight build first * Unneeded csmapi * Now trying to upload build * Trying a build and upload * Trying syntax change * trying ps * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update meta.yaml * Update bld.bat This is a test to see where appveyor is failing. * Update bld.bat * Update bld.bat * Update meta.yaml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Update .appveyor.yml * Adds if/else into appveyor (#146) * Adds if/else into appveyor * Update .appveyor.yml * Updates to use gcc7 * Update .travis.yml * Update .appveyor.yml * Fixed Line Scan construction and condensed plugin tests (#145) * First pass at test LS ISD * Initial LS plugin test base * fixed a test in frame plugin tests * Initial LS plugin test suite * Moved to single plugin test file * Added some new plugin tests * Fixed LS construction * Re-updated submodules * Reverted gtest * removed debuf prints left in and made getModelName functional (#148) * Changed line scan sensor model to new ISD spec (#149) * Changed line scan sensor model to new ISD spec * Updated LS to the proper spec * Changed model_name to name_model * Updated tests to use new name_ format in ISD * Updated LS test data to new spec * Fixed typo in ls test ISD * Moved framer to new ISD format. Added bad debug statements. * Updated LS to new spec * Fixed focal length epsilon name * Fixes 142 for framer * Removes unused state keyword counter * Updates Ls * Updates for parametercov and ref point * Adds sensor and platform names * Updates with collection identifier * updates for vector parameters in state * Added model state test for framer (#152) * Adds current param value and referencePt * unsupported pushed in * fixed nan issue (#153) * fixed nan issue * left in for loop * Removed switch statement for ls distortion: (#150) * Updated build recipe to use release and sha hash * Copied distortion inversion from ISIS3 Distortion Map (#161) * Fixed ground to image and image to ground not giving inverse results. (#162) * Copied distortion inversion from ISIS3 Distortion Map * mid-testing commit to fix LS i2g and g2i inconsistency * Fixed ground to image taking excessive iterations. Added simple line scan test and test stubs * Added more comments and replaced some hard coded tolerances * Added debug output to ground to image error message. (#168) * Initial steps to modularize losToEcf in UsgsAstroLsSensorModel * Try changing line endings * Re-converted line endings to dos * Modularize some of computeViewingPixel * Cleanup * Reconvert to dos line endings * Reformat member variables in header and switch more to use const pass-by-reference parameters to be consistent with rest of code * Updates in response to code review (part 1) * Update calculateRotationMatrixFromQuaternions -- removed invert parameter. Now, will always return the matrix that rotates the in the same direction as the quaternions. * Updates the version number in the meta. (#159) * Updates the version number in the meta. * Update meta.yaml * Build to hash This should be using the short hash now. * Update meta.yaml * Update meta.yaml * Finish Modularizing computeViewingPixel (#170) * Finishing modularization of UsgsAstroLsSensorModel::computeViewingPixel * Fixes per request * Removing ISIS references in variable names while I'm here * Example for LS testing (#171) * Removed odd height gradient descent search from image to ground when not intersecting. * Added first off body test. * Added angular velocity line scan test * Added failing, copy paste tests * Commented out line scan pan tests until 0 velocity bug is addressed * Removed extraneous focal epsilon from LS test ISDs * Extracts distortion models from the Line Scanner and Framer cameras into its own function set (#172) * Moved transverse distortion from line scanner into new file * Updated fixtures for distortion tests * Forgot a semicolon * Extracted radial destortion, and built associated tests * Removed invertDistortion commented code from Linescanner * Reconstructed distortion functions return results * Incorperated invertDistortion function * Corrected spelling, and removed commented out functions * Renamed all dpoint variables to distortionPoint * Removed computeUndistortedFocalPlaneCoordinates function * Renamed distortionPoint to undistortedPoint and distortedPoint where applicable * Fixed typo * Removed doc string parameters * Further extracted transverse destortion out of the Framer * Pulled losToEcf shared functions out into Utilities class (#174) * Initial steps to pull common camera model or math utility functions into a separate Utilities file. * dos2unix line endings again * ../include/usgscsm/UsgsAstroLsSensorModel.h * updates to modularized functions * dos2unix UsgsAstroFrameSensorModel * Forgot to actually add the Utilities class * Keep needed to unix2dos to re-fix line ending caracters * We have some dos line ending and some unix line endings in our code * Copied unit conversion from framer to ls (#177) * Makes GTest fully optional (#175) * removes unused setup.py (#178) * Refactored ISD parsing into utilities and added warnings (#179) * Added utility ISD parsing functions * ISD Parsing Test stub * Added ISD parsing method tests * Added ISD Parsing utilities to Framer * Replaced framer ISD parsing * added check for warning output to bad fram ISD test * Added a bit more testing * Removed ground to image 10m errormessage (#180) * Fixed state being written out and read in differently (#183) * Fixed state being written out and read in differently * Fixed canModelBeConstructedFromState * Updated README (#154) * Updated README * Added BAE license and removed open source license * Gtests for Utilities.cpp (#181) * Finishing modularization of UsgsAstroLsSensorModel::computeViewingPixel * Fixes per request * Removing ISIS references in variable names while I'm here * Beginning to implement tests for modularization changes in LS sensor model class * Adding tests for Utilities as well as a few minor modifications * Making suggested changes and adding test for caculateAttitudeCorrection * Fixing vector size * Updated LS ISD parsing code to match Framer (#186) * updated LS ISD parsing to match style in framer * reverted the 1 to 0 * Fixed attitude test, removed all cerr lines * removed image flip flag * added tests * Fixed computeDistortedFocalPlaneCoordinates test (#187) * Distortion Integration (#184) * Moved transverse distortion from line scanner into new file * Extracted radial destortion, and built associated tests * Reconstructed distortion functions return results * Corrected spelling, and removed commented out functions * Renamed all dpoint variables to distortionPoint * Removed computeUndistortedFocalPlaneCoordinates function * Renamed distortionPoint to undistortedPoint and distortedPoint where applicable * Removed doc string parameters * Added combined new distortion model * Half way through redefining parameters * Major update to distortion and coefficient storage * More debugging * More debugging * More debugging * debugging * debugging * Removed debugging messages and set the output value for remove distortion * Brought changes inline with dev * Updated isd parsing to handle either radial or transverse * Added Distortion Parse function * Split apply and remove distortion, and added the distortion type to each model state * Removed cmath * Removing prints, and other fixes * Updated how coefficients are parsed * Reverted notebook * Function namespace and parameter clean up * More function and parameter clean up * Reverted old code and updated distortion defaults * Small changes for distortion * Updated utilities and fixed failing test * Final LS tests and release prep (#188) * Added image locus tests * Pan LS tests now pass * updated release date in plugin * Forgot to update plugin release date test * added kaguya distortion model * address comments * default for KAGUYATC Distortion now zero vector of len 8 --- include/usgscsm/Distortion.h | 3 +- src/Distortion.cpp | 121 ++++++++++++++++++++++++++---- src/UsgsAstroFrameSensorModel.cpp | 1 + src/UsgsAstroLsSensorModel.cpp | 2 +- src/Utilities.cpp | 27 +++++++ 5 files changed, 137 insertions(+), 17 deletions(-) diff --git a/include/usgscsm/Distortion.h b/include/usgscsm/Distortion.h index 574a970..149e374 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 7a43ceb..150320b 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 4fdef52..c5e07fd 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 ead2b43..282843f 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 59f2636..eb5a674 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( -- GitLab