MRPT  1.9.9
ops_matrices.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
8  +------------------------------------------------------------------------+ */
9 #ifndef mrpt_math_matrix_ops_H
10 #define mrpt_math_matrix_ops_H
11
12 #include <mrpt/math/math_frwds.h> // forward declarations
13 #include <mrpt/math/eigen_frwds.h> // forward declarations
14
17
18 #include <mrpt/math/ops_containers.h> // Many generic operations
19
20 /** \file ops_matrices.h
21  * This file implements miscelaneous matrix and matrix/vector operations, and
22  * internal functions in mrpt::math::detail
23  */
24 namespace mrpt
25 {
26 namespace math
27 {
29  * @{ */
30
31 /** Transpose operator for matrices */
32 template <class Derived>
35 {
37 }
38
39 /** Unary inversion operator. */
40 template <class Derived>
43 {
44  return m.inv();
45 }
46
47 /** @} */ // end MRPT matrices operators
48
49 /** R = H * C * H^t (with C symmetric) */
50 template <typename MAT_H, typename MAT_C, typename MAT_R>
51 inline void multiply_HCHt(
52  const MAT_H& H, const MAT_C& C, MAT_R& R,
53  bool accumResultInOutput) // bool allow_submatrix_mult)
54 {
55  if (accumResultInOutput)
56  R += ((H * C.template selfadjointView<Eigen::Lower>()).eval() *
58  .eval()
60  else
61  R = ((H * C.template selfadjointView<Eigen::Lower>()).eval() *
63  .eval()
65 }
66
67 /** r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C) */
68 template <typename VECTOR_H, typename MAT_C>
69 typename MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H& H, const MAT_C& C)
70 {
71  return (H.matrix().adjoint() * C * H.matrix()).eval()(0, 0);
72 }
73
74 /** R = H^t * C * H (with C symmetric) */
75 template <typename MAT_H, typename MAT_C, typename MAT_R>
77  const MAT_H& H, const MAT_C& C, MAT_R& R,
78  bool accumResultInOutput) // bool allow_submatrix_mult)
79 {
80  if (accumResultInOutput)
81  R +=
83  H)
84  .eval()
86  else
88  H)
89  .eval()
91 }
92
93 /** Computes the mean vector and covariance from a list of samples in an NxM
94  * matrix, where each row is a sample, so the covariance is MxM.
95  * \param v The set of data as a NxM matrix, of types: CMatrixTemplateNumeric
96  * or CMatrixFixedNumeric
97  * \param out_mean The output M-vector for the estimated mean.
98  * \param out_cov The output MxM matrix for the estimated covariance matrix,
99  * this can also be either a fixed-size of dynamic size matrix.
100  * \sa mrpt::math::meanAndCovVec, math::mean,math::stddev, math::cov
101  */
102 template <class MAT_IN, class VECTOR, class MAT_OUT>
103 void meanAndCovMat(const MAT_IN& v, VECTOR& out_mean, MAT_OUT& out_cov)
104 {
105  const size_t N = v.rows();
106  ASSERTMSG_(N > 0, "The input matrix contains no elements");
107  const double N_inv = 1.0 / N;
108
109  const size_t M = v.cols();
110  ASSERTMSG_(M > 0, "The input matrix contains rows of length 0");
111
112  // First: Compute the mean
113  out_mean.assign(M, 0);
114  for (size_t i = 0; i < N; i++)
115  for (size_t j = 0; j < M; j++) out_mean[j] += v.coeff(i, j);
116  out_mean *= N_inv;
117
118  // Second: Compute the covariance
119  // Save only the above-diagonal part, then after averaging
120  // duplicate that part to the other half.
121  out_cov.zeros(M, M);
122  for (size_t i = 0; i < N; i++)
123  {
124  for (size_t j = 0; j < M; j++)
125  out_cov.get_unsafe(j, j) +=
126  square(v.get_unsafe(i, j) - out_mean[j]);
127
128  for (size_t j = 0; j < M; j++)
129  for (size_t k = j + 1; k < M; k++)
130  out_cov.get_unsafe(j, k) += (v.get_unsafe(i, j) - out_mean[j]) *
131  (v.get_unsafe(i, k) - out_mean[k]);
132  }
133  for (size_t j = 0; j < M; j++)
134  for (size_t k = j + 1; k < M; k++)
135  out_cov.get_unsafe(k, j) = out_cov.get_unsafe(j, k);
136  out_cov *= N_inv;
137 }
138
139 /** Computes the covariance matrix from a list of samples in an NxM matrix,
140  * where each row is a sample, so the covariance is MxM.
141  * \param v The set of data, as a NxM matrix.
142  * \param out_cov The output MxM matrix for the estimated covariance matrix.
143  * \sa math::mean,math::stddev, math::cov
144  */
145 template <class MATRIX>
146 inline Eigen::Matrix<typename MATRIX::Scalar, MATRIX::ColsAtCompileTime,
147  MATRIX::ColsAtCompileTime>
148  cov(const MATRIX& v)
149 {
150  Eigen::Matrix<double, MATRIX::ColsAtCompileTime, 1> m;
151  Eigen::Matrix<typename MATRIX::Scalar, MATRIX::ColsAtCompileTime,
152  MATRIX::ColsAtCompileTime>
153  C;
154  meanAndCovMat(v, m, C);
155  return C;
156 }
157
158 /** A useful macro for saving matrixes to a file while debugging. */
159 #define SAVE_MATRIX(M) M.saveToTextFile(#M ".txt");
160
161 /** Only for vectors/arrays "v" of length3, compute out = A * Skew(v), where
162  * Skew(v) is the skew symmetric matric generated from \a v (see
163  * mrpt::math::skew_symmetric3)
164  */
165 template <class MAT_A, class SKEW_3VECTOR, class MAT_OUT>
166 void multiply_A_skew3(const MAT_A& A, const SKEW_3VECTOR& v, MAT_OUT& out)
167 {
168  MRPT_START
169  ASSERT_EQUAL_(A.cols(), 3);
170  ASSERT_EQUAL_(v.size(), 3);
171  const size_t N = A.rows();
172  out.setSize(N, 3);
173  for (size_t i = 0; i < N; i++)
174  {
175  out.set_unsafe(
176  i, 0, A.get_unsafe(i, 1) * v[2] - A.get_unsafe(i, 2) * v[1]);
177  out.set_unsafe(
178  i, 1, -A.get_unsafe(i, 0) * v[2] + A.get_unsafe(i, 2) * v[0]);
179  out.set_unsafe(
180  i, 2, A.get_unsafe(i, 0) * v[1] - A.get_unsafe(i, 1) * v[0]);
181  }
182  MRPT_END
183 }
184
185 /** Only for vectors/arrays "v" of length3, compute out = Skew(v) * A, where
186  * Skew(v) is the skew symmetric matric generated from \a v (see
187  * mrpt::math::skew_symmetric3)
188  */
189 template <class SKEW_3VECTOR, class MAT_A, class MAT_OUT>
190 void multiply_skew3_A(const SKEW_3VECTOR& v, const MAT_A& A, MAT_OUT& out)
191 {
192  MRPT_START
193  ASSERT_EQUAL_(A.rows(), 3);
194  ASSERT_EQUAL_(v.size(), 3);
195  const size_t N = A.cols();
196  out.setSize(3, N);
197  for (size_t i = 0; i < N; i++)
198  {
199  out.set_unsafe(
200  0, i, -A.get_unsafe(1, i) * v[2] + A.get_unsafe(2, i) * v[1]);
201  out.set_unsafe(
202  1, i, A.get_unsafe(0, i) * v[2] - A.get_unsafe(2, i) * v[0]);
203  out.set_unsafe(
204  2, i, -A.get_unsafe(0, i) * v[1] + A.get_unsafe(1, i) * v[0]);
205  }
206  MRPT_END
207 }
208
209 // ------ Implementatin of detail functions -------------
210 namespace detail
211 {
212 /** Extract a submatrix - The output matrix must be set to the required size
213  * before call. */
214 template <class MATORG, class MATDEST>
216  const MATORG& M, const size_t first_row, const size_t first_col,
217  MATDEST& outMat)
218 {
219  const size_t NR = outMat.rows();
220  const size_t NC = outMat.cols();
221  ASSERT_BELOWEQ_(first_row + NR, M.rows());
222  ASSERT_BELOWEQ_(first_col + NC, M.cols());
223  for (size_t r = 0; r < NR; r++)
224  for (size_t c = 0; c < NC; c++)
225  outMat.get_unsafe(r, c) =
226  M.get_unsafe(first_row + r, first_col + c);
227 }
228
229 } // end of detail namespace
230
231 /** @} */ // end of grouping
232
233 } // End of math namespace
234 } // End of mrpt namespace
235
236 #endif
void multiply_HCHt(const MAT_H &H, const MAT_C &C, MAT_R &R, bool accumResultInOutput)
R = H * C * H^t (with C symmetric)
Definition: ops_matrices.h:51
double Scalar
Definition: KmUtils.h:44
#define MRPT_START
Definition: exceptions.h:262
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:166
MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C)
Definition: ops_matrices.h:69
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:103
T square(const T x)
Inline function for the square of a number.
void extractMatrix(const MATORG &M, const size_t first_row, const size_t first_col, MATDEST &outMat)
Extract a submatrix - The output matrix must be set to the required size before call.
Definition: ops_matrices.h:215
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
const GLubyte * c
Definition: glext.h:6313
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
void multiply_HtCH(const MAT_H &H, const MAT_C &C, MAT_R &R, bool accumResultInOutput)
R = H^t * C * H (with C symmetric)
Definition: ops_matrices.h:76
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
const float R
Eigen::MatrixBase< Derived >::PlainObject operator!(const Eigen::MatrixBase< Derived > &m)
Unary inversion operator.
Definition: ops_matrices.h:41
#define MRPT_END
Definition: exceptions.h:266
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > 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 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:190
#define ASSERT_BELOWEQ_(__A, __B)
Definition: exceptions.h:177
const Eigen::MatrixBase< Derived >::AdjointReturnType operator~(const Eigen::MatrixBase< Derived > &m)
Transpose operator for matrices.
Definition: ops_matrices.h:33

 Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020