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

New cahvor distortion model

parent 7728eebb

include/usgscsm/Distortion.h

+1 −1
Original line number Diff line number Diff line
@@ -6,7 +6,7 @@ 
#include <tuple>

#include <vector>



enum DistortionType { RADIAL, TRANSVERSE, KAGUYALISM, DAWNFC, LROLROCNAC };

enum DistortionType { RADIAL, TRANSVERSE, KAGUYALISM, DAWNFC, LROLROCNAC, CAHVOR };



// Transverse Distortion

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

src/Distortion.cpp

+81 −0
Original line number Diff line number Diff line
@@ -297,6 +297,28 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, 


      return;

    } break;



    // Compute undistorted focal plane coordinate given a distorted

    // coordinate set and the distortion coefficients along with an

    // x, and y offset as the fourth and fifth element

    case CAHVOR:

    {

      double shiftedDx = dx - opticalDistCoeffs[3];

      double shiftedDy = dy - opticalDistCoeffs[4];

      double rr = shiftedDx * shiftedDx + shiftedDy * shiftedDy;



      if (rr > tolerance)

      {

        double dr = opticalDistCoeffs[0] +

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



        ux = shiftedDx * (1.0 - dr);

        uy = shiftedDy * (1.0 - dr);

        ux += opticalDistCoeffs[3];

        uy += opticalDistCoeffs[4];

      }

    }

    break;

  }

}
@@ -519,5 +541,64 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, 


      return;

    } break;



    // 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. Also applies an initial offset with an

    // x, and y offset as the fourth and fifth element

    // This is untested manually

    case CAHVOR:

    {

      double shiftedUx = ux - opticalDistCoeffs[3];

      double shiftedUy = uy - opticalDistCoeffs[4];

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



      if (rp2 > tolerance)

      {

        double rp = sqrt(rp2);

        // 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

          // of the focal plane.  Just set the distorted position to the

          // undistorted position. Also, make sure the focal plane is less

          // than 1km, it is unreasonable for it to grow larger than that.

          if (iteration >= 20 || r > 1E9)

          {

            drOverR = 0.0;

            break;

          }



          r_prev = r;

          r2_prev = r * r;



          // Compute new fractional distortion:

          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 - r_prev) > desiredPrecision);



        dx = ux / (1.0 - drOverR);

        dy = uy / (1.0 - drOverR);

        dx += opticalDistCoeffs[3];

        dy += opticalDistCoeffs[4];

      }

    }

    break;

  }

}

src/UsgsAstroFrameSensorModel.cpp

+18 −2
Original line number Diff line number Diff line
@@ -321,7 +321,7 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToProximateImagingLocus( 
    csm::WarningList *warnings) const {

  MESSAGE_LOG(

      spdlog::level::info,

      "Computing imageToProximateImagingLocus(No ground) for point {}, {}, {}, "

      "Computing imageToProximateImagingLocus(No ground) for point {}, {}, "

      "with desired precision {}",

      imagePt.line, imagePt.samp, desiredPrecision);
@@ -362,7 +362,7 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus( 
    double *achievedPrecision, csm::WarningList *warnings) const {

  MESSAGE_LOG(

      spdlog::level::info,

      "Computing imageToProximateImagingLocus for {}, {}, {}, with desired "

      "Computing imageToRemoteImagingLocus for {}, {} with desired "

      "precision {}",

      imagePt.line, imagePt.samp, desiredPrecision);

  // Find the line,sample on the focal plane (mm)
@@ -373,15 +373,30 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus( 
      m_startingDetectorLine, &m_iTransS[0], &m_iTransL[0], focalPlaneX,

      focalPlaneY);



  MESSAGE_LOG(

      spdlog::level::trace,

      "Found focalPlaneX: {}, and focalPlaneY: {}",

      focalPlaneX, focalPlaneY);

      

  // Distort

  double undistortedFocalPlaneX, undistortedFocalPlaneY;

  removeDistortion(focalPlaneX, focalPlaneY, undistortedFocalPlaneX,

                   undistortedFocalPlaneY, m_opticalDistCoeffs,

                   m_distortionType);



  MESSAGE_LOG(

      spdlog::level::trace,

      "Found undistortedFocalPlaneX: {}, and undistortedFocalPlaneY: {}",

      undistortedFocalPlaneX, undistortedFocalPlaneY);



  // Get rotation matrix and transform to a body-fixed frame

  double m[3][3];

  calcRotationMatrix(m);

  MESSAGE_LOG(

      spdlog::level::trace,

      "Calculated rotation matrix [{}, {}, {}], [{}, {}, {}], [{}, {}, {}]",

      m[0][0], m[0][1], m[0][2], m[1][0], m[1][1], m[1][2], m[2][0], m[2][1],

      m[2][2]);

  std::vector<double> lookC{undistortedFocalPlaneX, undistortedFocalPlaneY,

                            m_focalLength};

  std::vector<double> lookB{
@@ -1250,6 +1265,7 @@ std::string UsgsAstroFrameSensorModel::constructStateFromIsd( 
  // We don't pass the pixel to focal plane transformation so invert the

  // focal plane to pixel transformation

  try {

    // "focal2pixel_lines":[0,136.4988453771169,0],"focal2pixel_samples":[136.4988453771169,0,0]

    double determinant = state["m_iTransL"][1].get<double>() *

                             state["m_iTransS"][2].get<double>() -

                         state["m_iTransL"][2].get<double>() *

src/Utilities.cpp

+20 −0
Original line number Diff line number Diff line
@@ -1053,6 +1053,8 @@ DistortionType getDistortionModel(int aleDistortionModel, 
      return DistortionType::DAWNFC;

    } else if (aleDistortionType == ale::DistortionType::LROLROCNAC) {

      return DistortionType::LROLROCNAC;

    }else if (aleDistortionType == ale::DistortionType::CAHVOR) {

      return DistortionType::CAHVOR;

    }

  } catch (...) {

    if (list) {
@@ -1193,6 +1195,24 @@ std::vector<double> getDistortionCoeffs(json isd, csm::WarningList *list) { 
        coefficients = std::vector<double>(1, 0.0);

      }

    } break;

    case DistortionType::CAHVOR: {

      try {

        coefficients = isd.at("optical_distortion")

                           .at("cahvor")

                           .at("coefficients")

                           .get<std::vector<double>>();



        return coefficients;

      } catch (...) {

        if (list) {

          list->push_back(csm::Warning(

              csm::Warning::DATA_NOT_AVAILABLE,

              "Could not parse the radial distortion model coefficients.",

              "Utilities::getDistortion()"));

        }

        coefficients = std::vector<double>(6, 0.0);

      }

    } break;

  }

  if (list) {

    list->push_back(