LeastSquares.cpp

Go to the documentation of this file.

#include "jama/jama_svd.h"
#include "jama/jama_qr.h"

#ifndef __sun__
#include "gmm/gmm_superlu_interface.h"
#endif

#include "LeastSquares.h"
#include "IException.h"
#include "IString.h"

namespace Isis {
  LeastSquares::LeastSquares(Isis::BasisFunction &basis, bool sparse,
                             int sparseRows, int sparseCols, bool jigsaw) {
    p_jigsaw = jigsaw;
    p_basis = &basis;
    p_solved = false;
    p_sparse = sparse;
    p_sigma0 = 0.;

#if defined(__sun__)
    p_sparse = false;
#endif

    if (p_sparse) {

      //  make sure sparse nrows/ncols have been set
      if (sparseRows == 0  ||  sparseCols == 0) {
        QString msg = "If solving using sparse matrices, you must enter the "
                      "number of rows/columns";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }

#ifndef __sun__
      gmm::resize(p_sparseA, sparseRows, sparseCols);
      gmm::resize(p_normals, sparseCols, sparseCols);
      gmm::resize(p_ATb, sparseCols, 1);
      p_xSparse.resize(sparseCols);

      if( p_jigsaw ) {
        p_epsilonsSparse.resize(sparseCols);
        std::fill_n(p_epsilonsSparse.begin(), sparseCols, 0.0);

        p_parameterWeights.resize(sparseCols);
      }

#endif
      p_sparseRows = sparseRows;
      p_sparseCols = sparseCols;
    }
    p_currentFillRow = -1;
  }

  LeastSquares::~LeastSquares() {
  }

  void LeastSquares::AddKnown(const std::vector<double> &data, double result,
                              double weight) {
    if((int) data.size() != p_basis->Variables()) {
      QString msg = "Number of elements in data does not match basis [" +
                        p_basis->Name() + "] requirements";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    p_expected.push_back(result);

    if (weight == 1) {
      p_sqrtWeight.push_back(weight);
    }
    else {
      p_sqrtWeight.push_back(sqrt(weight));
    }

    if(p_sparse) {
#ifndef __sun__
      FillSparseA(data);
#endif
    }
    else {
      p_input.push_back(data);
    }
  }



#ifndef __sun__
  void LeastSquares::FillSparseA(const std::vector<double> &data) {

    p_basis->Expand(data);

    p_currentFillRow++;

    //  ??? Speed this up using iterator instead of array indices???
    int ncolumns = (int)data.size();

    for(int c = 0;  c < ncolumns; c++) {
      p_sparseA(p_currentFillRow, c) = p_basis->Term(c) * p_sqrtWeight[p_currentFillRow];
    }
  }
#endif


  std::vector<double> LeastSquares::GetInput(int row) const {
    if((row >= Rows()) || (row < 0)) {
      QString msg = "Index out of bounds [Given = " + toString(row) + "]";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return p_input[row];
  }

  double LeastSquares::GetExpected(int row) const {
    if((row >= Rows()) || (row < 0)) {
      QString msg = "Index out of bounds [Given = " + toString(row) + "]";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return p_expected[row];
  }

  int LeastSquares::Rows() const {
    return (int)p_input.size();
  }

  int LeastSquares::Solve(Isis::LeastSquares::SolveMethod method) {

#if defined(__sun__)
    if(method == SPARSE) method = QRD;
#endif

    if((method == SPARSE  &&  p_sparseRows == 0)  ||
       (method != SPARSE  &&  Rows() == 0 )) {
      p_solved = false;
      QString msg = "No solution available because no input data was provided";
      throw IException(IException::Unknown, msg, _FILEINFO_);
    }

    if(method == SVD) {
      SolveSVD();
    }
    else if(method == QRD) {
      SolveQRD();
    }
    else if(method == SPARSE) {
#ifndef __sun__
      int column = SolveSparse();
      return column;
#endif
    }
    return 0;
  }

  void LeastSquares::SolveSVD() {

    // We are solving Ax=b ... start by creating A
    TNT::Array2D<double> A(p_input.size(), p_basis->Coefficients());
    for(int r = 0; r < A.dim1(); r++) {
      p_basis->Expand(p_input[r]);
      for(int c = 0; c < A.dim2(); c++) {
        A[r][c] = p_basis->Term(c) * p_sqrtWeight[r];
      }
    }

    // Ok use singular value decomposition to solve for the coefficients
    // A = [U][S][V']  where [U] is MxN, [S] is NxN, [V'] is NxN transpose
    // of [V].  We are solving for [A]x=b and need inverse of [A] such
    // that x = [invA]b. Since inverse may not exist we use the
    // pseudo-inverse [A+] from SVD which is [A+] = [V][invS][U']
    // Our coefficents are then x = [A+]b where b is p_b.
    JAMA::SVD<double> svd(A);

    TNT::Array2D<double> V;
    svd.getV(V);

    // The inverse of S is the 1 over each diagonal element of S
    TNT::Array2D<double> invS;
    svd.getS(invS);

    for(int i = 0; i < invS.dim1(); i++) {
      if(invS[i][i] != 0.0) invS[i][i] = 1.0 / invS[i][i];
    }

    // Transpose U
    TNT::Array2D<double> U;
    svd.getU(U);
    TNT::Array2D<double> 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];
      }
    }

    // Now multiply everything together to get [A+]
    TNT::Array2D<double> VinvS = TNT::matmult(V, invS);
    TNT::Array2D<double> Aplus = TNT::matmult(VinvS, transU);

    // Using Aplus and our b we can solve for the coefficients
    TNT::Array2D<double> b(p_expected.size(), 1);

    for(int r = 0; r < (int)p_expected.size(); r++) {
      b[r][0] = p_expected[r] * p_sqrtWeight[r];
    }

    TNT::Array2D<double> coefs = TNT::matmult(Aplus, b);

    // If the rank of the matrix is not large enough we don't
    // have enough coefficients for the solution
    if (coefs.dim1() < p_basis->Coefficients()) {
      QString msg = "Unable to solve least-squares using SVD method. No "
                    "solution available. Not enough knowns or knowns are "
                    "co-linear ... [Unknowns = " 
                    + toString(p_basis->Coefficients()) + "] [Knowns = " 
                    + toString(coefs.dim1()) + "]";
      throw IException(IException::Unknown, msg, _FILEINFO_);
    }

    // Set the coefficients in our basis equation
    std::vector<double> bcoefs;
    for (int i = 0; i < coefs.dim1(); i++) bcoefs.push_back(coefs[i][0]);

    p_basis->SetCoefficients(bcoefs);

    // Compute the errors
    for(int i = 0; i < (int)p_input.size(); i++) {
      double value = p_basis->Evaluate(p_input[i]);
      p_residuals.push_back(value - p_expected[i]);
      p_sigma0 += p_residuals[i]*p_residuals[i]*p_sqrtWeight[i]*p_sqrtWeight[i];
    }
    // calculate degrees of freedom (or redundancy)
    // DOF = # observations + # constrained parameters - # unknown parameters
    p_degreesOfFreedom = p_basis->Coefficients() - coefs.dim1();

    if( p_degreesOfFreedom > 0.0 )  {
      p_sigma0 = p_sigma0/(double)p_degreesOfFreedom;
    }

    // check for p_sigma0 < 0
    p_sigma0 = sqrt(p_sigma0);

    // All done
    p_solved = true;
  }

