JunoDistortionMap.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
 
#include "IString.h"
#include "JunoDistortionMap.h"
 
namespace Isis {
  JunoDistortionMap::JunoDistortionMap(Camera *parent)
      : CameraDistortionMap(parent, 1.0) {
  }
 
 
  JunoDistortionMap::~JunoDistortionMap() {
  }
 
 
  void JunoDistortionMap::SetDistortion(int naifIkCode) {
 
    // Use the pixel pitch to scale k1 and k2 coefficients to operate in focal
    // plane coordinates (millimeters). The coefficients found in the kernels
    // are based on detector coordinates (pixels).
 
    double pp = p_camera->PixelPitch();
    double p2 = pp * pp;
 
    // Currently k0 is non-existant in kernels (i.e equals zero). The try is
    // here in case this coefficient is needed for future distortion models.
    try {
      QString odk0 = "INS" + toString(naifIkCode) + "_DISTORTION_K0";
      p_odk.push_back(p_camera->Spice::getDouble(odk0));
 
    }
    catch (IException &e) {
      p_odk.push_back(0.0);
    }
 
    QString odk1 = "INS" + toString(naifIkCode) + "_DISTORTION_K1";
    p_odk.push_back(p_camera->Spice::getDouble(odk1) / p2);
    QString odk2 = "INS" + toString(naifIkCode) + "_DISTORTION_K2";
    p_odk.push_back(p_camera->Spice::getDouble(odk2) / (p2 * p2));
  }
 
 
  bool JunoDistortionMap::SetUndistortedFocalPlane(const double ux,
                                                   const double uy) {
 
    p_undistortedFocalPlaneX = ux;
    p_undistortedFocalPlaneY = uy;
 
    // Compute the distance from the focal plane center and if we are
    // close to the center then assume no distortion
    double r2 = (ux * ux) + (uy * uy);
    if (r2 <= 1.0E-6) {
      p_focalPlaneX = ux;
      p_focalPlaneY = uy;
      return true;
    }
 
    // The equation given in the IK computes the undistorted focal plane
    // ux = dx * (1 + k1*r^2), r^2 = dx^2 + dy^2
    double dr = 1 + p_odk[0] + p_odk[1]*r2 + p_odk[2]*r2*r2;
    p_focalPlaneX = ux * dr;
    p_focalPlaneY = uy * dr;
 
    return true;
 
  }
 
 
  bool JunoDistortionMap::SetFocalPlane(double dx,
                                        double dy) {
    p_focalPlaneX = dx;
    p_focalPlaneY = dy;
 
    // Get the distance from the focal plane center and if we are close
    // then skip the distortion
    double r2 = (dx * dx) + (dy * dy);
    if (r2 <= 1.0E-6) {
      p_undistortedFocalPlaneX = dx;
      p_undistortedFocalPlaneY = dy;
      return true;
    }
 
    bool converged = false;
    int i = 0;
    int maximumIterations = 15;
    double tolerance = p_camera->PixelPitch() / 100.0;
    double uxEstimate = dx;
    double uyEstimate = dy;
    double uxPrev = dx;
    double uyPrev = dy;
    double xDistortion = 0.0;
    double yDistortion = 0.0;
    double dr = 0.0;
    while (!converged) {
      dr = p_odk[0] + p_odk[1]*r2 + p_odk[2]*r2*r2;
      xDistortion = uxEstimate * dr;
      yDistortion = uyEstimate * dr;
      uxEstimate = dx - xDistortion;
      uyEstimate = dy - yDistortion;
      i++;
      if (fabs(uxEstimate - uxPrev) < tolerance &&
          fabs(uyEstimate - uyPrev) < tolerance ) {
        converged = true;
      }
      // If doesn't converge, don't do correction
      if (i > maximumIterations) {
        p_undistortedFocalPlaneX = dx;
        p_undistortedFocalPlaneY = dy;
        break;
      }
      r2 = (uxEstimate * uxEstimate) + (uyEstimate * uyEstimate);
      uxPrev = uxEstimate;
      uyPrev = uyEstimate;
    }
    p_undistortedFocalPlaneX = uxEstimate;
    p_undistortedFocalPlaneY = uyEstimate;
    return true;
  }
 
}