Isis 3.0 Object Programmers' Reference

Home

---

Affine.cpp

Go to the documentation of this file.

#include <vector>
#include <iostream>
#include <string>
#include "jama_svd.h"

#include "Affine.h"
#include "PolynomialBivariate.h"
#include "LeastSquares.h"
#include "iException.h"
#include "Constants.h"

using namespace std;

namespace Isis {
  Affine::Affine () {
    Identity();
    p_x = p_y = p_xp = p_yp = 0.0;
  }

  Affine::Affine(const Affine::AMatrix &a) {
    checkDims(a);
    p_matrix = a.copy();
    p_invmat = invert(p_matrix);
    p_x = p_y = p_xp = p_yp = 0.0;
  }

  Affine::~Affine() {}

  Affine::AMatrix Affine::getIdentity() {
    AMatrix ident(3,3, 0.0);
    for (int i = 0 ; i < ident.dim2() ; i++) {
      ident[i][i] = 1.0;
    }
    return (ident);
  }

  void Affine::Identity() {
    p_matrix = getIdentity();
    p_invmat = getIdentity();
  }

  void Affine::Solve (const double x[], const double y[], 
                      const double xp[], const double yp[], int n) {
    // We must solve two least squares equations
    PolynomialBivariate xpFunc(1);
    PolynomialBivariate ypFunc(1);
    LeastSquares xpLSQ(xpFunc);
    LeastSquares ypLSQ(ypFunc);

    // Push the knowns into the least squares class
    for (int i=0; i<n; i++) {
      vector<double> coord(2);
      coord[0] = x[i];
      coord[1] = y[i];
      xpLSQ.AddKnown(coord,xp[i]);
      ypLSQ.AddKnown(coord,yp[i]);
    }

    // Solve for A,B,C,D,E,F
    xpLSQ.Solve();
    ypLSQ.Solve();

    // Construct our affine matrix
    p_matrix[0][0] = xpFunc.Coefficient(1); // A
    p_matrix[0][1] = xpFunc.Coefficient(2); // B
    p_matrix[0][2] = xpFunc.Coefficient(0); // C
    p_matrix[1][0] = ypFunc.Coefficient(1); // D
    p_matrix[1][1] = ypFunc.Coefficient(2); // E
    p_matrix[1][2] = ypFunc.Coefficient(0); // F
    p_matrix[2][0] = 0.0;
    p_matrix[2][1] = 0.0;
    p_matrix[2][2] = 1.0;

    // invert the matrix
    p_invmat = invert(p_matrix);
  }

  void Affine::Translate (double tx, double ty) {
    AMatrix trans = getIdentity();

    trans[0][2] = tx;
    trans[1][2] = ty;
    p_matrix = TNT::matmult(trans,p_matrix);
    
    trans[0][2] = -tx;
    trans[1][2] = -ty;
    p_invmat = TNT::matmult(p_invmat,trans);
  }

  void Affine::Rotate(double angle) {
    AMatrix rot = getIdentity();

    double angleRadians = angle * Isis::PI / 180.0;
    rot[0][0] = cos(angleRadians);
    rot[0][1] = -sin(angleRadians);
    rot[1][0] = sin(angleRadians);
    rot[1][1] = cos(angleRadians);
    p_matrix = TNT::matmult(rot,p_matrix);

    angleRadians = -angleRadians;
    rot[0][0] = cos(angleRadians);
    rot[0][1] = -sin(angleRadians);
    rot[1][0] = sin(angleRadians);
    rot[1][1] = cos(angleRadians);
    p_invmat = TNT::matmult(p_invmat,rot);
  }

  void Affine::Scale (double scaleFactor) {
    AMatrix scale = getIdentity();
    scale[0][0] = scaleFactor;
    scale[1][1] = scaleFactor;
    p_matrix = TNT::matmult(scale,p_matrix);
    
    scale[0][0] = scaleFactor;
    scale[1][1] = scaleFactor;
    p_invmat = TNT::matmult(p_invmat,scale);
  }

  void Affine::Compute(double x, double y) {
    p_x = x;
    p_y = y;
    p_xp = p_matrix[0][0] * x + p_matrix[0][1] * y + p_matrix[0][2];
    p_yp = p_matrix[1][0] * x + p_matrix[1][1] * y + p_matrix[1][2];
  }

  void Affine::ComputeInverse(double xp, double yp) {
    p_xp = xp;
    p_yp = yp;
    p_x = p_invmat[0][0] * xp + p_invmat[0][1] * yp + p_invmat[0][2];
    p_y = p_invmat[1][0] * xp + p_invmat[1][1] * yp + p_invmat[1][2];
  }
   
  vector<double> Affine::Coefficients( int var ) {
    int index = var-1;
    vector <double> coef;
    coef.push_back(p_matrix[index][0]);
    coef.push_back(p_matrix[index][1]);
    coef.push_back(p_matrix[index][2]);
    return coef;
  }

  vector<double> Affine::InverseCoefficients( int var ) {
     int index = var-1;
     vector <double> coef;
     coef.push_back(p_invmat[index][0]);
     coef.push_back(p_invmat[index][1]);
     coef.push_back(p_invmat[index][2]);
     return coef;
   }

  void Affine::checkDims(const AMatrix &am) const throw (iException &) {
    if ((am.dim1() != 3) && (am.dim2() != 3)) {
      ostringstream mess;
      mess << "Affine matrices must be 3x3 - this one is " << am.dim1()
           << "x" << am.dim2();
      throw iException::Message(iException::Programmer, mess.str(), _FILEINFO_);
    }
    return;
  }

  Affine::AMatrix Affine::invert(const AMatrix &a) const throw (iException &) {
   // Now compute the inverse affine matrix using singular value
    // decomposition A = USV'.  So invA = V invS U'.  Where ' represents
    // the transpose of a matrix and invS is S with the recipricol of the
    // diagonal elements
    JAMA::SVD<double> svd(a);

    AMatrix V;
    svd.getV(V);

    // The inverse of S is 1 over each diagonal element of S
    AMatrix invS;
    svd.getS(invS);
    for (int i=0; i<invS.dim1(); i++) {
      if (invS[i][i] == 0.0) {
        string msg = "Affine transform not invertible";
        throw iException::Message(iException::Math,msg,_FILEINFO_);
      }
      invS[i][i] = 1.0 / invS[i][i];
    }

    // Transpose U
    AMatrix U;
    svd.getU(U);
    AMatrix transU(U.dim2(),U.dim1());
    for (int r=0; r<U.dim1(); r++) {
      for (int c=0; c<U.dim2(); c++) {
        transU[c][r] = U[r][c];
      }
    }

    // Multiply stuff together to get the inverse of the affine
    AMatrix VinvS = TNT::matmult(V,invS);
    return (TNT::matmult(VinvS,transU));
  }

} // end namespace isis