CMatrixTemplate.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/core/aligned_allocator.h>
#include <mrpt/math/math_frwds.h>  // forward declarations
#include <algorithm>  // swap()
#include <array>
#include <cstring>  // memset()

namespace mrpt::math
{
/** Auxiliary class used in CMatrixTemplate:size(), CMatrixTemplate::resize(),
 * CMatrixFixedNumeric::size(), CMatrixFixedNumeric::resize(), to mimic the
 * behavior of STL-containers */
struct CMatrixTemplateSize : public std::array<size_t, 2>
{
    using Base = std::array<size_t, 2>;
    using mrpt_autotype = CMatrixTemplateSize;

    inline CMatrixTemplateSize() : std::array<size_t, 2>() {}
    inline CMatrixTemplateSize(const size_t* d)
    {
        (*this)[0] = d[0];
        (*this)[1] = d[1];
    }

    inline bool operator==(const CMatrixTemplateSize& o) const
    {
        return Base::operator[](0) == o[0] && Base::operator[](1) == o[1];
    }
    inline bool operator!=(const CMatrixTemplateSize& o) const
    {
        return !(*this == o);
    }
    /** This operator allows the size(N,M) to be compared with a plain size_t
     * N*M  */
    inline operator size_t(void) const { return 2; }
};

/**  This template class provides the basic functionality for a general 2D
 *any-size, resizable container of numerical or non-numerical elements.
 * NOTES:
 *      - This class is not serializable since it is a template. For using
 *serialization, see mrpt::math::CMatrixNumeric
 *      - First row or column index is "0".
 *      - This class includes range checks with ASSERT_() if compiling with
 *"_DEBUG" or "MRPT_ALWAYS_CHECKS_DEBUG_MATRICES=1".
 *      - Please DO NOT use as template class type any other class. It can be
 *safely used the following types:
 *          - Elemental types (int,char,float,doble,...)
 *          - Data struct (Not classes!)
 *          - Any kind of pointers (user is responsible for allocating and
 *freeing
 *the memory addressed by pointers).
 *
 * \note Memory blocks for each row are 16-bytes aligned (since MRPT 0.7.0).
 * \note For a complete introduction to Matrices and vectors in MRPT, see:
 *http://www.mrpt.org/Matrices_vectors_arrays_and_Linear_Algebra_MRPT_and_Eigen_classes
 * \sa CMatrixTemplateNumeric
 * \ingroup mrpt_math_grp
 */
template <class T>
class CMatrixTemplate
{
   public:
    // type definitions
    /** The type of the matrix elements */
    using value_type = T;
    using reference = T&;
    using const_reference = const T&;
    using size_type = std::size_t;
    using difference_type = std::ptrdiff_t;

   protected:
    T** m_Val;
    size_t m_Rows, m_Cols;

    /** Internal use only: It reallocs the memory for the 2D matrix, maintaining
     * the previous contents if posible.
     */
    void realloc(size_t row, size_t col, bool newElementsToZero = false)
    {
        if (row != m_Rows || col != m_Cols || m_Val == nullptr)
        {
            size_t r;
            bool doZeroColumns = newElementsToZero && (col > m_Cols);
            size_t sizeZeroColumns = sizeof(T) * (col - m_Cols);

            // If we are reducing rows, free that memory:
            for (r = row; r < m_Rows; r++) mrpt::aligned_free(m_Val[r]);

            // Realloc the vector of pointers:
            if (!row)
            {
                mrpt::aligned_free(m_Val);
                m_Val = nullptr;
            }
            else
                m_Val = static_cast<T**>(
                    mrpt::aligned_realloc(m_Val, sizeof(T*) * row, 16));

            // How many new rows/cols?
            size_t row_size = col * sizeof(T);

            // Alloc new ROW pointers & resize previously existing rows, as
            // required:
            for (r = 0; r < row; r++)
            {
                if (r < m_Rows)
                {
                    // This was an existing row: Resize the memory:
                    m_Val[r] = static_cast<T*>(
                        mrpt::aligned_realloc(m_Val[r], row_size, 16));

                    if (doZeroColumns)
                    {
                        // Fill with zeros:
                        ::memset(&m_Val[r][m_Cols], 0, sizeZeroColumns);
                    }
                }
                else
                {
                    // This is a new row, alloc the memory for the first time:
                    m_Val[r] =
                        static_cast<T*>(mrpt::aligned_malloc(row_size, 16));
                    ::memset(m_Val[r], 0, row_size);
                }
            }
            // Done!
            m_Rows = row;
            m_Cols = col;
        }
    }

