Hyb2OncDistortionMap.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
 
#include <QDebug>
#include <QtGlobal>
#include <QtMath>
 
#include "Hyb2OncDistortionMap.h"
 
namespace Isis {
  Hyb2OncDistortionMap::Hyb2OncDistortionMap(Camera *parent, double zDirection)
      : CameraDistortionMap(parent, zDirection) {
  }
 
 
  Hyb2OncDistortionMap::~Hyb2OncDistortionMap() {
  }
 
 
  bool Hyb2OncDistortionMap::SetFocalPlane(double dx, double dy) {
    p_focalPlaneX = dx;
    p_focalPlaneY = dy;
 
    // reducing to principal point offset (xp,yp)
    double x = dx;// - p_xp;
    double y = dy;// - p_yp;
    //
    // Get the distance from the focal plane center and if we are close
    // then skip the distortion (this prevents division by zero)
    double r = (x * x) + (y * y);
    double r2 = r*r;
    double r4 = r2*r2;
    if (r <= 1.0E-6) {
      p_undistortedFocalPlaneX = dx;
      p_undistortedFocalPlaneY = dy;
      return true;
    }
 
    // apply distortion correction
    // r = x^2 + y^2
    // rprime = L0*r + L1*r^3 + L2*r^5, where Li are distortion coeffs
    // "dr" is rprime divided by r, used to reduce operations
    double dr = p_odk[0] + p_odk[1] * r2 + p_odk[2] * r4;
    p_undistortedFocalPlaneX = dr * x;
    p_undistortedFocalPlaneY = dr * y;
    return true;
  }
 
 
  bool Hyb2OncDistortionMap::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 no distortion is required
 
    bool converged = false;
    int iteration = 0;
    double tolMilliMeters = p_camera->PixelPitch() / 100.0;
    double x = ux;
    double y = uy;
    double r = (x * x) + (y * y);
    if (r <= 1.0E-6) {
      p_focalPlaneX = ux;
      p_focalPlaneY = uy;
      return true;
    }
    double rPrevious = r;
 
    while (!converged && qAbs(r - rPrevious) > tolMilliMeters) {
      double r2 = r*r;
      double r4 = r2*r2;
      double dr = p_odk[0] + p_odk[1] * r2 + p_odk[2] * r4;
      rPrevious = r;
      x = dr * x;
      y = dr * y;
      r = x*x + y*y;
 
      iteration++;
      if (iteration > 50) {
        converged = false;
        break;
      }
    }
 
    converged = true;
    p_focalPlaneX = x;
    p_focalPlaneY = y;
 
    return converged;
  }
 
}