KaguyaTcCameraDistortionMap.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
 
#include <QDebug>
#include <QtGlobal>
#include <QtMath>
 
#include "KaguyaTcCameraDistortionMap.h"
 
namespace Isis {
  KaguyaTcCameraDistortionMap::KaguyaTcCameraDistortionMap(Camera *parent, int naifIkCode)
      : CameraDistortionMap(parent) {
    QString odtxkey = "INS" + toString(naifIkCode) + "_DISTORTION_COEF_X";
    QString odtykey = "INS" + toString(naifIkCode) + "_DISTORTION_COEF_Y";
    QString boresightkey = "INS" + toString(naifIkCode) + "_BORESIGHT";
 
    for(int i = 0; i < 4; ++i) {
      p_odkx.push_back(p_camera->getDouble(odtxkey, i));
      p_odky.push_back(p_camera->getDouble(odtykey, i));
    }
 
    // add boresight x and y to coefficients vector
    p_odkx[0] = p_odkx[0] + p_camera->getDouble(boresightkey, 0);
    p_odky[0] = p_odky[0] + p_camera->getDouble(boresightkey, 1);
  }
 
 
 
  KaguyaTcCameraDistortionMap::~KaguyaTcCameraDistortionMap() {
  }
 
 
  bool KaguyaTcCameraDistortionMap::SetFocalPlane(double dx, double dy) {
    p_focalPlaneX = dx;
    p_focalPlaneY = dy;
 
    double x = dx;
    double y = dy;
 
    double r2 = x*x + y*y;
    double r = qSqrt(r2);
    double r3 = r2 * r;
 
    double dr_x = p_odkx[0] + p_odkx[1] * r + p_odkx[2] * r2 + p_odkx[3] * r3; //add boresight offset
    double dr_y = p_odky[0] + p_odky[1] * r + p_odky[2] * r2 + p_odky[3] * r3; //add boresight offset
 
    p_undistortedFocalPlaneX = x + dr_x;
    p_undistortedFocalPlaneY = y + dr_y;
 
    return true;
  }
 
 
  bool KaguyaTcCameraDistortionMap::SetUndistortedFocalPlane(const double ux,
      const double uy) {
 
    p_undistortedFocalPlaneX = ux;
    p_undistortedFocalPlaneY = uy;
 
    double xt = ux;
    double yt = uy;
 
    double xx, yy, r, rr, rrr, dr_x, dr_y;
    double xdistortion, ydistortion;
    double xdistorted, ydistorted;
    double xprevious, yprevious;
 
    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 below the given tolerance
    for (int i = 0; i < 50; i++) {
      xx = xt * xt;
      yy = yt * yt;
      rr = xx + yy;
      r = qSqrt(rr);
      rrr = rr * r;
 
      // Radial distortion
      // dr is the radial distortion contribution
      dr_x = p_odkx[0] + p_odkx[1] * r + p_odkx[2] * rr + p_odkx[3] * rrr; // why did hayabusa have a -1
      dr_y = p_odky[0] + p_odky[1] * r + p_odky[2] * rr + p_odky[3] * rrr; // why did hayabusa have a -1
 
      // Distortion at the current point location
      xdistortion = dr_x;
      ydistortion = dr_y;
 
      // updated image coordinates
      xt = ux - xdistortion;
      yt = uy - ydistortion;
 
      // distorted point corrected for principal point
      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_focalPlaneX = xdistorted;
      p_focalPlaneY = ydistorted;
    }
 
    return bConverged;
  }
}