MRPT  1.9.9
CMatrixFixed.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10
11 #include <mrpt/core/alignment_req.h> // MRPT_MAX_STATIC_ALIGN_BYTES
12 #include <mrpt/core/exceptions.h>
13 #include <mrpt/math/MatrixBase.h>
14 #include <mrpt/math/math_frwds.h> // Forward declarations
16 #include <mrpt/math/point_poses2vectors.h> // MRPT_MATRIX_CONSTRUCTORS_FROM_POSES()
19 #include <array>
20 #include <cstddef> // std::size_t
21
22 namespace mrpt::math
23 {
24 /** A compile-time fixed-size numeric matrix container.
25  * It uses a RowMajor element memory layout.
26  *
27  * \sa CMatrixDynamic (for dynamic-size matrices)
28  * \note For a complete introduction to Matrices and vectors in MRPT, see:
29  * https://www.mrpt.org/Matrices_vectors_arrays_and_Linear_Algebra_MRPT_and_Eigen_classes
30  * \ingroup mrpt_math_grp
31  */
32 template <typename T, std::size_t ROWS, std::size_t COLS>
33 class CMatrixFixed : public MatrixBase<T, CMatrixFixed<T, ROWS, COLS>>
34 {
35  private:
36  /** RowMajor matrix data */
37  using vec_t = std::array<T, ROWS * COLS>;
38  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) vec_t m_data;
39
40  public:
41  /** @name Matrix type definitions
42  * @{ */
43  /** The type of the matrix elements */
44  using value_type = T;
45  using Scalar = T;
46  using Index = int;
47  using reference = T&;
48  using const_reference = const T&;
49  using size_type = int;
51  constexpr static int RowsAtCompileTime = ROWS;
52  constexpr static int ColsAtCompileTime = COLS;
53  constexpr static int SizeAtCompileTime = ROWS * COLS;
54  constexpr static int is_mrpt_type = 1;
55  constexpr static int StorageOrder =
56  (ROWS != 1 && COLS == 1) ? 0 /*colMajor*/ : 1 /*rowMajor*/;
58  /** @} */
59
60  /** @name Iterators interface
61  * @{ */
62  using iterator = typename vec_t::iterator;
63  using const_iterator = typename vec_t::const_iterator;
64  iterator begin() { return m_data.begin(); }
65  iterator end() { return m_data.end(); }
66  const_iterator begin() const { return m_data.begin(); }
67  const_iterator end() const { return m_data.end(); }
68  const_iterator cbegin() const { return m_data.begin(); }
69  const_iterator cend() const { return m_data.end(); }
70  /** @} */
71
72  /** @name Constructors, assignment operators, initializers
73  * @{ */
74
75  /** Default constructor, initializes all elements to zero */
76  inline CMatrixFixed() { m_data.fill(0); }
77
78  /** Constructor which leaves the matrix uninitialized.
79  * Example of usage: CMatrixFixed<double,3,2>
80  * M(mrpt::math::UNINITIALIZED_MATRIX);
81  */
83
84  /** Initializes from a C array with RowMajor values */
85  template <size_t N>
86  explicit CMatrixFixed(const T (&vals)[N])
87  {
89  }
90
91  /** Initializes from a plain buffer with RowMajor values */
92  explicit CMatrixFixed(const T* data) { this->loadFromRawPointer(data); }
93
94  /** Convert from Eigen matrix */
95  template <class Derived>
97  {
98  *this = m;
99  }
100  /** Convert from Eigen product */
101  template <typename _Lhs, typename _Rhs, int Option>
103  {
104  *this = p.eval();
105  }
106  /** Convert from Eigen binary op */
107  template <typename Op, typename Lhs, typename Rhs>
109  {
110  *this = p.eval();
111  }
112
113  /** Convert from Eigen block */
114  template <typename VectorType, int Size>
116  {
117  *this = m;
118  }
119
120  /** Convenient ctor from size: in this class, it throws if size does not
121  * match compile-time size. It is provided for the sake of offering a
122  * uniform API with CMatrixDynamic. */
124  {
125  ASSERT_EQUAL_(cols, static_cast<size_type>(COLS));
126  ASSERT_EQUAL_(rows, static_cast<size_type>(ROWS));
127  }
128
130
131  template <class MAT>
132  void setFromMatrixLike(const MAT& m)
133  {
134  MRPT_START
135  setSize(m.rows(), m.cols());
136  for (Index r = 0; r < rows(); r++)
137  for (Index c = 0; c < cols(); c++) (*this)(r, c) = m(r, c);
138  MRPT_END
139  }
140
141  /** Assignment from an Eigen matrix */
142  template <class Derived>
144  {
145  MRPT_START
147  return *this;
148  MRPT_END
149  }
150  /** Assignment from an Eigen vector block */
151  template <typename VectorType, int Size>
153  {
154  MRPT_START
155  setFromMatrixLike(m.eval());
156  return *this;
157  MRPT_END
158  }
159
160  /** Assignment from a Dynamic matrix */
161  template <typename U>
163  {
164  MRPT_START
166  return *this;
167  MRPT_END
168  }
169
170  template <typename VECTOR>
172  {
173  MRPT_START
174  const auto LEN = std::size(vals);
175  ASSERT_EQUAL_(LEN, ROWS * COLS);
176  for (size_t r = 0, i = 0; r < ROWS; r++)
177  for (size_t c = 0; c < COLS; c++) m_data[r * COLS + c] = vals[i++];
178  MRPT_END
179  }
180
181  /** Initializes from a plain buffer with RowMajor values. Unsafe, prefer
182  * loadFromArray() wherever possible, to ensure buffer length checks. */
184  {
185  for (size_t r = 0, i = 0; r < ROWS; r++)
186  for (size_t c = 0; c < COLS; c++) m_data[r * COLS + c] = data[i++];
187  }
188
189  /** Throws if size does not match with the fixed matrix size */
190  void setSize(
191  size_t row, size_t col, [[maybe_unused]] bool zeroNewElements = false)
192  {
193  ASSERT_EQUAL_(row, ROWS);
194  ASSERT_EQUAL_(col, COLS);
195  }
196
197  void swap(CMatrixFixed& o) { m_data.swap(o.m_data); }
198
199  // These ones are to make template code compatible with Eigen & mrpt:
200  CMatrixFixed& derived() { return *this; }
201  const CMatrixFixed& derived() const { return *this; }
202  void conservativeResize(size_t row, size_t col) { setSize(row, col); }
203
204  void resize(size_t n)
205  {
206  if (ROWS == 1)
207  ASSERT_EQUAL_(COLS, n);
208  else if (COLS == 1)
209  ASSERT_EQUAL_(ROWS, n);
210  else
211  THROW_EXCEPTION("resize() can be invoked on 1xN or Nx1 only");
212  }
213
214  /** Throws if size does not match with the fixed matrix size */
215  inline void resize(
216  const matrix_size_t& siz, [[maybe_unused]] bool zeroNewElements = false)
217  {
218  resize(siz[0], siz[1]);
219  }
220  void resize(size_t row, size_t col)
221  {
222  ASSERT_EQUAL_(row, ROWS);
223  ASSERT_EQUAL_(col, COLS);
224  }
225
226  /** Number of rows in the matrix \sa rows() */
227  constexpr size_type rows() const { return ROWS; }
228
229  /** Number of columns in the matrix \sa rows() */
230  constexpr size_type cols() const { return COLS; }
231
232  /** Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x)) */
233  constexpr matrix_size_t size() const
234  {
235  matrix_size_t dims;
236  dims[0] = ROWS;
237  dims[1] = COLS;
238  return dims;
239  }
240
241  /** @} */
242
243  /** @name Matrix element access & modifiers
244  * @{ */
245
246  /** Get as an Eigen-compatible Eigen::Map object */
247  template <
248  typename EIGEN_MATRIX = eigen_t,
249  typename EIGEN_MAP = Eigen::Map<
250  EIGEN_MATRIX, MRPT_MAX_STATIC_ALIGN_BYTES, Eigen::InnerStride<1>>>
251  EIGEN_MAP asEigen()
252  {
253  static_assert(
254  std::is_same_v<EIGEN_MATRIX, eigen_t>,
255  "Please, do not override the default template arguments of this "
256  "method.");
257  return EIGEN_MAP(&m_data[0], ROWS, COLS);
258  }
259
260  /** \overload (const version) */
261  template <
262  typename EIGEN_MATRIX = eigen_t,
263  typename EIGEN_MAP = Eigen::Map<
264  const EIGEN_MATRIX, MRPT_MAX_STATIC_ALIGN_BYTES,
266  EIGEN_MAP asEigen() const
267  {
268  static_assert(
269  std::is_same_v<EIGEN_MATRIX, eigen_t>,
270  "Please, do not override the default template arguments of this "
271  "method.");
272  return EIGEN_MAP(&m_data[0], ROWS, COLS);
273  }
274
275  /** Access (row,col), without out-of-bounds check (except in Debug builds)
276  */
277  inline T& operator()(int row, int col)
278  {
279  ASSERTDEB_(static_cast<std::size_t>(row) < ROWS);
280  ASSERTDEB_(static_cast<std::size_t>(col) < COLS);
281  return m_data[row * COLS + col];
282  }
283  inline const T& operator()(int row, int col) const
284  {
285  ASSERTDEB_(static_cast<std::size_t>(row) < ROWS);
286  ASSERTDEB_(static_cast<std::size_t>(col) < COLS);
287  return m_data[row * COLS + col];
288  }
289
290  /** Access the i-th element, Row-Major order, without out-of-bounds check
291  * (except in Debug builds)
292  */
293  inline T& operator()(int i)
294  {
295  ASSERTDEB_(static_cast<std::size_t>(i) < ROWS * COLS);
296  return m_data[i];
297  }
298  inline const T& operator()(int i) const
299  {
300  ASSERTDEB_(static_cast<std::size_t>(i) < ROWS * COLS);
301  return m_data[i];
302  }
303
304  /** Access the [i-th] element (for 1xN or Nx1 matrices) */
305  inline T& operator[](int i)
306  {
307  ASSERT_(ROWS == 1 || COLS == 1);
308  ASSERTDEB_(static_cast<std::size_t>(i) < ROWS * COLS);
309  return m_data[i];
310  }
311  inline const T& operator[](int i) const
312  {
313  ASSERT_(ROWS == 1 || COLS == 1);
314  ASSERTDEB_(static_cast<std::size_t>(i) < ROWS * COLS);
315  return m_data[i];
316  }
317
320
321  /** Solves the linear system Ax=b, returns x, with A this **symmetric**
322  * matrix. \sa lu_solve() */
324
325  /** Solves the linear system Ax=b, returns x, with A this **asymmetric**
326  * matrix. \sa llt_solve() */
328
329  /** this += A<sup>T</sup> */
331  {
332  if constexpr (ROWS == COLS)
333  {
334  for (Index r = 0; r < static_cast<Index>(ROWS); r++)
335  for (Index c = 0; c < static_cast<Index>(COLS); c++)
336  (*this)(r, c) += A(c, r);
337  }
338  else
339  {
340  throw std::runtime_error("sum_At(): matrix must be square.");
341  }
342  }
343
344  /** @} */
345 };
346
347 /** @name Typedefs for common sizes
348  @{ */
371
389 /** @} */
390
391 } // namespace mrpt::math
392
393 namespace mrpt::typemeta
394 {
395 template <typename T, std::size_t N, std::size_t M>
397 {
398  constexpr static auto get()
399  {
400  return literal("CMatrixFixed<") + TTypeName<T>::get() + literal(",") +
403  }
404 };
405 } // namespace mrpt::typemeta
T & operator()(int row, int col)
Access (row,col), without out-of-bounds check (except in Debug builds)
Definition: CMatrixFixed.h:277
CMatrixFixed()
Default constructor, initializes all elements to zero.
Definition: CMatrixFixed.h:76
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
static constexpr int StorageOrder
Definition: CMatrixFixed.h:55
#define MRPT_START
Definition: exceptions.h:241
TConstructorFlags_Matrices
For usage in one of the constructors of CMatrixFixed or CMatrixDynamic (and derived classes)...
Definition: math_frwds.h:55
EIGEN_MAP asEigen() const
Definition: CMatrixFixed.h:266
constexpr matrix_size_t size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
Definition: CMatrixFixed.h:233
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
const_iterator cbegin() const
Definition: CMatrixFixed.h:68
GLenum GLsizei n
Definition: glext.h:5136
const CMatrixFixed & derived() const
Definition: CMatrixFixed.h:201
static constexpr int SizeAtCompileTime
Definition: CMatrixFixed.h:53
void conservativeResize(size_t row, size_t col)
Definition: CMatrixFixed.h:202
CMatrixFixed(const Eigen::CwiseBinaryOp< Op, Lhs, Rhs > &p)
Convert from Eigen binary op.
Definition: CMatrixFixed.h:108
void resize(const matrix_size_t &siz, [[maybe_unused]] bool zeroNewElements=false)
Throws if size does not match with the fixed matrix size.
Definition: CMatrixFixed.h:215
CMatrixFixed & derived()
Definition: CMatrixFixed.h:200
T & operator[](int i)
Access the [i-th] element (for 1xN or Nx1 matrices)
Definition: CMatrixFixed.h:305
const T & operator()(int i) const
Definition: CMatrixFixed.h:298
static constexpr int is_mrpt_type
Definition: CMatrixFixed.h:54
CMatrixFixed(TConstructorFlags_Matrices)
Constructor which leaves the matrix uninitialized.
Definition: CMatrixFixed.h:82
T & operator()(int i)
Access the i-th element, Row-Major order, without out-of-bounds check (except in Debug builds) ...
Definition: CMatrixFixed.h:293
A template to obtain the type of its argument as a string at compile time.
Definition: TTypeName.h:69
Definition: inflate.h:44
CMatrixFixed(const size_type rows, const size_type cols)
Convenient ctor from size: in this class, it throws if size does not match compile-time size...
Definition: CMatrixFixed.h:123
CMatrixFixed< T, ROWS, 1 > llt_solve(const CMatrixFixed< T, ROWS, 1 > &b) const
Solves the linear system Ax=b, returns x, with A this symmetric matrix.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
Definition: CMatrixFixed.h:171
void resize(size_t row, size_t col)
Definition: CMatrixFixed.h:220
This base provides a set of functions for maths stuff.
typename vec_t::const_iterator const_iterator
Definition: CMatrixFixed.h:63
CMatrixFixed(const Eigen::Product< _Lhs, _Rhs, Option > &p)
Convert from Eigen product.
Definition: CMatrixFixed.h:102
constexpr auto literal(const char(&lit)[N_PLUS_1]) -> string_literal< N_PLUS_1 - 1 >
Definition: static_string.h:46
CMatrixFixed< T, ROWS, 1 > lu_solve(const CMatrixFixed< T, ROWS, 1 > &b) const
Solves the linear system Ax=b, returns x, with A this asymmetric matrix.
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
const GLubyte * c
Definition: glext.h:6406
void setFromMatrixLike(const MAT &m)
Definition: CMatrixFixed.h:132
void sum_At(const CMatrixFixed< Scalar, ROWS, COLS > &A)
this += AT
Definition: CMatrixFixed.h:330
CMatrixFixed & operator=(const Eigen::VectorBlock< VectorType, Size > &m)
Assignment from an Eigen vector block.
Definition: CMatrixFixed.h:152
std::array< double, ROWS *COLS > vec_t
RowMajor matrix data.
Definition: CMatrixFixed.h:37
Eigen::Matrix< T, ROWS, COLS, StorageOrder, ROWS, COLS > eigen_t
Definition: CMatrixFixed.h:57
const_iterator end() const
Definition: CMatrixFixed.h:67
static constexpr int ColsAtCompileTime
Definition: CMatrixFixed.h:52
GLubyte GLubyte b
Definition: glext.h:6372
CMatrixFixed & operator=(const Eigen::MatrixBase< Derived > &m)
Assignment from an Eigen matrix.
Definition: CMatrixFixed.h:143
const T & operator()(int row, int col) const
Definition: CMatrixFixed.h:283
double value_type
The type of the matrix elements.
Definition: CMatrixFixed.h:44
CMatrixFixed(const T *data)
Initializes from a plain buffer with RowMajor values.
Definition: CMatrixFixed.h:92
CMatrixFixed(const T(&vals)[N])
Initializes from a C array with RowMajor values.
Definition: CMatrixFixed.h:86
_W64 int ptrdiff_t
Definition: glew.h:136
constexpr size_type rows() const
Number of rows in the matrix.
Definition: CMatrixFixed.h:227
const_iterator cend() const
Definition: CMatrixFixed.h:69
Base CRTP class for all MRPT matrices.
Definition: MatrixBase.h:23
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CMatrixFixed & operator=(const CMatrixDynamic< U > &m)
Assignment from a Dynamic matrix.
Definition: CMatrixFixed.h:162
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
const T & operator[](int i) const
Definition: CMatrixFixed.h:311
CMatrixFixed< float, ROWS, COLS > cast_float() const
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:190
GLenum GLenum GLvoid * row
Definition: glext.h:3580
#define MRPT_END
Definition: exceptions.h:245
CMatrixFixed(const Eigen::MatrixBase< Derived > &m)
Convert from Eigen matrix.
Definition: CMatrixFixed.h:96
CMatrixFixed(const Eigen::VectorBlock< VectorType, Size > &m)
Convert from Eigen block.
Definition: CMatrixFixed.h:115
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
#define MRPT_MATRIX_CONSTRUCTORS_FROM_POSES(_CLASS_)
constexpr size_type cols() const
Number of columns in the matrix.
Definition: CMatrixFixed.h:230
GLsizeiptr size
Definition: glext.h:3934
void swap(CMatrixFixed &o)
Definition: CMatrixFixed.h:197
This template class provides the basic functionality for a general 2D any-size, resizable container o...
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3550
GLfloat GLfloat p
Definition: glext.h:6398
CMatrixFixed< double, ROWS, COLS > cast_double() const
Auxiliary class used in CMatrixDynamic:size(), CMatrixDynamic::resize(), CMatrixFixed::size(), CMatrixFixed::resize(), to mimic the behavior of STL-containers.
Definition: matrix_size_t.h:20
constexpr string representation of a number.
Definition: num_to_string.h:44
static constexpr auto get()
Definition: TTypeName.h:71
void setSize(size_t row, size_t col, [[maybe_unused]] bool zeroNewElements=false)
Throws if size does not match with the fixed matrix size.
Definition: CMatrixFixed.h:190