g_inverse.cpp

Go to the documentation of this file.

//===========================================================================
//===========================================================================

#include <cmath>
#include <SharkDefs.h>
#include <LinAlg/LinAlg.h>

//===========================================================================
unsigned g_inverse
(
    const Array2D< double >& amatA,
    Array2D< double >& bmatA,
    unsigned maxIterations,
    double tolerance,
    bool ignoreThreshold
)
{
    unsigned i, j, k, m, n, r;

    m = amatA.rows();
    n = amatA.cols();

    Array2D< double > umatL(m, n);
    Array2D< double > vmatL(n, n);
    Array  < double > wvecL(n);

    bmatA.resize(n, m, false);

    if (m == 0 || n == 0) {
        return 0;
    }
    //calculate singular value decomposition
    //throw exception if the can't be calculated
    //within maxIterations iterations
    try {
        svd(amatA, umatL, vmatL, wvecL, maxIterations, ignoreThreshold);
    } catch (SharkException e) { 
        throw(e);
    }
    svdsort(umatL, vmatL, wvecL);
    //determine rank
    r = svdrank(amatA, umatL, vmatL, wvecL);
    //normalize singular values
    for (i = 0; i < r; i++) {
        if (wvecL(i) < tolerance)
            wvecL(i) = 0.;
        else
            wvecL(i) = 1. / wvecL(i);
    }
    for (; i < n; i++) {
        wvecL(i) = 0.;
    }

    for (i = 0; i < n; i++) {
        for (j = 0; j < m; j++) {
            double  t  = 0.;
            //calculate (pseudo-)inverse
            for (k = 0; k < n; k++) {
                t += vmatL(i, k) * wvecL(k) * umatL(j, k);
            }

            bmatA(i, j) = t;
        }
    }

    return r;
}

//===========================================================================
//===========================================================================

#include <cmath>
#include <SharkDefs.h>
#include <LinAlg/LinAlg.h>

void invertSymmPositiveDefinite(Array2D<double> &I, const Array2D< double >& ArrSymm) {
    Array2D<double> CholArraySymm, CholArraySymmInv, CholArraySymmInvT;

    CholeskyDecomposition(ArrSymm, CholArraySymm);

    CholArraySymmInv.resize(CholArraySymm, false);
    CholArraySymmInv = 0;

    unsigned m = CholArraySymm.dim(0);
    double s;
    unsigned i, j, k;

    for(j = 0; j < m; j++) 
        CholArraySymmInv(j ,j) = 1/CholArraySymm(j,j);

    for(j = 0; j < m; j++) {
        for(i = j+1; i < m; i++) {
            s = 0;
            for(k = 0; k < i; k++) {
                s += CholArraySymm(i, k) * CholArraySymmInv(k, j);
            }           
            CholArraySymmInv(i ,j) = -1/CholArraySymm(i, i)*s;
        }
    }

    CholArraySymmInvT = CholArraySymmInv;
    CholArraySymmInvT.transpose();
    
    matMat(I, CholArraySymmInvT, CholArraySymmInv);
}


//===========================================================================
void g_inverseCholesky(const Array2D< double >& A, Array2D< double >& outA, double thresholdFactor) {
    SIZE_CHECK(A.ndim() == 2)

    bool bTranspose = false;
    unsigned dimX = A.dim(0);
    unsigned dimY = A.dim(1);

    Array2D< double > AT = Array2D<double>(transpose(A));
    Array2D< double > AMat;

    if (dimX < dimY) {
        bTranspose = true;

        matMat( AMat, A, AT );
        dimY = dimX;
    }
    else 
        matMat( AMat, AT, A );
    

    double minDiagElem = AMat(0, 0);

    for (unsigned i = 1; i<dimY; ++i) 
        if (AMat(i,i) < minDiagElem)
            minDiagElem = AMat(i,i);

    minDiagElem = minDiagElem * thresholdFactor;

    unsigned m = dimY;
    unsigned n = dimY;
    unsigned r = 0;
    unsigned i, k, l;
    double s;


    Array2D< double > C;
    C.resize(m, m, false);
    C=0;

    for(k = 0; k < n; k++) {
        r++;
        for(i = k; i < n; i++) {
            s = 0;
            for(l = 0; l < r-1; l++) 
                s += C(i, l) * C(k, l);
    
            C(i, r-1) = AMat(i, k)  - s;
        }
        if (C(k, r-1)> minDiagElem) {
            C(k, r-1)  = sqrt(C(k, r-1));
            if (k<n)
                for(l = k+1; l < n; l++) 
                    C(l, r-1) =  C(l, r-1) / C(k, r-1);
        }
        else
            r--;
    }

    // delete m-r last cols
    Array2D< double > C2(m, r);

    for (unsigned i = 0; i < m; ++i)
        for (unsigned j = 0; j < r; ++j)
            C2(i,j)  =  C(i,j); 

        
    Array2D< double > ArrSymm, ArrSymmInv;
    Array2D<double> tmpMatrix, tmpMatrix2;

    Array2D< double > C2T = transpose(C2);
    matMat( ArrSymm, C2T, C2 );

#if 0
    invertSymm(ArrSymmInv, ArrSymm);
#else
    invertSymmPositiveDefinite(ArrSymmInv, ArrSymm);
#endif

    matMat( tmpMatrix, ArrSymmInv, ArrSymmInv );
    matMat( tmpMatrix2, tmpMatrix, C2T );
    matMat( tmpMatrix, C2, tmpMatrix2 );

    if (bTranspose) 
        matMat(outA, AT, tmpMatrix);
    else 
        matMat(outA, tmpMatrix, AT);
}

//===========================================================================
void g_inverseMoorePenrose(const Array2D< double >& A, Array2D< double >& outA) {
    SIZE_CHECK(A.ndim() == 2)

    bool bTranspose = false;
    unsigned dimX = A.dim(0);
    unsigned dimY = A.dim(1);

    Array2D< double > AT = Array2D<double>(transpose(A));
    Array2D< double > AMat;

    if (dimX < dimY) {
        bTranspose = true;
        matMat( AMat, A, AT );
        dimY = dimX;
    }
    else 
        matMat( AMat, AT, A );

    Array2D<double> AMatInv;
    invertSymmPositiveDefinite(AMatInv, AMat);

    if (bTranspose) 
        matMat(outA, AT, AMatInv);
    else
        matMat(outA, AMatInv, AT);

}