RosettaOsirisCameraDistortionMap.cpp

Go to the documentation of this file.

#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 );
  }
}