CMatrixFixed.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 <mrpt/core/alignment_req.h>  // MRPT_MAX_STATIC_ALIGN_BYTES
#include <mrpt/core/exceptions.h>
#include <mrpt/math/MatrixBase.h>
#include <mrpt/math/math_frwds.h>  // Forward declarations
#include <mrpt/math/matrix_size_t.h>
#include <mrpt/math/point_poses2vectors.h>  // MRPT_MATRIX_CONSTRUCTORS_FROM_POSES()
#include <mrpt/typemeta/TTypeName.h>
#include <mrpt/typemeta/num_to_string.h>
#include <array>
#include <cstddef>  // std::size_t

namespace mrpt::math
{
/** A compile-time fixed-size numeric matrix container.
 * It uses a RowMajor element memory layout.
 *
 * \sa CMatrixDynamic (for dynamic-size matrices)
 * \note For a complete introduction to Matrices and vectors in MRPT, see:
 * https://www.mrpt.org/Matrices_vectors_arrays_and_Linear_Algebra_MRPT_and_Eigen_classes
 * \ingroup mrpt_math_grp
 */
template <typename T, std::size_t ROWS, std::size_t COLS>
class CMatrixFixed : public MatrixBase<T, CMatrixFixed<T, ROWS, COLS>>
{
   private:
    /** RowMajor matrix data */
    using vec_t = std::array<T, ROWS * COLS>;
    alignas(MRPT_MAX_STATIC_ALIGN_BYTES) vec_t m_data;

   public:
    /** @name Matrix type definitions
     * @{ */
    /** The type of the matrix elements */
    using value_type = T;
    using Scalar = T;
    using Index = int;
    using reference = T&;
    using const_reference = const T&;
    using size_type = int;
    using difference_type = std::ptrdiff_t;
    constexpr static int RowsAtCompileTime = ROWS;
    constexpr static int ColsAtCompileTime = COLS;
    constexpr static int SizeAtCompileTime = ROWS * COLS;
    constexpr static int is_mrpt_type = 1;
    constexpr static int StorageOrder =
        (ROWS != 1 && COLS == 1) ? 0 /*colMajor*/ : 1 /*rowMajor*/;
    using eigen_t = Eigen::Matrix<T, ROWS, COLS, StorageOrder, ROWS, COLS>;
    /** @} */

    /** @name Iterators interface
     * @{ */
    using iterator = typename vec_t::iterator;
    using const_iterator = typename vec_t::const_iterator;
    iterator begin() { return m_data.begin(); }
    iterator end() { return m_data.end(); }
    const_iterator begin() const { return m_data.begin(); }
    const_iterator end() const { return m_data.end(); }
    const_iterator cbegin() const { return m_data.begin(); }
    const_iterator cend() const { return m_data.end(); }
    /** @} */

    /** @name Constructors, assignment operators, initializers
     *  @{ */

    /** Default constructor, initializes all elements to zero */
    inline CMatrixFixed() { m_data.fill(0); }

    /** Constructor which leaves the matrix uninitialized.
     *  Example of usage: CMatrixFixed<double,3,2>
     * M(mrpt::math::UNINITIALIZED_MATRIX);
     */
    inline CMatrixFixed(TConstructorFlags_Matrices) {}

    /** Initializes from a C array with RowMajor values */
    template <size_t N>
    explicit CMatrixFixed(const T (&vals)[N])
    {
        this->loadFromArray(vals);
    }

    /** Initializes from a plain buffer with RowMajor values */
    explicit CMatrixFixed(const T* data) { this->loadFromRawPointer(data); }

    /** Convert from Eigen matrix */
    template <class Derived>
    explicit CMatrixFixed(const Eigen::MatrixBase<Derived>& m)
    {
        *this = m;
    }
    /** Convert from Eigen product */
    template <typename _Lhs, typename _Rhs, int Option>
    explicit CMatrixFixed(const Eigen::Product<_Lhs, _Rhs, Option>& p)
    {
        *this = p.eval();
    }
    /** Convert from Eigen binary op */
    template <typename Op, typename Lhs, typename Rhs>
    explicit CMatrixFixed(const Eigen::CwiseBinaryOp<Op, Lhs, Rhs>& p)
    {
        *this = p.eval();
    }

