CameraDistortionMap.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
#include "IString.h"
#include "CameraDistortionMap.h"
 
namespace Isis {
  CameraDistortionMap::CameraDistortionMap(Camera *parent, double zDirection) {
    p_camera = parent;
    p_camera->SetDistortionMap(this);
    p_zDirection = zDirection;
  }
 
 
  CameraDistortionMap::~CameraDistortionMap() {
  }
 
 
  void CameraDistortionMap::SetDistortion(int naifIkCode) {
    QString odkkey = "INS" + toString(naifIkCode) + "_OD_K";
    for (int i = 0; i < 3; ++i) {
      p_odk.push_back(p_camera->Spice::getDouble(odkkey, i));
    }
  }
 
 
  bool CameraDistortionMap::SetFocalPlane(double dx, double dy) {
    p_focalPlaneX = dx;
    p_focalPlaneY = dy;
 
    // No coefficients == no distortion
    if (p_odk.size() <= 0) {
      p_undistortedFocalPlaneX = dx;
      p_undistortedFocalPlaneY = dy;
      return true;
    }
 
    // 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;
    }
 
    // Ok we need to apply distortion correction
    double drOverR = p_odk[0] + (r2 * (p_odk[1] + (r2 * p_odk[2])));
    p_undistortedFocalPlaneX = dx - (drOverR * dx);
    p_undistortedFocalPlaneY = dy - (drOverR * dy);
    return true;
  }
 
 
  bool CameraDistortionMap::SetUndistortedFocalPlane(const double ux,
      const double uy) {
    p_undistortedFocalPlaneX = ux;
    p_undistortedFocalPlaneY = uy;
 
    // No coefficients == nodistortion
    if (p_odk.size() <= 0) {
      p_focalPlaneX = ux;
      p_focalPlaneY = uy;
      return true;
    }
 
    // Compute the distance from the focal plane center and if we are
    // close to the center then no distortion is required
    double rp2 = (ux * ux) + (uy * uy);
    if (rp2 <= 1.0E-6) {
      p_focalPlaneX = ux;
      p_focalPlaneY = uy;
      return true;
    }
 
    // Ok make the correction, start by computing
    // fractional distortion at rp (r-prime)
    double rp = sqrt(rp2);
    double drOverR = p_odk[0] + (rp2 * (p_odk[1] + (rp2 * p_odk[2])));
 
    // Estimate r
    double r = rp + (drOverR * rp);
    double r_prev, r2_prev;
    double tolMilliMeters = p_camera->PixelPitch() / 100.0;
    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 >= 15 || r > 1E9) {
        drOverR = 0.0;
        break;
      }
 
      r_prev = r;
      r2_prev = r * r;
 
      // Compute new fractional distortion:
      drOverR = p_odk[0] + (r2_prev * (p_odk[1] + (r2_prev * p_odk[2])));
 
      r = rp + (drOverR * r_prev);  // Compute new estimate of r
      iteration++;
    }
    while (fabs(r - r_prev) > tolMilliMeters);
 
    p_focalPlaneX = ux / (1.0 - drOverR);
    p_focalPlaneY = uy / (1.0 - drOverR);
    return true;
  }
 
 
  std::vector<double> CameraDistortionMap::OpticalDistortionCoefficients() const {
    return p_odk;
  }
 
 
  double CameraDistortionMap::ZDirection() const {
    return p_zDirection;
  }
 
 
  double CameraDistortionMap::FocalPlaneX() const {
    return p_focalPlaneX;
  }
 
 
  double CameraDistortionMap::FocalPlaneY() const {
    return p_focalPlaneY;
  }
 
 
  double CameraDistortionMap::UndistortedFocalPlaneX() const {
    return p_undistortedFocalPlaneX;
  }
 
 
  double CameraDistortionMap::UndistortedFocalPlaneY() const {
    return p_undistortedFocalPlaneY;
  }
 
 
  double CameraDistortionMap::UndistortedFocalPlaneZ() const {
    return p_zDirection *p_camera->FocalLength();
  }
 
}