RosettaOsirisCameraDistortionMap.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
 
 
#include "RosettaOsirisCameraDistortionMap.h"
 
using namespace std;
 
namespace Isis {
  RosettaOsirisCameraDistortionMap::RosettaOsirisCameraDistortionMap(Camera *parent) :
                                                                   CameraDistortionMap(parent) {
    // Initialize to the identity transform
    m_toUnDistortedX = LinearAlgebra::zeroMatrix(4, 4);
    m_toUnDistortedY = LinearAlgebra::zeroMatrix(4, 4);
    m_toUnDistortedX(0, 1) = 1.0;
    m_toUnDistortedY(1, 0) = 1.0;
 
    m_boresightSample = 0.0;
    m_boresightLine = 0.0;
    m_pixelPitch = 1.0;
  }
 
  bool RosettaOsirisCameraDistortionMap::SetFocalPlane(const double dx, const double dy) {
    p_focalPlaneX = dx;
    p_focalPlaneY = dy;
 
    // The equations are in pixel coordinates so convert
    double dxPixel = focalXToLine(dx);
    double dyPixel = focalYToSample(dy);
 
    LinearAlgebra::Vector xTerms = LinearAlgebra::zeroVector(4);
    xTerms(0) = 1.0;
    xTerms(1) = dxPixel;
    xTerms(2) = dxPixel*dxPixel;
    xTerms(3) = dxPixel*dxPixel*dxPixel;
 
    LinearAlgebra::Vector yTerms = LinearAlgebra::zeroVector(4);
    yTerms(0) = 1.0;
    yTerms(1) = dyPixel;
    yTerms(2) = dyPixel*dyPixel;
    yTerms(3) = dyPixel*dyPixel*dyPixel;
 
    double ux = LinearAlgebra::dotProduct(
                          LinearAlgebra::multiply(m_toUnDistortedX, xTerms),
                          yTerms);
    double uy = LinearAlgebra::dotProduct(
                          LinearAlgebra::multiply(m_toUnDistortedY, xTerms),
                          yTerms);
 
    p_undistortedFocalPlaneX = lineToFocalX(ux);
    p_undistortedFocalPlaneY = sampleToFocalY(uy);
 
    return true;
  }
 
