CSparseMatrix.cpp

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 |
   +------------------------------------------------------------------------+ */

#include "base-precomp.h"  // Precompiled headers

#include <mrpt/math/CSparseMatrix.h>

using std::string;
using std::cout;
using std::endl;
using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::utils;

/** Copy constructor */
CSparseMatrix::CSparseMatrix(const CSparseMatrix& other)
{
        construct_from_existing_cs(other.sparse_matrix);
        copy(&other.sparse_matrix);
}

/** Copy constructor from an existing "cs" CSparse data structure */
CSparseMatrix::CSparseMatrix(const cs* const sm)
{
        construct_from_existing_cs(*sm);
        copy(sm);
}

/** Copy the data from an existing "cs" CSparse data structure */
void CSparseMatrix::copy(const cs* const sm)
{
        ASSERTMSG_(
                sm->nz == -1,
                "I expected a column-compressed sparse matrix, not a triplet form.")

        sparse_matrix.m = sm->m;
        sparse_matrix.n = sm->n;
        sparse_matrix.nz = sm->nz;
        sparse_matrix.nzmax = sm->nzmax;

        // Do it in a different way depending on it being triplet or compressed
        // format:
        ::memcpy(sparse_matrix.i, sm->i, sizeof(int) * sm->nzmax);
        ::memcpy(sparse_matrix.p, sm->p, sizeof(int) * (sm->n + 1));
        ::memcpy(sparse_matrix.x, sm->x, sizeof(double) * sm->nzmax);
}

/** Fast copy the data from an existing "cs" CSparse data structure, copying the
 * pointers and leaving NULLs in the source structure. */
void CSparseMatrix::copy_fast(cs* const sm)
{
        // This method will work for either triplet or compressed format:

        // Free previous contents, if any.
        internal_free_mem();

        // Fast copy / Move:
        sparse_matrix.m = sm->m;
        sparse_matrix.n = sm->n;
        sparse_matrix.nz = sm->nz;
        sparse_matrix.nzmax = sm->nzmax;

        sparse_matrix.i = sm->i;
        sparse_matrix.p = sm->p;
        sparse_matrix.x = sm->x;

        // Mark source as empty:
        sm->i = nullptr;
        sm->p = nullptr;
        sm->x = nullptr;
}

/** Fast swap contents with another sparse matrix */
void CSparseMatrix::swap(CSparseMatrix& other)
{
        // Fast copy / Move:
        std::swap(sparse_matrix.m, other.sparse_matrix.m);
        std::swap(sparse_matrix.n, sparse_matrix.n);
        std::swap(sparse_matrix.nz, other.sparse_matrix.nz);
        std::swap(sparse_matrix.nzmax, other.sparse_matrix.nzmax);

        std::swap(sparse_matrix.i, other.sparse_matrix.i);
        std::swap(sparse_matrix.p, other.sparse_matrix.p);
        std::swap(sparse_matrix.x, other.sparse_matrix.x);
}

// Dtor
CSparseMatrix::~CSparseMatrix() { internal_free_mem(); }
/** Erase all previous contents and leave the matrix as a "triplet" 1x1 matrix
 * without any data. */
void CSparseMatrix::clear(const size_t nRows, const size_t nCols)
{
        // Free old data:
        internal_free_mem();

        // Init as 1x1 triplet:
        sparse_matrix.nzmax = 1;
        sparse_matrix.m = nRows;
        sparse_matrix.n = nCols;
        sparse_matrix.i = (int*)malloc(sizeof(int) * sparse_matrix.nzmax);
        sparse_matrix.p = (int*)malloc(sizeof(int) * (sparse_matrix.n + 1));
        sparse_matrix.x = (double*)malloc(sizeof(double) * sparse_matrix.nzmax);
        sparse_matrix.nz = 0;  // >=0 -> triplet format.
}

/** free buffers (deallocate the memory of the i,p,x buffers) */
void CSparseMatrix::internal_free_mem()
{
        cs_free(sparse_matrix.i);
        cs_free(sparse_matrix.p);
        cs_free(sparse_matrix.x);
}

