SVD.cpp

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/


#include "SVD.hh"

#include <Base/Debug/Exception.hh>
#include <Base/Common/CompareFloatingPoint.hh>

#include <algorithm>
#include <cmath>
#include <vector>

#include "Lapack.hh"
#ifdef BIAS_HAVE_OPENCV
#include <cv.h>
#include <cxcore.h>
#endif
using namespace BIAS;
using namespace std;

SVD::SVD(const Matrix<double> &M, double ZeroThreshold,
         bool AbsoluteZeroThreshold) {
  Decomposed_=false;
  AbsoluteZeroThreshold_ =  AbsoluteZeroThreshold;
  Compute( M, ZeroThreshold);
}

#ifdef BIAS_HAVE_OPENCV
int SVD::ComputeOpenCV(const Matrix<double>& M,  double ZeroThreshold){
  ZeroThreshold_ = ZeroThreshold;
  A_ = M;
  unsigned cols = M.GetCols();
  unsigned rows = M.GetRows();
  CvMat* mat = cvCreateMat(rows,cols,CV_32FC1);
  cvZero(mat);
  //copy BIAS to OpenCV matrix
  for(unsigned i=0;i<rows;i++){
    for(unsigned j=0;j<cols;j++){
      CV_MAT_ELEM( *mat, float, i, j ) = (float)M[i][j];
    }
  }

  CvMat* U  = cvCreateMat(rows,rows,CV_32FC1); cvZero(U);
  CvMat* D  = cvCreateMat(rows,cols,CV_32FC1); cvZero(D);
  CvMat* V  = cvCreateMat(cols,cols,CV_32FC1); cvZero(V);
  cvSVD(mat, D, U, V, CV_SVD_V_T); // A = U D V^T

  U_.newsize(rows,rows);
  VT_.newsize(cols,cols);
  S_.newsize(rows);
  for(unsigned i=0;i<rows;i++){
    for(unsigned j=0;j<rows;j++){
      U_[i][j] = (double)CV_MAT_ELEM(*U,float,i,j);    
    }   
    if(i< rows && i < cols){
      //singular values are on diagonal of matrix D
      S_[i] = (double)CV_MAT_ELEM(*D,float,i,i);
    }
  }
  for(unsigned i=0;i<cols;i++){
    for(unsigned j=0;j<cols;j++){
      VT_[i][j] = (double)CV_MAT_ELEM(*V,float,i,j);
    }
  }
   
  cvReleaseMat(&U);
  cvReleaseMat(&D);
  cvReleaseMat(&V);
  cvReleaseMat(&mat);
  
  //need extra if -> ensure S_.size != 0
  if (S_[0] < numeric_limits<double>::epsilon())
    return -1;
  else 
    Decomposed_=true;

  return 0;
}
#endif
int SVD::Compute(const Matrix<double> & M, double ZeroThreshold) 
{
  Decomposed_=false;
  ZeroThreshold_ = ZeroThreshold;
  return General_Eigenproblem_GeneralMatrix_Lapack( M );
}


int SVD::General_Eigenproblem_GeneralMatrix_Lapack(const Matrix<double> & M) 
{
  // S_ will contain the singular values of A sorted with s(i) <= s(i+1)  
  // U_ will contain m x m left orthogonal/unitary matrix with 
  //    eigenvectors in columns  
  // VT_ will contain n x n right _transposed_ orthogonal/unitary matrix 
  //     with eigenvectors in rows (because of transpose)    
  A_=M;
  int res=0;
  res = General_singular_value_decomposition(A_, S_, U_, VT_);

  if ( S_.size() != 0 && U_.num_rows() != 0 &&  U_.num_cols() != 0 &&
       VT_.num_rows() != 0 &&  VT_.num_cols() != 0 && res == 0) {
    //need extra if -> ensure S_.size != 0
    if (S_[0] < numeric_limits<double>::epsilon()) {
      res = -1;
    } else Decomposed_=true;
  } else {
    res = -1;
  }
  
  return res;
}

