Commit c892d5dc authored by acpaquette's avatar acpaquette Committed by Jesse Mapel
Browse files

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
parent 25fc4fe9

include/usgscsm/Distortion.h

+18 −13
Original line number Diff line number Diff line
@@ -4,22 +4,27 @@ 
#include <vector>

#include <math.h>

#include <tuple>

#include <iostream>



// Transverse Distortion

std::tuple<double, double> removeDistortion(double dx, double dy,

                        const std::vector<double> &odtX, const std::vector<double> &odtY);



std::vector<std::vector<double>> distortionJacobian(double x, double y,

                        const std::vector<double> &odtX, const std::vector<double> &odtY);

enum DistortionType {

  RADIAL,

  TRANSVERSE

};



std::tuple<double, double> distortionFunction(double ux, double uy,

                        const std::vector<double> &odtX, const std::vector<double> &odtY);

// Transverse Distortion

void distortionJacobian(double x, double y, double *jacobian,

                        const std::vector<double> opticalDistCoeffs);



// Radial Distortion

std::tuple<double, double> removeDistortion(double inFocalPlaneX, double inFocalPlaneY,

                        const double opticalDistCoef[3], double tolerance = 1.0E-6);

void computeTransverseDistortion(double ux, double uy, double &dx, double &dy,

                                 const std::vector<double> opticalDistCoeffs);



std::tuple<double, double> invertDistortion(double inFocalPlaneX, double inFocalPlaneY,

                        const double opticalDistCoef[3], double desiredPrecision, double tolerance = 1.0E-6);

void removeDistortion(double dx, double dy, double &ux, double &uy,

                      const std::vector<double> opticalDistCoeffs,

                      DistortionType distortionType,

                      const double tolerance = 1.0E-6);



void applyDistortion(double ux, double uy, double &dx, double &dy,

                     const std::vector<double> opticalDistCoeffs,

                     DistortionType distortionType,

                     const double desiredPrecision = 0, const double tolerance = 1.0E-6);

#endif

include/usgscsm/UsgsAstroFrameSensorModel.h

+3 −2
Original line number Diff line number Diff line
@@ -9,6 +9,7 @@ 
#include "RasterGM.h"

#include "CorrelationModel.h"

#include "Distortion.h"

#include "Utilities.h"



#include <json.hpp>

using json = nlohmann::json;
@@ -330,8 +331,8 @@ protected: 
    std::vector<double> m_currentParameterCovariance;

    std::vector<csm::param::Type> m_parameterType;

    std::vector<double> m_noAdjustments;

    std::vector<double> m_odtX;

    std::vector<double> m_odtY;

    DistortionType m_distortionType;

    std::vector<double> m_opticalDistCoeffs;

    std::vector<double> m_transX;

    std::vector<double> m_transY;

    std::vector<double> m_spacecraftVelocity;

include/usgscsm/UsgsAstroLsSensorModel.h

+3 −1
Original line number Diff line number Diff line
@@ -30,6 +30,7 @@ 
#include <RasterGM.h>

#include <SettableEllipsoid.h>

#include <CorrelationModel.h>

#include "Distortion.h"



class UsgsAstroLsSensorModel : public csm::RasterGM, virtual public csm::SettableEllipsoid

{
@@ -78,7 +79,8 @@ public: 
   int          m_ikCode;

   double       m_focalLength;

   double       m_zDirection;

   double       m_opticalDistCoef[3];

   DistortionType m_distortionType;

   std::vector<double> m_opticalDistCoeffs;

   double       m_iTransS[3];

   double       m_iTransL[3];

