OsirisRexDistortionMap.cpp

Go to the documentation of this file.

#include <QDebug>
#include <QtGlobal>
#include <QtMath>

#include "NaifStatus.h"
#include "OsirisRexDistortionMap.h"
#include "CameraFocalPlaneMap.h"

namespace Isis {
  OsirisRexDistortionMap::OsirisRexDistortionMap(Camera *parent, double zDirection) 
      : CameraDistortionMap(parent, zDirection) {

    m_detectorOriginSample = p_camera->FocalPlaneMap()->DetectorSampleOrigin();
    m_detectorOriginLine = p_camera->FocalPlaneMap()->DetectorLineOrigin();
    m_pixelPitch = p_camera->PixelPitch();
  }


  OsirisRexDistortionMap::~OsirisRexDistortionMap() {
   }


  void OsirisRexDistortionMap::SetDistortion(int naifIkCode, QString filter="UNKNOWN") {
    // Load distortion coefficients, including filter if we have it.
    QString odkkey;

    if (filter.toUpper().compare("UNKNOWN") == 0) {
      odkkey = "INS" + toString(naifIkCode) + "_OD_K";
    }
    else {
      odkkey = "INS" + toString(naifIkCode) + "_OD_K_" + filter.trimmed().toUpper();
    }

    try {
      for (int i = 0; i < 5; ++i) {
         p_odk.push_back(p_camera->Spice::getDouble(odkkey, i));
      }
    } 
    catch (IException &e) {
      // This means that this is an older image without a filter provided
      // don't update p_odk, we will not apply the distortion in this case 
      m_distortionOriginSample = -1;
      m_distortionOriginLine = -1;
      return;
    }

    // Load center-of-distortion coordinates, including filter if we have it
    QString odcenterkey; 
    if (filter.toUpper().compare("UNKNOWN") == 0) {
      odcenterkey = "INS" + toString(naifIkCode) + "_OD_CENTER";
    }
    else {
      odcenterkey = "INS" + toString(naifIkCode) + "_OD_CENTER_" + filter.trimmed().toUpper(); 
    }
    m_distortionOriginSample = p_camera->Spice::getDouble(odcenterkey, 0);
    m_distortionOriginLine =   p_camera->Spice::getDouble(odcenterkey, 1);
  }


  bool OsirisRexDistortionMap::SetFocalPlane(double dx, double dy) {
    p_focalPlaneX = dx; 
    p_focalPlaneY = dy;

    // Only apply the distortion if we have the correct number of coefficients
    if (p_odk.size() < 2) {
      p_undistortedFocalPlaneX = dx;
      p_undistortedFocalPlaneY = dy;
      return true; 
    }

    double x0 = (m_distortionOriginLine - m_detectorOriginSample) * m_pixelPitch; 
    double y0 = (m_distortionOriginSample - m_detectorOriginLine) * m_pixelPitch;

    double xt = dx;
    double yt = dy;

    double xx, yy, r, rr, rrr, rrrr;
    double xdistortion, ydistortion;
    double xdistorted, ydistorted;
    double xprevious, yprevious;
    double drOverR;

    xprevious = 1000000.0;
    yprevious = 1000000.0;

    double tolerance = 0.000001;

    bool bConverged = false;

    // Iterating to introduce distortion...
    // We stop when the difference between distorted coordinates
    // in successive iterations is at or below the given tolerance.
    for(int i = 0; i < 50; i++) {
      xx = (xt-x0) * (xt-x0);
      yy = (yt-y0) * (yt-y0);
      rr = xx + yy;
      r = qSqrt(rr); 
      rrr = rr * r;
      rrrr = rr * rr;

      drOverR = p_odk[0] + p_odk[1]*r + p_odk[2]*rr + p_odk[3]*rrr + p_odk[4]*rrrr;

      // distortion at the current point location
      xdistortion = drOverR * (xt-x0);
      ydistortion = drOverR * (yt-y0);

      // updated image coordinates
      xt = dx - xdistortion;
      yt = dy - ydistortion;

      xdistorted = xt;
      ydistorted = yt;

      // check for convergence
      if((fabs(xt - xprevious) <= tolerance) && (fabs(yt - yprevious) <= tolerance)) {
        bConverged = true;
        break;
      }

      xprevious = xt;
      yprevious = yt;
    }

    if(bConverged) {
      p_undistortedFocalPlaneX = xdistorted;
      p_undistortedFocalPlaneY = ydistorted;
    }
    return bConverged;
  }


  bool OsirisRexDistortionMap::SetUndistortedFocalPlane(const double ux,
      const double uy) {
    p_undistortedFocalPlaneX = ux;
    p_undistortedFocalPlaneY = uy;

    // Only apply the distortion if we have the correct number of coefficients.
    if (p_odk.size() < 2) {
      p_focalPlaneX = ux;
      p_focalPlaneY = uy;
      return true; 
    }
    double x0 = (m_distortionOriginLine - m_detectorOriginSample) * m_pixelPitch;
    double y0 = (m_distortionOriginSample - m_detectorOriginLine) * m_pixelPitch;

    // Compute the distance from the focal plane center. If we are
    // close to the center then no distortion is required
    double x = ux;
    double y = uy;
    double r = qSqrt(((x-x0) * (x-x0)) + ((y - y0) * (y-y0)));

    if (r <= 1.0E-6) {
      p_focalPlaneX = ux;
      p_focalPlaneY = uy;
      return true;
    }

    double r2 = r*r;
    double r3 = r2*r;
    double r4 = r2*r2;

    double drOverR = p_odk[0] + p_odk[1]*r + p_odk[2]*r2 + p_odk[3]*r3 + p_odk[4]*r4;

    p_focalPlaneX = x + drOverR * (x-x0);
    p_focalPlaneY = y + drOverR * (y-y0); 
    return true;
  }
}