/** Initialization from a triplet "cs", which is first compressed */
void CSparseMatrix::construct_from_triplet(const cs& triplet)
{
        cs* sm = cs_compress(&triplet);
        copy_fast(sm);
        cs_spfree(sm);  // This will release just the "cs" structure itself, not the
        // internal buffers, by now set to NULL.
}

// To be called by constructors only, assume previous pointers are trash and
// overwrite them:
// This ONLY allocate the memory, doesn't fill it.
void CSparseMatrix::construct_from_existing_cs(const cs& sm)
{
        ASSERTMSG_(
                sm.nz == -1,
                "I expected a column-compressed sparse matrix, not a triplet form.")
        sparse_matrix.i = (int*)malloc(sizeof(int) * sm.nzmax);
        sparse_matrix.p = (int*)malloc(sizeof(int) * (sm.n + 1));
        sparse_matrix.x = (double*)malloc(sizeof(double) * sm.nzmax);
}

/** 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().
*/
CSparseMatrix::CSparseMatrix(const size_t nRows, const size_t nCols)
{
        sparse_matrix.nzmax = 1;
        sparse_matrix.m = nRows;
        sparse_matrix.n = nCols;
        sparse_matrix.i = (int*)malloc(sizeof(int) * sparse_matrix.nzmax);
        sparse_matrix.p = (int*)malloc(sizeof(int) * (sparse_matrix.n + 1));
        sparse_matrix.x = (double*)malloc(sizeof(double) * sparse_matrix.nzmax);
        sparse_matrix.nz = 0;
}

/** Insert an element into a "cs", return false on error. */
void CSparseMatrix::insert_entry(
        const size_t row, const size_t col, const double val)
{
        if (!isTriplet())
                THROW_EXCEPTION(
                        "insert_entry() is only available for sparse matrix in 'triplet' "
                        "format.")
        if (!cs_entry(&sparse_matrix, row, col, val))
                THROW_EXCEPTION(
                        "Error inserting element in sparse matrix (out of mem?)")
}

/** Copy operator from another existing object */
void CSparseMatrix::operator=(const CSparseMatrix& other)
{
        if (&other == this) return;

        cs_free(sparse_matrix.i);
        cs_free(sparse_matrix.p);
        cs_free(sparse_matrix.x);

        sparse_matrix.i = (int*)malloc(sizeof(int) * other.sparse_matrix.nzmax);
        sparse_matrix.p = (int*)malloc(sizeof(int) * (other.sparse_matrix.n + 1));
        sparse_matrix.x =
                (double*)malloc(sizeof(double) * other.sparse_matrix.nzmax);
        copy(&other.sparse_matrix);
}

void CSparseMatrix::add_AB(const CSparseMatrix& A, const CSparseMatrix& B)
{
        ASSERT_(
                A.getColCount() == B.getColCount() &&
                A.getRowCount() == B.getRowCount())

        cs* sm = cs_add(&(A.sparse_matrix), &(B.sparse_matrix), 1, 1);
        ASSERT_(sm)
        this->copy_fast(sm);
        cs_spfree(sm);
}

void CSparseMatrix::multiply_AB(const CSparseMatrix& A, const CSparseMatrix& B)
{
        ASSERT_(A.getColCount() == B.getRowCount())

        cs* sm = cs_multiply(&(A.sparse_matrix), &(B.sparse_matrix));
        ASSERT_(sm)
        this->copy_fast(sm);
        cs_spfree(sm);
}

void CSparseMatrix::multiply_Ab(
        const mrpt::math::CVectorDouble& b,
        mrpt::math::CVectorDouble& out_res) const
{
        ASSERT_EQUAL_(int(b.size()), int(getColCount()))
        out_res.resize(getRowCount());
        const double* y = &(b[0]);
        double* x = &(out_res[0]);
        cs_gaxpy(&sparse_matrix, y, x);
}