   double       m_detectorSampleOrigin;

include/usgscsm/Utilities.h

+4 −2
Original line number Diff line number Diff line 
#ifndef Utilities_h

#define Utilities_h



#include "Distortion.h"



#include <vector>

#include <math.h>

#include <tuple>
@@ -75,8 +77,8 @@ double getMinHeight(nlohmann::json isd, csm::WarningList *list=nullptr); 
double getMaxHeight(nlohmann::json isd, csm::WarningList *list=nullptr);

double getSemiMajorRadius(nlohmann::json isd, csm::WarningList *list=nullptr);

double getSemiMinorRadius(nlohmann::json isd, csm::WarningList *list=nullptr);

std::vector<double> getTransverseDistortionX(nlohmann::json isd, csm::WarningList *list=nullptr);

std::vector<double> getTransverseDistortionY(nlohmann::json isd, csm::WarningList *list=nullptr);

DistortionType getDistortionModel(nlohmann::json isd, csm::WarningList *list=nullptr);

std::vector<double> getDistortionCoeffs(nlohmann::json isd, csm::WarningList *list=nullptr);

std::vector<double> getRadialDistortion(nlohmann::json isd, csm::WarningList *list=nullptr);

std::vector<double> getSunPositions(nlohmann::json isd, csm::WarningList *list=nullptr);

std::vector<double> getSensorPositions(nlohmann::json isd, csm::WarningList *list=nullptr);

src/Distortion.cpp

+156 −173
Original line number Diff line number Diff line 
#include "Distortion.h"



/**

 * @brief Compute undistorted focal plane x/y.

 *

 * Computes undistorted focal plane (x,y) coordinates given a distorted focal plane (x,y)

 * coordinate. The undistorted coordinates are solved for using the Newton-Raphson

 * method for root-finding if the distortionFunction method is invoked.

 *

 * @param dx distorted focal plane x in millimeters

 * @param dy distorted focal plane y in millimeters

 * @param undistortedX The undistorted x coordinate, in millimeters.

 * @param undistortedY The undistorted y coordinate, in millimeters.

 *

 * @return if the conversion was successful

 * @todo Review the tolerance and maximum iterations of the root-

 *       finding algorithm.

 * @todo Review the handling of non-convergence of the root-finding

 *       algorithm.

 * @todo Add error handling for near-zero determinant.

*/

std::tuple<double, double> removeDistortion(double dx, double dy,

                        const std::vector<double> &odtX, const std::vector<double> &odtY) {

  // Solve the distortion equation using the Newton-Raphson method.

  // Set the error tolerance to about one millionth of a NAC pixel.

  const double tol = 1.4E-5;



  // The maximum number of iterations of the Newton-Raphson method.

  const int maxTries = 60;



  double x;

  double y;

  std::tuple<double, double> undistortedPoint(dx, dy);

  std::tuple<double, double> distortedPoint;



  // Initial guess at the root

  x = dx;

  y = dy;



  distortedPoint = distortionFunction(x, y, odtX, odtY);



  for (int count = 1; ((fabs(std::get<0>(distortedPoint)) +fabs(std::get<1>(distortedPoint))) > tol) && (count < maxTries); count++) {



    distortedPoint = distortionFunction(x, y, odtX, odtY);



    // fx = dx - fx;

    // fy = dy - fy;

    distortedPoint = std::make_tuple(dx - std::get<0>(distortedPoint), dy - std::get<1>(distortedPoint));



    std::vector<std::vector<double>> jacobian;



    jacobian = distortionJacobian(x, y, odtX, odtY);



    // Jxx * Jyy - Jxy * Jyx

    double determinant = jacobian[0][0] * jacobian[1][1] - jacobian[0][1] * jacobian[1][0];

    if (fabs(determinant) < 1E-6) {

      undistortedPoint = std::make_tuple(x, y);

      //

      // Near-zero determinant. Add error handling here.

      //

      //-- Just break out and return with no convergence

      return undistortedPoint;

    }



    //x = x + (Jyy * fx - Jxy * fy)

    x = x + (jacobian[1][1] * std::get<0>(distortedPoint) - jacobian[0][1] * std::get<1>(distortedPoint)) / determinant;

    // y = y + (Jxx * fy - Jyx * fx)

    y = y + (jacobian[0][0] * std::get<1>(distortedPoint) - jacobian[1][0] * std::get<0>(distortedPoint)) / determinant;

  }



  if ( (fabs(std::get<0>(distortedPoint)) + fabs(std::get<1>(distortedPoint))) <= tol) {

    // The method converged to a root.

    undistortedPoint = std::make_tuple(x, y);

  }

  // Otherwise method did not converge to a root within the maximum

  // number of iterations. Return with no distortion.

  return undistortedPoint;

}



/**

 * @description Jacobian of the distortion function. The Jacobian was computed

 * algebraically from the function described in the distortionFunction
@@ -92,8 +15,8 @@ std::tuple<double, double> removeDistortion(double dx, double dy, 
                     [1][0]: yx, [1][1]: yy

 */



std::vector<std::vector<double>> distortionJacobian(double x, double y,

                        const std::vector<double> &odtX, const std::vector<double> &odtY) {

void distortionJacobian(double x, double y, double *jacobian,

                        const std::vector<double> opticalDistCoeffs) {



  double d_dx[10];

  d_dx[0] = 0;
@@ -118,21 +41,20 @@ std::vector<std::vector<double>> distortionJacobian(double x, double y, 
  d_dy[8] = 2 * x * y;

  d_dy[9] = 3 * y * y;



  std::vector<std::vector<double>> jacobian(2, std::vector<double>(2));

  jacobian[0] = 0; // xx

  jacobian[1] = 0; // xy

  jacobian[2] = 0; // yx

  jacobian[3] = 0; // yy



  jacobian[0][0] = 0;

  jacobian[0][1] = 0;

  jacobian[1][0] = 0;

  jacobian[1][1] = 0;

  int xPointer = 0;

  int yPointer = opticalDistCoeffs.size() / 2;



  for (int i = 0; i < 10; i++) {

    jacobian[0][0] = jacobian[0][0] + d_dx[i] * odtX[i];

    jacobian[0][1] = jacobian[0][1] + d_dy[i] * odtX[i];

    jacobian[1][0] = jacobian[1][0] + d_dx[i] * odtY[i];

    jacobian[1][1] = jacobian[1][1] + d_dy[i] * odtY[i];

  for (int i = 0; i < 10; xPointer++, yPointer++, i++) {

    jacobian[0] = jacobian[0] + d_dx[i] * opticalDistCoeffs[xPointer];

    jacobian[1] = jacobian[1] + d_dy[i] * opticalDistCoeffs[xPointer];

    jacobian[2] = jacobian[2] + d_dx[i] * opticalDistCoeffs[yPointer];

    jacobian[3] = jacobian[3] + d_dy[i] * opticalDistCoeffs[yPointer];

  }



  return jacobian;

}



/**
@@ -142,13 +64,12 @@ std::vector<std::vector<double>> distortionJacobian(double x, double y, 
 *

 * @param ux Undistored x

 * @param uy Undistored y

 * @param odtX opticalDistCoef In X

 * @param odtY opticalDistCoef In Y

 * @param opticalDistCoeffs For both X and Y coefficients

 *

 * @returns distortedPoint Newly adjusted focal plane coordinates as an x, y tuple

 */

std::tuple<double, double> distortionFunction(double ux, double uy,

  const std::vector<double> &odtX, const std::vector<double> &odtY) {

void computeTransverseDistortion(double ux, double uy, double &dx, double &dy,

                                 const std::vector<double> opticalDistCoeffs) {



  double f[10];

  f[0] = 1;
@@ -162,64 +83,120 @@ std::tuple<double, double> distortionFunction(double ux, double uy, 
  f[8] = ux * uy * uy;

  f[9] = uy * uy * uy;



  std::tuple<double, double> distortedPoint(0.0, 0.0);

  for (int i = 0; i < 10; i++) {

    distortedPoint = std::make_tuple(std::get<0>(distortedPoint) + f[i] * odtX[i],

                             std::get<1>(distortedPoint) + f[i] * odtY[i]);

  }

  int xPointer = 0;

  int yPointer = opticalDistCoeffs.size() / 2;



  return distortedPoint;

  dx = 0.0;

  dy = 0.0;



  for (int i = 0; i < 10; xPointer++, yPointer++, i++) {

    dx = dx + f[i] * opticalDistCoeffs[xPointer];

    dy = dy + f[i] * opticalDistCoeffs[yPointer];

  }

}



/**

 * @description Compute undistorted focal plane coordinate given a distorted

 * coordinate set and the distortion coefficients

 *

 * @param inFocalPlaneX Distorted x

 * @param inFocalPlaneY Distorted y

 * @param opticalDistCoef distortion coefficients

 *

 * @returns undistortedPoint Newly adjusted focal plane coordinates as an x, y tuple

 */

std::tuple<double, double> removeDistortion(double inFocalPlaneX, double inFocalPlaneY,

  const double opticalDistCoef[3], double tolerance) {

  double rr = inFocalPlaneX * inFocalPlaneX + inFocalPlaneY * inFocalPlaneY;

  std::tuple<double, double> undistortedPoint(inFocalPlaneX, inFocalPlaneY);

void removeDistortion(double dx, double dy, double &ux, double &uy,

                      const std::vector<double> opticalDistCoeffs,

                      DistortionType distortionType,

                      const double tolerance) {

  ux = dx;

  uy = dy;



  switch (distortionType) {

    // Compute undistorted focal plane coordinate given a distorted

    // coordinate set and the distortion coefficients

    case RADIAL: {

      double rr = dx * dx + dy * dy;



      if (rr > tolerance)

      {

    double dr = opticalDistCoef[0] + (rr * (opticalDistCoef[1] + rr * opticalDistCoef[2]));

    undistortedPoint = std::make_tuple(inFocalPlaneX * (1.0 - dr), inFocalPlaneY * (1.0 - dr));

        double dr = opticalDistCoeffs[0] + (rr * (opticalDistCoeffs[1] + rr * opticalDistCoeffs[2]));

        ux = dx * (1.0 - dr);

        uy = dy * (1.0 - dr);

      }

    }

    break;

    // Computes undistorted focal plane (x,y) coordinates given a distorted focal plane (x,y)

    // coordinate. The undistorted coordinates are solved for using the Newton-Raphson

    // method for root-finding if the distortionFunction method is invoked.

    case TRANSVERSE: {

      // Solve the distortion equation using the Newton-Raphson method.

      // Set the error tolerance to about one millionth of a NAC pixel.

      // The maximum number of iterations of the Newton-Raphson method.

      const int maxTries = 20;



      double x;

      double y;

      double fx;

      double fy;

      double jacobian[4];



      // Initial guess at the root

      x = dx;

      y = dy;



      computeTransverseDistortion(x, y, fx, fy, opticalDistCoeffs);



      for (int count = 1; ((fabs(fx) +fabs(fy)) > tolerance) && (count < maxTries); count++) {



        computeTransverseDistortion(x, y, fx, fy, opticalDistCoeffs);



        fx = dx - fx;

        fy = dy - fy;



  return undistortedPoint;

        distortionJacobian(x, y, jacobian, opticalDistCoeffs);



        // Jxx * Jyy - Jxy * Jyx

        double determinant = jacobian[0] * jacobian[3] - jacobian[1] * jacobian[2];

        if (fabs(determinant) < 1E-6) {

          ux = x;

          uy = y;

          //

          // Near-zero determinant. Add error handling here.

          //

          //-- Just break out and return with no convergence

          return;

        }



/**

 * @description Compute undistorted focal plane coordinate given a distorted

 * focal plane coordinate. This method works by iteratively adding distortion

 * until the new distorted point, r, undistorts to within a tolerance of the

 * original point, rp.

 *

 * @param inFocalPlaneX Distorted x

 * @param inFocalPlaneY Distorted y

 * @param opticalDistCoef Distortion coefficients

 * @param desiredPrecision Convergence precision

 * @param tolerance Tolerance of r^2

 *

 * @returns undistortedPoint Newly adjusted focal plane coordinates as an x, y tuple

 */

std::tuple<double, double> invertDistortion(double inFocalPlaneX, double inFocalPlaneY,

  const double opticalDistCoef[3], double desiredPrecision, double tolerance) {

  double rp2 = (inFocalPlaneX * inFocalPlaneX) +

               (inFocalPlaneY * inFocalPlaneY);

  std::tuple<double, double> undistortedPoint;

        x = x + (jacobian[3] * fx - jacobian[1] * fy) / determinant;

        y = y + (jacobian[0] * fy - jacobian[2] * fx) / determinant;

      }



      if ((fabs(fx) + fabs(fy)) <= tolerance) {

        // The method converged to a root.

        ux = x;

        uy = y;



        return;

      }

      // Otherwise method did not converge to a root within the maximum

      // number of iterations

    }

    break;

  }

}



void applyDistortion(double ux, double uy, double &dx, double &dy,

                     const std::vector<double> opticalDistCoeffs,

                     DistortionType distortionType,

                     const double desiredPrecision, const double tolerance)

{

  dx = ux;

  dy = uy;



  switch (distortionType) {

    // Compute undistorted focal plane coordinate given a distorted

    // focal plane coordinate. This case works by iteratively adding distortion

    // until the new distorted point, r, undistorts to within a tolerance of the

    // original point, rp.

    case RADIAL: {

      double rp2 = (ux * ux) + (uy * uy);



      if (rp2 > tolerance) {

        double rp = sqrt(rp2);

        // Compute first fractional distortion using rp

    double drOverR = opticalDistCoef[0]

                  + (rp2 * (opticalDistCoef[1] + (rp2 * opticalDistCoef[2])));

        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;
@@ -239,16 +216,22 @@ std::tuple<double, double> invertDistortion(double inFocalPlaneX, double inFocal 
          r2_prev = r * r;



          // Compute new fractional distortion:

      drOverR = opticalDistCoef[0]

             + (r2_prev * (opticalDistCoef[1] + (r2_prev * opticalDistCoef[2])));

          drOverR = opticalDistCoeffs[0]

                 + (r2_prev * (opticalDistCoeffs[1] + (r2_prev * opticalDistCoeffs[2])));



          // Compute new estimate of r

          r = rp + (drOverR * r_prev);

          iteration++;

        }

        while (fabs(r * (1 - drOverR) - rp) > desiredPrecision);

    undistortedPoint = std::make_tuple(inFocalPlaneX / (1.0 - drOverR),

                                       inFocalPlaneY / (1.0 - drOverR));

        dx = ux / (1.0 - drOverR);

        dy = uy / (1.0 - drOverR);

      }

    }

    break;

    case TRANSVERSE: {

      computeTransverseDistortion(ux, uy, dx, dy, opticalDistCoeffs);

    }

    break;

  }

  return undistortedPoint;

}