nmrSVDEconomy.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, Anton Deguet
  Created on: 2005-10-18

  (C) Copyright 2005-2014 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 _nmrSVDEconomy_h
#define _nmrSVDEconomy_h

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

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


class nmrSVDEconomyDynamicData {

public:
    typedef unsigned int size_type;

    typedef vctFixedSizeVector<size_type, 2> nsize_type;

protected:
    vctDynamicVector<CISSTNETLIB_DOUBLE> WorkspaceMemory;

    vctDynamicVector<CISSTNETLIB_DOUBLE> OutputMemory;

    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) {
            const size_type minmn = (MMember < NMember) ? MMember : NMember;
            const size_type outputLength = MMember * minmn + NMember * NMember + minmn;
            this->OutputMemory.SetSize(outputLength);
            this->UReference.SetRef(MMember, minmn,
                                    (StorageOrderMember) ? minmn : 1,
                                    (StorageOrderMember) ? 1 : MMember,
                                    this->OutputMemory.Pointer(0));
            this->VtReference.SetRef(NMember, NMember,
                                     (StorageOrderMember) ? NMember : 1,
                                     (StorageOrderMember) ? 1 : NMember,
                                     this->OutputMemory.Pointer(MMember * minmn));
            this->SReference.SetRef(minmn,
                                    this->OutputMemory.Pointer(MMember * minmn + NMember * NMember), 1);
        } else {
            this->OutputMemory.SetSize(0);
        }
        // allocate workspace
        if (allocateWorkspace) {
            this->WorkspaceMemory.SetSize(WorkspaceSize(MMember, NMember));
            this->WorkspaceReference.SetRef(this->WorkspaceMemory);
        } else {
            this->WorkspaceMemory.SetSize(0);
        }
    }


    template <typename _matrixOwnerTypeU,
              typename _vectorOwnerTypeS,
              typename _matrixOwnerTypeVt>
    inline void ThrowUnlessOutputSizeIsCorrect(vctDynamicMatrixBase<_matrixOwnerTypeU, CISSTNETLIB_DOUBLE> & inU,
                                               vctDynamicVectorBase<_vectorOwnerTypeS, CISSTNETLIB_DOUBLE> & inS,
                                               vctDynamicMatrixBase<_matrixOwnerTypeVt, CISSTNETLIB_DOUBLE> & inVt) const
        throw(std::runtime_error)
    {
         // check sizes and storage order
        const size_type minmn = (MMember < NMember) ? MMember : NMember;
        if ((inU.rows() != MMember ) || (inU.cols() != minmn)) {
            cmnThrow(std::runtime_error("nmrSVDEconomyDynamicData: Size of matrix U is incorrect."));
        }
        if (inU.StorageOrder() != StorageOrderMember) {
            cmnThrow(std::runtime_error("nmrSVDEconomyDynamicData: Storage order of U is incorrect."));
        }
        if (!inU.IsCompact()) {
            cmnThrow(std::runtime_error("nmrSVDEconomyDynamicData: Matrix U must be compact."));
        }
        if (! inVt.IsSquare(NMember)) {
            cmnThrow(std::runtime_error("nmrSVDEconomyDynamicData: Size of matrix Vt is incorrect."));
        }
        if (inVt.StorageOrder() != StorageOrderMember) {
            cmnThrow(std::runtime_error("nmrSVDEconomyDynamicData: Storage order of Vt is incorrect."));
        }
        if (!inVt.IsCompact()) {
            cmnThrow(std::runtime_error("nmrSVDEconomyDynamicData: Matrix Vt must be compact."));
        }
        if (minmn != inS.size()) {
            cmnThrow(std::runtime_error("nmrSVDEconomyDynamicData: Size of vector S is incorrect."));
        }
        if (!inS.IsCompact()) {
            cmnThrow(std::runtime_error("nmrSVDEconomyDynamicData: Vector S must be compact."));
        }
    }


    template <typename _vectorOwnerTypeWorkspace>
    inline void
    ThrowUnlessWorkspaceSizeIsCorrect(vctDynamicVectorBase<_vectorOwnerTypeWorkspace, CISSTNETLIB_DOUBLE> & inWorkspace) const
        throw(std::runtime_error)
    {
        const size_type lwork = nmrSVDEconomyDynamicData::WorkspaceSize(MMember, NMember);
        if (lwork > inWorkspace.size()) {
            cmnThrow(std::runtime_error("nmrSVDEconomyDynamicData: Workspace is too small."));
        }
        if (!inWorkspace.IsCompact()) {
            cmnThrow(std::runtime_error("nmrSVDEconomyDynamicData: 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;
        return (lwork_1 > lwork_2) ? lwork_1 : lwork_2;
    }

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


    template <class _matrixOwnerTypeA>
    static inline
    nsize_type MatrixSSize(const vctDynamicConstMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A)
    {
        nsize_type matrixSize(A.rows(), (A.rows() < A.cols()) ? A.row() : A.cols());
        return matrixSize;
    }


    template <class _matrixOwnerTypeA, class _matrixOwnerTypeS, class _vectorOwnerTypeS>
    static inline
    vctDynamicMatrixBase<_matrixOwnerTypeS, CISSTNETLIB_DOUBLE> &
    UpdateMatrixS(const vctDynamicConstMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A,
                  const vctDynamicConstVectorBase<_vectorOwnerTypeS, CISSTNETLIB_DOUBLE> & vectorS,
                  vctDynamicMatrixBase<_matrixOwnerTypeS, CISSTNETLIB_DOUBLE> & matrixS)
        throw(std::runtime_error)
    {
        const size_type minmn = (A.rows() < A.cols()) ? A.rows() : A.cols();
        if ((minmn != matrixS.rows()) || (minmn != matrixS.cols())) {
            cmnThrow(std::runtime_error("nmrSVDEconomyDynamicData::UpdateMatrixS: Size of matrix S is incorrect."));
        }
        matrixS.SetAll(0.0);
        matrixS.Diagonal().Assign(vectorS);
        return matrixS;
    }

#ifndef SWIG
#ifndef DOXYGEN

    class Friend {
    private:
        nmrSVDEconomyDynamicData & Data;
    public:
        Friend(nmrSVDEconomyDynamicData &inData): Data(inData) {
        }
        inline vctDynamicVectorRef<CISSTNETLIB_DOUBLE> & S(void) {
            return Data.SReference;
        }
        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 // SWIG

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

    nmrSVDEconomyDynamicData(size_type m, size_type n, bool storageOrder)
    {
        this->Allocate(m, n, storageOrder);
    }

    template <class _matrixOwnerTypeA>
    nmrSVDEconomyDynamicData(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A)
    {
        this->Allocate(A);
    }

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

    template <typename _matrixOwnerTypeU,
              typename _vectorOwnerTypeS,
              typename _matrixOwnerTypeVt,
              typename _vectorOwnerTypeWorkspace>
    nmrSVDEconomyDynamicData(vctDynamicMatrixBase<_matrixOwnerTypeU, CISSTNETLIB_DOUBLE> & inU,
                      vctDynamicVectorBase<_vectorOwnerTypeS, CISSTNETLIB_DOUBLE> & inS,
                      vctDynamicMatrixBase<_matrixOwnerTypeVt, CISSTNETLIB_DOUBLE> & inVt,
                      vctDynamicVectorBase<_vectorOwnerTypeWorkspace, CISSTNETLIB_DOUBLE> & inWorkspace)
    {
        this->SetRef(inU, inS, inVt, inWorkspace);
    }

    template <typename _matrixOwnerTypeU,
              typename _vectorOwnerTypeS,
              typename _matrixOwnerTypeVt>
    nmrSVDEconomyDynamicData(vctDynamicMatrixBase<_matrixOwnerTypeU, CISSTNETLIB_DOUBLE> & inU,
                      vctDynamicVectorBase<_vectorOwnerTypeS, CISSTNETLIB_DOUBLE> & inS,
                      vctDynamicMatrixBase<_matrixOwnerTypeVt, CISSTNETLIB_DOUBLE> & inVt)
    {
        this->SetRefOutput(inU, inS, inVt);
    }


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

    template <class _matrixOwnerTypeA, class _vectorOwnerTypeWorkspace>
    inline void SetRefWorkspace(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A,
                                vctDynamicVectorBase<_vectorOwnerTypeWorkspace, CISSTNETLIB_DOUBLE> & inWorkspace)
    {
        this->SetDimension(A.rows(), A.cols(), A.StorageOrder());

        // allocate output and set references
        this->AllocateOutputWorkspace(true, false);

        // set reference on user provided workspace
        this->ThrowUnlessWorkspaceSizeIsCorrect(inWorkspace);
        this->WorkspaceReference.SetRef(inWorkspace);
    }

    void Allocate(size_type m, size_type n, bool storageOrder)
    {
        this->SetDimension(m, n, storageOrder);
        this->AllocateOutputWorkspace(true, true);
    }

    template <typename _matrixOwnerTypeU,
              typename _vectorOwnerTypeS,
              typename _matrixOwnerTypeVt,
              typename _vectorOwnerTypeWorkspace>
    void SetRef(vctDynamicMatrixBase<_matrixOwnerTypeU, CISSTNETLIB_DOUBLE> & inU,
                vctDynamicVectorBase<_vectorOwnerTypeS, CISSTNETLIB_DOUBLE> & inS,
                vctDynamicMatrixBase<_matrixOwnerTypeVt, CISSTNETLIB_DOUBLE> & inVt,
                vctDynamicVectorBase<_vectorOwnerTypeWorkspace, CISSTNETLIB_DOUBLE> & inWorkspace)
        throw(std::runtime_error)
    {
        this->SetDimension(inU.rows(), inVt.rows(), inU.StorageOrder());
        this->AllocateOutputWorkspace(false, false);
        this->ThrowUnlessOutputSizeIsCorrect(inU, inS, inVt);
        this->ThrowUnlessWorkspaceSizeIsCorrect(inWorkspace);

        this->SReference.SetRef(inS);
        this->UReference.SetRef(inU);
        this->VtReference.SetRef(inVt);
        this->WorkspaceReference.SetRef(inWorkspace);
    }


    template <typename _matrixOwnerTypeU,
              typename _vectorOwnerTypeS, typename _matrixOwnerTypeVt>
    void SetRefOutput(vctDynamicMatrixBase<_matrixOwnerTypeU, CISSTNETLIB_DOUBLE> & inU,
                      vctDynamicVectorBase<_vectorOwnerTypeS, CISSTNETLIB_DOUBLE> & inS,
                      vctDynamicMatrixBase<_matrixOwnerTypeVt, CISSTNETLIB_DOUBLE> & inVt)
        throw(std::runtime_error)
    {
        this->SetDimension(inU.rows(), inVt.rows(), inU.StorageOrder());
        this->ThrowUnlessOutputSizeIsCorrect(inU, inS, inVt);

        this->SReference.SetRef(inS);
        this->UReference.SetRef(inU);
        this->VtReference.SetRef(inVt);

        AllocateOutputWorkspace(false, true);
    }

    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;
    }
};










template <class _matrixOwnerType>
inline CISSTNETLIB_INTEGER nmrSVDEconomy(vctDynamicMatrixBase<_matrixOwnerType, CISSTNETLIB_DOUBLE> & A,
                                         nmrSVDEconomyDynamicData & data)
    throw (std::runtime_error)
{
    typename nmrSVDEconomyDynamicData::Friend dataFriend(data);
    CISSTNETLIB_INTEGER Info;
    char m_Jobu = 'S';
    char m_Jobvt = 'A';
    CISSTNETLIB_INTEGER m_Lwork = static_cast<CISSTNETLIB_INTEGER>(nmrSVDEconomyDynamicData::WorkspaceSize(dataFriend.M(),
                                                                                                           dataFriend.N()));
    /* check that storage order matches with Allocate() */
    if (A.StorageOrder() != dataFriend.StorageOrder()) {
        cmnThrow(std::runtime_error("nmrSVDEconomy: Storage order used for Allocate was different"));
    }
    /* check sizes */
    if ((dataFriend.M() != A.rows()) || (dataFriend.N() != A.cols())) {
        cmnThrow(std::runtime_error("nmrSVDEconomy: Size used for Allocate was different"));
    }
    /* check that the matrices are compact */
    if (! A.IsCompact()) {
        cmnThrow(std::runtime_error("nmrSVDEconomy: Requires a compact matrix"));
    }

    /* Based on storage order, permute U and Vt as well as dimension */
    CISSTNETLIB_DOUBLE *UPtr, *VtPtr;
    CISSTNETLIB_INTEGER m_Lda, m_Ldu, m_Ldvt;

    if (A.IsColMajor()) {
        m_Lda = (1 > dataFriend.M()) ? 1 : dataFriend.M();
        m_Ldu = dataFriend.M();
        m_Ldvt = dataFriend.N();
        UPtr = dataFriend.U().Pointer();
        VtPtr = dataFriend.Vt().Pointer();
    } else {
        m_Lda = (1 > dataFriend.N()) ? 1 : dataFriend.N();
        m_Ldu = dataFriend.N();
        m_Ldvt = dataFriend.M();
        UPtr = dataFriend.Vt().Pointer();
        VtPtr = dataFriend.U().Pointer();
    }

    // for versions based on gfortran/lapack, CISSTNETLIB_VERSION is
    // defined
#if defined(CISSTNETLIB_VERSION)
#if defined(CISSTNETLIB_VERSION_MAJOR)
#if (CISSTNETLIB_VERSION_MAJOR >= 3)
    cisstNetlib_dgesvd_(&m_Jobu, &m_Jobvt, &m_Ldu, &m_Ldvt,
                        A.Pointer(), &m_Lda, dataFriend.S().Pointer(),
                        UPtr, &m_Ldu,
                        VtPtr, &m_Ldvt,
                        dataFriend.Workspace().Pointer(), &m_Lwork, &Info);
#endif
#else // no major version
    dgesvd_(&m_Jobu, &m_Jobvt, &m_Ldu, &m_Ldvt,
            A.Pointer(), &m_Lda, dataFriend.S().Pointer(),
            UPtr, &m_Ldu,
            VtPtr, &m_Ldvt,
            dataFriend.Workspace().Pointer(), &m_Lwork, &Info);
#endif // CISSTNETLIB_VERSION
#else
   ftnlen jobu_len = (ftnlen)1, jobvt_len = (ftnlen)1;
   la_dzlapack_MP_sgesvd_nat(&m_Jobu, &m_Jobvt, &m_Ldu, &m_Ldvt,
                             A.Pointer(), &m_Lda, dataFriend.S().Pointer(),
                             UPtr, &m_Ldu,
                             VtPtr, &m_Ldvt,
                             dataFriend.Workspace().Pointer(), &m_Lwork, &Info,
                             jobu_len, jobvt_len);
#endif
    return Info;
}

template <class _matrixOwnerTypeA, class _matrixOwnerTypeU,
          class _vectorOwnerTypeS, class _matrixOwnerTypeVt,
          class _vectorOwnerTypeWorkspace>
inline CISSTNETLIB_INTEGER nmrSVDEconomy(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A,
                               vctDynamicMatrixBase<_matrixOwnerTypeU, CISSTNETLIB_DOUBLE> & U,
                               vctDynamicVectorBase<_vectorOwnerTypeS, CISSTNETLIB_DOUBLE> & S,
                               vctDynamicMatrixBase<_matrixOwnerTypeVt, CISSTNETLIB_DOUBLE> & Vt,
                               vctDynamicVectorBase<_vectorOwnerTypeWorkspace, CISSTNETLIB_DOUBLE> & Workspace)
{
    nmrSVDEconomyDynamicData svdData(U, S, Vt, Workspace);
    CISSTNETLIB_INTEGER ret_value = nmrSVDEconomy(A, svdData);
    return ret_value;
}

template <class _matrixOwnerTypeA, class _matrixOwnerTypeU,
          class _vectorOwnerTypeS, class _matrixOwnerTypeVt>
inline CISSTNETLIB_INTEGER nmrSVDEconomy(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A,
                               vctDynamicMatrixBase<_matrixOwnerTypeU, CISSTNETLIB_DOUBLE> & U,
                               vctDynamicVectorBase<_vectorOwnerTypeS, CISSTNETLIB_DOUBLE> & S,
                               vctDynamicMatrixBase<_matrixOwnerTypeVt, CISSTNETLIB_DOUBLE> & Vt)
{
    nmrSVDEconomyDynamicData svdData(U, S, Vt);
    CISSTNETLIB_INTEGER ret_value = nmrSVDEconomy(A, svdData);
    return ret_value;
}




#endif