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