SVD3x3.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 "SVD3x3.hh"


#ifdef WIN32
#  define _USE_MATH_DEFINES
#endif //WIN32
#include <math.h>

//#include <Base/Common/BIASpragma.hh>

using namespace BIAS;
using namespace std;

#define SVD_MAX_ITERATIONS 40

SVD3x3::SVD3x3()
{ 
  A_.newsize(3, 3);
  S_.newsize(3);
  U_.newsize(3, 3);
  VT_.newsize(3, 3);
  ZeroThreshold_=DEFAULT_DOUBLE_ZERO_THRESHOLD;
  Decomposed_=false;
}

SVD3x3::SVD3x3(const Matrix <double> & M, double ZeroThreshold)
{ 
  A_.newsize(3, 3);
  S_.newsize(3);
  U_.newsize(3, 3);
  VT_.newsize(3, 3);
  Compute(M, ZeroThreshold); 
}

#define PYTHAG(a,b) (fabs(a) > fabs(b) ? \
fabs(a)*sqrt(1.0+(fabs(b)/fabs(a))*(fabs(b)/fabs(a))) : \
(fabs(b) > 0 ? fabs(b)*sqrt(1.0+(fabs(a)/fabs(b))*(fabs(a)/fabs(b))) : 0.0))

#define MAX(a,b) ((a) > (b) ? (a) : (b))

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

