TaylorCameraDistortionMap.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
 
#include <cmath>
#include "IString.h"
#include "TaylorCameraDistortionMap.h"
 
using namespace std;
namespace Isis {
 
  TaylorCameraDistortionMap::TaylorCameraDistortionMap(Camera *parent, double zDirection):
    CameraDistortionMap(parent, zDirection) {
  }
 
  void TaylorCameraDistortionMap::SetDistortion(const int naifIkCode) {
    QString odtxkey = "INS" + toString(naifIkCode) + "_OD_T_X";
    QString odtykey = "INS" + toString(naifIkCode) + "_OD_T_Y";
    for(int i = 0; i < 10; ++i) {
      p_odtx.push_back(p_camera->getDouble(odtxkey, i));
      p_odty.push_back(p_camera->getDouble(odtykey, i));
    }
  }
 
  bool TaylorCameraDistortionMap::SetFocalPlane(const double dx, const double dy) {
    p_focalPlaneX = dx;
    p_focalPlaneY = dy;
 
    // No coefficients == no distortion
    if(p_odtx.size() <= 0 && p_odty.size() <= 0) {
      p_undistortedFocalPlaneX = dx;
      p_undistortedFocalPlaneY = dy;
      return true;
    }
 
    // Solve the distortion equation using the Newton-Raphson method.
    // Set the error tolerance to about one millionth of a NAC pixel.
    const double tol = 1.4E-5;
 
    // The maximum number of iterations of the Newton-Raphson method.
    const int maxTries = 20;
 
    double x;
    double y;
    double fx;
    double fy;
    double Jxx;
    double Jxy;
    double Jyx;
    double Jyy;
 
    // Initial guess at the root
    x = dx;
    y = dy;
 
    this->DistortionFunction(x, y, &fx, &fy);
 
    for(int count = 1; ((fabs(fx) + fabs(fy)) > tol) && (count < maxTries); count++) {
 
      this->DistortionFunction(x, y, &fx, &fy);
 
      fx = dx - fx;
      fy = dy - fy;
 
      this->DistortionFunctionJacobian(x, y, &Jxx, &Jxy, &Jyx, &Jyy);
 
      double determinant = Jxx * Jyy - Jxy * Jyx;
      if(determinant < 1E-6) {
        //
        // Near-zero determinant. Add error handling here.
        //
        //-- Just break out and return with no convergence
        break;
      }
 
      x = x + (Jyy * fx - Jxy * fy) / determinant;
      y = y + (Jxx * fy - Jyx * fx) / determinant;
    }
 
    if((fabs(fx) + fabs(fy)) <= tol) {
      // The method converged to a root.
      p_undistortedFocalPlaneX = x;
      p_undistortedFocalPlaneY = y;
    }
    else {
      // The method did not converge to a root within the maximum
      // number of iterations. Return with no distortion.
      p_undistortedFocalPlaneX = dx;
      p_undistortedFocalPlaneY = dy;
    }
 
    return true;
  }
 
  bool TaylorCameraDistortionMap::SetUndistortedFocalPlane(const double ux,
      const double uy) {
    p_undistortedFocalPlaneX = ux;
    p_undistortedFocalPlaneY = uy;
 
    // No coefficients == nodistortion
    if(p_odtx.size() <= 0 && p_odty.size() <= 0) {
      p_focalPlaneX = ux;
      p_focalPlaneY = uy;
      return true;
    }
 
    this->DistortionFunction(ux, uy, &p_focalPlaneX, &p_focalPlaneY);
 
    return true;
  }
 
  void TaylorCameraDistortionMap::DistortionFunction(double ux, double uy, double *dx, double *dy) {
 
    double f[10];
    f[0] = 1;
    f[1] = ux;
    f[2] = uy;
    f[3] = ux * ux;
    f[4] = ux * uy;
    f[5] = uy * uy;
    f[6] = ux * ux * ux;
    f[7] = ux * ux * uy;
    f[8] = ux * uy * uy;
    f[9] = uy * uy * uy;
 
    *dx = 0.0;
    *dy = 0.0;
 
    for(int i = 0; i < 10; i++) {
      *dx = *dx + f[i] * p_odtx[i];
      *dy = *dy + f[i] * p_odty[i];
    }
 
  }
 
  void TaylorCameraDistortionMap::DistortionFunctionJacobian(double x, double y, double *Jxx, double *Jxy, double *Jyx, double *Jyy) {
 
    double d_dx[10];
    d_dx[0] = 0;
    d_dx[1] = 1;
    d_dx[2] = 0;
    d_dx[3] = 2 * x;
    d_dx[4] = y;
    d_dx[5] = 0;
    d_dx[6] = 3 * x * x;
    d_dx[7] = 2 * x * y;
    d_dx[8] = y * y;
    d_dx[9] = 0;
    double d_dy[10];
    d_dy[0] = 0;
    d_dy[1] = 0;
    d_dy[2] = 1;
    d_dy[3] = 0;
    d_dy[4] = x;
    d_dy[5] = 2 * y;
    d_dy[6] = 0;
    d_dy[7] = x * x;
    d_dy[8] = 2 * x * y;
    d_dy[9] = 3 * y * y;
 
    *Jxx = 0.0;
    *Jxy = 0.0;
    *Jyx = 0.0;
    *Jyy = 0.0;
 
    for(int i = 0; i < 10; i++) {
      *Jxx = *Jxx + d_dx[i] * p_odtx[i];
      *Jxy = *Jxy + d_dy[i] * p_odtx[i];
      *Jyx = *Jyx + d_dx[i] * p_odty[i];
      *Jyy = *Jyy + d_dy[i] * p_odty[i];
    }
  }
 
}