nmrPInverse.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-10-18

  (C) Copyright 2005-2015 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 _nmrPInverse_h
#define _nmrPInverse_h

#include <cisstCommon/cmnThrow.h>
#include <cisstVector/vctFixedSizeMatrix.h>
#include <cisstVector/vctDynamicMatrix.h>
#include <cisstNumerical/nmrNetlib.h>
#include <cisstNumerical/nmrSVD.h>

// Always include last
#include <cisstNumerical/nmrExport.h>

/*
   ****************************************************************************
                                  DYNAMIC SIZE
   ****************************************************************************
 */

class nmrPInverseDynamicData {

public:
    typedef vct::size_type size_type;

protected:
    vctDynamicVector<CISSTNETLIB_DOUBLE> WorkspaceMemory;
    vctDynamicVector<CISSTNETLIB_DOUBLE> OutputMemory;
    vctDynamicMatrixRef<CISSTNETLIB_DOUBLE> PInverseReference;
    vctDynamicMatrixRef<CISSTNETLIB_DOUBLE> UReference;
    vctDynamicMatrixRef<CISSTNETLIB_DOUBLE> VtReference;
    vctDynamicVectorRef<CISSTNETLIB_DOUBLE> SReference;
    vctDynamicVectorRef<CISSTNETLIB_DOUBLE> WorkspaceReference;

    size_type MMember;
    size_type NMember;
    bool StorageOrderMember;

    inline void SetDimension(size_type m, size_type n, bool storageOrder)
    {
        StorageOrderMember = storageOrder;
        MMember = m;
        NMember = n;
    }

    inline void AllocateOutputWorkspace(bool allocateOutput, bool allocateWorkspace)
    {
        // allocate output
        if (allocateOutput) {
            OutputMemory.SetSize(MMember * NMember);
            PInverseReference.SetRef(NMember, MMember,
                                     (StorageOrderMember) ? MMember : 1,
                                     (StorageOrderMember) ? 1 : NMember,
                                     OutputMemory.Pointer());
        } else {
            OutputMemory.SetSize(0);
        }
        // allocate workspace
        if (allocateWorkspace) {
            this->WorkspaceMemory.SetSize(WorkspaceSize(MMember, NMember));
            this->SetRefSVD(this->WorkspaceMemory);
        } else {
            this->WorkspaceMemory.SetSize(0);
        }
    }


    template <class _vectorOwnerTypeWorkspace>
    inline void SetRefSVD(vctDynamicVectorBase<_vectorOwnerTypeWorkspace, CISSTNETLIB_DOUBLE> & workspace)
    {
        const size_type minmn = (MMember < NMember) ? MMember : NMember;
        size_type current = 0;
        UReference.SetRef(MMember, MMember,
                          (StorageOrderMember) ? MMember : 1,
                          (StorageOrderMember) ? 1 : MMember,
                          workspace.Pointer(current));
        current += (MMember * MMember);
        VtReference.SetRef(NMember, NMember,
                           (StorageOrderMember) ? NMember : 1,
                           (StorageOrderMember) ? 1 : NMember,
                           workspace.Pointer(current));
        current += (NMember * NMember);
        SReference.SetRef(minmn,
                          workspace.Pointer(current),
                          1);
        current += minmn;
        WorkspaceReference.SetRef(nmrSVDDynamicData::WorkspaceSize(MMember, NMember),
                                  workspace.Pointer(current),
                                  1);
    }



    template <typename _matrixOwnerTypePInverse>
    inline void ThrowUnlessOutputSizeIsCorrect(vctDynamicMatrixBase<_matrixOwnerTypePInverse, CISSTNETLIB_DOUBLE> & pInverse) const
        throw(std::runtime_error)
    {
        // check sizes and storage order
        if ((MMember != pInverse.cols()) || (NMember != pInverse.rows())) {
            cmnThrow(std::runtime_error("nmrPInverseDynamicData: Size of matrix pInverse is incorrect."));
        }
        if (pInverse.StorageOrder() != StorageOrderMember) {
            cmnThrow(std::runtime_error("nmrPInverseDynamicData: Storage order of pInverse is incorrect."));
        }
    }

