MRPT  1.9.9
ops_matrices.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 |
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 #include <mrpt/math/CMatrixFixed.h>
14 #include <mrpt/math/mat2eig.h>
15 #include <mrpt/math/math_frwds.h> // forward declarations
16 #include <mrpt/math/ops_containers.h> // Many generic operations
17 
18 /** \file ops_matrices.h
19  * This file implements miscelaneous matrix and matrix/vector operations, and
20  * internal functions in mrpt::math::detail
21  */
22 namespace mrpt
23 {
24 namespace math
25 {
26 /** R = H * C * H^t */
27 template <typename MAT_H, typename MAT_C, typename MAT_R>
28 inline void multiply_HCHt(
29  const MAT_H& H, const MAT_C& C, MAT_R& R, bool accumResultInOutput = false)
30 {
31  auto res = (mat2eig(H) * mat2eig(C) * mat2eig(H).transpose()).eval();
32  if (accumResultInOutput)
33  R.asEigen() += res;
34  else
35  {
36  R.resize(res.rows(), res.cols());
37  R.asEigen() = res;
38  }
39 }
40 
41 /** return a fixed-size matrix with the result of: H * C * H^t */
42 template <std::size_t H_ROWS, std::size_t H_COLS, typename Scalar>
46 {
48  R.asEigen() = (mat2eig(H) * mat2eig(C) * mat2eig(H).transpose()).eval();
49  return R;
50 }
51 
52 /** r (a scalar) = H * C * H^t (with a column vector H and a symmetric
53  * matrix C)
54  */
55 template <typename VECTOR_H, typename MAT_C>
56 typename MAT_C::Scalar multiply_HtCH_scalar(const VECTOR_H& H, const MAT_C& C)
57 {
58  return (mat2eig(H).transpose() * mat2eig(C) * mat2eig(H))(0, 0);
59 }
60 
61 /** r (a scalar) = H^t * C * H (with a row vector H and a symmetric matrix
62  * C) */
63 template <typename VECTOR_H, typename MAT_C>
64 typename MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H& H, const MAT_C& C)
65 {
66  return (mat2eig(H) * mat2eig(C) * mat2eig(H).transpose())(0, 0);
67 }
68 
69 /** Computes the mean vector and covariance from a list of samples in an NxM
70  * matrix, where each row is a sample, so the covariance is MxM.
71  * \param v The set of data as a NxM matrix, of types: CMatrixDynamic
72  * or CMatrixFixed
73  * \param out_mean The output M-vector for the estimated mean.
74  * \param out_cov The output MxM matrix for the estimated covariance matrix,
75  * this can also be either a fixed-size of dynamic size matrix.
76  * \sa mrpt::math::meanAndCovVec, math::mean,math::stddev, math::cov
77  */
78 template <class MAT_IN, class VECTOR, class MAT_OUT>
79 void meanAndCovMat(const MAT_IN& v, VECTOR& out_mean, MAT_OUT& out_cov)
80 {
81  const size_t N = v.rows();
82  ASSERTMSG_(N > 0, "The input matrix contains no elements");
83  const double N_inv = 1.0 / N;
84 
85  const size_t M = v.cols();
86  ASSERTMSG_(M > 0, "The input matrix contains rows of length 0");
87 
88  // First: Compute the mean
89  out_mean.assign(M, 0);
90  for (size_t i = 0; i < N; i++)
91  for (size_t j = 0; j < M; j++) out_mean[j] += v.coeff(i, j);
92  out_mean *= N_inv;
93 
94  // Second: Compute the covariance
95  // Save only the above-diagonal part, then after averaging
96  // duplicate that part to the other half.
97  out_cov.setZero(M, M);
98  for (size_t i = 0; i < N; i++)
99  {
100  for (size_t j = 0; j < M; j++)
101  out_cov(j, j) += square(v(i, j) - out_mean[j]);
102 
103  for (size_t j = 0; j < M; j++)
104  for (size_t k = j + 1; k < M; k++)
105  out_cov(j, k) +=
106  (v(i, j) - out_mean[j]) * (v(i, k) - out_mean[k]);
107  }
108  for (size_t j = 0; j < M; j++)
109  for (size_t k = j + 1; k < M; k++) out_cov(k, j) = out_cov(j, k);
110  out_cov *= N_inv;
111 }
112 
113 /** Computes a row with the mean values of each column in the matrix and the
114  * associated vector with the standard deviation of each column.
115  * \sa mean,meanAndStdAll \exception std::exception If the matrix/vector is
116  * empty.
117  * \param unbiased_variance Standard deviation is sum(vals-mean)/K, with
118  * K=N-1 or N for unbiased_variance=true or false, respectively.
119  */
120 template <class MAT_IN, class VEC>
122  const MAT_IN& m, VEC& outMeanVector, VEC& outStdVector,
123  const bool unbiased_variance = true)
124 {
125  const auto N = m.rows(), M = m.cols();
126  if (N == 0) throw std::runtime_error("meanAndStdColumns: Empty container.");
127  const double N_inv = 1.0 / N;
128  const double N_ =
129  unbiased_variance ? (N > 1 ? 1.0 / (N - 1) : 1.0) : 1.0 / N;
130  outMeanVector.resize(M);
131  outStdVector.resize(M);
132  for (decltype(m.cols()) i = 0; i < M; i++)
133  {
134  outMeanVector[i] = m.asEigen().col(i).array().sum() * N_inv;
135  outStdVector[i] = std::sqrt(
136  (m.asEigen().col(i).array() - outMeanVector[i]).square().sum() *
137  N_);
138  }
139 }
140 
141 /** Computes the covariance matrix from a list of samples in an NxM matrix,
142  * where each row is a sample, so the covariance is MxM.
143  * \param v The set of data, as a NxM matrix.
144  * \param out_cov The output MxM matrix for the estimated covariance matrix.
145  * \sa math::mean,math::stddev, math::cov
146  */
147 template <class MATRIX>
148 CMatrixDouble cov(const MATRIX& v)
149 {
150  CVectorDouble m;
151  CMatrixDouble C;
152  meanAndCovMat(v, m, C);
153  return C;
154 }
155 
156 /** A useful macro for saving matrixes to a file while debugging. */
157 #define SAVE_MATRIX(M) M.saveToTextFile(#M ".txt");
158 
159 /** Only for vectors/arrays "v" of length3, compute out = A * Skew(v), where
160  * Skew(v) is the skew symmetric matric generated from \a v (see
161  * mrpt::math::skew_symmetric3)
162  */
163 template <class MAT_A, class SKEW_3VECTOR, class MAT_OUT>
164 void multiply_A_skew3(const MAT_A& A, const SKEW_3VECTOR& v, MAT_OUT& out)
165 {
166  MRPT_START
167  ASSERT_EQUAL_(A.cols(), 3);
168  ASSERT_EQUAL_(v.size(), 3);
169  const size_t N = A.rows();
170  out.setSize(N, 3);
171  for (size_t i = 0; i < N; i++)
172  {
173  out(i, 0) = A(i, 1) * v[2] - A(i, 2) * v[1];
174  out(i, 1) = -A(i, 0) * v[2] + A(i, 2) * v[0];
175  out(i, 2) = A(i, 0) * v[1] - A(i, 1) * v[0];
176  }
177  MRPT_END
178 }
179 
180 /** Only for vectors/arrays "v" of length3, compute out = Skew(v) * A, where
181  * Skew(v) is the skew symmetric matric generated from \a v (see
182  * mrpt::math::skew_symmetric3)
183  */
184 template <class SKEW_3VECTOR, class MAT_A, class MAT_OUT>
185 void multiply_skew3_A(const SKEW_3VECTOR& v, const MAT_A& A, MAT_OUT& out)
186 {
187  MRPT_START
188  ASSERT_EQUAL_(A.rows(), 3);
189  ASSERT_EQUAL_(v.size(), 3);
190  const size_t N = A.cols();
191  out.setSize(3, N);
192  for (size_t i = 0; i < N; i++)
193  {
194  out(0, i) = -A(1, i) * v[2] + A(2, i) * v[1];
195  out(1, i) = A(0, i) * v[2] - A(2, i) * v[0];
196  out(2, i) = -A(0, i) * v[1] + A(1, i) * v[0];
197  }
198  MRPT_END
199 }
200 
201 /** Computes the Laplacian of a square graph weight matrix.
202  * The laplacian matrix is L = D - W, with D a diagonal matrix with the
203  * degree of each node, W the edge weights.
204  */
205 template <typename MATIN, typename MATOUT>
206 void laplacian(const MATIN& g, MATOUT& ret)
207 {
208  if (g.rows() != g.cols())
209  throw std::runtime_error("laplacian: Defined for square matrixes only");
210  const auto N = g.rows();
211  ret = g;
212  ret *= -1;
213  for (typename MATIN::Index i = 0; i < N; i++)
214  {
215  typename MATIN::Scalar deg = 0;
216  for (typename MATIN::Index j = 0; j < N; j++) deg += g(j, i);
217  ret(i, i) += deg;
218  }
219 }
220 
221 /** Get a submatrix from a square matrix, by collecting the elements
222  * M(idxs,idxs), where idxs is a sequence
223  * {block_indices(i):block_indices(i)+BLOCKSIZE-1} for all "i" up to the
224  * size of block_indices. A perfect application of this method is in
225  * extracting covariance matrices of a subset of variables from the full
226  * covariance matrix. \sa extractSubmatrix, extractSubmatrixSymmetrical
227  */
228 template <std::size_t BLOCKSIZE, typename MAT, typename MATRIX>
230  const MAT& m, const std::vector<size_t>& block_indices, MATRIX& out)
231 {
232  if (BLOCKSIZE < 1)
233  throw std::runtime_error(
234  "extractSubmatrixSymmetricalBlocks: BLOCKSIZE must be >=1");
235  if (m.cols() != m.rows())
236  throw std::runtime_error(
237  "extractSubmatrixSymmetricalBlocks: Matrix is not square.");
238 
239  const size_t N = block_indices.size();
240  const size_t nrows_out = N * BLOCKSIZE;
241  out.resize(nrows_out, nrows_out);
242  if (!N) return; // Done
243  for (size_t dst_row_blk = 0; dst_row_blk < N; ++dst_row_blk)
244  {
245  for (size_t dst_col_blk = 0; dst_col_blk < N; ++dst_col_blk)
246  {
247 #if defined(_DEBUG)
248  if (block_indices[dst_col_blk] * BLOCKSIZE + BLOCKSIZE - 1 >=
249  size_t(m.cols()))
250  throw std::runtime_error(
251  "extractSubmatrixSymmetricalBlocks: Indices out of "
252  "range!");
253 #endif
254  out.asEigen().template block<BLOCKSIZE, BLOCKSIZE>(
255  dst_row_blk * BLOCKSIZE, dst_col_blk * BLOCKSIZE) =
256  m.asEigen().template block<BLOCKSIZE, BLOCKSIZE>(
257  block_indices[dst_row_blk] * BLOCKSIZE,
258  block_indices[dst_col_blk] * BLOCKSIZE);
259  }
260  }
261 }
262 
263 //! \overload for BLOCKSIZE determined at runtime
264 template <typename MAT, typename MATRIX>
266  const MAT& m, const std::size_t BLOCKSIZE,
267  const std::vector<size_t>& block_indices, MATRIX& out)
268 {
269  if (BLOCKSIZE < 1)
270  throw std::runtime_error(
271  "extractSubmatrixSymmetricalBlocks: BLOCKSIZE must be >=1");
272  if (m.cols() != m.rows())
273  throw std::runtime_error(
274  "extractSubmatrixSymmetricalBlocks: Matrix is not square.");
275 
276  const size_t N = block_indices.size();
277  const size_t nrows_out = N * BLOCKSIZE;
278  out.resize(nrows_out, nrows_out);
279  if (!N) return; // Done
280  for (size_t dst_row_blk = 0; dst_row_blk < N; ++dst_row_blk)
281  {
282  for (size_t dst_col_blk = 0; dst_col_blk < N; ++dst_col_blk)
283  {
284 #if defined(_DEBUG)
285  if (block_indices[dst_col_blk] * BLOCKSIZE + BLOCKSIZE - 1 >=
286  size_t(m.cols()))
287  throw std::runtime_error(
288  "extractSubmatrixSymmetricalBlocks: Indices out of "
289  "range!");
290 #endif
291  out.block(
292  dst_row_blk * BLOCKSIZE, dst_col_blk * BLOCKSIZE, BLOCKSIZE,
293  BLOCKSIZE) =
294  m.block(
295  block_indices[dst_row_blk] * BLOCKSIZE,
296  block_indices[dst_col_blk] * BLOCKSIZE, BLOCKSIZE,
297  BLOCKSIZE);
298  }
299  }
300 }
301 
302 /** Get a submatrix from a square matrix, by collecting the elements
303  * M(idxs,idxs), where idxs is the sequence of indices passed as argument. A
304  * perfect application of this method is in extracting covariance matrices
305  * of a subset of variables from the full covariance matrix. \sa
306  * extractSubmatrix, extractSubmatrixSymmetricalBlocks
307  */
308 template <typename MAT, typename MATRIX>
310  const MAT& m, const std::vector<size_t>& indices, MATRIX& out)
311 {
312  if (m.cols() != m.rows())
313  throw std::runtime_error(
314  "extractSubmatrixSymmetrical: Matrix is not square.");
315 
316  const size_t N = indices.size();
317  const size_t nrows_out = N;
318  out.resize(nrows_out, nrows_out);
319  if (!N) return; // Done
320  for (size_t dst_row_blk = 0; dst_row_blk < N; ++dst_row_blk)
321  for (size_t dst_col_blk = 0; dst_col_blk < N; ++dst_col_blk)
322  out(dst_row_blk, dst_col_blk) =
323  m(indices[dst_row_blk], indices[dst_col_blk]);
324 }
325 
326 /** @} */ // end of grouping
327 
328 } // namespace math
329 } // namespace mrpt
MAT_C::Scalar multiply_HtCH_scalar(const VECTOR_H &H, const MAT_C &C)
r (a scalar) = H * C * H^t (with a column vector H and a symmetric matrix C)
Definition: ops_matrices.h:56
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
double Scalar
Definition: KmUtils.h:43
#define MRPT_START
Definition: exceptions.h:241
const Derived & mat2eig(const Eigen::EigenBase< Derived > &m)
Returns an Eigen-compatible type, despite its argument already is an Eigen matrix, or an mrpt-math matrix/vector.
Definition: mat2eig.h:20
void laplacian(const MATIN &g, MATOUT &ret)
Computes the Laplacian of a square graph weight matrix.
Definition: ops_matrices.h:206
This file implements several operations that operate element-wise on individual or pairs of container...
void multiply_A_skew3(const MAT_A &A, const SKEW_3VECTOR &v, MAT_OUT &out)
Only for vectors/arrays "v" of length3, compute out = A * Skew(v), where Skew(v) is the skew symmetri...
Definition: ops_matrices.h:164
GLuint GLuint GLsizei GLenum const GLvoid * indices
Definition: glext.h:3532
void extractSubmatrixSymmetricalBlocks(const MAT &m, const std::vector< size_t > &block_indices, MATRIX &out)
Get a submatrix from a square matrix, by collecting the elements M(idxs,idxs), where idxs is a sequen...
Definition: ops_matrices.h:229
MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
r (a scalar) = H^t * C * H (with a row vector H and a symmetric matrix C)
Definition: ops_matrices.h:64
void meanAndCovMat(const MAT_IN &v, VECTOR &out_mean, MAT_OUT &out_cov)
Computes the mean vector and covariance from a list of samples in an NxM matrix, where each row is a ...
Definition: ops_matrices.h:79
T square(const T x)
Inline function for the square of a number.
const size_t BLOCKSIZE
Definition: nanoflann.hpp:460
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
GLubyte g
Definition: glext.h:6372
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
CMatrixDouble cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:148
void meanAndStdColumns(const MAT_IN &m, VEC &outMeanVector, VEC &outStdVector, const bool unbiased_variance=true)
Computes a row with the mean values of each column in the matrix and the associated vector with the s...
Definition: ops_matrices.h:121
void extractSubmatrixSymmetrical(const MAT &m, const std::vector< size_t > &indices, MATRIX &out)
Get a submatrix from a square matrix, by collecting the elements M(idxs,idxs), where idxs is the sequ...
Definition: ops_matrices.h:309
GLsizei GLboolean transpose
Definition: glext.h:4150
const GLdouble * v
Definition: glext.h:3684
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void extractSubmatrixSymmetricalBlocksDyn(const MAT &m, const std::size_t BLOCKSIZE, const std::vector< size_t > &block_indices, MATRIX &out)
Definition: ops_matrices.h:265
const float R
#define MRPT_END
Definition: exceptions.h:245
void multiply_skew3_A(const SKEW_3VECTOR &v, const MAT_A &A, MAT_OUT &out)
Only for vectors/arrays "v" of length3, compute out = Skew(v) * A, where Skew(v) is the skew symmetri...
Definition: ops_matrices.h:185
GLuint res
Definition: glext.h:7385
void multiply_HCHt(const MAT_H &H, const MAT_C &C, MAT_R &R, bool accumResultInOutput=false)
R = H * C * H^t.
Definition: ops_matrices.h:28



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ee555d257 Fri Aug 16 10:05:39 2019 +0200 at vie ago 16 10:10:14 CEST 2019