CSparseMatrix.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */
#ifndef CSparseMatrix_H
#define CSparseMatrix_H

#include <mrpt/utils/utils_defs.h>
#include <mrpt/utils/exceptions.h>
#include <mrpt/utils/CUncopiable.h>

#include <mrpt/math/math_frwds.h>
#include <mrpt/utils/types_math.h>
#include <mrpt/math/CSparseMatrixTemplate.h>
#include <mrpt/math/CMatrixTemplateNumeric.h>
#include <mrpt/math/CMatrixFixedNumeric.h>
#include <cstring> // memcpy

// Include CSparse lib headers, either from the system or embedded:
extern "C"{
#if MRPT_HAS_SUITESPARSE
#       define NCOMPLEX    // In MRPT we don't need complex numbers, so avoid the annoying warning: 'cs_ci_house' has C-linkage specified, but returns UDT 'std::complex<double>' which is incompatible with C
#       include "cs.h"
#else
#       include <mrpt/otherlibs/CSparse/cs.h>
#endif
}

namespace mrpt
{
        namespace math
        {
                /** Used in mrpt::math::CSparseMatrix */
                class CExceptionNotDefPos : public mrpt::utils::CMRPTException
                {
                public:
                        CExceptionNotDefPos(const std::string &s) : CMRPTException(s) {  }
                };


                /** A sparse matrix structure, wrapping T. Davis' CSparse library (part of suitesparse)
                  *  The type of the matrix entries is fixed to "double".
                  *
                  *  There are two formats for the non-zero entries in this matrix:
                  *             - A "triplet" matrix: a set of (r,c)=val triplet entries.
                  *             - A column-compressed sparse (CCS) matrix.
                  *
                  *  The latter is the "normal" format, which is expected by all mathematical operations defined
                  *   in this class. There're three ways of initializing and populating a sparse matrix:
                  *
                  *   <ol>
                  *    <li> <b>As a triplet (empty), then add entries, then compress:</b>
                  *         \code
                  *             CSparseMatrix  SM(100,100);
                  *             SM.insert_entry(i,j, val);  // or
                  *             SM.insert_submatrix(i,j, MAT); //  ...
                  *             SM.compressFromTriplet();
                  *         \endcode
                  *    </li>
                  *    <li> <b>As a triplet from a CSparseMatrixTemplate<double>:</b>
                  *         \code
                  *             CSparseMatrixTemplate<double>  data;
                  *             data(row,col) = val;
                  *             ...
                  *             CSparseMatrix  SM(data);
                  *         \endcode
                  *    </li>
                  *    <li> <b>From an existing dense matrix:</b>
                  *         \code
                  *             CMatrixDouble data(100,100); // or
                  *             CMatrixFloat  data(100,100); // or
                  *             CMatrixFixedNumeric<double,4,6>  data; // etc...
                  *             CSparseMatrix  SM(data);
                  *         \endcode
                  *    </li>
                  *
                  *   </ol>
                  *
                  *  Due to its practical utility, there is a special inner class CSparseMatrix::CholeskyDecomp to handle Cholesky-related methods and data.
                  *
                  * \note This class was initially adapted from "robotvision", by Hauke Strasdat, Steven Lovegrove and Andrew J. Davison. See http://www.openslam.org/robotvision.html
                  * \note CSparse is maintained by Timothy Davis: http://people.sc.fsu.edu/~jburkardt/c_src/csparse/csparse.html .
                  * \note See also his book "Direct methods for sparse linear systems". http://books.google.es/books?id=TvwiyF8vy3EC&pg=PA12&lpg=PA12&dq=cs_compress&source=bl&ots=od9uGJ793j&sig=Wa-fBk4sZkZv3Y0Op8FNH8PvCUs&hl=es&ei=UjA0TJf-EoSmsQay3aXPAw&sa=X&oi=book_result&ct=result&resnum=8&ved=0CEQQ6AEwBw#v=onepage&q&f=false
                  *
                  * \sa mrpt::math::MatrixBlockSparseCols, mrpt::math::CMatrixFixedNumeric, mrpt::math::CMatrixTemplateNumeric, etc.
                  * \ingroup mrpt_base_grp
                  */
                class BASE_IMPEXP CSparseMatrix
                {
                private:
                        cs sparse_matrix;

