MRPT  2.0.1
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-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
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;
50  using difference_type = std::ptrdiff_t;
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  {
88  this->loadFromArray(vals);
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>
171  void loadFromArray(const VECTOR& vals)
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. */
183  void loadFromRawPointer(const T* data)
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  /** Return raw pointer to row-major data buffer. All matrix cells can be
276  * assumed to be stored contiguously in memory, i.e. row stride = column
277  * count. */
278  const T* data() const
279  {
280  ASSERT_(!m_data.empty());
281  return &m_data[0];
282  }
283  /// \overload
284  T* data()
285  {
286  ASSERT_(!m_data.empty());
287  return &m_data[0];
288  }
289 
290  /** Access (row,col), without out-of-bounds check (except in Debug builds)
291  */
292  inline T& operator()(int row, int col)
293  {
294  ASSERTDEB_(static_cast<std::size_t>(row) < ROWS);
295  ASSERTDEB_(static_cast<std::size_t>(col) < COLS);
296  return m_data[row * COLS + col];
297  }
298  inline const T& operator()(int row, int col) const
299  {
300  ASSERTDEB_(static_cast<std::size_t>(row) < ROWS);
301  ASSERTDEB_(static_cast<std::size_t>(col) < COLS);
302  return m_data[row * COLS + col];
303  }
304 
305  /** Access the i-th element, Row-Major order, without out-of-bounds check
306  * (except in Debug builds)
307  */
308  inline T& operator()(int i)
309  {
310  ASSERTDEB_(static_cast<std::size_t>(i) < ROWS * COLS);
311  return m_data[i];
312  }
313  inline const T& operator()(int i) const
314  {
315  ASSERTDEB_(static_cast<std::size_t>(i) < ROWS * COLS);
316  return m_data[i];
317  }
318 
319  /** Access the [i-th] element (for 1xN or Nx1 matrices) */
320  inline T& operator[](int i)
321  {
322  ASSERT_(ROWS == 1 || COLS == 1);
323  ASSERTDEB_(static_cast<std::size_t>(i) < ROWS * COLS);
324  return m_data[i];
325  }
326  inline const T& operator[](int i) const
327  {
328  ASSERT_(ROWS == 1 || COLS == 1);
329  ASSERTDEB_(static_cast<std::size_t>(i) < ROWS * COLS);
330  return m_data[i];
331  }
332 
335 
336  /** Solves the linear system Ax=b, returns x, with A this **symmetric**
337  * matrix. \sa lu_solve() */
339 
340  /** Solves the linear system Ax=b, returns x, with A this **asymmetric**
341  * matrix. \sa llt_solve() */
343 
344  /** this += A<sup>T</sup> */
346  {
347  if constexpr (ROWS == COLS)
348  {
349  for (Index r = 0; r < static_cast<Index>(ROWS); r++)
350  for (Index c = 0; c < static_cast<Index>(COLS); c++)
351  (*this)(r, c) += A(c, r);
352  }
353  else
354  {
355  throw std::runtime_error("sum_At(): matrix must be square.");
356  }
357  }
358 
359  /** @} */
360 };
361 
362 /** @name Typedefs for common sizes
363  @{ */
389 
407 /** @} */
408 
409 } // namespace mrpt::math
410 
411 namespace mrpt::typemeta
412 {
413 template <typename T, std::size_t N, std::size_t M>
415 {
416  constexpr static auto get()
417  {
418  return literal("CMatrixFixed<") + TTypeName<T>::get() + literal(",") +
421  }
422 };
423 } // namespace mrpt::typemeta
T & operator()(int row, int col)
Access (row,col), without out-of-bounds check (except in Debug builds)
Definition: CMatrixFixed.h:292
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
const T * data() const
Return raw pointer to row-major data buffer.
Definition: CMatrixFixed.h:278
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
size_t size(const MATRIXLIKE &m, const int dim)
const_iterator cbegin() const
Definition: CMatrixFixed.h:68
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:320
const T & operator()(int i) const
Definition: CMatrixFixed.h:313
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:308
A template to obtain the type of its argument as a string at compile time.
Definition: TTypeName.h:69
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
void loadFromArray(const VECTOR &vals)
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
void setFromMatrixLike(const MAT &m)
Definition: CMatrixFixed.h:132
void sum_At(const CMatrixFixed< Scalar, ROWS, COLS > &A)
this += AT
Definition: CMatrixFixed.h:345
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
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:298
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
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
const T & operator[](int i) const
Definition: CMatrixFixed.h:326
CMatrixFixed< float, ROWS, COLS > cast_float() const
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:190
#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
void swap(CMatrixFixed &o)
Definition: CMatrixFixed.h:197
This template class provides the basic functionality for a general 2D any-size, resizable container o...
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
void loadFromRawPointer(const T *data)
Initializes from a plain buffer with RowMajor values.
Definition: CMatrixFixed.h:183
static constexpr int RowsAtCompileTime
Definition: CMatrixFixed.h:51
const_iterator begin() const
Definition: CMatrixFixed.h:66



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020