// -- Solve the matrix-vector system M x = y, returning x.
Vector<double> SVD::Solve(const Vector<double>& y) const
{
  /// @bug bug in this function (woelk)
  BEXCEPTION("Error, there is a bug in this function, use Solve(Matrix, "
             "vector, Vector instead");
  Vector<double> x(VT_.num_cols());      // solution matrix vector
  Vector<double> UTy;

  //cout << "U_=" << U_ << " VT_= " << VT_ << " S_=" << S_;

  if(Decomposed_) {
    if (y.size() <= U_.num_rows()){
      Matrix<double> UT=U_.Transpose();
      // at first multiply with U^T
      if (U_.num_rows() < U_.num_cols()) {// augment y with extra rows of
        Vector<double> yy(U_.num_rows(), 0.0);      // zeros, so that it match
        for (int k=0; k< y.size();k++) 
          yy[k] = y[k]; // cols of u.transpose.
        UTy = UT * yy;
      } else {
        UTy = UT * y;
      }
           
      // multiply with diagonal 1/S_
      int i=0;
      for (; i < UTy.size(); i++)      
        x[i]=(fabs(S_[i]) >= ZeroThreshold_) ? UTy[i]/S_[i] : 0;//UTy[i]*S_[i];
      
      for (;i<x.size();i++)  x[i]=0;
      
      return VT_.Transpose() * x;   // premultiply with v.
      
    } else {
      BIASERR("size of right hand side of Ax = y is too big. Length(y) = "
              << y.size());
      x.newsize(0);
      return x;
    }
  } else {
    BIASERR("SVD holds no decomposed matrix.");
    x.newsize(0);
    return x;
  }
}

#ifdef BIAS_HAVE_OPENCV
    /** \brief Calculates new inverse with OpenCV cvInvert*/
Matrix<double> SVD::InvertOpenCV(const Matrix<double>& A)
{
  unsigned cols = A.GetCols();
  unsigned rows = A.GetRows();
  Matrix<double> B(rows,cols,0);
  CvMat* a = cvCreateMat(rows,cols,CV_32FC1);
  cvZero(a);
  //copy BIAS to OpenCV matrix
  for(unsigned i=0;i<rows;i++){
    for(unsigned j=0;j<cols;j++){
      CV_MAT_ELEM( *a, float, i, j ) = (float)A[i][j];
    }
  }
  CvMat* b = cvCreateMat(rows,cols,CV_32FC1);
  cvZero(b);
  //CV_LU - Gaussian elimination with optimal pivot element 
  //CV_SVD - Singular decomposition method 
  double ret = cvInvert(a,b,CV_SVD);
  if(ret==0.0) BIASWARN("Could not invert Matrix");
  //copy OpenCV to BIAS matrix
  for(unsigned i=0;i<rows;i++){
    for(unsigned j=0;j<cols;j++){
      B[i][j] = (double)CV_MAT_ELEM( *b, float, i, j );
    }
  }
  cvReleaseMat(&a);
  cvReleaseMat(&b);

  return B;
}
#endif

Matrix<double> SVD::Invert()
{
  if (!Decomposed_) {//no decomposition, return 0x0-matrix
    Matrix<double> res(0,0);
    return res;
  }

  Matrix<double> V(VT_.num_cols(), VT_.num_rows());
  Matrix<double> UT(U_.num_cols(), U_.num_rows());
  Matrix<double> SdT(VT_.num_rows(), U_.num_cols());
  SdT.SetZero();

  if (AbsoluteZeroThreshold_) {
    for (register int i = 0; i < S_.size(); i++){
      SdT[i][i] = (S_[i] < ZeroThreshold_ && S_[i] > -ZeroThreshold_)
                  ? (0.0) : (1.0/S_[i]);
    }
  } else {
    for (register int i = 0; i < S_.size(); i++){
      SdT[i][i]=(S_[i]/S_[0] < ZeroThreshold_ && S_[i]/S_[0] > -ZeroThreshold_)
                 ? (0.0) : (1.0/S_[i]);
    }
  }
  V = VT_.Transpose();
  UT = U_.Transpose();

  return V * SdT * UT;
}