   public:
    /**
     * Checks whether the rows [r-N,r+N] and the columns [c-N,c+N] are present
     * in the matrix.
     */
    template <size_t N>
    inline void ASSERT_ENOUGHROOM(size_t r, size_t c) const
    {
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
        ASSERT_((r >= N) && (r + N < rows()) && (c >= N) && (c + N < cols()));
#endif
    }
    /*! Fill all the elements with a given value (Note: named "fillAll" since
     * "fill" will be used by child classes) */
    void fillAll(const T& val)
    {
        for (size_t r = 0; r < m_Rows; r++)
            for (size_t c = 0; c < m_Cols; c++) m_Val[r][c] = val;
    }

    /** Swap with another matrix very efficiently (just swaps a pointer and two
     * integer values). */
    inline void swap(CMatrixTemplate<T>& o)
    {
        std::swap(m_Val, o.m_Val);
        std::swap(m_Rows, o.m_Rows);
        std::swap(m_Cols, o.m_Cols);
    }

    /** Constructors */
    CMatrixTemplate(const CMatrixTemplate& m)
        : m_Val(nullptr), m_Rows(0), m_Cols(0)
    {
        (*this) = m;
    }

    CMatrixTemplate(size_t row = 1, size_t col = 1)
        : m_Val(nullptr), m_Rows(0), m_Cols(0)
    {
        realloc(row, col);
    }

    /** Copy constructor & crop from another matrix
     */
    CMatrixTemplate(
        const CMatrixTemplate& m, const size_t cropRowCount,
        const size_t cropColCount)
        : m_Val(nullptr), m_Rows(0), m_Cols(0)
    {
        ASSERT_(m.m_Rows >= cropRowCount);
        ASSERT_(m.m_Cols >= cropColCount);
        realloc(cropRowCount, cropColCount);
        for (size_t i = 0; i < m_Rows; i++)
            for (size_t j = 0; j < m_Cols; j++) m_Val[i][j] = m.m_Val[i][j];
    }

    /** Constructor from a given size and a C array. The array length must match
     *cols x row.
     * \code
     *  const double numbers[] = {
     *    1,2,3,
     *    4,5,6 };
     *   CMatrixDouble   M(3,2, numbers);
     * \endcode
     */
    template <typename V, size_t N>
    CMatrixTemplate(size_t row, size_t col, V (&theArray)[N])
        : m_Val(nullptr), m_Rows(0), m_Cols(0)
    {
        MRPT_COMPILE_TIME_ASSERT(N != 0);
        realloc(row, col);
        if (m_Rows * m_Cols != N)
            THROW_EXCEPTION(
                format(
                    "Mismatch between matrix size %lu x %lu and array of "
                    "length %lu",
                    static_cast<long unsigned>(m_Rows),
                    static_cast<long unsigned>(m_Cols),
                    static_cast<long unsigned>(N)))
        size_t idx = 0;
        for (size_t i = 0; i < m_Rows; i++)
            for (size_t j = 0; j < m_Cols; j++)
                m_Val[i][j] = static_cast<T>(theArray[idx++]);
    }

