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