Matrix<double> SVD::Invert(int rank)
{
  if (!Decomposed_) {//no decomposition, return 0x0-matrix
    Matrix<double> res(0,0);
    return res;
  }

  Matrix<double> V(VT_.num_cols(), VT_.num_rows());
  Matrix<double> UT(U_.num_cols(), U_.num_rows());
  Matrix<double> SdT(VT_.num_rows(), U_.num_cols());
  SdT.SetZero();

  for (register int i = 0; i < rank; i++){
    SdT[i][i] = (1.0/S_[i]);
  }
  for (register int i = rank; i < S_.size(); i++){
    SdT[i][i] = 0;
  }
  V = VT_.Transpose();
  UT = U_.Transpose();

  return V * SdT * UT;
}


Matrix<double> SVD::Invert(Matrix<double> A)
{
  if (Compute(A)==0)
    return Invert();
  else return Matrix<double>(0,0);
} 


Matrix<double> SVD::Sqrt()
{
#ifdef BIAS_DEBUG
  // check symmetry and symmetric positive definit'ness
  // 1. symmetry
  bool valid = (A_.GetCols() == A_.GetRows());
  if (valid){
    const int num = A_.GetCols();
    for (int r=0; r<num; r++){
      for (int c=r; c<num; c++){
        // dont use double epsilon but 1e-10 for numeric comparison
        valid = valid && Equal(A_[r][c], A_[c][r], 1e-10);
        if (!valid) {
          cout << setprecision(20) << A_[r][c] << "\t"<< setprecision(20) << A_[c][r]<<endl;
          break;
          break;
        }
      }
    }
  }
  if (!valid){
    //BIASERR("matrix is not symmetric "<<A_);
    BEXCEPTION("matrix is not symmetric "/*<<A_*/);
  }
  // 2. positive definit = all eigenvalues are positive
  Vector<double> eig = Upper_symmetric_eigenvalue_solve(A_);
  for (register int i=0; i<eig.size(); i++){
    valid = valid && (GreaterEqual(eig[i], 0.0, 1e-10));
    if (!valid){
      cout << "eigenvalue "<<setprecision(20)<<eig[i]<<endl;
      break;
    }
  }
  if (!valid){
    //BIASERR("matrix is not positive definit "<<A_);
    BEXCEPTION("matrix is not positive definit "/*<<A_*/);
  }
#endif
  if (!Decomposed_){
    int res = 0;
    if ((res = Compute(A_))!=0) {
      BIASERR("SVD failed with result "<<res);
      return  Matrix<double>(VT_.num_rows(), U_.num_cols(), MatrixZero);
    }
  }
  // [U S V] = svd(A); sqrtm(A) = U * sqrt(S) * V'
  Matrix<double> SqrtS(VT_.num_rows(), U_.num_cols());
  SqrtS.SetZero();
  if (AbsoluteZeroThreshold_) {
    for (register int i=0; i<S_.size(); i++){
      SqrtS[i][i] = (fabs(S_[i])<ZeroThreshold_)?(0.0):(sqrt(S_[i]));
    }
  } else {
    for (register int i=0; i<S_.size(); i++){
      SqrtS[i][i] = (fabs(S_[i]/S_[0])<ZeroThreshold_)?(0.0):(sqrt(S_[i]));
    }
  }

  return U_ * SqrtS * VT_;
}

Matrix<double> SVD::Sqrt(const Matrix<double>& A)
{
  if (Compute(A)==0)
    return Sqrt();
  else return Matrix<double>(0,0);
} 