                        /** Initialization from a dense matrix of any kind existing in MRPT. */
                        template <class MATRIX>
                        void construct_from_mrpt_mat(const MATRIX & C)
                        {
                                std::vector<int> row_list, col_list;  // Use "int" for convenience so we can do a memcpy below...
                                std::vector<double> content_list;
                                const int nCol = C.getColCount();
                                const int nRow = C.getRowCount();
                                for (int c=0; c<nCol; ++c)
                                {
                                        col_list.push_back(row_list.size());
                                        for (int r=0; r<nRow; ++r)
                                                if (C.get_unsafe(r,c)!=0)
                                                {
                                                        row_list.push_back(r);
                                                        content_list.push_back(C(r,c));
                                                }
                                }
                                col_list.push_back(row_list.size());

                                sparse_matrix.m = nRow;
                                sparse_matrix.n = nCol;
                                sparse_matrix.nzmax = content_list.size();
                                sparse_matrix.i = (int*)malloc(sizeof(int)*row_list.size());
                                sparse_matrix.p = (int*)malloc(sizeof(int)*col_list.size());
                                sparse_matrix.x = (double*)malloc(sizeof(double)*content_list.size());

                                ::memcpy(sparse_matrix.i, &row_list[0], sizeof(row_list[0])*row_list.size() );
                                ::memcpy(sparse_matrix.p, &col_list[0], sizeof(col_list[0])*col_list.size() );
                                ::memcpy(sparse_matrix.x, &content_list[0], sizeof(content_list[0])*content_list.size() );

                                sparse_matrix.nz = -1;  // <0 means "column compressed", ">=0" means triplet.
                        }

                        /** Initialization from a triplet "cs", which is first compressed */
                        void construct_from_triplet(const cs & triplet);

                        /** To be called by constructors only, assume previous pointers are trash and overwrite them */
                        void construct_from_existing_cs(const cs &sm);

                        /** free buffers (deallocate the memory of the i,p,x buffers) */
                        void internal_free_mem();

                        /** Copy the data from an existing "cs" CSparse data structure */
                        void  copy(const cs  * const sm);

                        /** Fast copy the data from an existing "cs" CSparse data structure, copying the pointers and leaving NULLs in the source structure. */
                        void  copy_fast(cs  * const sm);

                public:

                        /** @name Constructors, destructor & copy operations
                            @{  */

                        /** Create an initially empty sparse matrix, in the "triplet" form.
                          *  Notice that you must call "compressFromTriplet" after populating the matrix and before using the math operatons on this matrix.
                          *  The initial size can be later on extended with insert_entry() or setRowCount() & setColCount().
                          * \sa insert_entry, setRowCount, setColCount
                          */
                        CSparseMatrix(const size_t nRows=0, const size_t nCols=0);

                        /** A good way to initialize a sparse matrix from a list of non NULL elements.
                          *  This constructor takes all the non-zero entries in "data" and builds a column-compressed sparse representation.
                          */
                        template <typename T>
                        inline CSparseMatrix(const CSparseMatrixTemplate<T> & data)
                        {
                                ASSERTMSG_(!data.empty(), "Input data must contain at least one non-zero element.")
                                sparse_matrix.i = NULL; // This is to know they shouldn't be tried to free()
                                sparse_matrix.p = NULL;
                                sparse_matrix.x = NULL;

                                // 1) Create triplet matrix
                                CSparseMatrix  triplet(data.getRowCount(),data.getColCount());
                                // 2) Put data in:
                                for (typename CSparseMatrixTemplate<T>::const_iterator it=data.begin();it!=data.end();++it)
                                        triplet.insert_entry_fast(it->first.first,it->first.second, it->second);

                                // 3) Compress:
                                construct_from_triplet(triplet.sparse_matrix);
                        }


                        // We can't do a simple "template <class ANYMATRIX>" since it would be tried to match against "cs*"...

                        /** Constructor from a dense matrix of any kind existing in MRPT, creating a "column-compressed" sparse matrix. */
                        template <typename T,size_t N,size_t M> inline explicit CSparseMatrix(const CMatrixFixedNumeric<T,N,M> &MAT) { construct_from_mrpt_mat(MAT); }

                        /** Constructor from a dense matrix of any kind existing in MRPT, creating a "column-compressed" sparse matrix. */
                        template <typename T>  inline explicit CSparseMatrix(const CMatrixTemplateNumeric<T> &MAT) { construct_from_mrpt_mat(MAT); }

                        /** Copy constructor */
                        CSparseMatrix(const CSparseMatrix & other);

                        /** Copy constructor from an existing "cs" CSparse data structure */
                        explicit CSparseMatrix(const cs  * const sm);

                        /** Destructor */
                        virtual ~CSparseMatrix();

                        /** Copy operator from another existing object */
                        void operator = (const CSparseMatrix & other);

                        /** Fast swap contents with another sparse matrix */
                        void swap(CSparseMatrix & other);

