DawnFcDistortionMap.cpp

#include "DawnFcDistortionMap.h"
#include "CameraFocalPlaneMap.h"

using namespace std;

namespace Isis {
  DawnFcDistortionMap::DawnFcDistortionMap(Camera *parent, double k1, double zDirection) : CameraDistortionMap(parent, zDirection) {
    p_k1 = k1;
  }

  bool DawnFcDistortionMap::SetUndistortedFocalPlane(const double ux, const double uy) {
    double offsetSqrd;

    p_undistortedFocalPlaneX = ux;
    p_undistortedFocalPlaneY = uy;

    offsetSqrd = ux * ux + uy * uy;

    p_focalPlaneX = ux * (1.0 + p_k1 * offsetSqrd);
    p_focalPlaneY = uy * (1.0 + p_k1 * offsetSqrd);
    return true;
  }

  bool DawnFcDistortionMap::SetFocalPlane(const double dx, const double dy) {
    double offsetSqrd;
    int    numAttempts;
    double delta;
    bool    done;

    p_focalPlaneX = dx;
    p_focalPlaneY = dy;

    /****************************************************************************
    * Pre-loop intializations
    ****************************************************************************/

    numAttempts = 1;
    delta = 0.00001;

    offsetSqrd = dy * dy + dx * dx;
    double guess_dx, guess_dy;
    double guess_ux, guess_uy;

    /****************************************************************************
    * Loop ...
    ****************************************************************************/
    do {
      guess_ux = dx / (1.0 + p_k1 * offsetSqrd);
      guess_uy = dy / (1.0 + p_k1 * offsetSqrd);

      offsetSqrd = guess_uy * guess_uy + guess_ux * guess_ux;

      guess_dx = guess_ux * (1.0 + p_k1 * offsetSqrd);
      guess_dy = guess_uy * (1.0 + p_k1 * offsetSqrd);

      done = true;
      if(abs(guess_dy - dy) > delta) {
        done = false;
      }

      if(abs(guess_dx - dx) > delta) {
        done = false;
      }

      /* Not converging so bomb */
      numAttempts++;
      if(numAttempts > 20) {
        return false;
      }
    }
    while(!done);

    /****************************************************************************
    * Sucess ...
    ****************************************************************************/

    p_undistortedFocalPlaneX = guess_ux;
    p_undistortedFocalPlaneY = guess_uy;
    return true;
  }
}