nmrPInverseSolver.h

Go to the documentation of this file.

/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*-    */
/* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */

/*
  
  Author(s):    Ankur Kapoor
  Created on: 2005-07-29

  (C) Copyright 2005-2007 Johns Hopkins University (JHU), All Rights
  Reserved.

--- begin cisst license - do not edit ---

This software is provided "as is" under an open source license, with
no warranty.  The complete license can be found in license.txt and
http://www.cisst.org/cisst/license.txt.

--- end cisst license ---
*/


#ifndef _nmrPInverseSolver_h
#define _nmrPInverseSolver_h

#include <cisstVector/vctDynamicMatrix.h>
#include <cisstNumerical/nmrSVDSolver.h>


class nmrPInverseSolver {
    // we have this class so that we reserve memory only one
    // would help if svd of a same size matrix (or a matrix)
    // that doesnt change much is desired.

protected:
    CISSTNETLIB_INTEGER M;
    CISSTNETLIB_INTEGER N;
    CISSTNETLIB_INTEGER MaxMN;
    CISSTNETLIB_INTEGER MinMN;
    CISSTNETLIB_DOUBLE EPS;
    vctDynamicMatrix<CISSTNETLIB_DOUBLE> S;
    vctDynamicMatrix<CISSTNETLIB_DOUBLE> U;
    vctDynamicMatrix<CISSTNETLIB_DOUBLE> V;
    vctDynamicMatrix<CISSTNETLIB_DOUBLE> PInverseA;
    bool StorageOrder;

#ifdef CISST_COMPILER_IS_MSVC
#pragma warning (push)
#pragma warning (disable: 4996)
#endif // CISST_COMPILER_IS_MSVC

    nmrSVDSolver svd;

#ifdef CISST_COMPILER_IS_MSVC
#pragma warning (pop)
#endif // CISST_COMPILER_IS_MSVC


public:
    nmrPInverseSolver(void):
        M(0),
        N(0),
        StorageOrder(VCT_COL_MAJOR)
    {
        Allocate(M, N, StorageOrder);
    }


    nmrPInverseSolver(CISSTNETLIB_INTEGER m, CISSTNETLIB_INTEGER n, bool storageOrder = VCT_COL_MAJOR) {
        Allocate(m, n, storageOrder);
    }

    
    nmrPInverseSolver(const vctDynamicMatrix<CISSTNETLIB_DOUBLE> &A) {
        Allocate(A);
    }


    inline void Allocate(CISSTNETLIB_INTEGER m, CISSTNETLIB_INTEGER n, bool storageOrder) {
        StorageOrder = storageOrder;
        M = m;
        N = n;
        MaxMN = std::max(M, N);
        MinMN = std::min(M, N);
        svd.Allocate(M, N, storageOrder);
        S.SetSize(MinMN, 1, storageOrder);
        U.SetSize(M, M, storageOrder);
        V.SetSize(N, N, storageOrder);
        PInverseA.SetSize(N, M, storageOrder);
        /* compute machine precision */
        EPS = 1.0;
        do {
            EPS = EPS / 2.0;
        } while (EPS + 1.0 != 1.0);
        EPS = EPS * 2.0;
    }


    inline void Allocate(const vctDynamicMatrix<CISSTNETLIB_DOUBLE> &A) {
        Allocate(A.rows(), A.cols(), A.IsRowMajor());
    }


    template <class _matrixOwnerType>
    inline void Solve(vctDynamicMatrixBase<_matrixOwnerType, CISSTNETLIB_DOUBLE> &A) 
        throw (std::runtime_error) {
        /* all the checking is done by SVD */
        svd.Solve(A);
        S.Assign(svd.GetS());
        U.Assign(svd.GetU());
        V.Assign(svd.GetVt().Transpose());
        CISSTNETLIB_DOUBLE eps = EPS * svd.GetS().at(0, 0);
        PInverseA.SetAll(0);
        CISSTNETLIB_DOUBLE singularValue;
        for (int irank = 0; irank < MinMN; irank++) {
            if ((singularValue = S(irank, 0)) > eps) {
                for (int j = 0; j < M; j++) {
                    for (int i = 0; i < N; i++) {
                        PInverseA(i, j) = PInverseA(i, j)
                            + V(i, irank)*U(j, irank)/singularValue;
                    }
                }
            }
        }
    }
    
    inline const vctDynamicMatrix<CISSTNETLIB_DOUBLE> &GetS(void) const {
        return S;
    }

    inline const vctDynamicMatrix<CISSTNETLIB_DOUBLE> &GetU(void) const {
        return U;
    }

    inline const vctDynamicMatrix<CISSTNETLIB_DOUBLE> &GetV(void) const {
        return V;
    }

    inline const vctDynamicMatrix<CISSTNETLIB_DOUBLE> &GetPInverse(void) const {
        return PInverseA;
    }
};


#ifdef CISST_COMPILER_IS_MSVC
class CISST_DEPRECATED nmrPInverseSolver;
#endif // CISST_COMPILER_IS_MSVC


#endif // _nmrPInverseSolver_h