                        /** Erase all previous contents and leave the matrix as a "triplet" ROWS x COLS matrix without any nonzero entry. */
                        void clear(const size_t nRows=1, const size_t nCols=1);

                        /** @}  */

                        /** @name Math operations (the interesting stuff...)
                                @{ */

                        void add_AB(const CSparseMatrix & A,const CSparseMatrix & B); //!< this = A+B
                        void multiply_AB(const CSparseMatrix & A,const CSparseMatrix & B); //!< this = A*B
                        void multiply_Ab(const mrpt::math::CVectorDouble &b, mrpt::math::CVectorDouble &out_res) const; //!< out_res = this * b

                        inline CSparseMatrix operator + (const CSparseMatrix & other) const
                        {
                                CSparseMatrix RES;
                                RES.add_AB(*this,other);
                                return RES;
                        }
                        inline CSparseMatrix operator * (const CSparseMatrix & other) const
                        {
                                CSparseMatrix RES;
                                RES.multiply_AB(*this,other);
                                return RES;
                        }
                        inline mrpt::math::CVectorDouble operator * (const mrpt::math::CVectorDouble & other) const {
                                mrpt::math::CVectorDouble res;
                                multiply_Ab(other,res);
                                return res;
                        }
                        inline void operator += (const CSparseMatrix & other) {
                                this->add_AB(*this,other);
                        }
                        inline void operator *= (const CSparseMatrix & other) {
                                this->multiply_AB(*this,other);
                        }
                        CSparseMatrix transpose() const;

                        /** @}  */


                        /** @ Access the matrix, get, set elements, etc.
                            @{ */

                        /** ONLY for TRIPLET matrices: insert a new non-zero entry in the matrix.
                          *  This method cannot be used once the matrix is in column-compressed form.
                          *  The size of the matrix will be automatically extended if the indices are out of the current limits.
                          * \sa isTriplet, compressFromTriplet
                          */
                        void insert_entry(const size_t row, const size_t col, const double val );

                        /** This was an optimized version, but is now equivalent to insert_entry() due to the need to be compatible with unmodified CSparse system libraries. */
                        inline void insert_entry_fast(const size_t row, const size_t col, const double val )  { insert_entry(row,col,val); }

                        /** ONLY for TRIPLET matrices: insert a given matrix (in any of the MRPT formats) at a given location of the sparse matrix.
                          *  This method cannot be used once the matrix is in column-compressed form.
                          *  The size of the matrix will be automatically extended if the indices are out of the current limits.
                          *  Since this is inline, it can be very efficient for fixed-size MRPT matrices.
                          * \sa isTriplet, compressFromTriplet, insert_entry
                          */
                        template <class MATRIX>
                        inline void insert_submatrix(const size_t row, const size_t col, const MATRIX &M )
                        {
                                if (!isTriplet()) THROW_EXCEPTION("insert_entry() is only available for sparse matrix in 'triplet' format.")
                                const size_t nR = M.getRowCount();
                                const size_t nC = M.getColCount();
                                for (size_t r=0;r<nR;r++)
                                        for (size_t c=0;c<nC;c++)
                                                insert_entry_fast(row+r,col+c, M.get_unsafe(r,c));
                                // If needed, extend the size of the matrix:
                                sparse_matrix.m = std::max(sparse_matrix.m, int(row+nR));
                                sparse_matrix.n = std::max(sparse_matrix.n, int(col+nC));
                        }


                        /** ONLY for TRIPLET matrices: convert the matrix in a column-compressed form.
                          * \sa insert_entry
                          */
                        void compressFromTriplet();

                        /** Return a dense representation of the sparse matrix.
                          * \sa saveToTextFile_dense
                          */
                        void get_dense(CMatrixDouble &outMat) const;

                        /** Static method to convert a "cs" structure into a dense representation of the sparse matrix.
                          */
                        static void cs2dense(const cs& SM, CMatrixDouble &outMat);

                        /** save as a dense matrix to a text file \return False on any error.
                          */
                        bool saveToTextFile_dense(const std::string &filName);

                        /** Save sparse structure to a text file loadable from MATLAB (can be called on triplet or CCS matrices).
                          *
                          *  The format of the text file is:
                          *  \code
                          *   NUM_ROWS   NUM_COLS   NUM_NON_ZERO_MAX
                          *   row_1   col_1   value_1
                          *   row_2   col_2   value_2
                          *   ...
                          *  \endcode
                          *
                          *  Instructions for loading from MATLAB in triplet form will be automatically writen to the
                          *  output file as comments in th first lines:
                          *
                          *   \code
                          *      D=load('file.txt');
                          *      SM=spconvert(D(2:end,:));
                          *        or, to always preserve the actual matrix size m x n:
                 *      m=D(1,1); n=D(1,2); nzmax=D(1,3);
                          *      Di=D(2:end,1); Dj=D(2:end,2); Ds=D(2:end,3);
                          *      M=sparse(Di,Dj,Ds, m,n, nzmax);
                          *   \endcode
                          * \return False on any error.
                          */
                        bool saveToTextFile_sparse(const std::string &filName);