    /** Constructor from a given size and a STL container (std::vector,
     * std::list,...) with the initial values. The vector length must match cols
     * x row.
     */
    template <typename V>
    CMatrixTemplate(size_t row, size_t col, const V& theVector)
        : m_Val(nullptr), m_Rows(0), m_Cols(0)
    {
        const size_t N = theVector.size();
        realloc(row, col);
        if (m_Rows * m_Cols != N)
            THROW_EXCEPTION(
                format(
                    "Mismatch between matrix size %lu x %lu and array of "
                    "length %lu",
                    static_cast<long unsigned>(m_Rows),
                    static_cast<long unsigned>(m_Cols),
                    static_cast<long unsigned>(N)))
        typename V::const_iterator it = theVector.begin();
        for (size_t i = 0; i < m_Rows; i++)
            for (size_t j = 0; j < m_Cols; j++)
                m_Val[i][j] = static_cast<T>(*(it++));
    }

    /** Destructor */
    virtual ~CMatrixTemplate() { realloc(0, 0); }
    /** Assignment operator from another matrix */
    CMatrixTemplate& operator=(const CMatrixTemplate& m)
    {
        realloc(m.m_Rows, m.m_Cols);
        for (size_t i = 0; i < m_Rows; i++)
            for (size_t j = 0; j < m_Cols; j++) m_Val[i][j] = m.m_Val[i][j];
        return *this;
    }

    /** Assignment operator for initializing from a C array (The matrix must be
     *set to the correct size before invoking this asignament)
     * \code
     *   CMatrixDouble   M(3,2);
     *  const double numbers[] = {
     *    1,2,3,
     *    4,5,6 };
     *  M = numbers;
     * \endcode
     *  Refer also to the constructor with initialization data
     *CMatrixTemplate::CMatrixTemplate
     */
    template <typename V, size_t N>
    CMatrixTemplate& operator=(V (&theArray)[N])
    {
        MRPT_COMPILE_TIME_ASSERT(N != 0);
        if (m_Rows * m_Cols != N)
        {
            THROW_EXCEPTION(
                format(
                    "Mismatch between matrix size %lu x %lu and array of "
                    "length %lu",
                    m_Rows, m_Cols, N))
        }
        size_t idx = 0;
        for (size_t i = 0; i < m_Rows; i++)
            for (size_t j = 0; j < m_Cols; j++)
                m_Val[i][j] = static_cast<T>(theArray[idx++]);
        return *this;
    }

    /** Number of rows in the matrix
     * \sa rows(), getColCount, nr, nc
     */
    inline size_t rows() const { return m_Rows; }
    /** Number of columns in the matrix
     * \sa rows(), getColCount, nr, nc
     */
    inline size_t cols() const { return m_Cols; }
    /** Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x)) */
    inline CMatrixTemplateSize size() const
    {
        CMatrixTemplateSize dims;
        dims[0] = m_Rows;
        dims[1] = m_Cols;
        return dims;
    }

    /** Changes the size of matrix, maintaining the previous contents. */
    void setSize(size_t row, size_t col, bool zeroNewElements = false)
    {
        realloc(row, col, zeroNewElements);
    }

    /** This method just checks has no effects in this class, but raises an
     * exception if the expected size does not match */
    inline void resize(
        const CMatrixTemplateSize& siz, bool zeroNewElements = false)
    {
        setSize(siz[0], siz[1], zeroNewElements);
    }

    /** Subscript operator to get/set individual elements
     */
    inline T& operator()(size_t row, size_t col)
    {
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
        if (row >= m_Rows || col >= m_Cols)
            THROW_EXCEPTION(
                format(
                    "Indexes (%lu,%lu) out of range. Matrix is %lux%lu",
                    static_cast<unsigned long>(row),
                    static_cast<unsigned long>(col),
                    static_cast<unsigned long>(m_Rows),
                    static_cast<unsigned long>(m_Cols)));
#endif
        return m_Val[row][col];
    }

