CameraDistortionMap.cpp

Go to the documentation of this file.

#include "IString.h"
#include "CameraDistortionMap.h"

namespace Isis {
  CameraDistortionMap::CameraDistortionMap(Camera *parent, double zDirection) {
    p_camera = parent;
    p_camera->SetDistortionMap(this);
    p_zDirection = zDirection;
  }

  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 a 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;
  }

}