CSparseMatrix CSparseMatrix::transpose() const
{
        cs* sm = cs_transpose(&sparse_matrix, 1);
        ASSERT_(sm)
        CSparseMatrix SM(sm);
        cs_spfree(sm);
        return SM;
}

/** Static method to convert a "cs" structure into a dense representation of the
 * sparse matrix.
*/
void CSparseMatrix::cs2dense(const cs& SM, CMatrixDouble& d_M)
{
        d_M.zeros(SM.m, SM.n);
        if (SM.nz >= 0)  // isTriplet ??
        {  // It's in triplet form.
                for (int idx = 0; idx < SM.nz; ++idx)
                        d_M(SM.i[idx], SM.p[idx]) +=
                                SM.x[idx];  // += since the convention is that duplicate (i,j)
                // entries add to each other.
        }
        else
        {  // Column compressed format:
                ASSERT_(SM.x)  // JL: Could it be nullptr and be OK???

                for (int j = 0; j < SM.n; j++)
                {
                        const int p0 = SM.p[j];
                        const int p1 = SM.p[j + 1];
                        for (int p = p0; p < p1; p++)
                                d_M(SM.i[p], j) += SM.x[p];  // += since the convention is that
                        // duplicate (i,j) entries add to
                        // each other.
                }
        }
}

void CSparseMatrix::get_dense(CMatrixDouble& d_M) const
{
        cs2dense(sparse_matrix, d_M);
}

void CSparseMatrix::compressFromTriplet()
{
        if (!isTriplet())
                THROW_EXCEPTION(
                        "compressFromTriplet(): Matrix is already in column-compressed "
                        "format.")

        cs* sm = cs_compress(&this->sparse_matrix);
        copy_fast(sm);
        cs_spfree(sm);  // This will release just the "cs" structure itself, not the
        // internal buffers, now set to NULL.
}

/** save as a dense matrix to a text file \return False on any error.
*/
bool CSparseMatrix::saveToTextFile_dense(const std::string& filName)
{
        CMatrixDouble dense;
        this->get_dense(dense);
        try
        {
                dense.saveToTextFile(filName);
                return true;
        }
        catch (...)
        {
                return false;
        }
}

// Format:
//   NUM_ROWS   NUM_COLS   NUM_NON_ZERO_MAX
//   row_1   col_1   value_1
//   row_2   col_2   value_2
bool CSparseMatrix::saveToTextFile_sparse(const std::string& filName)
{
        FILE* f = fopen(filName.c_str(), "wt");
        if (!f) return false;

        // Help notes:
        fprintf(
                f,
                "\
# This sparse matrix can be loaded in Octave/Matlab as follows:\n\
# D=load('file.txt');\n\
# SM=spconvert(D(2:end,:));\n\
#  or...\n\
# m=D(1,1); n=D(1,2); nzmax=D(1,3);\n\
# Di=D(2:end,1); Dj=D(2:end,2); Ds=D(2:end,3);\n\
# SM=sparse(Di,Dj,Ds, m,n, nzmax);\n\n");

        // First line:
        fprintf(
                f, "%i %i %i\n", sparse_matrix.m, sparse_matrix.n,
                sparse_matrix.nzmax);  // Rows, cols, nzmax

        // Data lines:
        if (sparse_matrix.nz >= 0)  // isTriplet ??
        {  // It's in triplet form.
                for (int i = 0; i < sparse_matrix.nzmax; i++)
                        if (sparse_matrix.x[i] != 0)
                                fprintf(
                                        f, "%4i %4i %e\n", 1 + sparse_matrix.i[i],
                                        1 + sparse_matrix.p[i], sparse_matrix.x[i]);
        }
        else
        {  // Column compressed format:
                ASSERT_(sparse_matrix.x)  // JL: Could it be nullptr and be OK???

                for (int j = 0; j < sparse_matrix.n; j++)
                {
                        const int p0 = sparse_matrix.p[j];
                        const int p1 = sparse_matrix.p[j + 1];
                        for (int p = p0; p < p1; p++)
                                fprintf(
                                        f, "%4i %4i %e\n", 1 + sparse_matrix.i[p], 1 + j,
                                        sparse_matrix.x[p]);
                }
        }

        fclose(f);
        return true;
}