Matrix<double> SVD::SqrtT()
{
#ifdef BIAS_DEBUG
  // check symmetry and symmetric positive definit'ness
  // 1. symmetry
  bool valid = (A_.GetCols() == A_.GetRows());
  if (valid){
    const int num = A_.GetCols();
    for (int r=0; r<num; r++){
      for (int c=r; c<num; c++){
        valid = valid && Equal(A_[r][c], A_[c][r], 1e-12);
        if (!valid) {
          cout<<" Invalid matrix ("<<num<<"x"<<num<<") element at "
              <<r<<" "<<c<<endl;
          cout << setprecision(20) << A_[r][c] << "\t"<< setprecision(20) 
               << A_[c][r]<<endl;
          break;
        }
      }
      if (!valid) { break; }
    }
  }
  if (!valid){
    BEXCEPTION("matrix is not symmetric "<<A_<<endl
               <<"Fix this by calling A_.MakeSymmetric() in CALLING class");
  }

  // 2. positive definit = all eigenvalues are positive
  Vector<double> eig = Upper_symmetric_eigenvalue_solve(A_);
  for (register int i=0; i<eig.size(); i++){
    valid = valid && (GreaterEqual(eig[i], 0.0, 1e-12));
    if (!valid){
      //cout << "eigenvalue "<<setprecision(20)<<eig[i]<<endl;
      break;
    }
  }
  if (!valid){
    //BIASERR("matrix is not positive definit "<<A_);
    //BEXCEPTION("matrix is not positive definit "/*<<A_*/);
  }
#endif
  
  if (!Decomposed_){
    if (Compute(A_)!=0) {
      // error in decomposition
      return Matrix<double>(0,0); // empty matrix
    }
  }
  // [U S V] = svd(A); sqrtm(A) = U * sqrt(S)
  Matrix<double> SqrtS(VT_.num_rows(), U_.num_cols(), MatrixZero);  
  if (AbsoluteZeroThreshold_) {
    for (register int i=0; i<S_.size(); i++){
      SqrtS[i][i] = (fabs(S_[i])<ZeroThreshold_)?(0.0):(sqrt(S_[i]));
    }
  } else {
    for (register int i=0; i<S_.size(); i++){
      SqrtS[i][i] = (fabs(S_[i]/S_[0])<ZeroThreshold_)?(0.0):(sqrt(S_[i]));
    }
  }

  return U_ * SqrtS;
}

Matrix<double> SVD::SqrtT(const Matrix<double>& A)
{
  if (Compute(A)==0)
    return SqrtT();
  else return Matrix<double>(0,0);
} 


const int SVD::AbsNullspaceDim() const 
{
  int dim = 0;
  int index = S_.size()-1; 

  //cerr << "ZeroThreshold_: "<<ZeroThreshold_<<endl;
  while (index >= 0 && fabs(S_[index]) < ZeroThreshold_ ) {
    // singular values are sorted in descending order is still (around zero
    index--;
    dim++;
  }
  
  if (A_.num_cols()>A_.num_rows()){
      dim+=A_.num_cols()-A_.num_rows();
  }
  return dim;
}

const int SVD::RelNullspaceDim() const 
{ 
  int dim = 0;
  int index = S_.size()-1; 

  // use greatest singular value as reference
  const double RefSingValue(S_[0]);
  if (RefSingValue<=DBL_EPSILON) {
    // the greatest singular value is zero => all are zero
    return 0;
  } 
    
  while ((S_[index]/RefSingValue)<ZeroThreshold_){
    dim++;
    if (--index<1) break; // S[0]/S[0]==1 always => dont need to check that
  }

  // check if we have an "underdetermined equation system" by dimensionality
  if (A_.num_cols()>A_.num_rows()){
    dim += A_.num_cols()-A_.num_rows();
  }
   
  return dim;
}


const int SVD::AbsRightNullspaceDim() const { 
  return AbsNullspaceDim();
}

const int SVD::RelRightNullspaceDim() const { 
    return RelNullspaceDim();
}

const int SVD::AbsLeftNullspaceDim() const 
{ 
  int dim = 0;
  int index = S_.size()-1; 

  //cerr << "ZeroThreshold_: "<<ZeroThreshold_<<endl;
  while (index >= 0 && fabs(S_[index]) < ZeroThreshold_ ) {
    // singular values are sorted in descending order
    index--;
    dim++;
  }
  // compensate null space for rectangular matrices 
  if (A_.num_rows()>A_.num_cols()){
    dim += A_.num_rows()-A_.num_cols();
  }
  return dim;
}