/** this function is based on svdcmp from the numerical recipes in C */
int SVD3x3::Compute(const Matrix<double> & M, double ZeroThreshold) 
{
  BIASASSERT(M.num_rows()==3);
  BIASASSERT(M.num_cols()==3);

  Decomposed_=false;
  ZeroThreshold_ = ZeroThreshold;
  U_=A_=M;
  int res=0;
  double **a=U_.GetDataArray1(), **v=VT_.GetDataArray1();
  double *w=S_.GetData()-1; 
    int flag=0, i=0, its=0, j=0, jj=0 , k=0, l=0, nm=0;
    double c=0.0, f=0.0, h=0.0, s=0.0, x=0.0, y=0.0, z=0.0;
    double norma=0.0, g=0.0, sc=0.0;
    double tmp[4];

    for (i=1; i<=3; i++) {
        l=i+1;
        tmp[i]=sc*g;
        g=s=sc=0.0;
        if (i <= 3) {
            for (k=i; k<=3; k++) 
        sc += fabs(a[k][i]);
            if (sc) {
                for (k=i; k<=3; k++) {
                    a[k][i] /= sc;
          // here s and a[k][i] are exactly the same for O2 and O3
                    s += a[k][i]*a[k][i];
          // s is now slightly different for O2 and O3 !
                }
                f=a[i][i];
                g = -SIGN(sqrt(s),f);
                h=f*g-s;
                a[i][i]=f-g;
                if (i != 3) {
                    for (j=l; j<=3; j++) {
                        for (s=0.0,k=i; k<=3; k++) 
              s += a[k][i]*a[k][j];
                        f=s/h;
                        for (k=i; k<=3; k++) 
              a[k][j] += f*a[k][i];
                    }
                }
                for (k=i; k<=3; k++) 
          a[k][i] *= sc;
            }
        }
        w[i]=sc*g;
        g=s=sc=0.0;
        if (i < 3) {
            for (k=l; k<=3; k++) 
        sc += fabs(a[i][k]);
            if (sc) {
                for (k=l; k<=3; k++) {
                    a[i][k] /= sc;
                    s += a[i][k]*a[i][k];
                }
                f=a[i][l];
                g = -SIGN(sqrt(s),f);
                h=f*g-s;
                a[i][l]=f-g;
                for (k=l; k<=3; k++) 
          tmp[k]=a[i][k]/h;
                if (i != 3) {
                    for (j=l; j<=3; j++) {
                        for (s=0.0,k=l; k<=3; k++) 
              s += a[j][k]*a[i][k];
                        for (k=l; k<=3; k++) 
              a[j][k] += s*tmp[k];
                    }
                }
                for (k=l; k<=3; k++) 
          a[i][k] *= sc;
            }
        }
        norma=MAX(norma,(fabs(w[i])+fabs(tmp[i])));
    }
    for (i=3; i>=1; i--) {
        if (i < 3) {
            if (g) {
                for (j=l; j<=3; j++)
                    v[i][j]=(a[i][j]/a[i][l])/g;
                for (j=l; j<=3; j++) {
                    for (s=0.0,k=l; k<=3; k++) 
            s += a[i][k]*v[j][k];
                    for (k=l; k<=3; k++) 
            v[j][k] += s*v[i][k];
                }
            }
            for (j=l; j<=3; j++) 
        v[i][j]=v[j][i]=0.0;
        }
        v[i][i]=1.0;
        g=tmp[i];
        l=i;
    }
    for (i=3; i>=1; i--) {
        l=i+1;
        g=w[i];
        if (i < 3)
            for (j=l; j<=3; j++) 
        a[i][j]=0.0;
        if (g) {
            g=1.0/g;
            if (i != 3) {
                for (j=l; j<=3; j++) {
                    for (s=0.0, k=l; k<=3; k++) 
            s += a[k][i]*a[k][j];
                    f=(s/a[i][i])*g;
                    for (k=i; k<=3; k++) 
            a[k][j] += f*a[k][i];
                }
            }
            for (j=i; j<=3; j++) 
        a[j][i] *= g;
        } else {
            for (j=i; j<=3; j++) 
        a[j][i]=0.0;
        }
        ++a[i][i];
    }
    for (k=3; k>=1; k--) {
        for (its=1; its<=SVD_MAX_ITERATIONS; its++) {
            flag=1;
            for (l=k;l>=1;l--) {
                nm=l-1;
                if (fabs(tmp[l])+norma == norma) {
          // for l==k==2 we never reach this point, because lhs and rhs 
          // differ here by 1e-18 !
                    flag=0;
                    break;
                }
                if (fabs(w[nm])+norma == norma) 
          break;
            }
            if (flag) {
                c=0.0;
                s=1.0;
                for (i=l; i<=k; i++) {
                    f=s*tmp[i];
                    if (fabs(f)+norma != norma) {
                        g=w[i];
                        h=PYTHAG(f,g);
                        w[i]=h;
                        h=1.0/h;
                        c=g*h;
                        s=(-f*h);
                        for (j=1; j<=3; j++) {
                            y=a[j][nm];
                            z=a[j][i];
                            a[j][nm]=y*c+z*s;
                            a[j][i]=z*c-y*s;
                        }
                    }
                }
            }
            z=w[k];
      // check for negative singular values and adapt them and V accordingly
            if (l == k) {
                if (z < 0.0) {
                    w[k] = -z;
                    for (j=1; j<=3; j++) 
            v[k][j]=(-v[k][j]);
                }
                break;
            }
            if (its == SVD_MAX_ITERATIONS) {
        BIASERR("No convergence in "<<SVD_MAX_ITERATIONS<<" iterations");
        res=-1;
        break;
      }
            x=w[l];
            nm=k-1;
            y=w[nm];
            g=tmp[nm];
            h=tmp[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;
            for (j=l; j<=nm; j++) {
                i=j+1;
                g=tmp[i];
                y=w[i];
                h=s*g;
                g=c*g;
                z=PYTHAG(f,h);
                tmp[j]=z;
                c=f/z;
                s=h/z;
                f=x*c+g*s;
                g=g*c-x*s;
                h=y*s;
                y=y*c;
                for (jj=1; jj<=3; jj++) {
                    x=v[j][jj];
                    z=v[i][jj];
                    v[j][jj]=x*c+z*s;
                    v[i][jj]=z*c-x*s;
                }
                z=PYTHAG(f,h);
                w[j]=z;
                if (z) {
                    z=1.0/z;
                    c=f*z;
                    s=h*z;
                }
                f=(c*g)+(s*y);
                x=(c*y)-(s*g);
                for (jj=1; jj<=3; jj++) {
                    y=a[jj][j];
                    z=a[jj][i];
                    a[jj][j]=y*c+z*s;
                    a[jj][i]=z*c-y*s;
                }
            }
            tmp[l]=0.0;
            tmp[k]=f;
            w[k]=x;
        }
    }

  // now sort singular values
  // we wouldnt need the fabs here if svd always worked correctly,
  // for most reasonable ordering in case of fail, check for sign
  i = (fabs(S_[0]) < fabs(S_[1])) ? 1 : 0;
  if (fabs(S_[i]) < fabs(S_[2]))
    i = 2;
  if (i != 0)
    _Swap(0, i);
  if (fabs(S_[1]) < fabs(S_[2]))
    _Swap(1, 2);

  if (res==0) Decomposed_=true;
  
  return res;
}
#undef SIGN
#undef MAX
#undef PYTHAG