    /** Subscript operator to get individual elements
     */
    inline const T& operator()(size_t row, size_t col) const
    {
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
        if (row >= m_Rows || col >= m_Cols)
            THROW_EXCEPTION(
                format(
                    "Indexes (%lu,%lu) out of range. Matrix is %lux%lu",
                    static_cast<unsigned long>(row),
                    static_cast<unsigned long>(col),
                    static_cast<unsigned long>(m_Rows),
                    static_cast<unsigned long>(m_Cols)));
#endif
        return m_Val[row][col];
    }

    /** Subscript operator to get/set an individual element from a row or column
     * matrix.
     * \exception std::exception If the object is not a column or row matrix.
     */
    inline T& operator()(size_t ith)
    {
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
        ASSERT_(m_Rows == 1 || m_Cols == 1);
#endif
        if (m_Rows == 1)
        {
// A row matrix:
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
            if (ith >= m_Cols)
                THROW_EXCEPTION_FMT(
                    "Index %u out of range!", static_cast<unsigned>(ith));
#endif
            return m_Val[0][ith];
        }
        else
        {
// A columns matrix:
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
            if (ith >= m_Rows)
                THROW_EXCEPTION_FMT(
                    "Index %u out of range!", static_cast<unsigned>(ith));
#endif
            return m_Val[ith][0];
        }
    }

    /** Subscript operator to get/set an individual element from a row or column
     * matrix.
     * \exception std::exception If the object is not a column or row matrix.
     */
    inline T operator()(size_t ith) const
    {
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
        ASSERT_(m_Rows == 1 || m_Cols == 1);
#endif
        if (m_Rows == 1)
        {
// A row matrix:
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
            if (ith >= m_Cols)
                THROW_EXCEPTION_FMT(
                    "Index %u out of range!", static_cast<unsigned>(ith));
#endif
            return m_Val[0][ith];
        }
        else
        {
// A columns matrix:
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
            if (ith >= m_Rows)
                THROW_EXCEPTION_FMT(
                    "Index %u out of range!", static_cast<unsigned>(ith));
#endif
            return m_Val[ith][0];
        }
    }

    /** Fast but unsafe method to write a value in the matrix
     */
    inline void set_unsafe(size_t row, size_t col, const T& v)
    {
#ifdef _DEBUG
        if (row >= m_Rows || col >= m_Cols)
            THROW_EXCEPTION(
                format(
                    "Indexes (%lu,%lu) out of range. Matrix is %lux%lu",
                    static_cast<unsigned long>(row),
                    static_cast<unsigned long>(col),
                    static_cast<unsigned long>(m_Rows),
                    static_cast<unsigned long>(m_Cols)));
#endif
        m_Val[row][col] = v;
    }

    /** Fast but unsafe method to read a value from the matrix
     */
    inline const T& get_unsafe(size_t row, size_t col) const
    {
#ifdef _DEBUG
        if (row >= m_Rows || col >= m_Cols)
            THROW_EXCEPTION(
                format(
                    "Indexes (%lu,%lu) out of range. Matrix is %lux%lu",
                    static_cast<unsigned long>(row),
                    static_cast<unsigned long>(col),
                    static_cast<unsigned long>(m_Rows),
                    static_cast<unsigned long>(m_Cols)));
#endif
        return m_Val[row][col];
    }

    /** Fast but unsafe method to get a reference from the matrix
     */
    inline T& get_unsafe(size_t row, size_t col)
    {
#ifdef _DEBUG
        if (row >= m_Rows || col >= m_Cols)
            THROW_EXCEPTION(
                format(
                    "Indexes (%lu,%lu) out of range. Matrix is %lux%lu",
                    static_cast<unsigned long>(row),
                    static_cast<unsigned long>(col),
                    static_cast<unsigned long>(m_Rows),
                    static_cast<unsigned long>(m_Cols)));
#endif
        return m_Val[row][col];
    }

