MRPT  1.9.9
MatrixBase.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 
12 
13 namespace mrpt::math
14 {
15 /** Base CRTP class for all MRPT matrices.
16  *
17  * See MatrixVectorBase
18  *
19  * \sa CMatrixFixed
20  * \ingroup mrpt_math_grp
21  */
22 template <typename Scalar, class Derived>
23 class MatrixBase : public MatrixVectorBase<Scalar, Derived>
24 {
25  public:
26  Derived& mbDerived() { return static_cast<Derived&>(*this); }
27  const Derived& mbDerived() const
28  {
29  return static_cast<const Derived&>(*this);
30  }
31 
32  /** Resize to NxN, set all entries to zero, except the main diagonal which
33  * is set to `value` */
34  void setDiagonal(const std::size_t N, const Scalar value)
35  {
36  mbDerived().resize(N, N);
37  for (typename Derived::Index r = 0; r < mbDerived().rows(); r++)
38  for (typename Derived::Index c = 0; c < mbDerived().cols(); c++)
39  mbDerived()(r, c) = (r == c) ? value : 0;
40  }
41  /** Set all entries to zero, except the main diagonal which is set to
42  * `value` */
43  void setDiagonal(const Scalar value)
44  {
45  ASSERT_EQUAL_(mbDerived().cols(), mbDerived().rows());
46  setDiagonal(mbDerived().cols(), value);
47  }
48  /** Resizes to NxN, with N the length of the input vector, set all entries
49  * to zero, except the main diagonal which is set to values in the vector.
50  */
51  void setDiagonal(const std::vector<Scalar>& diags)
52  {
53  const std::size_t N = diags.size();
54  mbDerived().setZero(N, N);
55  for (std::size_t i = 0; i < N; i++) mbDerived()(i, i) = diags[i];
56  }
57  void setIdentity()
58  {
59  ASSERT_EQUAL_(mbDerived().rows(), mbDerived().cols());
60  setDiagonal(mbDerived().cols(), 1);
61  }
62  void setIdentity(const std::size_t N) { setDiagonal(N, 1); }
63 
64  static Derived Identity()
65  {
66  ASSERTMSG_(
67  Derived::RowsAtCompileTime > 0 && Derived::ColsAtCompileTime > 0,
68  "Identity() without arguments can be used only for fixed-size "
69  "matrices/vectors");
70  Derived m;
71  m.setIdentity();
72  return m;
73  }
74  static Derived Identity(const std::size_t N)
75  {
76  Derived m;
77  m.setIdentity(N);
78  return m;
79  }
80 
81  /** this = A*B, with A & B of the same type of this.
82  * For products of different matrix types, use the regular * operator (which
83  * requires the `<Eigen/Dense>` header) */
84  void matProductOf_AB(const Derived& A, const Derived& B);
85 
86  /** @name Operations that DO require `#include <Eigen/Dense>` in user code
87  * @{ */
88 
89  auto col(int colIdx)
90  {
91  internalAssertEigenDefined<Derived>();
92  return mbDerived().asEigen().col(colIdx);
93  }
94  auto col(int colIdx) const
95  {
96  internalAssertEigenDefined<Derived>();
97  return mbDerived().asEigen().col(colIdx);
98  }
99 
100  auto row(int rowIdx)
101  {
102  internalAssertEigenDefined<Derived>();
103  return mbDerived().asEigen().row(rowIdx);
104  }
105  auto row(int rowIdx) const
106  {
107  internalAssertEigenDefined<Derived>();
108  return mbDerived().asEigen().row(rowIdx);
109  }
110 
111  template <typename VECTOR_LIKE>
112  void extractRow(int rowIdx, VECTOR_LIKE& v) const
113  {
114  ASSERT_BELOW_(rowIdx, mbDerived().rows());
115  v.resize(mbDerived().cols());
116  for (typename Derived::Index i = 0; i < mbDerived().cols(); i++)
117  v[i] = mbDerived().coeff(rowIdx, i);
118  }
119  template <typename VECTOR_LIKE>
120  VECTOR_LIKE extractRow(int rowIdx) const
121  {
122  VECTOR_LIKE v;
123  extractRow(rowIdx, v);
124  return v;
125  }
126 
127  template <typename VECTOR_LIKE>
128  void extractColumn(int colIdx, VECTOR_LIKE& v) const
129  {
130  ASSERT_BELOW_(colIdx, mbDerived().cols());
131  v.resize(mbDerived().rows());
132  for (typename Derived::Index i = 0; i < mbDerived().rows(); i++)
133  v[i] = mbDerived().coeff(i, colIdx);
134  }
135  template <typename VECTOR_LIKE>
136  VECTOR_LIKE extractColumn(int colIdx) const
137  {
138  VECTOR_LIKE c;
139  extractColumn(colIdx, c);
140  return c;
141  }
142 
143  /** @} */
144 
145  /** @name Standalone operations (do NOT require `#include <Eigen/Dense>`)
146  * @{ */
147  /** Determinant of matrix. */
148  Scalar det() const;
149 
150  /** Returns the inverse of a general matrix using LU */
151  Derived inverse() const;
152 
153  /** Returns the inverse of a symmetric matrix using LLt */
154  Derived inverse_LLt() const;
155 
156  /** Finds the rank of the matrix via LU decomposition.
157  * Uses Eigen's default threshold unless `threshold>0`. */
158  int rank(Scalar threshold = 0) const;
159 
160  /** Cholesky M=U<sup>T</sup> * U decomposition for symmetric matrix
161  * (upper-half of the matrix is actually ignored.
162  * \return false if Cholesky fails
163  */
164  bool chol(Derived& U) const;
165 
166  /** Computes the eigenvectors and eigenvalues for a square, general matrix.
167  * Use eig_symmetric() for symmetric matrices for better accuracy and
168  * performance.
169  * Eigenvectors are the columns of the returned matrix, and their order
170  * matches that of returned eigenvalues.
171  * \param[in] sorted If true, eigenvalues (and eigenvectors) will be sorted
172  * in ascending order.
173  * \param[out] eVecs The container where eigenvectors will be stored.
174  * \param[out] eVals The container where eigenvalues will be stored.
175  * \return false if eigenvalues could not be determined.
176  */
177  bool eig(
178  Derived& eVecs, std::vector<Scalar>& eVals, bool sorted = true) const;
179 
180  /** Read: eig()
181  * \note This only uses the **lower-triangular** part of the matrix */
182  bool eig_symmetric(
183  Derived& eVecs, std::vector<Scalar>& eVals, bool sorted = true) const;
184 
185  /** Returns the maximum value in the diagonal. */
186  Scalar maximumDiagonal() const;
187  /** Returns the minimum value in the diagonal. */
188  Scalar minimumDiagonal() const;
189 
190  /** Returns the trace of the matrix (not necessarily square). */
191  Scalar trace() const;
192 
193  /** Removes columns of the matrix.
194  * This "unsafe" version assumes indices sorted in ascending order. */
195  void unsafeRemoveColumns(const std::vector<std::size_t>& idxs);
196 
197  /** Removes columns of the matrix. Indices may be unsorted and duplicated */
198  void removeColumns(const std::vector<std::size_t>& idxsToRemove);
199 
200  /** Removes rows of the matrix.
201  * This "unsafe" version assumes indices sorted in ascending order. */
202  void unsafeRemoveRows(const std::vector<std::size_t>& idxs);
203 
204  /** Removes rows of the matrix. Indices may be unsorted and duplicated */
205  void removeRows(const std::vector<std::size_t>& idxsToRemove);
206 
207  /** Copies the given input submatrix/vector into this matrix/vector,
208  * starting at the given top-left coordinates. */
209  template <typename OTHERMATVEC>
211  const int row_start, const int col_start, const OTHERMATVEC& submat)
212  {
213  ASSERT_BELOWEQ_(row_start + submat.rows(), mbDerived().rows());
214  ASSERT_BELOWEQ_(col_start + submat.cols(), mbDerived().cols());
215  for (int r = 0; r < submat.rows(); r++)
216  for (int c = 0; c < submat.cols(); c++)
217  mbDerived()(r + row_start, c + col_start) = submat(r, c);
218  }
219  /** Like insertMatrix(), but inserts `submat'` (transposed) */
220  template <typename OTHERMATVEC>
222  const int row_start, const int col_start, const OTHERMATVEC& submat)
223  {
224  ASSERT_BELOWEQ_(row_start + submat.cols(), mbDerived().rows());
225  ASSERT_BELOWEQ_(col_start + submat.rows(), mbDerived().cols());
226  for (int r = 0; r < submat.cols(); r++)
227  for (int c = 0; c < submat.rows(); c++)
228  mbDerived()(r + row_start, c + col_start) = submat(c, r);
229  }
230 
231  /** const blockCopy(): Returns a *copy* of the given block */
232  template <int BLOCK_ROWS, int BLOCK_COLS>
234  int start_row = 0, int start_col = 0) const
235  {
236  return extractMatrix<BLOCK_ROWS, BLOCK_COLS>(start_row, start_col);
237  }
238  /** const blockCopy(): Returns a *copy* of the given block (non templated
239  * version, dynamic sizes) */
241  int start_row, int start_col, int BLOCK_ROWS, int BLOCK_COLS) const
242  {
243  return extractMatrix(start_row, start_col, BLOCK_ROWS, BLOCK_COLS);
244  }
245 
246  template <int BLOCK_ROWS, int BLOCK_COLS>
248  const int start_row = 0, const int start_col = 0) const
249  {
250  ASSERT_BELOWEQ_(start_row + BLOCK_ROWS, mbDerived().rows());
251  ASSERT_BELOWEQ_(start_col + BLOCK_COLS, mbDerived().cols());
252 
254  for (int r = 0; r < BLOCK_ROWS; r++)
255  for (int c = 0; c < BLOCK_COLS; c++)
256  ret(r, c) = mbDerived()(r + start_row, c + start_col);
257  return ret;
258  }
259 
261  const int BLOCK_ROWS, const int BLOCK_COLS, const int start_row,
262  const int start_col) const
263  {
264  ASSERT_BELOWEQ_(start_row + BLOCK_ROWS, mbDerived().rows());
265  ASSERT_BELOWEQ_(start_col + BLOCK_COLS, mbDerived().cols());
266 
267  CMatrixDynamic<Scalar> ret(BLOCK_ROWS, BLOCK_COLS);
268  for (int r = 0; r < BLOCK_ROWS; r++)
269  for (int c = 0; c < BLOCK_COLS; c++)
270  ret(r, c) = mbDerived()(r + start_row, c + start_col);
271  return ret;
272  }
273 
274  /** this = A * A<sup>T</sup> */
275  template <typename MAT_A>
276  void matProductOf_AAt(const MAT_A& A)
277  {
278  using Index = typename Derived::Index;
279  const auto N = A.rows(), Ninner = A.cols();
280  mbDerived().resize(N, N);
281  for (Index r = 0; r < N; r++)
282  {
283  // Only 1/2 of computations required:
284  for (Index c = r; c < N; c++)
285  {
286  typename Derived::Scalar s = 0;
287  for (Index i = 0; i < Ninner; i++) s += A(r, i) * A(c, i);
288  mbDerived()(r, c) = s;
289  mbDerived()(c, r) = s;
290  }
291  }
292  }
293  /** this = A<sup>T</sup> * A */
294  template <typename MAT_A>
295  void matProductOf_AtA(const MAT_A& A)
296  {
297  using Index = typename Derived::Index;
298  const auto N = A.cols(), Ninner = A.rows();
299  mbDerived().resize(N, N);
300  for (Index r = 0; r < N; r++)
301  {
302  // Only 1/2 of computations required:
303  for (Index c = r; c < N; c++)
304  {
305  typename Derived::Scalar s = 0;
306  for (Index i = 0; i < Ninner; i++) s += A(i, r) * A(i, c);
307  mbDerived()(r, c) = s;
308  mbDerived()(c, r) = s;
309  }
310  }
311  }
312 
313  /** @} */
314 };
315 
316 } // namespace mrpt::math
void matProductOf_AtA(const MAT_A &A)
this = AT * A
Definition: MatrixBase.h:295
bool eig(Derived &eVecs, std::vector< Scalar > &eVals, bool sorted=true) const
Computes the eigenvectors and eigenvalues for a square, general matrix.
CMatrixFixed< Scalar, BLOCK_ROWS, BLOCK_COLS > blockCopy(int start_row=0, int start_col=0) const
const blockCopy(): Returns a copy of the given block
Definition: MatrixBase.h:233
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
double Scalar
Definition: KmUtils.h:43
static Derived Identity()
Definition: MatrixBase.h:64
auto col(int colIdx)
Definition: MatrixBase.h:89
auto col(int colIdx) const
Definition: MatrixBase.h:94
void unsafeRemoveColumns(const std::vector< std::size_t > &idxs)
Removes columns of the matrix.
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
bool chol(Derived &U) const
Cholesky M=UT * U decomposition for symmetric matrix (upper-half of the matrix is actually ignored...
void insertMatrix(const int row_start, const int col_start, const OTHERMATVEC &submat)
Copies the given input submatrix/vector into this matrix/vector, starting at the given top-left coord...
Definition: MatrixBase.h:210
auto row(int rowIdx) const
Definition: MatrixBase.h:105
Derived & mbDerived()
Definition: MatrixBase.h:26
int rank(Scalar threshold=0) const
Finds the rank of the matrix via LU decomposition.
bool eig_symmetric(Derived &eVecs, std::vector< Scalar > &eVals, bool sorted=true) const
Read: eig()
void matProductOf_AAt(const MAT_A &A)
this = A * AT
Definition: MatrixBase.h:276
CMatrixFixed< Scalar, BLOCK_ROWS, BLOCK_COLS > extractMatrix(const int start_row=0, const int start_col=0) const
Definition: MatrixBase.h:247
This base provides a set of functions for maths stuff.
Derived inverse() const
Returns the inverse of a general matrix using LU.
Scalar minimumDiagonal() const
Returns the minimum value in the diagonal.
CMatrixDynamic< Scalar > extractMatrix(const int BLOCK_ROWS, const int BLOCK_COLS, const int start_row, const int start_col) const
Definition: MatrixBase.h:260
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
auto row(int rowIdx)
Definition: MatrixBase.h:100
VECTOR_LIKE extractRow(int rowIdx) const
Definition: MatrixBase.h:120
Scalar det() const
Determinant of matrix.
Scalar maximumDiagonal() const
Returns the maximum value in the diagonal.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
void extractColumn(int colIdx, VECTOR_LIKE &v) const
Definition: MatrixBase.h:128
void matProductOf_AB(const Derived &A, const Derived &B)
this = A*B, with A & B of the same type of this.
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
CMatrixDynamic< Scalar > blockCopy(int start_row, int start_col, int BLOCK_ROWS, int BLOCK_COLS) const
const blockCopy(): Returns a copy of the given block (non templated version, dynamic sizes) ...
Definition: MatrixBase.h:240
void removeColumns(const std::vector< std::size_t > &idxsToRemove)
Removes columns of the matrix.
Base CRTP class for all MRPT matrices.
Definition: MatrixBase.h:23
VECTOR_LIKE extractColumn(int colIdx) const
Definition: MatrixBase.h:136
void setDiagonal(const std::vector< Scalar > &diags)
Resizes to NxN, with N the length of the input vector, set all entries to zero, except the main diago...
Definition: MatrixBase.h:51
void setDiagonal(const Scalar value)
Set all entries to zero, except the main diagonal which is set to value
Definition: MatrixBase.h:43
void unsafeRemoveRows(const std::vector< std::size_t > &idxs)
Removes rows of the matrix.
Scalar trace() const
Returns the trace of the matrix (not necessarily square).
Base CRTP class for all MRPT vectors and matrices.
#define ASSERT_BELOWEQ_(__A, __B)
Definition: exceptions.h:161
void removeRows(const std::vector< std::size_t > &idxsToRemove)
Removes rows of the matrix.
const Derived & mbDerived() const
Definition: MatrixBase.h:27
This template class provides the basic functionality for a general 2D any-size, resizable container o...
static Derived Identity(const std::size_t N)
Definition: MatrixBase.h:74
void extractRow(int rowIdx, VECTOR_LIKE &v) const
Definition: MatrixBase.h:112
void insertMatrixTransposed(const int row_start, const int col_start, const OTHERMATVEC &submat)
Like insertMatrix(), but inserts ‘submat’` (transposed)
Definition: MatrixBase.h:221
void setDiagonal(const std::size_t N, const Scalar value)
Resize to NxN, set all entries to zero, except the main diagonal which is set to value ...
Definition: MatrixBase.h:34
void setIdentity(const std::size_t N)
Definition: MatrixBase.h:62



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020