    template <typename _vectorOwnerTypeWorkspace>
    inline void
    ThrowUnlessWorkspaceSizeIsCorrect(vctDynamicVectorBase<_vectorOwnerTypeWorkspace, CISSTNETLIB_DOUBLE> & workspace) const
        throw(std::runtime_error)
    {
        const size_type lwork = nmrSVDDynamicData::WorkspaceSize(MMember, NMember);
        if (lwork > workspace.size()) {
            cmnThrow(std::runtime_error("nmrPInverseDynamicData: Workspace is too small."));
        }
        if (!workspace.IsCompact()) {
            cmnThrow(std::runtime_error("nmrPInverseDynamicData: Workspace must be compact."));
        }
    }


public:
    static inline size_type WorkspaceSize(size_type m, size_type n)
    {
        const size_type minmn = (m < n) ? m : n;
        const size_type maxmn = (m > n) ? m : n;
        const size_type lwork_1 = 3 * minmn + maxmn;
        const size_type lwork_2 = 5 * minmn;
        const size_type lwork = (lwork_1 > lwork_2) ? lwork_1 : lwork_2;
        //     u       vt      s       workspace   p = s^+ ut
        return m * m + n * n + minmn + lwork + n * m;
    }

    template <class _matrixOwnerTypeA>
    static inline size_type WorkspaceSize(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> &A)
    {
        return nmrPInverseDynamicData::WorkspaceSize(A.rows(), A.cols());
    }

#ifndef SWIG
#ifndef DOXYGEN

    class Friend {
    private:
        nmrPInverseDynamicData & Data;
    public:
        Friend(nmrPInverseDynamicData &data): Data(data) {
        }
        inline vctDynamicVectorRef<CISSTNETLIB_DOUBLE> &S(void) {
            return Data.SReference;
        }
        inline vctDynamicMatrixRef<CISSTNETLIB_DOUBLE> &PInverse(void) {
            return Data.PInverseReference;
        }
        inline vctDynamicMatrixRef<CISSTNETLIB_DOUBLE> &U(void) {
            return Data.UReference;
        }
        inline vctDynamicMatrixRef<CISSTNETLIB_DOUBLE> &Vt(void) {
            return Data.VtReference;
        }
        inline vctDynamicVectorRef<CISSTNETLIB_DOUBLE> &Workspace(void) {
            return Data.WorkspaceReference;
        }
        inline size_type M(void) {
            return Data.MMember;
        }
        inline size_type N(void) {
            return Data.NMember;
        }
        inline bool StorageOrder(void) {
            return Data.StorageOrderMember;
        }
    };
    friend class Friend;
#endif // DOXYGEN
#endif // #ifndef SWIG

    nmrPInverseDynamicData():
        MMember(static_cast<size_type>(0)),
        NMember(static_cast<size_type>(0)),
        StorageOrderMember(VCT_COL_MAJOR)
    {
        AllocateOutputWorkspace(false, false);
    };


    template <class _matrixOwnerTypeA>
    nmrPInverseDynamicData(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> &A)
    {
        this->Allocate(A.rows(), A.cols(), A.StorageOrder());
    }

    template <class _matrixOwnerTypeA, class _vectorOwnerTypeWorkspace>
    nmrPInverseDynamicData(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A,
                           vctDynamicVectorBase<_vectorOwnerTypeWorkspace, CISSTNETLIB_DOUBLE> & workspace)
    {
        this->SetRefWorkspace(A, workspace);
    }

    template <class _matrixOwnerTypeA,
              class _matrixOwnerTypePInverse,
              class _vectorOwnerTypeWorkspace>
    nmrPInverseDynamicData(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & CMN_UNUSED(A),
                           vctDynamicMatrixBase<_matrixOwnerTypePInverse, CISSTNETLIB_DOUBLE> & pInverse,
                           vctDynamicVectorBase<_vectorOwnerTypeWorkspace, CISSTNETLIB_DOUBLE> & workspace)
    {
        this->SetRef(pInverse, workspace);
    }
    template <class _matrixOwnerTypeA, class _matrixOwnerTypePInverse>
    nmrPInverseDynamicData(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & CMN_UNUSED(A),
                           vctDynamicMatrixBase<_matrixOwnerTypePInverse, CISSTNETLIB_DOUBLE> & pInverse)
    {
        this->SetRefOutput(pInverse);
    }