  void LeastSquares::SolveQRD() {

    // We are solving Ax=b ... start by creating an MxN matrix, A
    TNT::Array2D<double> A(p_input.size(), p_basis->Coefficients());
    for(int r = 0; r < A.dim1(); r++) {
      p_basis->Expand(p_input[r]);
      for(int c = 0; c < A.dim2(); c++) {
        A[r][c] = p_basis->Term(c) * p_sqrtWeight[r];
      }
    }

    // Ok use  to solve for the coefficients
    // [A] = [Q][R]  where [Q] is MxN and orthogonal and  [R] is an NxN,
    // upper triangular matrix.  TNT provides the solve method that inverts
    // [Q] and backsolves [R] to get the coefficients in the vector x.
    // That is, we solve the system Rx = Q^T b
    JAMA::QR<double> qr(A);

    // Using A and our b we can solve for the coefficients
    TNT::Array1D<double> b(p_expected.size());
    for(int r = 0; r < (int)p_expected.size(); r++) {
      b[r] = p_expected[r] * p_sqrtWeight[r];
    }// by construction, we know the size of b is equal to M, so b is conformant

    // Check to make sure the matrix is full rank before solving
    // -- rectangular matrices must be full rank in order for the solve method
    //    to be successful
    int full = qr.isFullRank();
    if(full == 0) {
      QString msg = "Unable to solve-least squares using QR Decomposition. "
                    "The upper triangular R matrix is not full rank";
      throw IException(IException::Unknown, msg, _FILEINFO_);
    }

    TNT::Array1D<double> coefs = qr.solve(b);

    // Set the coefficients in our basis equation
    std::vector<double> bcoefs;
    for(int i = 0; i < coefs.dim1(); i++) {
      bcoefs.push_back(coefs[i]);
    }
    p_basis->SetCoefficients(bcoefs);

    // Compute the errors
    for(int i = 0; i < (int)p_input.size(); i++) {
      double value = p_basis->Evaluate(p_input[i]);
      p_residuals.push_back(value - p_expected[i]);
    }

    // All done
    p_solved = true;
  }




#ifndef __sun__
  int LeastSquares::SolveSparse() {

    // form "normal equations" matrix by multiplying ATA
    gmm::mult(gmm::transposed(p_sparseA), p_sparseA, p_normals);

    //  Test for any columns with all 0's
    //  Return column number so caller can determine the appropriate error.
    int numNonZeros;
    for(int c = 0; c < p_sparseCols; c++) {
      numNonZeros = gmm::nnz(gmm::sub_matrix(p_normals,
                             gmm::sub_interval(0,p_sparseCols),
                             gmm::sub_interval(c,1)));

      if(numNonZeros == 0) return c + 1;
    }

    // Create the right-hand-side column vector 'b'
    gmm::dense_matrix<double> b(p_sparseRows, 1);

    // multiply each element of 'b' by it's associated weight
    for ( int r = 0; r < p_sparseRows; r++ )
      b(r,0) = p_expected[r] * p_sqrtWeight[r];

    // form ATb
    gmm::mult(gmm::transposed(p_sparseA), b, p_ATb);

    // apply parameter weighting if Jigsaw (bundle adjustment)
    if ( p_jigsaw ) {
      for( int i = 0; i < p_sparseCols; i++) {
        double weight = p_parameterWeights[i];

        if( weight <= 0.0 )
          continue;

        p_normals[i][i] += weight;
        p_ATb[i] -= p_epsilonsSparse[i]*weight;
      }
    }

//    printf("printing rhs\n");
//    for( int i = 0; i < m_nSparseCols; i++ )
//      printf("%20.10e\n",m_ATb[i]);

    // decompose normal equations matrix
    p_SLU_Factor.build_with(p_normals);

    // solve with decomposed normals and right hand side
    // int perm = 0;  //  use natural ordering
    int perm = 2;     //  confirm meaning and necessity of
//  double recond;    //  variables perm and recond
    p_SLU_Factor.solve(p_xSparse,gmm::mat_const_col(p_ATb,0), perm);
    // Set the coefficients in our basis equation
    p_basis->SetCoefficients(p_xSparse);

    // if Jigsaw (bundle adjustment)
    // add corrections into epsilon vector (keeping track of total corrections)
    if ( p_jigsaw ) {
      for( int i = 0; i < p_sparseCols; i++ )
        p_epsilonsSparse[i] += p_xSparse[i];
    }

    // test print solution
//    printf("printing solution vector and epsilons\n");
//    for( int a = 0; a < p_sparseCols; a++ )
//      printf("soln[%d]: %lf epsilon[%d]: %lf\n",a,p_xSparse[a],a,p_epsilonsSparse[a]);

//    printf("printing design matrix A\n");
//    for (int i = 0; i < p_sparseRows; i++ )
//    {
//      for (int j = 0; j < p_sparseCols; j++ )
//      {
//        if ( j == p_sparseCols-1 )
//          printf("%20.20e \n",(double)(p_sparseA(i,j)));
//        else
//          printf("%20.20e ",(double)(p_sparseA(i,j)));
//      }
//    }

    // Compute the image coordinate residuals and sum into Sigma0
    // (note this is exactly what was being done before, but with less overhead - I think)
    // ultimately, we should not be using the A matrix but forming the normals
    // directly. Then we'll have to compute the residuals by back projection

    p_residuals.resize(p_sparseRows);
    gmm::mult(p_sparseA, p_xSparse, p_residuals);
    p_sigma0 = 0.0;

    for ( int i = 0; i < p_sparseRows; i++ ) {
        p_residuals[i] = p_residuals[i]/p_sqrtWeight[i];
        p_residuals[i] -= p_expected[i];
        p_sigma0 += p_residuals[i]*p_residuals[i]*p_sqrtWeight[i]*p_sqrtWeight[i];
    }

    // if Jigsaw (bundle adjustment)
    // add contibution to Sigma0 from constrained parameters
    if ( p_jigsaw ) {
      double constrained_vTPv = 0.0;

      for ( int i = 0; i < p_sparseCols; i++ ) {
        double weight = p_parameterWeights[i];

        if ( weight <= 0.0 )
          continue;

        constrained_vTPv += p_epsilonsSparse[i]*p_epsilonsSparse[i]*weight;
      }
      p_sigma0 += constrained_vTPv;
    }
    // calculate degrees of freedom (or redundancy)
    // DOF = # observations + # constrained parameters - # unknown parameters
    p_degreesOfFreedom = p_sparseRows + p_constrainedParameters - p_sparseCols;

    if( p_degreesOfFreedom <= 0.0 ) {
      printf("Observations: %d\nConstrained: %d\nParameters: %d\nDOF: %d\n",
             p_sparseRows,p_constrainedParameters,p_sparseCols,p_degreesOfFreedom);
      p_sigma0 = 1.0;
    }
    else
      p_sigma0 = p_sigma0/(double)p_degreesOfFreedom;

    // check for p_sigma0 < 0
    p_sigma0 = sqrt(p_sigma0);

    // kle testing - output residuals and some stats
    printf("Sigma0 = %20.10lf\nNumber of Observations = %d\nNumber of Parameters = %d\nNumber of Constrained Parameters = %d\nDOF = %d\n",p_sigma0,p_sparseRows,p_sparseCols,p_constrainedParameters,p_degreesOfFreedom);
//    printf("printing residuals\n");
//    for( int k = 0; k < p_sparseRows; k++ )
//    {
//      printf("%lf %lf\n",p_residuals[k],p_residuals[k+1]);
//      k++;
//    }

    // All done
    p_solved = true;
    return 0;
  }
#endif

#ifndef __sun__
  bool LeastSquares::SparseErrorPropagation ()
  {
    // clear memory
    gmm::clear(p_ATb);
    gmm::clear(p_xSparse);

    // for each column of the inverse, solve with a right-hand side consisting
    // of a column of the identity matrix, putting each resulting solution vectfor
    // into the corresponding column of the inverse matrix
    for ( int i = 0; i < p_sparseCols; i++ )
    {
      if( i > 0 )
        p_ATb(i-1,0) = 0.0;

      p_ATb(i,0) = 1.0;

      // solve with decomposed normals and right hand side
      p_SLU_Factor.solve(p_xSparse,gmm::mat_const_col(p_ATb,0));

      // put solution vector x into current column of inverse matrix
      gmm::copy(p_xSparse, gmm::mat_row(p_normals, i));
    }

    // scale inverse by Sigma0 squared to get variance-covariance matrix
    // if simulated data, we don't scale (effectively scaling by 1.0)
    //    printf("scaling by Sigma0\n");
    gmm::scale(p_normals,(p_sigma0*p_sigma0));

//    printf("covariance matrix\n");
//    for (int i = 0; i < p_sparseCols; i++ )
//    {
//      for (int j = 0; j < p_sparseCols; j++ )
//      {
//        if ( j == p_sparseCols-1 )
//          printf("%20.20e \n",(double)(p_sparseInverse(i,j)));
//        else
//          printf("%20.20e ",(double)(p_sparseInverse(i,j)));
//      }
//    }

    // standard deviations from covariance matrix
//    printf("parameter standard deviations\n");
//  for (int i = 0; i < p_sparseCols; i++ )
//    {
//      printf("Sigma Parameter %d = %20.20e \n",i+1,sqrt((double)(p_sparseInverse(i,i))));
//    }

    return true;
  }
#endif


  void LeastSquares::Reset ()
  {
    if ( p_sparse ) {
      gmm::clear(p_sparseA);
      gmm::clear(p_ATb);
      gmm::clear(p_normals);
      p_currentFillRow = -1;
    }
    else {
      p_input.clear();
      //      p_sigma0 = 0.;
    }
      p_sigma0 = 0.;
    p_residuals.clear();
    p_expected.clear();
    p_sqrtWeight.clear();
    p_solved = false;
  }



  double LeastSquares::Evaluate(const std::vector<double> &data) {
    if(!p_solved) {
      QString msg = "Unable to evaluate until a solution has been computed";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return p_basis->Evaluate(data);
  }

  std::vector<double> LeastSquares::Residuals() const {
    if(!p_solved) {
      QString msg = "Unable to return residuals until a solution has been computed";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return p_residuals;
  }

  double LeastSquares::Residual(int i) const {
    if(!p_solved) {
      QString msg = "Unable to return residuals until a solution has been computed";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return p_residuals[i];
  }

  void LeastSquares::Weight(int index, double weight) {
    if(weight == 1) {
      p_sqrtWeight[index] = weight;
    }
    else {
      p_sqrtWeight[index] = sqrt(weight);
    }
  }

} // end namespace isis