                        // Very basic, standard methods that MRPT methods expect for any matrix:
                        inline size_t getRowCount() const { return sparse_matrix.m; }
                        inline size_t getColCount() const { return sparse_matrix.n; }

                        /** Change the number of rows in the matrix (can't be lower than current size) */
                        inline void setRowCount(const size_t nRows) { ASSERT_(nRows>=(size_t)sparse_matrix.m); sparse_matrix.m = nRows; }
                        inline void setColCount(const size_t nCols) { ASSERT_(nCols>=(size_t)sparse_matrix.n); sparse_matrix.n = nCols; }

                        /** Returns true if this sparse matrix is in "triplet" form. \sa isColumnCompressed */
                        inline bool isTriplet() const { return sparse_matrix.nz>=0; }  // <0 means "column compressed", ">=0" means triplet.

                        /** Returns true if this sparse matrix is in "column compressed" form. \sa isTriplet */
                        inline bool isColumnCompressed() const { return sparse_matrix.nz<0; }  // <0 means "column compressed", ">=0" means triplet.

                        /** @} */


                        /** @name Cholesky factorization
                            @{  */

                        /** Auxiliary class to hold the results of a Cholesky factorization of a sparse matrix.
                          *  This implementation does not allow updating/downdating.
                          *
                          *  Usage example:
                          *   \code
                          *     CSparseMatrix  SM(100,100);
                          *     SM.insert_entry(i,j, val); ...
                          *     SM.compressFromTriplet();
                          *
                          *     // Do Cholesky decomposition:
                          *             CSparseMatrix::CholeskyDecomp  CD(SM);
                          *     CD.get_inverse();
                          *     ...
                          *   \endcode
                          *
                          * \note Only the upper triangular part of the input matrix is accessed.
                          * \note This class was initially adapted from "robotvision", by Hauke Strasdat, Steven Lovegrove and Andrew J. Davison. See http://www.openslam.org/robotvision.html
                          * \note This class designed to be "uncopiable".
                          * \sa The main class: CSparseMatrix
                          */
                        class BASE_IMPEXP CholeskyDecomp  : public mrpt::utils::CUncopiable
                        {
                        private:
                                css * m_symbolic_structure;
                                csn * m_numeric_structure;
                                const CSparseMatrix *m_originalSM;  //!< A const reference to the original matrix used to build this decomposition.

                        public:
                                /** Constructor from a square definite-positive sparse matrix A, which can be use to solve Ax=b
                                  *   The actual Cholesky decomposition takes places in this constructor.
                                  *  \note Only the upper triangular part of the matrix is accessed.
                                  *  \exception std::runtime_error On non-square input matrix.
                                  *  \exception mrpt::math::CExceptionNotDefPos On non-definite-positive matrix as input.
                                  */
                                CholeskyDecomp(const CSparseMatrix &A);

                                /** Destructor */
                                virtual ~CholeskyDecomp();

                                /** Return the L matrix (L*L' = M), as a dense matrix. */
                                inline CMatrixDouble get_L() const { CMatrixDouble L; get_L(L); return L; }

                                /** Return the L matrix (L*L' = M), as a dense matrix. */
                                void get_L(CMatrixDouble &out_L) const;

                                /** Return the vector from a back-substitution step that solves: Ux=b   */
                                template <class VECTOR>
                                inline VECTOR backsub(const VECTOR &b) const { VECTOR res; backsub(b,res); return res; }

                                /** Return the vector from a back-substitution step that solves: Ux=b. Vectors can be Eigen::VectorXd or mrpt::math::CVectorDouble   */
                                void backsub(const Eigen::VectorXd &b, Eigen::VectorXd &result_x) const;

                                /** overload for double pointers which assume the user has reserved the output memory for \a result */
                                void backsub(const double *b, double *result, const size_t N) const;

                                /** Update the Cholesky factorization from an updated vesion of the original input, square definite-positive sparse matrix.
                                  *  NOTE: This new matrix MUST HAVE exactly the same sparse structure than the original one.
                                  */
                                void update(const CSparseMatrix &new_SM);
                        };


                        /** @} */

                }; // end class CSparseMatrix

        }
}
#endif