    template <class _matrixOwnerTypeA>
    inline void Allocate(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> &A)
    {
        this->SetDimension(A.rows(), A.cols(), A.StorageOrder());
        this->AllocateOutputWorkspace(true, true);
    }


    template <class _matrixOwnerTypeA, class _vectorOwnerTypeWorkspace>
    inline void SetRefWorkspace(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> &A,
                                vctDynamicVectorBase<_vectorOwnerTypeWorkspace, CISSTNETLIB_DOUBLE> &workspace)
    {
        this->SetDimension(A.rows(), A.cols(), A.StorageOrder());
        this->AllocateOutputWorkspace(true, false);
        // call helper method to set references for SVD components
        this->ThrowUnlessWorkspaceSizeIsCorrect(workspace);
        this->SetRefSVD(workspace);
    }

    template <class _matrixOwnerTypePInverse,
              class _vectorOwnerTypeWorkspace>
    void SetRef(vctDynamicMatrixBase<_matrixOwnerTypePInverse, CISSTNETLIB_DOUBLE> &pInverse,
                vctDynamicVectorBase<_vectorOwnerTypeWorkspace, CISSTNETLIB_DOUBLE> &workspace)
    {
        this->SetDimension(pInverse.cols(), pInverse.rows(), pInverse.StorageOrder());
        this->AllocateOutputWorkspace(false, false);
        // set reference on output
        this->ThrowUnlessOutputSizeIsCorrect(pInverse);
        this->PInverseReference.SetRef(pInverse);
        // set reference on workspace
        this->ThrowUnlessWorkspaceSizeIsCorrect(workspace);
        this->SetRefSVD(workspace);
    }

    template <class _matrixOwnerTypePInverse>
    void SetRefOutput(vctDynamicMatrixBase<_matrixOwnerTypePInverse, CISSTNETLIB_DOUBLE> &pInverse)
    {
        this->SetDimension(pInverse.cols(), pInverse.rows(), pInverse.StorageOrder());
        this->AllocateOutputWorkspace(false, true);
        this->ThrowUnlessOutputSizeIsCorrect(pInverse);
        this->PInverseReference.SetRef(pInverse);
    }



public:
    inline const vctDynamicVectorRef<CISSTNETLIB_DOUBLE> &S(void) const {
        return SReference;
    }
    inline const vctDynamicMatrixRef<CISSTNETLIB_DOUBLE> &U(void) const {
        return UReference;
    }
    inline const vctDynamicMatrixRef<CISSTNETLIB_DOUBLE> &Vt(void) const {
        return VtReference;
    }
    inline const vctDynamicMatrixRef<CISSTNETLIB_DOUBLE> &PInverse(void) const {
        return PInverseReference;
    }
};


#ifndef SWIG
/*
   ****************************************************************************
                                  FIXED SIZE
   ****************************************************************************
 */

template <vct::size_type _rows, vct::size_type _cols, bool _storageOrder>
class nmrPInverseFixedSizeData
{
public:
    typedef vct::size_type size_type;
#ifndef DOXYGEN
    enum {MIN_MN = (_rows < _cols) ? _rows : _cols};
    enum {LWORK_1 = (3 * MIN_MN + (_rows > _cols)) ? _rows : _cols};
    enum {LWORK_2 = 5 * MIN_MN};
    enum {LWORK_3 =
          (static_cast<size_type>(LWORK_1) > static_cast<size_type>(LWORK_2))
          ?
          static_cast<size_type>(LWORK_1)
          :
          static_cast<size_type>(LWORK_2)};
    enum {LWORK = _rows * _rows + _cols * _cols + static_cast<size_type>(MIN_MN) + static_cast<size_type>(LWORK_3) + _rows * _cols};
#endif // DOXYGEN

