Affine.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
 
#include "Affine.h"
 
#include <vector>
#include <iostream>
#include <sstream>
#include <string>
 
#include <jama/jama_svd.h>
 
#include "Constants.h"
#include "IException.h"
#include "IString.h"
#include "LeastSquares.h"
#include "PolynomialBivariate.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);
 
    // Invert for inverse translation
    p_invmat = invert(p_matrix);
  }
 
  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 {
    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(IException::Programmer, mess.str(), _FILEINFO_);
    }
    return;
  }
 
  Affine::AMatrix Affine::invert(const AMatrix &a) const {
    // 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(IException::Unknown, 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