const int SVD::RelLeftNullspaceDim() const 
{ 
  int dim = 0;
  int index = S_.size()-1; 
  
  if (GetSingularValue(0)==0){
    dim=NullspaceDim();
  } else {
    double maxs=S_[0];
    while (index>=0 && fabs(S_[index]/maxs)<ZeroThreshold_){
        index--;
        dim++;
    }
    // compensate null space for rectangular matrices 
    if (A_.num_rows()>A_.num_cols()){
        dim+=A_.num_rows()-A_.num_cols();
    }
  } 
  return dim;
}

unsigned int SVD::Rank()
{
  BIASDOUT(D_SVD_RANK, "S_.Size(): "<<S_.Size()<<" NullspaceDim(): "
           << NullspaceDim()<<"  A_.num_cols(): "<<A_.num_cols()
           <<"   A_.num_rows(): "<<A_.num_rows()<<"  S_: "<<S_);
  unsigned int lowdim=A_.num_rows()<A_.num_cols()?A_.num_rows():A_.num_cols();
  return (lowdim-NullspaceDim());
}


int SVD::Solve(Matrix<double>& A, Vector<double>& b, Vector<double>& x)
{
  int res = 0;
  // make squared symmetric matrix A^T * A
  Matrix<double> ATA(A.num_cols(), A.num_cols());
  //ATA = A.Transpose()*A;
  A.GetSystemMatrix(ATA);

  // compute ATA = U * S * V^T
  Compute(ATA);
  Vector<double> d(A.num_rows()), z(A.num_cols());
  Vector<double> S = GetS();
  BIASCDOUT(D_SVD_SOLVE, "S: "<<S<<endl); 
  
  // A * x = b  => x = (A^T * A )^-1 * A^T * b
  //                 = (U * S * V^T)^-1 * A^T * b 
  // by symmetry we know: A^T * A = (A^T * A)^T 
  //                 = (V * S * U^T)^-1 * A^T * b 
  // since U and V^T are square orthogonal matrices here
  //                 = U^-T * S^-1 * V^T * A^T * b
  //                 = U    * S^-1 *       d
  d = GetVT() * A.Transpose() * b;
  
  // compute z = S^-1 * d
  BIASCDOUT(D_SVD_SOLVE, "d: "<<d<<endl);
  if (AbsoluteZeroThreshold_) {
    for (int i=0; i<A.num_cols(); i++) {
      if (fabs(S[i])>ZeroThreshold_) {
        z[i] = d[i]/S[i];
      } else {
        z[i] = 0.0;
        res--;
      }
    }
  } else {
    for (int i=0; i<A.num_cols(); i++) {
      if (fabs(S[i]/S_[0])>ZeroThreshold_) {
        z[i] = d[i]/S[i];
      } else {
        z[i] = 0.0;
        res--;
      }
    }
  }

  if (res<0) {
      BIASDOUT(D_SVD_SOLVE,"Error in SVD, degenerate case in Solve ("
            << -res <<" dim. solution space), S=" 
            << S);
  }
  
  // get final solution x = U * z
  x = GetU() * z;
  return res;
}


BIAS::Vector<double> SVD::GetEigenValues()
{
  std::vector<double> eigValues(VT_.GetRows());
  // now we need the eigenvalues (with sign !)
  // compute this by projecting the (normalized) eigenvectors onto their images
  Vector<double> eigenvector, eigenimage;
  for (unsigned int i=0; i<VT_.GetRows(); i++) {
    // get ith eigenvector and its image
    eigenvector=VT_.GetRow(i);
    eigenimage = A_ * eigenvector; 
    // compute scalar product
    eigValues[i] = eigenimage * eigenvector;
  }
  std::sort(eigValues.begin(),eigValues.end());
  BIAS::Vector<double> result(VT_.GetRows(),&eigValues[0]);
  
  return result;
}