Main MRPT website > C++ reference
MRPT logo
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-2014, 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> // Fordward declarations
13 
14 #include <mrpt/math/CMatrix.h>
15 #include <mrpt/math/CMatrixD.h>
16 #include <mrpt/utils/CStream.h>
17 
20 
21 #include <mrpt/math/ops_containers.h> // Many generic operations
22 
23 #include <mrpt/utils/metaprogramming.h> // for copy_container_typecasting()
24 
25 
26 /** \file ops_matrices.h
27  * This file implements miscelaneous matrix and matrix/vector operations, plus internal functions in mrpt::math::detail
28  *
29  */
30 
31 namespace mrpt
32 {
33  namespace math
34  {
35  /** \addtogroup container_ops_grp
36  * @{ */
37 
38  /** @name Operators for binary streaming of MRPT matrices
39  @{ */
40 
41  /** Read operator from a CStream. The format is compatible with that of CMatrix & CMatrixD */
42  template <size_t NROWS,size_t NCOLS>
44  CMatrix aux;
45  in.ReadObject(&aux);
46  ASSERTMSG_(M.cols()==aux.cols() && M.rows()==aux.rows(), format("Size mismatch: deserialized is %ux%u, expected is %ux%u",(unsigned)aux.getRowCount(),(unsigned)aux.getColCount(),(unsigned)NROWS,(unsigned)NCOLS))
47  M = aux;
48  return in;
49  }
50  /** Read operator from a CStream. The format is compatible with that of CMatrix & CMatrixD */
51  template <size_t NROWS,size_t NCOLS>
53  CMatrixD aux;
54  in.ReadObject(&aux);
55  ASSERTMSG_(M.cols()==aux.cols() && M.rows()==aux.rows(), format("Size mismatch: deserialized is %ux%u, expected is %ux%u",(unsigned)aux.getRowCount(),(unsigned)aux.getColCount(),(unsigned)NROWS,(unsigned)NCOLS))
56  M = aux;
57  return in;
58  }
59 
60  /** Write operator for writing into a CStream. The format is compatible with that of CMatrix & CMatrixD */
61  template <size_t NROWS,size_t NCOLS>
62  mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const CMatrixFixedNumeric<float,NROWS,NCOLS> & M) {
63  CMatrix aux = CMatrixFloat(M);
64  out.WriteObject(&aux);
65  return out;
66  }
67  /** Write operator for writing into a CStream. The format is compatible with that of CMatrix & CMatrixD */
68  template <size_t NROWS,size_t NCOLS>
69  mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const CMatrixFixedNumeric<double,NROWS,NCOLS> & M) {
70  CMatrixD aux = CMatrixDouble(M);
71  out.WriteObject(&aux);
72  return out;
73  }
74 
75  /** @} */ // end MRPT matrices stream operators
76 
77 
78  /** @name Operators for text streaming of MRPT matrices
79  @{ */
80 
81 
82  /** Dumps the matrix to a text ostream, adding a final "\n" to Eigen's default output. */
83  template <typename T,size_t NROWS,size_t NCOLS>
84  inline std::ostream & operator << (std::ostream & s, const CMatrixFixedNumeric<T,NROWS,NCOLS>& m)
85  {
86  Eigen::IOFormat fmt; fmt.matSuffix="\n";
87  return s << m.format(fmt);
88  }
89 
90  /** Dumps the matrix to a text ostream, adding a final "\n" to Eigen's default output. */
91  template<typename T>
92  inline std::ostream & operator << (std::ostream & s, const CMatrixTemplateNumeric<T>& m)
93  {
94  Eigen::IOFormat fmt; fmt.matSuffix="\n";
95  return s << m.format(fmt);
96  }
97 
98  /** Dumps the vector as a row to a text ostream, with the format: "[v1 v2 v3... vN]" */
99  template<typename T>
100  inline std::ostream & operator << (std::ostream & s, const mrpt::dynamicsize_vector<T>& m)
101  {
102  Eigen::IOFormat fmt; fmt.rowSeparator=" "; fmt.matPrefix="["; fmt.matSuffix="]";
103  return s << m.format(fmt);
104  }
105 
106  /** @} */ // end MRPT matrices stream operators
107 
108 
109  /** Transpose operator for matrices */
110  template <class Derived>
111  inline const typename Eigen::MatrixBase<Derived>::AdjointReturnType operator ~(const Eigen::MatrixBase<Derived> &m) {
112  return m.adjoint();
113  }
114 
115  /** Unary inversion operator. */
116  template <class Derived>
117  inline typename Eigen::MatrixBase<Derived>::PlainObject operator !(const Eigen::MatrixBase<Derived> &m) {
118  return m.inv();
119  }
120 
121  /** @} */ // end MRPT matrices operators
122 
123 
124  /** R = H * C * H^t (with C symmetric) */
125  template <typename MAT_H, typename MAT_C, typename MAT_R>
126  inline void multiply_HCHt(
127  const MAT_H &H,
128  const MAT_C &C,
129  MAT_R &R,
130  bool accumResultInOutput ) // bool allow_submatrix_mult)
131  {
132  if (accumResultInOutput)
133  R += ( (H * C.template selfadjointView<Eigen::Lower>()).eval() * H.adjoint()).eval().template selfadjointView<Eigen::Lower>();
134  else
135  R = ( (H * C.template selfadjointView<Eigen::Lower>()).eval() * H.adjoint()).eval().template selfadjointView<Eigen::Lower>();
136  }
137 
138  /** r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C) */
139  template <typename VECTOR_H, typename MAT_C>
140  typename MAT_C::Scalar
141  multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
142  {
143  return (H.matrix().adjoint() * C * H.matrix()).eval()(0,0);
144  }
145 
146  /** R = H^t * C * H (with C symmetric) */
147  template <typename MAT_H, typename MAT_C, typename MAT_R>
149  const MAT_H &H,
150  const MAT_C &C,
151  MAT_R &R,
152  bool accumResultInOutput) // bool allow_submatrix_mult)
153  {
154  if (accumResultInOutput)
155  R += ( (H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() * H).eval().template selfadjointView<Eigen::Lower>();
156  else
157  R = ( (H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() * H).eval().template selfadjointView<Eigen::Lower>();
158  }
159 
160 
161  /** 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.
162  * \param v The set of data as a NxM matrix, of types: CMatrixTemplateNumeric or CMatrixFixedNumeric
163  * \param out_mean The output M-vector for the estimated mean.
164  * \param out_cov The output MxM matrix for the estimated covariance matrix, this can also be either a fixed-size of dynamic size matrix.
165  * \sa mrpt::math::meanAndCovVec, math::mean,math::stddev, math::cov
166  */
167  template<class MAT_IN,class VECTOR, class MAT_OUT>
169  const MAT_IN & v,
170  VECTOR & out_mean,
171  MAT_OUT & out_cov
172  )
173  {
174  const size_t N = v.rows();
175  ASSERTMSG_(N>0,"The input matrix contains no elements");
176  const double N_inv = 1.0/N;
177 
178  const size_t M = v.cols();
179  ASSERTMSG_(M>0,"The input matrix contains rows of length 0");
180 
181  // First: Compute the mean
182  out_mean.assign(M,0);
183  for (size_t i=0;i<N;i++)
184  for (size_t j=0;j<M;j++)
185  out_mean[j]+=v.coeff(i,j);
186  out_mean*=N_inv;
187 
188  // Second: Compute the covariance
189  // Save only the above-diagonal part, then after averaging
190  // duplicate that part to the other half.
191  out_cov.zeros(M,M);
192  for (size_t i=0;i<N;i++)
193  {
194  for (size_t j=0;j<M;j++)
195  out_cov.get_unsafe(j,j)+=square(v.get_unsafe(i,j)-out_mean[j]);
196 
197  for (size_t j=0;j<M;j++)
198  for (size_t k=j+1;k<M;k++)
199  out_cov.get_unsafe(j,k)+=(v.get_unsafe(i,j)-out_mean[j])*(v.get_unsafe(i,k)-out_mean[k]);
200  }
201  for (size_t j=0;j<M;j++)
202  for (size_t k=j+1;k<M;k++)
203  out_cov.get_unsafe(k,j) = out_cov.get_unsafe(j,k);
204  out_cov*=N_inv;
205  }
206 
207  /** Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample, so the covariance is MxM.
208  * \param v The set of data, as a NxM matrix.
209  * \param out_cov The output MxM matrix for the estimated covariance matrix.
210  * \sa math::mean,math::stddev, math::cov
211  */
212  template<class MATRIX>
213  inline Eigen::Matrix<typename MATRIX::Scalar,MATRIX::ColsAtCompileTime,MATRIX::ColsAtCompileTime>
214  cov( const MATRIX &v )
215  {
216  Eigen::Matrix<double,MATRIX::ColsAtCompileTime,1> m;
217  Eigen::Matrix<typename MATRIX::Scalar,MATRIX::ColsAtCompileTime,MATRIX::ColsAtCompileTime> C;
218  meanAndCovMat(v,m,C);
219  return C;
220  }
221 
222  /** A useful macro for saving matrixes to a file while debugging. */
223  #define SAVE_MATRIX(M) M.saveToTextFile(#M ".txt");
224 
225 
226  /** 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)
227  */
228  template <class MAT_A,class SKEW_3VECTOR,class MAT_OUT>
229  void multiply_A_skew3(const MAT_A &A,const SKEW_3VECTOR &v, MAT_OUT &out)
230  {
231  MRPT_START
232  ASSERT_EQUAL_(size(A,2),3)
233  ASSERT_EQUAL_(v.size(),3)
234  const size_t N = size(A,1);
235  out.setSize(N,3);
236  for (size_t i=0;i<N;i++)
237  {
238  out.set_unsafe(i,0, A.get_unsafe(i,1)*v[2]-A.get_unsafe(i,2)*v[1] );
239  out.set_unsafe(i,1,-A.get_unsafe(i,0)*v[2]+A.get_unsafe(i,2)*v[0] );
240  out.set_unsafe(i,2, A.get_unsafe(i,0)*v[1]-A.get_unsafe(i,1)*v[0] );
241  }
242  MRPT_END
243  }
244 
245  /** 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)
246  */
247  template <class SKEW_3VECTOR,class MAT_A,class MAT_OUT>
248  void multiply_skew3_A(const SKEW_3VECTOR &v, const MAT_A &A,MAT_OUT &out)
249  {
250  MRPT_START
251  ASSERT_EQUAL_(size(A,1),3)
252  ASSERT_EQUAL_(v.size(),3)
253  const size_t N = size(A,2);
254  out.setSize(3,N);
255  for (size_t i=0;i<N;i++)
256  {
257  out.set_unsafe(0,i,-A.get_unsafe(1,i)*v[2]+A.get_unsafe(2,i)*v[1] );
258  out.set_unsafe(1,i, A.get_unsafe(0,i)*v[2]-A.get_unsafe(2,i)*v[0] );
259  out.set_unsafe(2,i,-A.get_unsafe(0,i)*v[1]+A.get_unsafe(1,i)*v[0] );
260  }
261  MRPT_END
262  }
263 
264 
265  // ------ Implementatin of detail functions -------------
266  namespace detail
267  {
268  /** Extract a submatrix - The output matrix must be set to the required size before call. */
269  template <class MATORG, class MATDEST>
271  const MATORG &M,
272  const size_t first_row,
273  const size_t first_col,
274  MATDEST &outMat)
275  {
276  const size_t NR = outMat.getRowCount();
277  const size_t NC = outMat.getColCount();
278  ASSERT_BELOWEQ_( first_row+NR, M.getRowCount() )
279  ASSERT_BELOWEQ_( first_col+NC, M.getColCount() )
280  for (size_t r=0;r<NR;r++)
281  for (size_t c=0;c<NC;c++)
282  outMat.get_unsafe(r,c) = M.get_unsafe(first_row+r,first_col+c);
283  }
284 
285  } // end of detail namespace
286 
287  /** @} */ // end of grouping
288 
289  } // End of math namespace
290 } // End of mrpt namespace
291 
292 
293 #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:126
#define ASSERT_BELOWEQ_(__A, __B)
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:33
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
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:229
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:141
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:168
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:36
A numeric matrix of compile-time fixed size.
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:270
#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:214
CSerializablePtr ReadObject()
Reads an object from stream, its class determined at runtime, and returns a smart pointer to the obje...
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:148
BASE_IMPEXP ::mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CMatrixPtr &pObj)
#define MRPT_START
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:158
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
size_t size(const MATRIXLIKE &m, int dim)
Definition: bits.h:46
Eigen::MatrixBase< Derived >::PlainObject operator!(const Eigen::MatrixBase< Derived > &m)
Unary inversion operator.
Definition: ops_matrices.h:117
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
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:248
const Eigen::MatrixBase< Derived >::AdjointReturnType operator~(const Eigen::MatrixBase< Derived > &m)
Transpose operator for matrices.
Definition: ops_matrices.h:111
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:31
#define ASSERTMSG_(f, __ERROR_MSG)
CMatrixTemplateNumeric< float > CMatrixFloat
Declares a matrix of float numbers (non serializable).



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo