CSparseMatrixTemplate.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <map>
#include <vector>

namespace mrpt::math
{
/** A sparse matrix container (with cells of any type), with iterators.
 *  This class stores only those elements created by assigning them a value,
 * for example: "M(2,3)=8;".
 *
 *  This class doesn't implement math operations since it's a generic sparse
 * container, but it can be
 *   used to initialize the contents of a CSparse library-based matrix of type
 * mrpt::math::CSparseMatrix.
 *
 *  Note that reading non-existing cell elements will return the default value
 * (0 for numbers)
 *   and that cell will remain non-created in the matrix.
 *
 *  There is an additional method "exists(i,j)" to check whether a given
 * element exists in the matrix.
 *
 *  \sa mrpt::math::MatrixBlockSparseCols, mrpt::math::CSparseMatrix,
 * CSparseSymmetricalMatrix
 *  \note Methods marked as "Doesn't check bounds" mean that if an access to an
 * element out of the matrix size is tried, an empty element will be assumed,
 * but this will not raise any invalid memory access.
 * \ingroup mrpt_math_grp
 */
template <class T>
class CSparseMatrixTemplate
{
    // Public typedefs
   public:
    /**
     * Internal map type, used to store the actual matrix.
     */
    using SparseMatrixMap = typename std::map<std::pair<size_t, size_t>, T>;
    /**
     * Const iterator to move through the matrix.
     * \sa CSparseMatrixTemplate::const_reverse_iterator
     */
    using const_iterator = typename SparseMatrixMap::const_iterator;
    /**
     * Const reverse iterator to move through the matrix.
     * \sa CSparseMatrixTemplate::const_iterator
     */
    using const_reverse_iterator =
        typename SparseMatrixMap::const_reverse_iterator;

   protected:
    /**
     * Size of the matrix.
     */
    size_t mRows{0}, mColumns{0};
    /**
     * Actual matrix.
     */
    SparseMatrixMap objectList;

   public:
    /**
     * Basic constructor with no data. Size is set to (0,0).
     */
    CSparseMatrixTemplate() = default;
    /**
     * Constructor with default size.
     */
    CSparseMatrixTemplate(size_t nR, size_t nC) : mRows(nR), mColumns(nC) {}
    /**
     * Element access operator. Doesn't check bounds.
     */
    inline T operator()(size_t r, size_t c) const
    {
        auto it = objectList.find(std::make_pair(r, c));
        if (it == objectList.end())
            return T();
        else
            return it->second;
    }

    /** Element access operator. Checks bounds.
     */
    inline bool exists(size_t r, size_t c) const
    {
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
        if (r >= mRows || c >= mColumns) throw std::logic_error("Out of range");
#endif
        return (objectList.find(std::make_pair(r, c)) != objectList.end());
    }

    /**
     * Reference access operator. Checks for bounds.
     */
    inline T& operator()(size_t r, size_t c)
    {  //-V659
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
        if (r >= mRows || c >= mColumns) throw std::logic_error("Out of range");
#endif
        return objectList[std::make_pair(r, c)];
    }
    /**
     * Returns the amount of rows in this matrix.
     * \sa getColCount,getRow
     */
    inline size_t rows() const { return mRows; }
    /**
     * Returns the amount of columns in this matrix.
     * \sa rows()
     */
    inline size_t cols() const { return mColumns; }
    /**
     * Extracts a full row from the matrix.
     * \sa rows(),getColumn,setRow
     * \throw std::logic_error on out of range.
     */
    template <typename VECTOR>
    void getRow(size_t nRow, VECTOR& vec) const
    {
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
        if (nRow >= mRows) throw std::logic_error("Out of range");
#endif
        vec.resize(mColumns);
        size_t nextIndex = 0;
        for (typename SparseMatrixMap::const_iterator it = objectList.begin();
             it != objectList.end(); ++it)
        {
            const std::pair<size_t, size_t>& index = it->first;
            if (index.first < nRow)
                continue;
            else if (index.first == nRow)
            {
                for (size_t i = nextIndex; i < index.second; i++) vec[i] = T();
                vec[index.second] = it->second;
                nextIndex = index.second + 1;
            }
            else
            {
                for (size_t i = nextIndex; i < mColumns; i++) vec[i] = T();
                break;
            }
        }
    }
    /**
     * Extracts a full column from the matrix.
     * \sa getColCount,getRow,setColumn
     * \throw std::logic_error on out of range.
     */
    template <typename VECTOR>
    void getColumn(size_t nCol, VECTOR& vec) const
    {
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
        if (nCol >= mColumns) throw std::logic_error("Out of range");
#endif
        vec.resize(mRows);
        size_t nextIndex = 0;
        for (typename SparseMatrixMap::const_iterator it = objectList.begin();
             it != objectList.end(); ++it)
        {
            const std::pair<size_t, size_t>& index = it->first;
            if (index.second == nCol)
            {
                for (size_t i = nextIndex; i < index.first; i++) vec[i] = T();
                vec[index.first] = it->second;
                nextIndex = index.first + 1;
            }
        }
        for (size_t i = nextIndex; i < mRows; i++) vec[i] = T();
    }
    /**
     * Inserts an element into the matrix.
     * \sa operator()(size_t,size_t)
     */
    inline void insert(size_t row, size_t column, const T& obj)
    {
        operator()(row, column) = obj;
    }