    /** Convert from Eigen block */
    template <typename VectorType, int Size>
    explicit CMatrixFixed(const Eigen::VectorBlock<VectorType, Size>& m)
    {
        *this = m;
    }

    /** Convenient ctor from size: in this class, it throws if size does not
     * match compile-time size. It is provided for the sake of offering a
     * uniform API with CMatrixDynamic. */
    CMatrixFixed(const size_type rows, const size_type cols)
    {
        ASSERT_EQUAL_(cols, static_cast<size_type>(COLS));
        ASSERT_EQUAL_(rows, static_cast<size_type>(ROWS));
    }

    MRPT_MATRIX_CONSTRUCTORS_FROM_POSES(CMatrixFixed)

    template <class MAT>
    void setFromMatrixLike(const MAT& m)
    {
        MRPT_START
        setSize(m.rows(), m.cols());
        for (Index r = 0; r < rows(); r++)
            for (Index c = 0; c < cols(); c++) (*this)(r, c) = m(r, c);
        MRPT_END
    }

    /** Assignment from an Eigen matrix */
    template <class Derived>
    CMatrixFixed& operator=(const Eigen::MatrixBase<Derived>& m)
    {
        MRPT_START
        setFromMatrixLike(m);
        return *this;
        MRPT_END
    }
    /** Assignment from an Eigen vector block */
    template <typename VectorType, int Size>
    CMatrixFixed& operator=(const Eigen::VectorBlock<VectorType, Size>& m)
    {
        MRPT_START
        setFromMatrixLike(m.eval());
        return *this;
        MRPT_END
    }

    /** Assignment from a Dynamic matrix */
    template <typename U>
    CMatrixFixed& operator=(const CMatrixDynamic<U>& m)
    {
        MRPT_START
        setFromMatrixLike(m);
        return *this;
        MRPT_END
    }

    template <typename VECTOR>
    void loadFromArray(const VECTOR& vals)
    {
        MRPT_START
        const auto LEN = std::size(vals);
        ASSERT_EQUAL_(LEN, ROWS * COLS);
        for (size_t r = 0, i = 0; r < ROWS; r++)
            for (size_t c = 0; c < COLS; c++) m_data[r * COLS + c] = vals[i++];
        MRPT_END
    }

    /** Initializes from a plain buffer with RowMajor values. Unsafe, prefer
     * loadFromArray() wherever possible, to ensure buffer length checks. */
    void loadFromRawPointer(const T* data)
    {
        for (size_t r = 0, i = 0; r < ROWS; r++)
            for (size_t c = 0; c < COLS; c++) m_data[r * COLS + c] = data[i++];
    }

    /** Throws if size does not match with the fixed matrix size */
    void setSize(
        size_t row, size_t col, [[maybe_unused]] bool zeroNewElements = false)
    {
        ASSERT_EQUAL_(row, ROWS);
        ASSERT_EQUAL_(col, COLS);
    }

    void swap(CMatrixFixed& o) { m_data.swap(o.m_data); }

    // These ones are to make template code compatible with Eigen & mrpt:
    CMatrixFixed& derived() { return *this; }
    const CMatrixFixed& derived() const { return *this; }
    void conservativeResize(size_t row, size_t col) { setSize(row, col); }

    void resize(size_t n)
    {
        if (ROWS == 1)
            ASSERT_EQUAL_(COLS, n);
        else if (COLS == 1)
            ASSERT_EQUAL_(ROWS, n);
        else
            THROW_EXCEPTION("resize() can be invoked on 1xN or Nx1 only");
    }

    /** Throws if size does not match with the fixed matrix size */
    inline void resize(
        const matrix_size_t& siz, [[maybe_unused]] bool zeroNewElements = false)
    {
        resize(siz[0], siz[1]);
    }
    void resize(size_t row, size_t col)
    {
        ASSERT_EQUAL_(row, ROWS);
        ASSERT_EQUAL_(col, COLS);
    }

    /** Number of rows in the matrix \sa rows() */
    constexpr size_type rows() const { return ROWS; }

    /** Number of columns in the matrix \sa rows() */
    constexpr size_type cols() const { return COLS; }

