Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
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 internal functions in mrpt::math::detail
22  */
23 namespace mrpt
24 {
25  namespace math
26  {
27  /** \addtogroup container_ops_grp
28  * @{ */
29 
30  /** Transpose operator for matrices */
31  template <class Derived>
32  inline const typename Eigen::MatrixBase<Derived>::AdjointReturnType operator ~(const Eigen::MatrixBase<Derived> &m) {
33  return m.adjoint();
34  }
35 
36  /** Unary inversion operator. */
37  template <class Derived>
38  inline typename Eigen::MatrixBase<Derived>::PlainObject operator !(const Eigen::MatrixBase<Derived> &m) {
39  return m.inv();
40  }
41 
42  /** @} */ // end MRPT matrices operators
43 
44 
45  /** R = H * C * H^t (with C symmetric) */
46  template <typename MAT_H, typename MAT_C, typename MAT_R>
47  inline void multiply_HCHt(
48  const MAT_H &H,
49  const MAT_C &C,
50  MAT_R &R,
51  bool accumResultInOutput ) // bool allow_submatrix_mult)
52  {
53  if (accumResultInOutput)
54  R += ( (H * C.template selfadjointView<Eigen::Lower>()).eval() * H.adjoint()).eval().template selfadjointView<Eigen::Lower>();
55  else
56  R = ( (H * C.template selfadjointView<Eigen::Lower>()).eval() * H.adjoint()).eval().template selfadjointView<Eigen::Lower>();
57  }
58 
59  /** r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C) */
60  template <typename VECTOR_H, typename MAT_C>
61  typename MAT_C::Scalar
62  multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
63  {
64  return (H.matrix().adjoint() * C * H.matrix()).eval()(0,0);
65  }
66 
67  /** R = H^t * C * H (with C symmetric) */
68  template <typename MAT_H, typename MAT_C, typename MAT_R>
70  const MAT_H &H,
71  const MAT_C &C,
72  MAT_R &R,
73  bool accumResultInOutput) // bool allow_submatrix_mult)
74  {
75  if (accumResultInOutput)
76  R += ( (H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() * H).eval().template selfadjointView<Eigen::Lower>();
77  else
78  R = ( (H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() * H).eval().template selfadjointView<Eigen::Lower>();
79  }
80 
81 
82  /** Computes the mean vector and covariance from a list of samples in an NxM matrix, where each row is a sample, so the covariance is MxM.
83  * \param v The set of data as a NxM matrix, of types: CMatrixTemplateNumeric or CMatrixFixedNumeric
84  * \param out_mean The output M-vector for the estimated mean.
85  * \param out_cov The output MxM matrix for the estimated covariance matrix, this can also be either a fixed-size of dynamic size matrix.
86  * \sa mrpt::math::meanAndCovVec, math::mean,math::stddev, math::cov
87  */
88  template<class MAT_IN,class VECTOR, class MAT_OUT>
90  const MAT_IN & v,
91  VECTOR & out_mean,
92  MAT_OUT & out_cov
93  )
94  {
95  const size_t N = v.rows();
96  ASSERTMSG_(N>0,"The input matrix contains no elements");
97  const double N_inv = 1.0/N;
98 
99  const size_t M = v.cols();
100  ASSERTMSG_(M>0,"The input matrix contains rows of length 0");
101 
102  // First: Compute the mean
103  out_mean.assign(M,0);
104  for (size_t i=0;i<N;i++)
105  for (size_t j=0;j<M;j++)
106  out_mean[j]+=v.coeff(i,j);
107  out_mean*=N_inv;
108 
109  // Second: Compute the covariance
110  // Save only the above-diagonal part, then after averaging
111  // duplicate that part to the other half.
112  out_cov.zeros(M,M);
113  for (size_t i=0;i<N;i++)
114  {
115  for (size_t j=0;j<M;j++)
116  out_cov.get_unsafe(j,j)+=square(v.get_unsafe(i,j)-out_mean[j]);
117 
118  for (size_t j=0;j<M;j++)
119  for (size_t k=j+1;k<M;k++)
120  out_cov.get_unsafe(j,k)+=(v.get_unsafe(i,j)-out_mean[j])*(v.get_unsafe(i,k)-out_mean[k]);
121  }
122  for (size_t j=0;j<M;j++)
123  for (size_t k=j+1;k<M;k++)
124  out_cov.get_unsafe(k,j) = out_cov.get_unsafe(j,k);
125  out_cov*=N_inv;
126  }
127 
128  /** Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample, so the covariance is MxM.
129  * \param v The set of data, as a NxM matrix.
130  * \param out_cov The output MxM matrix for the estimated covariance matrix.
131  * \sa math::mean,math::stddev, math::cov
132  */
133  template<class MATRIX>
134  inline Eigen::Matrix<typename MATRIX::Scalar,MATRIX::ColsAtCompileTime,MATRIX::ColsAtCompileTime>
135  cov( const MATRIX &v )
136  {
137  Eigen::Matrix<double,MATRIX::ColsAtCompileTime,1> m;
138  Eigen::Matrix<typename MATRIX::Scalar,MATRIX::ColsAtCompileTime,MATRIX::ColsAtCompileTime> C;
139  meanAndCovMat(v,m,C);
140  return C;
141  }
142 
143  /** A useful macro for saving matrixes to a file while debugging. */
144  #define SAVE_MATRIX(M) M.saveToTextFile(#M ".txt");
145 
146 
147  /** Only for vectors/arrays "v" of length3, compute out = A * Skew(v), where Skew(v) is the skew symmetric matric generated from \a v (see mrpt::math::skew_symmetric3)
148  */
149  template <class MAT_A,class SKEW_3VECTOR,class MAT_OUT>
150  void multiply_A_skew3(const MAT_A &A,const SKEW_3VECTOR &v, MAT_OUT &out)
151  {
152  MRPT_START
153  ASSERT_EQUAL_(size(A,2),3)
154  ASSERT_EQUAL_(v.size(),3)
155  const size_t N = size(A,1);
156  out.setSize(N,3);
157  for (size_t i=0;i<N;i++)
158  {
159  out.set_unsafe(i,0, A.get_unsafe(i,1)*v[2]-A.get_unsafe(i,2)*v[1] );
160  out.set_unsafe(i,1,-A.get_unsafe(i,0)*v[2]+A.get_unsafe(i,2)*v[0] );
161  out.set_unsafe(i,2, A.get_unsafe(i,0)*v[1]-A.get_unsafe(i,1)*v[0] );
162  }
163  MRPT_END
164  }
165 
166  /** Only for vectors/arrays "v" of length3, compute out = Skew(v) * A, where Skew(v) is the skew symmetric matric generated from \a v (see mrpt::math::skew_symmetric3)
167  */
168  template <class SKEW_3VECTOR,class MAT_A,class MAT_OUT>
169  void multiply_skew3_A(const SKEW_3VECTOR &v, const MAT_A &A,MAT_OUT &out)
170  {
171  MRPT_START
172  ASSERT_EQUAL_(size(A,1),3)
173  ASSERT_EQUAL_(v.size(),3)
174  const size_t N = size(A,2);
175  out.setSize(3,N);
176  for (size_t i=0;i<N;i++)
177  {
178  out.set_unsafe(0,i,-A.get_unsafe(1,i)*v[2]+A.get_unsafe(2,i)*v[1] );
179  out.set_unsafe(1,i, A.get_unsafe(0,i)*v[2]-A.get_unsafe(2,i)*v[0] );
180  out.set_unsafe(2,i,-A.get_unsafe(0,i)*v[1]+A.get_unsafe(1,i)*v[0] );
181  }
182  MRPT_END
183  }
184 
185 
186  // ------ Implementatin of detail functions -------------
187  namespace detail
188  {
189  /** Extract a submatrix - The output matrix must be set to the required size before call. */
190  template <class MATORG, class MATDEST>
192  const MATORG &M,
193  const size_t first_row,
194  const size_t first_col,
195  MATDEST &outMat)
196  {
197  const size_t NR = outMat.getRowCount();
198  const size_t NC = outMat.getColCount();
199  ASSERT_BELOWEQ_( first_row+NR, M.getRowCount() )
200  ASSERT_BELOWEQ_( first_col+NC, M.getColCount() )
201  for (size_t r=0;r<NR;r++)
202  for (size_t c=0;c<NC;c++)
203  outMat.get_unsafe(r,c) = M.get_unsafe(first_row+r,first_col+c);
204  }
205 
206  } // end of detail namespace
207 
208  /** @} */ // end of grouping
209 
210  } // End of math namespace
211 } // End of mrpt namespace
212 
213 
214 #endif
#define ASSERT_EQUAL_(__A, __B)
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:47
#define ASSERT_BELOWEQ_(__A, __B)
size_t size(const MATRIXLIKE &m, const int dim)
Definition: bits.h:43
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:150
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:62
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:89
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
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:191
#define MRPT_END
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:135
const GLubyte * c
Definition: glext.h:5590
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:69
#define MRPT_START
const GLdouble * v
Definition: glext.h:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
const float R
Eigen::MatrixBase< Derived >::PlainObject operator!(const Eigen::MatrixBase< Derived > &m)
Unary inversion operator.
Definition: ops_matrices.h:38
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:169
const Eigen::MatrixBase< Derived >::AdjointReturnType operator~(const Eigen::MatrixBase< Derived > &m)
Transpose operator for matrices.
Definition: ops_matrices.h:32
#define ASSERTMSG_(f, __ERROR_MSG)
double Scalar
Definition: KmUtils.h:41



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019