  bool RosettaOsirisCameraDistortionMap::SetUndistortedFocalPlane(const double ux, const double uy) {
    // image coordinates prior to introducing distortion
    p_undistortedFocalPlaneX = ux;
    p_undistortedFocalPlaneY = uy;
 
    // The equations are in pixel coordinates so convert
    double uxPixel = focalXToLine(ux);
    double uyPixel = focalYToSample(uy);
 
    // Loop setup
    double tolerance = 1e-7;
    int iteration = 1;
    int maxIterations = 20;
    bool done = false;
 
    LinearAlgebra::Vector undistortedCoordinate = LinearAlgebra::zeroVector(2);
    undistortedCoordinate(0) = uxPixel;
    undistortedCoordinate(1) = uyPixel;
    // Use the undistorted coordinate as the initial point
    LinearAlgebra::Vector distortedCoordinate = undistortedCoordinate;
 
    LinearAlgebra::Vector xTerms = LinearAlgebra::zeroVector(4);
    LinearAlgebra::Vector yTerms = LinearAlgebra::zeroVector(4);
    LinearAlgebra::Vector delXTerms = LinearAlgebra::zeroVector(4);
    LinearAlgebra::Vector delYTerms = LinearAlgebra::zeroVector(4);
 
    LinearAlgebra::Vector objectFuncValue = LinearAlgebra::zeroVector(2);
    LinearAlgebra::Vector updateStep = LinearAlgebra::zeroVector(2);
    LinearAlgebra::Matrix negJacobian = LinearAlgebra::zeroMatrix(2, 2);
    LinearAlgebra::Matrix negInvJacobian = LinearAlgebra::zeroMatrix(2, 2);
 
    // Compute the term vectors
    //   These are recomputed at the END of each iteration, so compute them
    //   before the first iteration
    xTerms(0) = 1.0;
    xTerms(1) = distortedCoordinate(0);
    xTerms(2) = distortedCoordinate(0)*distortedCoordinate(0);
    xTerms(3) = distortedCoordinate(0)*distortedCoordinate(0)*distortedCoordinate(0);
 
    yTerms(0) = 1.0;
    yTerms(1) = distortedCoordinate(1);
    yTerms(2) = distortedCoordinate(1)*distortedCoordinate(1);
    yTerms(3) = distortedCoordinate(1)*distortedCoordinate(1)*distortedCoordinate(1);
 
    delXTerms(1) = 1.0;
    delXTerms(2) = 2.0*distortedCoordinate(0);
    delXTerms(3) = 3.0*distortedCoordinate(0)*distortedCoordinate(0);
 
    delYTerms(1) = 1.0;
    delYTerms(2) = 2.0*distortedCoordinate(1);
    delYTerms(3) = 3.0*distortedCoordinate(1)*distortedCoordinate(1);
 
    // Compute the object function, the distance between the redistorted and undistorted
    objectFuncValue(0) = undistortedCoordinate(0)
                          - LinearAlgebra::dotProduct( LinearAlgebra::multiply(m_toUnDistortedX, xTerms),
                                                      yTerms );
    objectFuncValue(1) = undistortedCoordinate(1)
                          - LinearAlgebra::dotProduct( LinearAlgebra::multiply(m_toUnDistortedY, xTerms),
                                                      yTerms );
 
    while(!done) {
 
      // Compute the negative jacobian
      //   The variable terms and object function value have already been
      //   computed priort to checking for convergence at the end of the
      //   previous iteration. These are not needed to check for convergence
      //   so, compute them now.
      negJacobian(0, 0) = LinearAlgebra::dotProduct( LinearAlgebra::multiply(m_toUnDistortedX, delXTerms),
                                                     yTerms );
      negJacobian(0, 1) = LinearAlgebra::dotProduct( LinearAlgebra::multiply(m_toUnDistortedX, xTerms),
                                                     delYTerms );
      negJacobian(1, 0) = LinearAlgebra::dotProduct( LinearAlgebra::multiply(m_toUnDistortedY, delXTerms),
                                                     yTerms );
      negJacobian(1, 1) = LinearAlgebra::dotProduct( LinearAlgebra::multiply(m_toUnDistortedY, xTerms),
                                                     delYTerms );
 
      // Invert the negative jacobian
      //   If it is not invertible, then fail
      double det = negJacobian(0, 0) * negJacobian(1, 1) - negJacobian(1, 0) * negJacobian(0, 1);
      if ( fabs(det) < 1e-15 ) {
        return false;
      }
      negInvJacobian(0, 0) = negJacobian(1, 1) / det;
      negInvJacobian(0, 1) = - negJacobian(0, 1) / det;
      negInvJacobian(1, 0) = - negJacobian(1, 0) / det;
      negInvJacobian(1, 1) = negJacobian(0, 0) / det;
 
      // Compute the update step
      updateStep = LinearAlgebra::multiply(negInvJacobian, objectFuncValue);
 
      // Update and check for convergence
      distortedCoordinate += updateStep;
 
      // Compute the term vectors
      xTerms(0) = 1.0;
      xTerms(1) = distortedCoordinate(0);
      xTerms(2) = distortedCoordinate(0)*distortedCoordinate(0);
      xTerms(3) = distortedCoordinate(0)*distortedCoordinate(0)*distortedCoordinate(0);
 
      yTerms(0) = 1.0;
      yTerms(1) = distortedCoordinate(1);
      yTerms(2) = distortedCoordinate(1)*distortedCoordinate(1);
      yTerms(3) = distortedCoordinate(1)*distortedCoordinate(1)*distortedCoordinate(1);
 
      delXTerms(1) = 1.0;
      delXTerms(2) = 2.0*distortedCoordinate(0);
      delXTerms(3) = 3.0*distortedCoordinate(0)*distortedCoordinate(0);
 
      delYTerms(1) = 1.0;
      delYTerms(2) = 2.0*distortedCoordinate(1);
      delYTerms(3) = 3.0*distortedCoordinate(1)*distortedCoordinate(1);
 
      // Compute the object function, the distance between the redistorted and undistorted
      objectFuncValue(0) = undistortedCoordinate(0)
                           - LinearAlgebra::dotProduct( LinearAlgebra::multiply(m_toUnDistortedX, xTerms),
                                                        yTerms );
      objectFuncValue(1) = undistortedCoordinate(1)
                           - LinearAlgebra::dotProduct( LinearAlgebra::multiply(m_toUnDistortedY, xTerms),
                                                        yTerms );
 
      if ( LinearAlgebra::magnitude(objectFuncValue) < tolerance ||
           iteration > maxIterations ) {
        done = true;
      }
      iteration++;
    }
 
    p_focalPlaneX = lineToFocalX( distortedCoordinate(0) );
    p_focalPlaneY = sampleToFocalY( distortedCoordinate(1) );
 
    return true;
  }
 
 
  void RosettaOsirisCameraDistortionMap::setUnDistortedXMatrix(LinearAlgebra::Matrix xMat) {
    m_toUnDistortedX = xMat;
  }
 
 
  void RosettaOsirisCameraDistortionMap::setUnDistortedYMatrix(LinearAlgebra::Matrix yMat) {
    m_toUnDistortedY = yMat;
  }
 
 
  void RosettaOsirisCameraDistortionMap::setBoresight(double sample, double line) {
    m_boresightSample = sample;
    m_boresightLine = line;
  }
 
 
  void RosettaOsirisCameraDistortionMap::setPixelPitch(double pitch) {
    m_pixelPitch = pitch;
  }
 
 
  double RosettaOsirisCameraDistortionMap::focalXToLine(double x) {
    return ( x / m_pixelPitch + m_boresightLine );
  }
 
 
  double RosettaOsirisCameraDistortionMap::focalYToSample(double y) {
    return ( y / m_pixelPitch + m_boresightSample);
  }
 
 
  double RosettaOsirisCameraDistortionMap::lineToFocalX(double line) {
    return ( ( line - m_boresightLine ) * m_pixelPitch );
  }
 
 
  double RosettaOsirisCameraDistortionMap::sampleToFocalY(double sample) {
    return ( ( sample - m_boresightSample) * m_pixelPitch );
  }
}