LroWideAngleCameraDistortionMap.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
 
#include <cmath>
 
#include <sstream>
#include <iomanip>
#include <boost/foreach.hpp>
 
#include "IString.h"
#include "LroWideAngleCameraDistortionMap.h"
 
using namespace std;
 
namespace Isis {
  LroWideAngleCameraDistortionMap::LroWideAngleCameraDistortionMap(Camera *parent,
                                                                   int naifIkCode) :
                                                                   CameraDistortionMap(parent) {
    // Initialize distortion coefficients to 0 to avoid segfaults
    QString odkkey = "INS" + toString(naifIkCode) + "_OD_K";
    for (int i = 0; i < 3; ++i) {
      p_odk.push_back(0);
    }
  }
 
 
  void LroWideAngleCameraDistortionMap::addFilter(int naifIkCode) {
    QString odkkey = "INS" + QString::number(naifIkCode) + "_OD_K";
 
    std::vector<double> v_odk;
    for(int i = 0; i < 3; i++) {
      v_odk.push_back(p_camera->getDouble(odkkey, i));
    }
 
    m_odkFilters.push_back(v_odk);
  }
 
 
  void LroWideAngleCameraDistortionMap::setBand(int vband) {
    if ( (vband <= 0) || (vband > m_odkFilters.size()) ) {
      QString mess = "Invalid band (" + QString::number(vband) + " requested " +
                     " Must be <= " + QString::number(m_odkFilters.size());
      throw IException(IException::Programmer, mess, _FILEINFO_);
    }
 
    //  Install new parameters
    p_odk = m_odkFilters[vband-1];
 
    return;
 
  }
 
  bool LroWideAngleCameraDistortionMap::SetFocalPlane(const double dx, const double dy) {
    p_focalPlaneX = dx;
    p_focalPlaneY = dy;
 
    double dk1 = p_odk[0];
    double dk2 = p_odk[1];
    double dk3 = p_odk[2];
 
    double rr = dx * dx + dy * dy;
 
    double dr = 1.0 + dk1 * rr + dk2 * rr * rr + dk3 * rr * rr * rr;
 
    // This was in here when the model used to be dx/dr so that the model
    // would never divide by zero.
    if ( dr == 0.0 ) return false;
 
    // Compute the undistorted positions
    p_undistortedFocalPlaneX = dx * dr;
    p_undistortedFocalPlaneY = dy * dr;
 
    return true;
  }
 
  bool LroWideAngleCameraDistortionMap::SetUndistortedFocalPlane(const double ux, const double uy) {
    // image coordinates prior to introducing distortion
    p_undistortedFocalPlaneX = ux;
    p_undistortedFocalPlaneY = uy;
 
    double xt = ux;
    double yt = uy;
 
    double rr, dr;
    double xdistorted, ydistorted;
    double xprevious, yprevious;
 
    xprevious = 1000000.0;
    yprevious = 1000000.0;
    // Changed this line to 10^-6... it allows the outer pixels to be found
    // when mapping back to the sensor
    double tolerance = 1.0e-6;
 
    bool bConverged = false;
 
    double dk1 = p_odk[0];
    double dk2 = p_odk[1];
    double dk3 = p_odk[2];
 
    // 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++) {
      rr = xt * xt + yt * yt;
      // dr is the radial distortion contribution
      dr = 1.0 + dk1 * rr + dk2 * rr * rr + dk3 * rr * rr * rr;
 
      // introducing distortion
      xt = ux / dr;
      yt = uy / dr;
 
      // distorted 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;
  }
}