    typedef vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _rows, _cols, _storageOrder> MatrixTypeA;
    typedef vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _cols, _rows, _storageOrder> MatrixTypePInverse;
    typedef vctFixedSizeVector<CISSTNETLIB_DOUBLE, LWORK> VectorTypeWorkspace;
    typedef vctFixedSizeMatrixRef<CISSTNETLIB_DOUBLE, _rows, _rows, _storageOrder ? _rows : 1, _storageOrder ? 1 : _rows> MatrixTypeU;
    typedef vctFixedSizeMatrixRef<CISSTNETLIB_DOUBLE, _cols, _cols, _storageOrder ? _cols : 1, _storageOrder ? 1 : _cols> MatrixTypeVt;
    typedef vctFixedSizeVectorRef<CISSTNETLIB_DOUBLE, MIN_MN, 1> VectorTypeS;
    typedef vctFixedSizeVectorRef<CISSTNETLIB_DOUBLE, LWORK_3, 1> VectorTypeSVDWorkspace;
    typedef vctFixedSizeMatrixRef<CISSTNETLIB_DOUBLE, _cols, _rows, _storageOrder? _cols : 1, _storageOrder ? 1 : _rows> MatrixTypeP;
private:
    MatrixTypePInverse PInverseMember;

    VectorTypeWorkspace WorkspaceMember;

    MatrixTypeU UReference;
    MatrixTypeVt VtReference;
    VectorTypeS SReference;
    VectorTypeSVDWorkspace SVDWorkspaceReference;
    MatrixTypeP PReference;
    //*}

public:

#ifndef DOXYGEN
    /* This class is not intended to be a top-level API.  It has been
      provided to avoid making the templated PInverse function as a
      friend of this class, which turns out to be not so easy with
      Visual C++ 7. Instead the Friend class provides a cumbersome way
      to get non-const references to the private data.  In order to
      get non-const references the user has to first create a object
      of nmrPInverseFixedSizeData::Friend and then user get* method on
      that object. Our philosophy here is that this should be deterent
      for a general user and should ring alarm bells in a reasonable
      programmer.
    */
    class Friend {
    private:
        nmrPInverseFixedSizeData<_rows, _cols, _storageOrder> & Data;
    public:
        Friend(nmrPInverseFixedSizeData<_rows, _cols, _storageOrder> &data): Data(data) {
        }
        inline MatrixTypePInverse & PInverse(void) {
            return Data.PInverseMember;
        }
        inline VectorTypeS & S(void) {
            return Data.SReference;
        }
        inline MatrixTypeU & U(void) {
            return Data.UReference;
        }
        inline MatrixTypeVt & Vt(void) {
            return Data.VtReference;
        }
        inline VectorTypeWorkspace & Workspace(void) {
            return Data.WorkspaceMember;
        }
        inline MatrixTypeP & P(void) {
            return Data.PReference;
        }
    };
    friend class Friend;
#endif

    nmrPInverseFixedSizeData():
        UReference(WorkspaceMember.Pointer(0)),
        VtReference(WorkspaceMember.Pointer(_rows * _rows)),
        SReference(WorkspaceMember.Pointer(_rows * _rows + _cols * _cols)),
        SVDWorkspaceReference(WorkspaceMember.Pointer(_rows * _rows + _cols * _cols + MIN_MN)),
        PReference(WorkspaceMember.Pointer(_rows * _rows + _cols * _cols + MIN_MN + LWORK_3))
    {};

    inline const MatrixTypePInverse & PInverse(void) const {
        return PInverseMember;
    }
    inline const VectorTypeS & S(void) const {
        return SReference;
    }
    inline const MatrixTypeU & U(void) const {
        return UReference;
    }
    inline const MatrixTypeVt & Vt(void) const {
        return VtReference;
    }
    inline const MatrixTypeP & P(void) const {
        return PReference;
    }
};
#endif  // #ifndef SWIG



