CSparseMatrix.cpp

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

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

#include <mrpt/math/CSparseMatrix.h>
#include <string>
#include <iostream>

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

/** 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.cols() == B.cols() && A.rows() == B.rows());

    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.cols() == B.rows());

    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(cols()));
    out_res.resize(rows());
    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.cols() == SM.rows());
    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
// ==============================