CSparseMatrix.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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 |
   +------------------------------------------------------------------------+ */
#pragma once
 
#include <mrpt/math/math_frwds.h>
#include <mrpt/math/CSparseMatrixTemplate.h>
#include <mrpt/math/CMatrixTemplateNumeric.h>
#include <mrpt/math/CMatrixFixedNumeric.h>
#include <cstring>  // memcpy
#include <stdexcept>
 
// 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 */
struct CExceptionNotDefPos : public std::runtime_error
{
        CExceptionNotDefPos(const char* s) : std::runtime_error(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_math_grp
 */
class 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.cols();
                const int nRow = C.rows();
                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 nullptr
         * 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 =
                        nullptr;  // This is to know they shouldn't be tried to free()
                sparse_matrix.p = nullptr;
                sparse_matrix.x = nullptr;
 
                // 1) Create triplet matrix
                CSparseMatrix triplet(data.rows(), data.cols());
                // 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...)
                @{ */
 
        /** this = A+B */
        void add_AB(const CSparseMatrix& A, const CSparseMatrix& B);
        /** this = A*B */
        void multiply_AB(const CSparseMatrix& A, const CSparseMatrix& B);
        /** out_res = this * b */
        void multiply_Ab(
                const mrpt::math::CVectorDouble& b,
                mrpt::math::CVectorDouble& out_res) const;
 
        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.rows();
                const size_t nC = M.cols();
                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 rows() const { return sparse_matrix.m; }
        inline size_t cols() 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 CholeskyDecomp
        {
           private:
                css* m_symbolic_structure;
                csn* m_numeric_structure;
                /** A const reference to the original matrix used to build this
                 * decomposition. */
                const CSparseMatrix* m_originalSM;
 
           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);
                CholeskyDecomp(const CholeskyDecomp& A) = delete;
 
                CholeskyDecomp& operator=(const CholeskyDecomp&) = delete;
 
                /** 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);
        };
        static_assert(
                !std::is_copy_constructible<CholeskyDecomp>::value &&
                        !std::is_copy_assignable<CholeskyDecomp>::value,
                "Copy Check");
 
        /** @} */
 
};  // end class CSparseMatrix
}  // namespace math
}  // namespace mrpt