template <class _matrixOwnerType>
inline CISSTNETLIB_INTEGER nmrPInverse(vctDynamicMatrixBase<_matrixOwnerType, CISSTNETLIB_DOUBLE> &A, nmrPInverseDynamicData &data) throw (std::runtime_error)
{
    typedef vct::size_type size_type;

    typename nmrPInverseDynamicData::Friend dataFriend(data);
    CISSTNETLIB_INTEGER ret_value;
    /* check that the size and storage order matches with Allocate() */
    if (A.StorageOrder() != dataFriend.StorageOrder()) {
        cmnThrow(std::runtime_error("nmrPInverse Solve: Storage order used for Allocate was different"));
    }
    if ((A.rows() != dataFriend.M()) || (A.cols() != dataFriend.N())) {
        cmnThrow(std::runtime_error("nmrPInverse Solve: Size used for Allocate was different"));
    }
    const size_type rows = A.rows();
    const size_type cols = A.cols();
    const size_type minmn = (rows < cols) ? rows : cols;
    const size_type maxmn = (rows > cols) ? rows : cols;

    ret_value = nmrSVD(A, dataFriend.U(), dataFriend.S(),
                       dataFriend.Vt(), dataFriend.Workspace());
    const CISSTNETLIB_DOUBLE eps = cmnTypeTraits<CISSTNETLIB_DOUBLE>::Tolerance() * dataFriend.S().at(0) * maxmn;

    dataFriend.PInverse().SetAll(0);
    CISSTNETLIB_DOUBLE singularValue;
    size_type irank, i, j;
    for (irank = 0; irank < minmn; irank++) {
        if ((singularValue = dataFriend.S().at(irank)) > eps) {
            for (j = 0; j < rows; j++) {
                for (i = 0; i < cols; i++) {
                    dataFriend.PInverse().at(i, j) = dataFriend.PInverse().at(i, j)
                        + dataFriend.Vt().at(irank, i) * dataFriend.U().at(j, irank) / singularValue;
                }
            }
        }
    }
    return ret_value;
}


template <class _matrixOwnerTypeA, class _matrixOwnerTypePInverse, class _vectorOwnerTypeWorkspace>
inline CISSTNETLIB_INTEGER nmrPInverse(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> &A,
                             vctDynamicMatrixBase<_matrixOwnerTypePInverse, CISSTNETLIB_DOUBLE> &PInverse, vctDynamicVectorBase<_vectorOwnerTypeWorkspace, CISSTNETLIB_DOUBLE> &Workspace)
{
    nmrPInverseDynamicData svdData(A, PInverse, Workspace);
    CISSTNETLIB_INTEGER ret_value = nmrPInverse(A, svdData);
    return ret_value;
}


template <class _matrixOwnerTypeA, class _matrixOwnerTypePInverse>
inline CISSTNETLIB_INTEGER nmrPInverse(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> &A,
                             vctDynamicMatrixBase<_matrixOwnerTypePInverse, CISSTNETLIB_DOUBLE> &PInverse)
{
    nmrPInverseDynamicData svdData(A, PInverse);
    CISSTNETLIB_INTEGER ret_value = nmrPInverse(A, svdData);
    return ret_value;
}


#ifndef SWIG

