svd.h

Go to the documentation of this file.

#ifndef SVD_H

#define SVD_H

// Compute the Single Value Decomposition of an arbitrary matrix A
// That is compute the 3 matrices U,W,V with U column orthogonal (m,n) 
// ,W a diagonal matrix and V an orthogonal square matrix s.t. 
// A = U.W.Vt. From this decomposition it is trivial to compute the 
// inverse of A as Ainv = V.Winv.tranpose(U).
//
// s = diagonal elements of W
// work1 = workspace, length must be A.num_rows
// work2 = workspace, length must be A.num_cols

#include "tntmath.h"

#define SVD_MAX_ITER 200

namespace TNT
{

template <class MaTRiX, class VecToR >
void SVD(MaTRiX &A, MaTRiX &U, VecToR &s, MaTRiX &V, VecToR &work1, VecToR &work2, int maxiter=SVD_MAX_ITER) {

    int m = A.num_rows();
    int n = A.num_cols();
    int nu = min(m,n);

    VecToR& work = work1;
    VecToR& e = work2;

    U = 0;
    s = 0;

    int i=0, j=0, k=0;

    // Reduce A to bidiagonal form, storing the diagonal elements
    // in s and the super-diagonal elements in e.

    int nct = min(m-1,n);
    int nrt = max(0,min(n-2,m));
    for (k = 0; k < max(nct,nrt); k++) {
        if (k < nct) {

            // Compute the transformation for the k-th column and
            // place the k-th diagonal in s[k].
            // Compute 2-norm of k-th column without under/overflow.
            s[k] = 0;
            for (i = k; i < m; i++) {
                s[k] = hypot(s[k],A[i][k]);
            }
            if (s[k] != 0.0) {
                if (A[k][k] < 0.0) {
                    s[k] = -s[k];
                }
                for (i = k; i < m; i++) {
                    A[i][k] /= s[k];
                }
                A[k][k] += 1.0;
            }
            s[k] = -s[k];
        }
        for (j = k+1; j < n; j++) {
            if ((k < nct) && (s[k] != 0.0))  {

            // Apply the transformation.

                typename MaTRiX::value_type t = 0;
                for (i = k; i < m; i++) {
                    t += A[i][k]*A[i][j];
                }
                t = -t/A[k][k];
                for (i = k; i < m; i++) {
                    A[i][j] += t*A[i][k];
                }
            }

            // Place the k-th row of A into e for the
            // subsequent calculation of the row transformation.

            e[j] = A[k][j];
        }
        if (k < nct) {

            // Place the transformation in U for subsequent back
            // multiplication.

            for (i = k; i < m; i++)
                U[i][k] = A[i][k];
        }
        if (k < nrt) {

            // Compute the k-th row transformation and place the
            // k-th super-diagonal in e[k].
            // Compute 2-norm without under/overflow.
            e[k] = 0;
            for (i = k+1; i < n; i++) {
                e[k] = hypot(e[k],e[i]);
            }
            if (e[k] != 0.0) {
                if (e[k+1] < 0.0) {
                    e[k] = -e[k];
                }
                for (i = k+1; i < n; i++) {
                    e[i] /= e[k];
                }
                e[k+1] += 1.0;
            }
            e[k] = -e[k];
            if ((k+1 < m) & (e[k] != 0.0)) {

            // Apply the transformation.

                for (i = k+1; i < m; i++) {
                    work[i] = 0.0;
                }
                for (j = k+1; j < n; j++) {
                    for (i = k+1; i < m; i++) {
                        work[i] += e[j]*A[i][j];
                    }
                }
                for (j = k+1; j < n; j++) {
                    typename MaTRiX::value_type t = -e[j]/e[k+1];
                    for (i = k+1; i < m; i++) {
                        A[i][j] += t*work[i];
                    }
                }
            }

            // Place the transformation in V for subsequent
            // back multiplication.

            for (i = k+1; i < n; i++)
                V[i][k] = e[i];
        }
    }

    // Set up the final bidiagonal matrix or order p.

    int p = min(n,m+1);
    if (nct < n) {
        s[nct] = A[nct][nct];
    }
    if (m < p) {
        s[p-1] = 0.0;
    }
    if (nrt+1 < p) {
        e[nrt] = A[nrt][p-1];
    }
    e[p-1] = 0.0;

    // If required, generate U.

    for (j = nct; j < nu; j++) {
        for (i = 0; i < m; i++) {
            U[i][j] = 0.0;
        }
        U[j][j] = 1.0;
    }
    for (k = nct-1; k >= 0; k--) {
        if (s[k] != 0.0) {
            for (j = k+1; j < nu; j++) {
                typename MaTRiX::value_type t = 0;
                for (i = k; i < m; i++) {
                    t += U[i][k]*U[i][j];
                }
                t = -t/U[k][k];
                for (i = k; i < m; i++) {
                    U[i][j] += t*U[i][k];
                }
            }
            for (i = k; i < m; i++ ) {
                U[i][k] = -U[i][k];
            }
            U[k][k] = 1.0 + U[k][k];
            for (i = 0; i < k-1; i++) {
                U[i][k] = 0.0;
            }
        } else {
            for (i = 0; i < m; i++) {
                U[i][k] = 0.0;
            }
            U[k][k] = 1.0;
        }
    }

    // If required, generate V.

    for (k = n-1; k >= 0; k--) {
        if ((k < nrt) & (e[k] != 0.0)) {
            for (j = k+1; j < nu; j++) {
                typename MaTRiX::value_type t = 0;
                for (i = k+1; i < n; i++) {
                    t += V[i][k]*V[i][j];
                }
                t = -t/V[k+1][k];
                for (i = k+1; i < n; i++) {
                    V[i][j] += t*V[i][k];
                }
            }
        }
        for (i = 0; i < n; i++) {
            V[i][k] = 0.0;
        }
        V[k][k] = 1.0;
    }

    // Main iteration loop for the singular values.

    int pp = p-1;
    int iter = 0;
    typename MaTRiX::value_type eps = pow(2.0,-52.0);
    while (p > 0) {
        int kase=0;
        k=0;

        // Test for maximum iterations to avoid infinite loop
        if(maxiter == 0)
            break;
        maxiter--;

        // This section of the program inspects for
        // negligible elements in the s and e arrays.  On
        // completion the variables kase and k are set as follows.

        // kase = 1   if s(p) and e[k-1] are negligible and k<p
        // kase = 2   if s(k) is negligible and k<p
        // kase = 3   if e[k-1] is negligible, k<p, and
        //                s(k), ..., s(p) are not negligible (qr step).
        // kase = 4   if e(p-1) is negligible (convergence).

        for (k = p-2; k >= -1; k--) {
            if (k == -1) {
                break;
            }
            if (TNT::abs(e[k]) <= eps*(TNT::abs(s[k]) + TNT::abs(s[k+1]))) {
                e[k] = 0.0;
                break;
            }
        }
        if (k == p-2) {
            kase = 4;
        } else {
            int ks;
            for (ks = p-1; ks >= k; ks--) {
                if (ks == k) {
                    break;
                }
                typename MaTRiX::value_type t = (ks != p ? TNT::abs(e[ks]) : 0.) + 
                              (ks != k+1 ? TNT::abs(e[ks-1]) : 0.);
                if (TNT::abs(s[ks]) <= eps*t)  {
                    s[ks] = 0.0;
                    break;
                }
            }
            if (ks == k) {
                kase = 3;
            } else if (ks == p-1) {
                kase = 1;
            } else {
                kase = 2;
                k = ks;
            }
        }
        k++;

        // Perform the task indicated by kase.

        switch (kase) {

            // Deflate negligible s(p).

            case 1: {
                typename MaTRiX::value_type f = e[p-2];
                e[p-2] = 0.0;
                for (j = p-2; j >= k; j--) {
                    typename MaTRiX::value_type t = hypot(s[j],f);
                    typename MaTRiX::value_type cs = s[j]/t;
                    typename MaTRiX::value_type sn = f/t;
                    s[j] = t;
                    if (j != k) {
                        f = -sn*e[j-1];
                        e[j-1] = cs*e[j-1];
                    }

                    for (i = 0; i < n; i++) {
                        t = cs*V[i][j] + sn*V[i][p-1];
                        V[i][p-1] = -sn*V[i][j] + cs*V[i][p-1];
                        V[i][j] = t;
                    }
                }
            }
            break;

            // Split at negligible s(k).

            case 2: {
                typename MaTRiX::value_type f = e[k-1];
                e[k-1] = 0.0;
                for (j = k; j < p; j++) {
                    typename MaTRiX::value_type t = hypot(s[j],f);
                    typename MaTRiX::value_type cs = s[j]/t;
                    typename MaTRiX::value_type sn = f/t;
                    s[j] = t;
                    f = -sn*e[j];
                    e[j] = cs*e[j];

                    for (i = 0; i < m; i++) {
                        t = cs*U[i][j] + sn*U[i][k-1];
                        U[i][k-1] = -sn*U[i][j] + cs*U[i][k-1];
                        U[i][j] = t;
                    }
                }
            }
            break;

            // Perform one qr step.

            case 3: {

                // Calculate the shift.

                typename MaTRiX::value_type scale = max(max(max(max(
                          TNT::abs(s[p-1]),TNT::abs(s[p-2])),TNT::abs(e[p-2])), 
                          TNT::abs(s[k])),TNT::abs(e[k]));
                typename MaTRiX::value_type sp = s[p-1]/scale;
                typename MaTRiX::value_type spm1 = s[p-2]/scale;
                typename MaTRiX::value_type epm1 = e[p-2]/scale;
                typename MaTRiX::value_type sk = s[k]/scale;
                typename MaTRiX::value_type ek = e[k]/scale;
                typename MaTRiX::value_type b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)/2.0;
                typename MaTRiX::value_type c = (sp*epm1)*(sp*epm1);
                typename MaTRiX::value_type shift = 0.0;
                if ((b != 0.0) || (c != 0.0)) {
                    shift = sqrt(b*b + c);
                    if (b < 0.0) {
                        shift = -shift;
                    }
                    shift = c/(b + shift);
                }
                typename MaTRiX::value_type f = (sk + sp)*(sk - sp) + shift;
                typename MaTRiX::value_type g = sk*ek;

                // Chase zeros.

                for (j = k; j < p-1; j++) {
                    typename MaTRiX::value_type t = hypot(f,g);
                    /* division by zero checks added to avoid NaN (brecht) */
                    typename MaTRiX::value_type cs = (t == 0.0f)? 0.0f: f/t;
                    typename MaTRiX::value_type sn = (t == 0.0f)? 0.0f: g/t;
                    if (j != k) {
                        e[j-1] = t;
                    }
                    f = cs*s[j] + sn*e[j];
                    e[j] = cs*e[j] - sn*s[j];
                    g = sn*s[j+1];
                    s[j+1] = cs*s[j+1];

                    for (i = 0; i < n; i++) {
                        t = cs*V[i][j] + sn*V[i][j+1];
                        V[i][j+1] = -sn*V[i][j] + cs*V[i][j+1];
                        V[i][j] = t;
                    }

                    t = hypot(f,g);
                    /* division by zero checks added to avoid NaN (brecht) */
                    cs = (t == 0.0f)? 0.0f: f/t;
                    sn = (t == 0.0f)? 0.0f: g/t;
                    s[j] = t;
                    f = cs*e[j] + sn*s[j+1];
                    s[j+1] = -sn*e[j] + cs*s[j+1];
                    g = sn*e[j+1];
                    e[j+1] = cs*e[j+1];
                    if (j < m-1) {
                        for (i = 0; i < m; i++) {
                            t = cs*U[i][j] + sn*U[i][j+1];
                            U[i][j+1] = -sn*U[i][j] + cs*U[i][j+1];
                            U[i][j] = t;
                        }
                    }
                }
                e[p-2] = f;
                iter = iter + 1;
            }
            break;

            // Convergence.

            case 4: {

                // Make the singular values positive.

                if (s[k] <= 0.0) {
                    s[k] = (s[k] < 0.0 ? -s[k] : 0.0);

                    for (i = 0; i <= pp; i++)
                        V[i][k] = -V[i][k];
                }

                // Order the singular values.

                while (k < pp) {
                    if (s[k] >= s[k+1]) {
                        break;
                    }
                    typename MaTRiX::value_type t = s[k];
                    s[k] = s[k+1];
                    s[k+1] = t;
                    if (k < n-1) {
                        for (i = 0; i < n; i++) {
                            t = V[i][k+1]; V[i][k+1] = V[i][k]; V[i][k] = t;
                        }
                    }
                    if (k < m-1) {
                        for (i = 0; i < m; i++) {
                            t = U[i][k+1]; U[i][k+1] = U[i][k]; U[i][k] = t;
                        }
                    }
                    k++;
                }
                iter = 0;
                p--;
            }
            break;
        }
    }
}

}

#endif