    /** Inserts submatrix at a given location */
    template <class MATRIX_LIKE>
    inline void insertMatrix(size_t row, size_t column, const MATRIX_LIKE& mat)
    {
        for (size_t nr = 0; nr < mat.rows(); nr++)
            for (size_t nc = 0; nc < mat.cols(); nc++)
                operator()(row + nr, column + nc) = mat(nr, nc);
    }

    // Public interface only supports const iterators. This way, no user of this
    // class will be able to freely modify it contents.
    /**
     * Returns an iterator which points to the starting point of the matrix.
     * It's a const_iterator, so that the usar isn't able to modify the matrix
     * content into an invalid state.
     * \sa end,rbegin,rend
     */
    inline const_iterator begin() const { return objectList.begin(); }
    /**
     * Returns an iterator which points to the end of the matrix. It's a
     * const_iterator, so that the usar isn't able to modify the matrix content
     * into an invalid state.
     * \sa begin,rbegin,rend
     */
    inline const_iterator end() const { return objectList.end(); }
    /**
     * Returns an iterator which points to the end of the matrix, and can be
     * used to move backwards. It's a const_reverse_iterator, so that the usar
     * isn't able to modify the matrix content into an invalid state.
     * \sa begin,end,rend
     */
    inline const_reverse_iterator rbegin() const { return objectList.rbegin(); }
    /**
     * Returns an iterator which points to the starting point of the matrix,
     * although it's the upper limit of the matrix since it's a reverse
     * iterator. Also, it's a const_reverse_iterator, so that the usar isn't
     * able to modify the matrix content into an invalid state.
     * \sa begin,end,rbegin
     */
    inline const_reverse_iterator rend() const { return objectList.rend(); }
    /**
     * Inserts a full row into the matrix. The third argument is used to
     * specify a null object (which won't be inserted, since the matrix is
     * sparse).
     * \sa getRow
     * \throw std::logic_error on out of range or wrong sized vector.
     */
    template <typename VECTOR>
    void setRow(size_t nRow, const VECTOR& vec, const T& nullObject = T())
    {
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
        if (nRow >= mRows) throw std::logic_error("Out of range");
#endif
        size_t N = vec.size();
        if (N != mColumns) throw std::logic_error("Wrong-sized vector");
        for (size_t i = 0; i < N; i++)
        {
            const T& obj = vec[i];
            std::pair<size_t, size_t> index = std::make_pair(nRow, i);
            if (obj == nullObject)
                objectList.erase(index);
            else
                objectList[index] = obj;
        }
    }
    /**
     * Inserts a full column into the matrix. The third argument is used to
     * specify a null object (which won't be inserted, since the matrix is
     * sparse).
     * \sa getColumn
     * \throw std::logic_error on out of range or wrong sized vector.
     */
    template <typename VECTOR>
    void setColumn(size_t nCol, const VECTOR& vec, const T& nullObject = T())
    {
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
        if (nCol >= mColumns) throw std::logic_error("Out of range");
#endif
        size_t N = vec.size();
        if (N != mRows) throw std::logic_error("Wrong-sized vector");
        for (size_t i = 0; i < N; i++)
        {
            const T& obj = vec[i];
            std::pair<size_t, size_t> index = std::make_pair(i, nCol);
            if (obj == nullObject)
                objectList.erase(index);
            else
                objectList[index] = obj;
        }
    }
    /**
     * Changes the size of the matrix.
     */
    void resize(size_t nRows, size_t nCols)
    {
        // if (mRows<0||mColumns<0) throw std::logic_error("Invalid range"); //
        // This case never happens!
        if (mRows == nRows && mColumns == nCols) return;
        mRows = nRows;
        mColumns = nCols;
        std::vector<std::pair<size_t, size_t>> toErase;
        for (auto it = objectList.begin(); it != objectList.end(); ++it)
        {
            const std::pair<size_t, size_t>& i = it->first;
            if (i.first >= nRows || i.second >= nCols)
                toErase.push_back(it->first);
        }
        for (auto it = toErase.begin(); it != toErase.end(); ++it)
            objectList.erase(*it);
    }
    /**
     * Extracts a submatrix form the matrix.
     * \sa operator()(size_t,size_t)
     * \throw std::logic_error on invalid bounds.
     */
    CSparseMatrixTemplate<T> operator()(
        size_t firstRow, size_t lastRow, size_t firstColumn,
        size_t lastColumn) const
    {
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
        if (lastRow >= mRows || lastColumn >= mColumns)
            throw std::logic_error("Out of range");
        if (firstRow > lastRow || firstColumn > lastColumn)
            throw std::logic_error("Invalid size");
#endif
        CSparseMatrixTemplate<T> res = CSparseMatrixTemplate<T>(
            lastRow + 1 - firstRow, lastColumn + 1 - firstColumn);
        for (typename SparseMatrixMap::const_iterator it = begin(); it != end();
             ++it)
        {
            const std::pair<size_t, size_t>& i = it->first;
            if (i.first >= firstRow && i.first <= lastRow &&
                i.second >= firstColumn && i.second <= lastColumn)
                res(i.first - firstRow, i.second - firstColumn) = it->second;
        }
        return res;
    }
    /**
     * Gets a vector containing all the elements of the matrix, ignoring their
     * position.
     */
    template <typename VECTOR>
    void asVector(VECTOR& vec) const
    {
        size_t N = objectList.size();
        vec.resize(0);
        vec.reserve(N);
        for (const_iterator it = objectList.begin(); it != objectList.end();
             ++it)
            vec.push_back(it->second);
    }
    /**
     * Gets the amount of non-null elements inside the matrix.
     * \sa getNullElements,isNull,isNotNull
     */
    inline size_t getNonNullElements() const { return objectList.size(); }
    /** Are there no elements set to !=0 ?
     * \sa getNullElements,isNull,isNotNull
     */
    inline bool empty() const { return objectList.empty(); }
    /**
     * Gets the amount of null elements inside the matrix.
     * \sa getNonNullElements,isNull,isNotNull
     */
    inline size_t getNullElements() const
    {
        return mRows * mColumns - getNonNullElements();
    }
    /**
     * Checks whether an element of the matrix is the default object.
     * \sa getNonNullElements,getNullElements,isNotNull
     * \throw std::logic_error on out of range
     */
    inline bool isNull(size_t nRow, size_t nCol) const
    {
        if (nRow >= mRows || nCol >= mColumns)
            throw std::logic_error("Out of range");
        return objectList.count(std::make_pair(nRow, nCol)) == 0;
    }
    /**
     * Checks whether an element of the matrix is not the default object.
     * \sa getNonNullElements,getNullElements,isNull
     */
    inline bool isNotNull(size_t nRow, size_t nCol) const
    {
        if (nRow >= mRows || nCol >= mColumns)
            throw std::logic_error("Out of range");
        return objectList.count(std::make_pair(nRow, nCol)) > 0;
    }
    /**
     * Completely removes all elements, although maintaining the matrix's size.
     */
    inline void clear() { objectList.clear(); }
    /**
     * Checks each non-null elements against the basic objects, erasing
     * unnecesary references to it.
     */
    void purge(T nullObject = T())
    {
        std::vector<std::pair<size_t, size_t>> nulls;
        for (const_iterator it = begin(); it != end(); ++it)
            if (it->second == nullObject) nulls.push_back(it->first);
        for (auto it = nulls.begin(); it != nulls.end(); ++it)
            objectList.erase(*it);
    }
};  // end of sparse matrix

/** A sparse matrix container for square symmetrical content around the main
 * diagonal.
 *  This class saves half of the space with respect to CSparseMatrixTemplate
 * since only those entries (c,r) such as c>=r are really stored,
 *   but both (c,r) and (r,c) can be retrieved or set and both redirect to the
 * same internal cell container.
 *  \sa CSparseMatrixTemplate
 */
template <class T>
class CSparseSymmetricalMatrix : public CSparseMatrixTemplate<T>
{
   public:
    CSparseSymmetricalMatrix() : CSparseMatrixTemplate<T>() {}
    explicit CSparseSymmetricalMatrix(const CSparseSymmetricalMatrix& o)
        : CSparseMatrixTemplate<T>(o)
    {
    }
    explicit CSparseSymmetricalMatrix(const CSparseMatrixTemplate<T>& o)
        : CSparseMatrixTemplate<T>(o)
    {
    }
    virtual ~CSparseSymmetricalMatrix() = default;
    void resize(size_t matrixSize)
    {
        CSparseMatrixTemplate<T>::resize(matrixSize, matrixSize);
    }

    inline T operator()(size_t r, size_t c) const
    {
        if (c < r) std::swap(r, c);  // Symmetrical matrix
        typename CSparseMatrixTemplate<T>::const_iterator it =
            CSparseMatrixTemplate<T>::objectList.find(std::make_pair(r, c));
        if (it == CSparseMatrixTemplate<T>::objectList.end())
            return T();
        else
            return it->second;
    }
    inline T& operator()(size_t r, size_t c)
    {  //-V659
        if (c < r) std::swap(r, c);  // Symmetrical matrix
        if (r >= CSparseMatrixTemplate<T>::mRows ||
            c >= CSparseMatrixTemplate<T>::mColumns)
            throw std::logic_error("Out of range");
        return CSparseMatrixTemplate<T>::objectList[std::make_pair(r, c)];
    }

};  // end of CSparseSymmetricalMatrix
}  // namespace mrpt::math