template <vct::size_type _rows, vct::size_type _cols, vct::size_type _work, bool _storageOrder, class _dataPtrType>
inline CISSTNETLIB_INTEGER nmrPInverse(vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _rows, _cols, _storageOrder> & A,
                             vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _cols, _rows, _storageOrder> & pInverse,
                             vctFixedSizeVectorBase<_work, 1, CISSTNETLIB_DOUBLE, _dataPtrType> & workspace)
{
    typedef vct::size_type size_type;
    const size_type lwork = nmrPInverseFixedSizeData<_rows, _cols, _storageOrder>::LWORK;
    const size_type lwork_3 = nmrPInverseFixedSizeData<_rows, _cols, _storageOrder>::LWORK_3;
    const size_type minmn = nmrPInverseFixedSizeData<_rows, _cols, _storageOrder>::MIN_MN;
    const size_type maxmn = (_rows > _cols) ? _rows : _cols;
    //Assert if requirement is greater than size provided!
    CMN_ASSERT(lwork <= _work);
    // split Work into submatrices and vectors
    // for this we simply use the nmrSVD with dynamic ref as parameters as we dont have
    // nmrSVD with fixedsizeref as parameters
    vctDynamicMatrixRef<CISSTNETLIB_DOUBLE> ARef(A);
    vctDynamicMatrixRef<CISSTNETLIB_DOUBLE> URef(_rows, _rows,
                                     (_storageOrder) ? _rows : 1,
                                     (_storageOrder) ? 1 : _rows,
                                     workspace.Pointer(0));
    vctDynamicMatrixRef<CISSTNETLIB_DOUBLE> VtRef(_cols, _cols,
                                      (_storageOrder) ? _cols : 1,
                                      (_storageOrder) ? 1 : _cols,
                                      workspace.Pointer(_rows*_rows));
    vctDynamicVectorRef<CISSTNETLIB_DOUBLE> SRef(minmn,
                                     workspace.Pointer(_rows * _rows + _cols * _cols));
    vctDynamicVectorRef<CISSTNETLIB_DOUBLE> SVDWorkspaceRef(lwork_3,
                                                workspace.Pointer(_rows * _rows + _cols * _cols + minmn));
    vctDynamicMatrixRef<CISSTNETLIB_DOUBLE> PRef(_cols, _rows, (_storageOrder) ? _rows : 1, (_storageOrder) ? 1 : _cols,
                                     workspace.Pointer(_rows * _rows + _cols * _cols + minmn + lwork_3));
    vctDynamicMatrixRef<CISSTNETLIB_DOUBLE> pInverseRef(pInverse);
    CISSTNETLIB_INTEGER ret_value;
    ret_value = nmrSVD(ARef, URef, SRef, VtRef, SVDWorkspaceRef);
    const CISSTNETLIB_DOUBLE eps = cmnTypeTraits<CISSTNETLIB_DOUBLE>::Tolerance() * SRef(0) * maxmn;
    /*
    vctDynamicMatrix<CISSTNETLIB_DOUBLE> SM (_cols, _rows, _storageOrder);
    SM.SetAll(0.);
    for (size_type irank = 0; irank < minmn; irank++) {
        if (SRef(irank) > eps) {
            SM(irank, irank) = 1.0/SRef(irank);
        }
    }
    PRef.ProductOf(SM, URef.Transpose());
    */
    size_type irank;
    PRef.SetAll(0.);
    for (irank = 0; irank < minmn; irank++) {
        if (SRef(irank) > eps) {
            PRef.Row(irank).ProductOf(URef.Transpose().Row(irank), 1.0/SRef(irank));
        } else {
            PRef.Row(irank).SetAll(0.);
        }
    }
    pInverseRef.ProductOf(VtRef.Transpose(), PRef);

    return ret_value;
}

template <vct::size_type _rows, vct::size_type _cols, bool _storageOrder>
inline CISSTNETLIB_INTEGER nmrPInverse(vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _rows, _cols, _storageOrder> &A,
                             vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _cols, _rows, _storageOrder> &pInverse)
{
    typename nmrPInverseFixedSizeData<_rows, _cols, _storageOrder>::VectorTypeWorkspace workspace;
    CISSTNETLIB_INTEGER ret_value = nmrPInverse(A, pInverse, workspace);
    return ret_value;
}

template <vct::size_type _rows, vct::size_type _cols, bool _storageOrder>
inline CISSTNETLIB_INTEGER nmrPInverse(vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _rows, _cols, _storageOrder> &A,
                             nmrPInverseFixedSizeData<_rows, _cols, _storageOrder> &data)
{
    typename nmrPInverseFixedSizeData<_rows, _cols, _storageOrder>::Friend dataFriend(data);
    /* all the checking is done by SVD */
    CISSTNETLIB_INTEGER ret_value = nmrPInverse(A, dataFriend.PInverse(), dataFriend.Workspace());
    return ret_value;
}
#endif  // #ifndef SWIG



#endif