// ===============  START OF:   CSparseMatrix::CholeskyDecomp  inner class
// ==============================

/** Constructor from a square semidefinite-positive sparse matrix.
*   The actual Cholesky decomposition takes places in this constructor.
*  \exception std::runtime_error On non-square input matrix.
*  \exception mrpt::math::CExceptionNotDefPos On non-semidefinite-positive
* matrix as input.
*/
CSparseMatrix::CholeskyDecomp::CholeskyDecomp(const CSparseMatrix& SM)
        : m_symbolic_structure(nullptr),
          m_numeric_structure(nullptr),
          m_originalSM(&SM)
{
        ASSERT_(SM.getColCount() == SM.getRowCount())
        ASSERT_(SM.isColumnCompressed())

        // symbolic decomposition:
        m_symbolic_structure =
                cs_schol(1 /* order */, &m_originalSM->sparse_matrix);

        // numeric decomposition:
        m_numeric_structure =
                cs_chol(&m_originalSM->sparse_matrix, m_symbolic_structure);

        if (!m_numeric_structure)
                throw mrpt::math::CExceptionNotDefPos(
                        "CSparseMatrix::CholeskyDecomp: Not positive definite matrix.");
}

// Destructor:
CSparseMatrix::CholeskyDecomp::~CholeskyDecomp()
{
        cs_nfree(m_numeric_structure);
        cs_sfree(m_symbolic_structure);
}

/** Return the L matrix (L*L' = M), as a dense matrix. */
void CSparseMatrix::CholeskyDecomp::get_L(CMatrixDouble& L) const
{
        CSparseMatrix::cs2dense(*m_numeric_structure->L, L);
}

/** Return the vector from a back-substitution step that solves: Ux=b   */
void CSparseMatrix::CholeskyDecomp::backsub(
        const Eigen::VectorXd& b, Eigen::VectorXd& sol) const
{
        ASSERT_(b.size() > 0)
        sol.resize(b.size());
        this->backsub(&b[0], &sol[0], b.size());
}

/** Return the vector from a back-substitution step that solves: Ux=b   */
void CSparseMatrix::CholeskyDecomp::backsub(
        const double* b, double* sol, const size_t N) const
{
        ASSERT_(N > 0)
        std::vector<double> tmp(N);

        cs_ipvec(
                m_symbolic_structure->pinv, &b[0], &tmp[0], N); /* tmp = PERMUT*b */
        // permute con. pivoting
        cs_lsolve(m_numeric_structure->L, &tmp[0]); /* tmp = L\tmp */
        cs_ltsolve(m_numeric_structure->L, &tmp[0]); /* tmp = L'\tmp */
        cs_pvec(
                m_symbolic_structure->pinv, &tmp[0], &sol[0],
                N); /* sol = PERMUT'*tmp */
        // unpermute con. pivoting

        // Result is now in "sol".
}

/** 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 CSparseMatrix::CholeskyDecomp::update(const CSparseMatrix& new_SM)
{
        ASSERTMSG_(
                m_originalSM->sparse_matrix.nzmax == new_SM.sparse_matrix.nzmax,
                "New matrix doesn't have the same sparse structure!");
        ASSERTMSG_(
                m_originalSM->sparse_matrix.n == new_SM.sparse_matrix.n,
                "New matrix doesn't have the same sparse structure!");

        m_originalSM = &new_SM;  // Just copy the reference.

        // Release old data:
        cs_nfree(m_numeric_structure);
        m_numeric_structure = nullptr;

        // numeric decomposition:
        m_numeric_structure =
                cs_chol(&m_originalSM->sparse_matrix, m_symbolic_structure);
        if (!m_numeric_structure)
                throw mrpt::math::CExceptionNotDefPos(
                        "CholeskyDecomp::update: Not positive definite matrix.");
}

// ===============    END OF: CSparseMatrix::CholeskyDecomp  inner class
// ==============================