    /** Fast but unsafe method to obtain a pointer to a given row of the matrix
     * (Use only in time critical applications)
     */
    inline T* get_unsafe_row(size_t row)
    {
#ifdef _DEBUG
        if (row >= m_Rows)
            THROW_EXCEPTION(
                format(
                    "Row index %lu out of range. Matrix is %lux%lu",
                    static_cast<unsigned long>(row),
                    static_cast<unsigned long>(m_Rows),
                    static_cast<unsigned long>(m_Cols)));
#endif
        return m_Val[row];
    }

    /** Fast but unsafe method to obtain a pointer to a given row of the matrix
     * (Use only in critical applications)
     */
    inline const T* get_unsafe_row(size_t row) const { return m_Val[row]; }
    /** Subscript operator to get a submatrix
     */
    inline CMatrixTemplate<T> operator()(
        const size_t row1, const size_t row2, const size_t col1,
        const size_t col2) const
    {
        CMatrixTemplate<T> val(0, 0);
        extractSubmatrix(row1, row2, col1, col2, val);
        return val;
    }

    /** Get a submatrix, given its bounds
     * \sa extractSubmatrixSymmetricalBlocks
     */
    void extractSubmatrix(
        const size_t row1, const size_t row2, const size_t col1,
        const size_t col2, CMatrixTemplate<T>& out) const
    {
        int nrows = int(row2) - int(row1) + 1;
        int ncols = int(col2) - int(col1) + 1;
        if (nrows <= 0 || ncols <= 0)
        {
            out.realloc(0, 0);
            return;
        }
        if (row2 >= m_Rows || col2 >= m_Cols)
            THROW_EXCEPTION("Indices out of range!");
        out.realloc(nrows, ncols);
        for (int i = 0; i < nrows; i++)
            for (int j = 0; j < ncols; j++)
                out.m_Val[i][j] = m_Val[i + row1][j + col1];
    }
    /// @overload
    template <class EIGEN_MATRIX>
    void extractSubmatrix(
        const size_t row1, const size_t row2, const size_t col1,
        const size_t col2, EIGEN_MATRIX& out) const
    {
        int nrows = int(row2) - int(row1) + 1;
        int ncols = int(col2) - int(col1) + 1;
        if (nrows <= 0 || ncols <= 0)
        {
            out = typename EIGEN_MATRIX::PlainObject();
            return;
        }
        if (row2 >= m_Rows || col2 >= m_Cols)
            THROW_EXCEPTION("Indices out of range!");
        out.resize(nrows, ncols);
        for (int i = 0; i < nrows; i++)
            for (int j = 0; j < ncols; j++)
                out.coeffRef(i, j) = m_Val[i + row1][j + col1];
    }

    /** Gets a series of contiguous rows.
     * \exception std::logic_error On index out of bounds
     * \sa extractRow
     * \sa extractColumns
     */
    inline void extractRows(
        size_t firstRow, size_t lastRow, CMatrixTemplate<T>& out) const
    {
        out.setSize(lastRow - firstRow + 1, m_Cols);
        detail::extractMatrix(*this, firstRow, 0, out);
    }

    /** Gets a series of contiguous columns.
     * \exception std::logic_error On index out of bounds
     * \sa extractColumn
     * \sa extractRows
     */
    inline void extractColumns(
        size_t firstCol, size_t lastCol, CMatrixTemplate<T>& out) const
    {
        out.setSize(m_Rows, lastCol - firstCol + 1);
        detail::extractMatrix(*this, 0, firstCol, out);
    }

    /** Returns a given column to a vector (without modifying the matrix)
     * \exception std::exception On index out of bounds
     */
    void extractCol(size_t nCol, std::vector<T>& out, int startingRow = 0) const
    {
        size_t i, n;
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
        if (nCol >= m_Cols)
            THROW_EXCEPTION("extractCol: Column index out of bounds");
#endif

        n = m_Rows - startingRow;
        out.resize(n);

        for (i = 0; i < n; i++) out[i] = m_Val[i + startingRow][nCol];
    }

    /** Gets a given column to a vector (without modifying the matrix)
     * \exception std::exception On index out of bounds
     */
    void extractCol(
        size_t nCol, CMatrixTemplate<T>& out, int startingRow = 0) const
    {
        size_t i, n;
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
        if (nCol >= m_Cols)
            THROW_EXCEPTION("extractCol: Column index out of bounds");
#endif

        n = m_Rows - startingRow;
        out.setSize(n, 1);

        for (i = 0; i < n; i++) out(i, 0) = m_Val[i + startingRow][nCol];
    }

    /** Appends a new row to the MxN matrix from a 1xN vector.
     *  The lenght of the vector must match the width of the matrix, unless
     * it's empty: in that case the matrix is resized to 1xN.
     *  \code
     *    CMatrixDouble  M(0,0);
     *    CVectorDouble  v(7),w(7);
     *    // ...
     *    M.appendRow(v);
     *    M.appendRow(w);
     *  \endcode
     * \exception std::exception On incorrect vector length.
     * \sa extractRow
     * \sa appendCol
     */
    void appendRow(const std::vector<T>& in)
    {
        size_t i, n, row;

        n = m_Cols;
        row = m_Rows;

        if (m_Cols == 0 || m_Rows == 0)
        {
            ASSERT_(!in.empty());
            n = m_Cols = in.size();
        }
        else
        {
            ASSERT_(in.size() == m_Cols);
        }

        realloc(row + 1, n);

        for (i = 0; i < n; i++) m_Val[row][i] = in[i];
    }

    /** Appends a new column to the matrix from a vector.
     * The length of the vector must match the number of rows of the matrix,
     * unless it is (0,0).
     * \exception std::exception On size mismatch.
     * \sa extractCol
     * \sa appendRow
     */
    void appendCol(const std::vector<T>& in)
    {
        size_t r = m_Rows, c = m_Cols;
        if (m_Cols == 0 || m_Rows == 0)
        {
            ASSERT_(!in.empty());
            r = in.size();
            c = 0;
        }
        else
            ASSERT_(in.size() == m_Rows);
        realloc(r, c + 1);
        for (size_t i = 0; i < m_Rows; i++) m_Val[i][m_Cols - 1] = in[i];
    }

    /** Inserts a column from a vector, replacing the current contents of that
     * column.
     * \exception std::exception On index out of bounds
     * \sa extractCol
     */
    void insertCol(size_t nCol, const std::vector<T>& in)
    {
        if (nCol >= m_Cols)
            THROW_EXCEPTION("insertCol: Row index out of bounds");

        size_t n = in.size();
        ASSERT_(m_Rows >= in.size());

        for (size_t i = 0; i < n; i++) m_Val[i][nCol] = in[i];
    }

    /** Returns a vector containing the matrix's values.
     */
    void getAsVector(std::vector<T>& out) const
    {
        out.clear();
        out.reserve(m_Rows * m_Cols);
        for (size_t i = 0; i < m_Rows; i++)
            out.insert(out.end(), &(m_Val[i][0]), &(m_Val[i][m_Cols]));
    }

};  // end of class CMatrixTemplate

/** Declares a matrix of booleans (non serializable).
 *  \sa CMatrixDouble, CMatrixFloat, CMatrixB
 */
// using CMatrixBool = CMatrixTemplate<bool>;
class CMatrixBool : public CMatrixTemplate<bool>
{
   public:
    /** Constructor */
    CMatrixBool(size_t row = 1, size_t col = 1);
    /** Copy constructor */
    CMatrixBool(const CMatrixTemplate<bool>& m);
    /** Assignment operator for float matrixes */
    CMatrixBool& operator=(const CMatrixTemplate<bool>& m);
};

}