    /** Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x)) */
    constexpr matrix_size_t size() const
    {
        matrix_size_t dims;
        dims[0] = ROWS;
        dims[1] = COLS;
        return dims;
    }

    /** @} */

    /** @name Matrix element access & modifiers
     *  @{ */

    /** Get as an Eigen-compatible Eigen::Map object  */
    template <
        typename EIGEN_MATRIX = eigen_t,
        typename EIGEN_MAP = Eigen::Map<
            EIGEN_MATRIX, MRPT_MAX_STATIC_ALIGN_BYTES, Eigen::InnerStride<1>>>
    EIGEN_MAP asEigen()
    {
        static_assert(
            std::is_same_v<EIGEN_MATRIX, eigen_t>,
            "Please, do not override the default template arguments of this "
            "method.");
        return EIGEN_MAP(&m_data[0], ROWS, COLS);
    }

    /** \overload (const version) */
    template <
        typename EIGEN_MATRIX = eigen_t,
        typename EIGEN_MAP = Eigen::Map<
            const EIGEN_MATRIX, MRPT_MAX_STATIC_ALIGN_BYTES,
            Eigen::InnerStride<1>>>
    EIGEN_MAP asEigen() const
    {
        static_assert(
            std::is_same_v<EIGEN_MATRIX, eigen_t>,
            "Please, do not override the default template arguments of this "
            "method.");
        return EIGEN_MAP(&m_data[0], ROWS, COLS);
    }

    /** Return raw pointer to row-major data buffer. All matrix cells can be
     * assumed to be stored contiguously in memory, i.e. row stride = column
     * count. */
    const T* data() const
    {
        ASSERT_(!m_data.empty());
        return &m_data[0];
    }
    /// \overload
    T* data()
    {
        ASSERT_(!m_data.empty());
        return &m_data[0];
    }

    /** Access (row,col), without out-of-bounds check (except in Debug builds)
     */
    inline T& operator()(int row, int col)
    {
        ASSERTDEB_(static_cast<std::size_t>(row) < ROWS);
        ASSERTDEB_(static_cast<std::size_t>(col) < COLS);
        return m_data[row * COLS + col];
    }
    inline const T& operator()(int row, int col) const
    {
        ASSERTDEB_(static_cast<std::size_t>(row) < ROWS);
        ASSERTDEB_(static_cast<std::size_t>(col) < COLS);
        return m_data[row * COLS + col];
    }

    /** Access the i-th element, Row-Major order, without out-of-bounds check
     * (except in Debug builds)
     */
    inline T& operator()(int i)
    {
        ASSERTDEB_(static_cast<std::size_t>(i) < ROWS * COLS);
        return m_data[i];
    }
    inline const T& operator()(int i) const
    {
        ASSERTDEB_(static_cast<std::size_t>(i) < ROWS * COLS);
        return m_data[i];
    }

    /** Access the [i-th] element (for 1xN or Nx1 matrices) */
    inline T& operator[](int i)
    {
        ASSERT_(ROWS == 1 || COLS == 1);
        ASSERTDEB_(static_cast<std::size_t>(i) < ROWS * COLS);
        return m_data[i];
    }
    inline const T& operator[](int i) const
    {
        ASSERT_(ROWS == 1 || COLS == 1);
        ASSERTDEB_(static_cast<std::size_t>(i) < ROWS * COLS);
        return m_data[i];
    }

    CMatrixFixed<float, ROWS, COLS> cast_float() const;
    CMatrixFixed<double, ROWS, COLS> cast_double() const;

    /** Solves the linear system Ax=b, returns x, with A this **symmetric**
     * matrix. \sa lu_solve() */
    CMatrixFixed<T, ROWS, 1> llt_solve(const CMatrixFixed<T, ROWS, 1>& b) const;

    /** Solves the linear system Ax=b, returns x, with A this **asymmetric**
     * matrix. \sa llt_solve() */
    CMatrixFixed<T, ROWS, 1> lu_solve(const CMatrixFixed<T, ROWS, 1>& b) const;

