svd.cpp

#include "svd.h"
#include "util.h"
#include "linalg.h"
#include <cmath>


#define SIGN(a,b) ((b) > 0.0 ? fabs(a) : -fabs(a))

static double maxarg1, maxarg2;
#define FMAX(a,b) (maxarg1 = (a), maxarg2 = (b), (maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2))

static int iminarg1, iminarg2;
#define IMIN(a,b) (iminarg1 = (a), iminarg2 = (b), (iminarg1) < (iminarg2) ? (iminarg1) : (iminarg2))

static double sqrarg;
#define SQR(a) ((sqrarg = (a)) == 0.0 ? 0.0 : sqrarg * sqrarg)

double pythag(double a, double b) {
  // computes (a^2+b^2)^(1/2) without destructive under/over-flow
  double absa = fabs(a);
  double absb = fabs(b);
  if (absa>absb) { 
    return absa*sqrt(1.0+SQR(absb/absa)); 
  }
  else {
    return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+SQR(absa/absb)));
  }
}

// SVD-decomposition A = U*W*V', where U,V orthogonal and W diagonal. The matrix U is
// replacing A and notice that V and not the transpose V' is returned. 
// Blackbox-algorithm from NR.
void svdcmp(matrix &A, matrix &DD, matrix &VV) {
  int flag,i,its,j,jj,k,l = 0,nm = 0;
  double anorm,c,f,g,h,s,scale,x,y,z;

  // Dimensions of A;
  int m = A.row();
  int n = A.column();
  
  matrix W(n,n);
  matrix V(n,n);

  safevector<double> rv1(n);

  g=scale=anorm=0.0;

  //  cerr << "Start of algorithm\n";
  
  // householder reduction to bidiagonal form
  for (i=0; i<n; i++) {
    l=i+1;
    rv1[i] = scale*g;
    g=s=scale=0.0;
    if (i<m) {
      for (k=i; k<m; k++)   { scale += fabs(A.get(k,i));   }
      if (scale) {
        for (k=i; k<m; k++) {
          //      cerr << "A( " << k << ", " << i << ")";
          A.set(A.get(k,i)/scale, k,i);
          s += A.get(k,i)*A.get(k,i);
        }
        f = A.get(i,i);
        g = -SIGN((double)sqrt(s),double(f));
        h = f*g-s;
        A.set(f-g,i,i);
        for (j=l; j<n; j++) {
          for (s=0.0, k=i; k<m; k++) {  s += A.get(k,i)*A.get(k,j);  }
          f = s/h;
          for (k=i; k<m; k++) {  A.set(A.get(k,j)+f*A.get(k,i),k,j);  }
        }
        for (k=i; k<m; k++) {  A.set(A.get(k,i)*scale,k,i); }
      } // if (scale)
    } // if (i<=m)

    W.set(scale*g,i,i);
    g=s=scale=0.0;
    if (i<m && i!=n-1) {
      for (k=l; k<n; k++) { scale += fabs(A.get(i,k)); }
      if (scale) {
        for(k=l; k<n; k++) {
        A.set(A.get(i,k)/scale,i,k);
        s += A.get(i,k)*A.get(i,k);
        }
        f = A.get(i,l);
        g = -SIGN(sqrt(s),f);
        h = f*g-s;
        A.set(f-g,i,l);
        for (k=l; k<n; k++) {
          rv1[k]=A.get(i,k)/h;
        }
        for (j=l; j<m; j++) {
          for (s=0.0, k=l; k<n; k++) { s+= A.get(j,k)*A.get(i,k); }
          for (k=l; k<n; k++) { A.set(s*rv1[k]+A.get(j,k),j,k); }
        }
        for (k=l; k<n; k++) { A.set(A.get(i,k)*scale, i,k); }
      }// if (scale)
    } // if(i<=m && i!=n)
    anorm = FMAX(anorm, (fabs(W.get(i,i))+fabs(rv1[i])));
  } //  for (i=1; i<=n; i++) {


  //  cerr << "End of part 1\n";

  for (i=n-1; i>=0; i--) { // Accumulation of right-hand transformations.
    if (i<n-1) {
      if (g) {
        for (j=l; j<n; j++) { // Double division to avoid possible underflow 
          V.set((A.get(i,j)/A.get(i,l))/g ,j,i);
        }
        for (j=l; j<n; j++) {
          for (s=0.0, k=l; k<n; k++) { s +=  A.get(i,k)*V.get(k,j); }
          for (k=l; k<n; k++) { V.set(V.get(k,j)+s*V.get(k,i), k,j); }
        }
      } // if (g)
      for (j=l; j<n; j++) { V.set(0,i,j); V.set(0,j,i); }
    } // if (i<n)
    V.set(1.0,i,i);
    g = rv1[i];
    l = i;
  } // for (i=n; i>=1; i--);

  //  cerr << "End of part 2\n";
     
  for (i=IMIN(m,n)-1; i>=0; i--) { // Accumulation of left-hand transformations
    l = i+1;
    g = W.get(i,i);
    for (j=l; j<n; j++) { A.set(0.0, i,j); }
    if (g) {
      g = 1.0/g;
      for (j=l; j<n; j++) {
        for (s=0.0, k=l; k<m; k++) {  s += A.get(k,i)*A.get(k,j); }
        f = (s/A.get(i,i))*g;
        for (k=i; k<m; k++) { A.set(A.get(k,j)+f*A.get(k,i), k,j); }
      }
      for (j=i; j<m; j++) { A.set(g*A.get(j,i), j,i);  }
    } // if (g)
    else { 
      for (j=i; j<m; j++) { A.set(0.0, j,i); }
    }
    A.set(A.get(i,i)+1, i,i);
  } // for (i=IMIN(m,n); i>=1; i--)

  //  cerr << "End of part 3\n";
  
  for (k=n-1; k>=0; k--) { // Diagonalization of the bidiagonal form; Loop over singular values, and over all allowed iterations
    for (its=1; its<=100; its++) {
      flag = 1;
      for (l=k; l>=0; l--) { // Test for splitting
        //      cerr << "\nl=" << l << "   k=" << k <<  endl;
        nm = l-1;          // Note that rv1[0] is always zero.
        if ((double)(fabs(rv1[l])+anorm) == anorm) {
          flag = 0;
          break;
        }
        if ((double)(fabs(W.get(nm,nm))+anorm) == anorm) { break; }
      } // for (l=k; l>=1; l--)
      if (l<0) l++;
      if (flag) { // cancelation of rv1[l], if l>1.
        //      cerr << "\nl=" << l << "   k=" << k <<  endl;
        c = 0.0;
        s = 1.0;
        for (i=l; i<=k; i++) {
          f = s*rv1[i];
          rv1[i] = c*rv1[i];
          if ((double)(fabs(f)+anorm) == anorm) { break; }
          //      cerr << "W.rows=" << W.row() << " W.cols=" << W.column() << "  i=" << i << endl;
          g = W.get(i,i);
          h = pythag(f,g);
          //      cerr << "W.rows=" << W.row() << " W.cols=" << W.column() << "  i=" << i << endl;
          W.set(h,i,i);
          h = 1.0/h;
          c = g*h;
          s = -f*h;
          for (j=0; j<m; j++) {
            y = A.get(j,nm);
            z = A.get(j,i);
            A.set(y*c+z*c, j, nm);
            A.set(z*c-y*s, j,i);
          }
        } // for (i=1; i<=k; i++)
      } // if (flag)
      
      //      cerr << "End of part 4\n";
  
      z = W.get(k,k);
      if (l == k) { // Convergence
        if (z < 0.0) { // Singular values are made nonnegative
          W.set(-z, k,k);
          for (j=0; j<n; j++) { V.set(-V.get(j,k), j,k); }
        }
        break;
      } // if (l == k)
      
      if (its == 100) {
        std::cerr << "No convergence in 100 svdcmp iterations\n";
        exit(1);
      }
      
      //      cerr << "End of part 5\n";

      x = W.get(l,l);   // Shift from bottom 2-by-2 mirror
      nm = k-1;
      y = W.get(nm,nm);
      g = rv1[nm];
      h = rv1[k];
        f = ((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
      g = pythag(f,1.0);
      f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
      c=s=1.0;  // Next QR transformation
      for (j=l; j<=nm; j++) {
        i = j+1;
        g = rv1[i];
        y = W.get(i,i);
        h = s*g;
        g = c*g;
        z = pythag(f,h);
        rv1[j] = z;
        c = f/z;
        s = h/z;
        f = x*c+g*s;
        g = g*c-x*s;
        h = y*s;
        y *= c;
        for (jj=0; jj<n; jj++) {
          x = V.get(jj,j);
          z = V.get(jj,i);
          V.set(x*c+z*s, jj,j);
          V.set(z*c-x*s, jj,i);
        }
        z = pythag(f,h);
        W.set(z, j,j);
        if  (z) {  // Rotation can be arbitrary if z=0;
          z = 1.0/z;
          c = f*z;
          s = h*z;
        }
        f = c*g+s*y;
        x = c*y-s*g;

        for (jj=0; jj<m; jj++) {
          y = A.get(jj,j);
          z = A.get(jj,i);
          A.set(y*c+z*s, jj,j);
          A.set(z*c-y*s, jj,i);
        }
      } // for (j=1; k<=nm; j++)
      rv1[l] = 0.0;
      rv1[k] = f;
      W.set(x, k,k);   
    } // for (its=1; its<=30; its++)
  } // for (k=n; k>=1; k--)

  DD  = W;
  VV = V;
} // end of function      


/*
void redsvd(matrix &A, matrix &D, matrix &V) {

  double threshold = 1e-6;

  svdcmp(A, D, V);

//   cerr << "dim(V) = " << V.height() << "x" << V.width() << ";  ";
//   cerr << "dim(D) = " << D.height() << "x" << D.width() << ";  ";
//   cerr << "dim(A) = " << A.height() << "x" << A.width() << ";  ";
//   cerr << endl << endl;
  
  //  cerr << "D_original = " << endl << D << endl;
  unsigned N=D.width();
  safevector<unsigned> deleteindex;
  safevector<double> diagonal;
  for (unsigned i=0; i<N; i++) {
    if (fabs(D(i,i))>threshold) {
      diagonal.push_back(D(i,i));
    }
    else deleteindex.push_back(i);
  }

  //  cerr << "D.size = " << diagonal.size() << endl;
  Matrix<double> DD(diagonal.size(), diagonal.size());
  for (unsigned i=0; i<diagonal.size(); i++) {
    DD(i,i) = diagonal[i];
  }
  D = DD;

  cerr << "Deleteindex: " << deleteindex << endl;
  A.remove_col(deleteindex);
  V.remove_col(deleteindex);
    cerr << "A(pseudoinv)=" << endl << A << endl;
    cerr << "V(pseudoinv)=" << endl << V << endl;


//   cerr << "U=" << endl << U << endl;
//   cerr << "V=" << endl << V << endl;
} // redsvd
*/


matrix pseudoinverse(matrix const& A, double threshold) {

  matrix U=A, D,V;
  svdcmp(U,D,V);

  //  cerr << endl << endl << "D=" << endl << D << endl;
  matrix iD(D.row(), D.row()); 
  for (int i=0; i<D.row(); i++) { // Truncate (near) singular values
    if (fabs(D.get(i,i))>threshold)
      iD.set((double)1/D.get(i,i), i,i);
  }
    matrix res = V*iD;
    res = res*U.trans();
    return res;
}


#ifdef TEST

int main(int argc, char *argv[]) {
  try {

    /*
    matrix A(4,4);
    A.set(1,0,0);   A.set(0,0,1);     A.set(1,0,2);     A.set(0,0,3); 
    A.set(0,1,0);   A.set(1,1,1);     A.set(0,1,2);     A.set(1,1,3); 
    A.set(2,2,0);   A.set(0,2,1);     A.set(2,2,2);     A.set(0,2,3); 
    A.set(0,3,0);   A.set(0,3,1);     A.set(1,3,2);     A.set(1,3,3); 
    */

    matrix B(1,4);
    B.set(1,0,0);   B.set(0,0,1);     B.set(1,0,2);     B.set(0,0,3); 
    matrix A = B.trans();


    std::cerr.precision(5);
    std::cerr << "A = "; A.print();
    std::cerr << "-------------------------------------------------------------------" << std::endl;

    matrix U = A;
    matrix V(1,1), D(1,1);
    svdcmp(U,D,V);

    for (int i=0; i<D.row(); i++) { // Truncate (near) singular values
      if (fabs(D.get(i,i))<1e-6)
        D.set(0,i,i) ;
    }

    matrix newA = U*D*V.trans();
    std::cerr << "newA = "; newA.print();
    std::cerr << "U'U = "; (U.trans()*U).print();
    std::cerr << "V'V = "; (V.trans()*V).print();

    std::cerr << "-------------------------------------------------------------------" << std::endl;

    matrix b(4,1); 
    b.set(0.0, 0,0);
    b.set(2.0, 1,0);
    b.set(1.0, 2,0);
    b.set(7.0, 3,0);
     
    matrix Aplus = pseudoinverse(A);

    std::cerr << "Solving Ax = b = "; (b.trans()).print(); std::cerr << std::endl;
    matrix x = Aplus*b;
    std::cerr << "Apinv*b = x'  = "; x.trans().print(); std::cerr << std::endl;
    std::cerr << "A*x' = "; (A*x).trans().print(); std::cerr << std::endl;

  } catch(std::string const& err) {
    std::cerr << err << std::endl;
    return 1;
  }
  return 0;
}


#endif//TEST
   
      

                

---