    /** this += A<sup>T</sup> */
    void sum_At(const CMatrixFixed<Scalar, ROWS, COLS>& A)
    {
        if constexpr (ROWS == COLS)
        {
            for (Index r = 0; r < static_cast<Index>(ROWS); r++)
                for (Index c = 0; c < static_cast<Index>(COLS); c++)
                    (*this)(r, c) += A(c, r);
        }
        else
        {
            throw std::runtime_error("sum_At(): matrix must be square.");
        }
    }

    /** @} */
};

/** @name Typedefs for common sizes
    @{ */
using CMatrixDouble22 = CMatrixFixed<double, 2, 2>;
using CMatrixDouble23 = CMatrixFixed<double, 2, 3>;
using CMatrixDouble32 = CMatrixFixed<double, 3, 2>;
using CMatrixDouble33 = CMatrixFixed<double, 3, 3>;
using CMatrixDouble44 = CMatrixFixed<double, 4, 4>;
using CMatrixDouble66 = CMatrixFixed<double, 6, 6>;
using CMatrixDouble77 = CMatrixFixed<double, 7, 7>;
using CMatrixDouble13 = CMatrixFixed<double, 1, 3>;
using CMatrixDouble31 = CMatrixFixed<double, 3, 1>;
using CMatrixDouble34 = CMatrixFixed<double, 3, 4>;
using CMatrixDouble36 = CMatrixFixed<double, 3, 6>;
using CMatrixDouble12 = CMatrixFixed<double, 1, 2>;
using CMatrixDouble21 = CMatrixFixed<double, 2, 1>;
using CMatrixDouble61 = CMatrixFixed<double, 6, 1>;
using CMatrixDouble16 = CMatrixFixed<double, 1, 6>;
using CMatrixDouble71 = CMatrixFixed<double, 7, 1>;
using CMatrixDouble17 = CMatrixFixed<double, 1, 7>;
using CMatrixDouble51 = CMatrixFixed<double, 5, 1>;
using CMatrixDouble15 = CMatrixFixed<double, 1, 5>;
using CMatrixDouble41 = CMatrixFixed<double, 4, 1>;
using CMatrixDouble43 = CMatrixFixed<double, 4, 3>;
using CMatrixDouble6_12 = CMatrixFixed<double, 6, 12>;
using CMatrixDouble12_6 = CMatrixFixed<double, 12, 6>;
using CMatrixDouble39 = CMatrixFixed<double, 3, 9>;
using CMatrixDouble93 = CMatrixFixed<double, 9, 3>;

using CMatrixFloat22 = CMatrixFixed<float, 2, 2>;
using CMatrixFloat23 = CMatrixFixed<float, 2, 3>;
using CMatrixFloat32 = CMatrixFixed<float, 3, 2>;
using CMatrixFloat33 = CMatrixFixed<float, 3, 3>;
using CMatrixFloat44 = CMatrixFixed<float, 4, 4>;
using CMatrixFloat66 = CMatrixFixed<float, 6, 6>;
using CMatrixFloat77 = CMatrixFixed<float, 7, 7>;
using CMatrixFloat13 = CMatrixFixed<float, 1, 3>;
using CMatrixFloat31 = CMatrixFixed<float, 3, 1>;
using CMatrixFloat12 = CMatrixFixed<float, 1, 2>;
using CMatrixFloat21 = CMatrixFixed<float, 2, 1>;
using CMatrixFloat61 = CMatrixFixed<float, 6, 1>;
using CMatrixFloat16 = CMatrixFixed<float, 1, 6>;
using CMatrixFloat71 = CMatrixFixed<float, 7, 1>;
using CMatrixFloat17 = CMatrixFixed<float, 1, 7>;
using CMatrixFloat51 = CMatrixFixed<float, 5, 1>;
using CMatrixFloat15 = CMatrixFixed<float, 1, 5>;
/**  @} */

}  // namespace mrpt::math

namespace mrpt::typemeta
{
template <typename T, std::size_t N, std::size_t M>
struct TTypeName<mrpt::math::CMatrixFixed<T, N, M>>
{
    constexpr static auto get()
    {
        return literal("CMatrixFixed<") + TTypeName<T>::get() + literal(",") +
               literal(num_to_string<N>::value) + literal(",") +
               literal(num_to_string<M>::value) + literal(">");
    }
};
}  // namespace mrpt::typemeta