Main MRPT website > C++ reference
MRPT logo
ops_matrices.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef mrpt_math_matrix_ops_H
29 #define mrpt_math_matrix_ops_H
30 
31 #include <mrpt/math/math_frwds.h> // Fordward declarations
32 
33 #include <mrpt/math/CMatrix.h>
34 #include <mrpt/math/CMatrixD.h>
35 #include <mrpt/utils/CStream.h>
36 
37 #include <mrpt/math/CMatrixTemplateNumeric.h>
38 #include <mrpt/math/CMatrixFixedNumeric.h>
39 
40 #include <mrpt/math/ops_containers.h> // Many generic operations
41 
42 #include <mrpt/utils/metaprogramming.h> // for copy_container_typecasting()
43 
44 
45 /** \file ops_matrices.h
46  * This file implements miscelaneous matrix and matrix/vector operations, plus internal functions in mrpt::math::detail
47  *
48  */
49 
50 namespace mrpt
51 {
52  namespace math
53  {
54  /** \addtogroup container_ops_grp
55  * @{ */
56 
57  /** @name Operators for binary streaming of MRPT matrices
58  @{ */
59 
60  /** Read operator from 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 &in, CMatrixFixedNumeric<float,NROWS,NCOLS> & M) {
63  CMatrix aux;
64  in.ReadObject(&aux);
65  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))
66  M = aux;
67  return in;
68  }
69  /** Read operator from a CStream. The format is compatible with that of CMatrix & CMatrixD */
70  template <size_t NROWS,size_t NCOLS>
71  mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in, CMatrixFixedNumeric<double,NROWS,NCOLS> & M) {
72  CMatrixD aux;
73  in.ReadObject(&aux);
74  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))
75  M = aux;
76  return in;
77  }
78 
79  /** Write operator for writing into a CStream. The format is compatible with that of CMatrix & CMatrixD */
80  template <size_t NROWS,size_t NCOLS>
81  mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const CMatrixFixedNumeric<float,NROWS,NCOLS> & M) {
82  CMatrix aux = CMatrixFloat(M);
83  out.WriteObject(&aux);
84  return out;
85  }
86  /** Write operator for writing into a CStream. The format is compatible with that of CMatrix & CMatrixD */
87  template <size_t NROWS,size_t NCOLS>
88  mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const CMatrixFixedNumeric<double,NROWS,NCOLS> & M) {
89  CMatrixD aux = CMatrixDouble(M);
90  out.WriteObject(&aux);
91  return out;
92  }
93 
94  /** @} */ // end MRPT matrices stream operators
95 
96 
97  /** @name Operators for text streaming of MRPT matrices
98  @{ */
99 
100 
101  /** Dumps the matrix to a text ostream, adding a final "\n" to Eigen's default output. */
102  template <typename T,size_t NROWS,size_t NCOLS>
103  inline std::ostream & operator << (std::ostream & s, const CMatrixFixedNumeric<T,NROWS,NCOLS>& m)
104  {
105  Eigen::IOFormat fmt; fmt.matSuffix="\n";
106  return s << m.format(fmt);
107  }
108 
109  /** Dumps the matrix to a text ostream, adding a final "\n" to Eigen's default output. */
110  template<typename T>
111  inline std::ostream & operator << (std::ostream & s, const CMatrixTemplateNumeric<T>& m)
112  {
113  Eigen::IOFormat fmt; fmt.matSuffix="\n";
114  return s << m.format(fmt);
115  }
116 
117  /** Dumps the vector as a row to a text ostream, with the format: "[v1 v2 v3... vN]" */
118  template<typename T>
119  inline std::ostream & operator << (std::ostream & s, const mrpt::dynamicsize_vector<T>& m)
120  {
121  Eigen::IOFormat fmt; fmt.rowSeparator=" "; fmt.matPrefix="["; fmt.matSuffix="]";
122  return s << m.format(fmt);
123  }
124 
125  /** @} */ // end MRPT matrices stream operators
126 
127 
128  /** Transpose operator for matrices */
129  template <class Derived>
130  inline const typename Eigen::MatrixBase<Derived>::AdjointReturnType operator ~(const Eigen::MatrixBase<Derived> &m) {
131  return m.adjoint();
132  }
133 
134  /** Unary inversion operator. */
135  template <class Derived>
136  inline typename Eigen::MatrixBase<Derived>::PlainObject operator !(const Eigen::MatrixBase<Derived> &m) {
137  return m.inv();
138  }
139 
140  /** @} */ // end MRPT matrices operators
141 
142 
143  /** R = H * C * H^t (with C symmetric) */
144  template <typename MAT_H, typename MAT_C, typename MAT_R>
145  inline void multiply_HCHt(
146  const MAT_H &H,
147  const MAT_C &C,
148  MAT_R &R,
149  bool accumResultInOutput ) // bool allow_submatrix_mult)
150  {
151  if (accumResultInOutput)
152  R += ( (H * C.template selfadjointView<Eigen::Lower>()).eval() * H.adjoint()).eval().template selfadjointView<Eigen::Lower>();
153  else
154  R = ( (H * C.template selfadjointView<Eigen::Lower>()).eval() * H.adjoint()).eval().template selfadjointView<Eigen::Lower>();
155  }
156 
157  /** r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C) */
158  template <typename VECTOR_H, typename MAT_C>
159  typename MAT_C::value_type
160  multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
161  {
162  return (H.matrix().adjoint() * C * H.matrix()).eval()(0,0);
163  }
164 
165  /** R = H^t * C * H (with C symmetric) */
166  template <typename MAT_H, typename MAT_C, typename MAT_R>
167  void multiply_HtCH(
168  const MAT_H &H,
169  const MAT_C &C,
170  MAT_R &R,
171  bool accumResultInOutput) // bool allow_submatrix_mult)
172  {
173  if (accumResultInOutput)
174  R += ( (H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() * H).eval().template selfadjointView<Eigen::Lower>();
175  else
176  R = ( (H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() * H).eval().template selfadjointView<Eigen::Lower>();
177  }
178 
179 
180  /** 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.
181  * \param v The set of data as a NxM matrix, of types: CMatrixTemplateNumeric or CMatrixFixedNumeric
182  * \param out_mean The output M-vector for the estimated mean.
183  * \param out_cov The output MxM matrix for the estimated covariance matrix, this can also be either a fixed-size of dynamic size matrix.
184  * \sa mrpt::math::meanAndCovVec, math::mean,math::stddev, math::cov
185  */
186  template<class MAT_IN,class VECTOR, class MAT_OUT>
187  void meanAndCovMat(
188  const MAT_IN & v,
189  VECTOR & out_mean,
190  MAT_OUT & out_cov
191  )
192  {
193  const size_t N = v.rows();
194  ASSERTMSG_(N>0,"The input matrix contains no elements");
195  const double N_inv = 1.0/N;
196 
197  const size_t M = v.cols();
198  ASSERTMSG_(M>0,"The input matrix contains rows of length 0");
199 
200  // First: Compute the mean
201  out_mean.assign(M,0);
202  for (size_t i=0;i<N;i++)
203  for (size_t j=0;j<M;j++)
204  out_mean[j]+=v.coeff(i,j);
205  out_mean*=N_inv;
206 
207  // Second: Compute the covariance
208  // Save only the above-diagonal part, then after averaging
209  // duplicate that part to the other half.
210  out_cov.zeros(M,M);
211  for (size_t i=0;i<N;i++)
212  {
213  for (size_t j=0;j<M;j++)
214  out_cov.get_unsafe(j,j)+=square(v.get_unsafe(i,j)-out_mean[j]);
215 
216  for (size_t j=0;j<M;j++)
217  for (size_t k=j+1;k<M;k++)
218  out_cov.get_unsafe(j,k)+=(v.get_unsafe(i,j)-out_mean[j])*(v.get_unsafe(i,k)-out_mean[k]);
219  }
220  for (size_t j=0;j<M;j++)
221  for (size_t k=j+1;k<M;k++)
222  out_cov.get_unsafe(k,j) = out_cov.get_unsafe(j,k);
223  out_cov*=N_inv;
224  }
225 
226  /** Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample, so the covariance is MxM.
227  * \param v The set of data, as a NxM matrix.
228  * \param out_cov The output MxM matrix for the estimated covariance matrix.
229  * \sa math::mean,math::stddev, math::cov
230  */
231  template<class MATRIX>
232  inline Eigen::Matrix<typename MATRIX::Scalar,MATRIX::ColsAtCompileTime,MATRIX::ColsAtCompileTime>
233  cov( const MATRIX &v )
234  {
235  Eigen::Matrix<double,MATRIX::ColsAtCompileTime,1> m;
236  Eigen::Matrix<typename MATRIX::Scalar,MATRIX::ColsAtCompileTime,MATRIX::ColsAtCompileTime> C;
237  meanAndCovMat(v,m,C);
238  return C;
239  }
240 
241  /** A useful macro for saving matrixes to a file while debugging. */
242  #define SAVE_MATRIX(M) M.saveToTextFile(#M ".txt");
243 
244 
245  /** 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)
246  */
247  template <class MAT_A,class SKEW_3VECTOR,class MAT_OUT>
248  void multiply_A_skew3(const MAT_A &A,const SKEW_3VECTOR &v, MAT_OUT &out)
249  {
250  MRPT_START
251  ASSERT_EQUAL_(size(A,2),3)
252  ASSERT_EQUAL_(v.size(),3)
253  const size_t N = size(A,1);
254  out.setSize(N,3);
255  for (size_t i=0;i<N;i++)
256  {
257  out.set_unsafe(i,0, A.get_unsafe(i,1)*v[2]-A.get_unsafe(i,2)*v[1] );
258  out.set_unsafe(i,1,-A.get_unsafe(i,0)*v[2]+A.get_unsafe(i,2)*v[0] );
259  out.set_unsafe(i,2, A.get_unsafe(i,0)*v[1]-A.get_unsafe(i,1)*v[0] );
260  }
261  MRPT_END
262  }
263 
264  /** 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)
265  */
266  template <class SKEW_3VECTOR,class MAT_A,class MAT_OUT>
267  void multiply_skew3_A(const SKEW_3VECTOR &v, const MAT_A &A,MAT_OUT &out)
268  {
269  MRPT_START
270  ASSERT_EQUAL_(size(A,1),3)
271  ASSERT_EQUAL_(v.size(),3)
272  const size_t N = size(A,2);
273  out.setSize(3,N);
274  for (size_t i=0;i<N;i++)
275  {
276  out.set_unsafe(0,i,-A.get_unsafe(1,i)*v[2]+A.get_unsafe(2,i)*v[1] );
277  out.set_unsafe(1,i, A.get_unsafe(0,i)*v[2]-A.get_unsafe(2,i)*v[0] );
278  out.set_unsafe(2,i,-A.get_unsafe(0,i)*v[1]+A.get_unsafe(1,i)*v[0] );
279  }
280  MRPT_END
281  }
282 
283 
284  // ------ Implementatin of detail functions -------------
285  namespace detail
286  {
287  /** Extract a submatrix - The output matrix must be set to the required size before call. */
288  template <class MATORG, class MATDEST>
289  void extractMatrix(
290  const MATORG &M,
291  const size_t first_row,
292  const size_t first_col,
293  MATDEST &outMat)
294  {
295  const size_t NR = outMat.getRowCount();
296  const size_t NC = outMat.getColCount();
297  ASSERT_BELOWEQ_( first_row+NR, M.getRowCount() )
298  ASSERT_BELOWEQ_( first_col+NC, M.getColCount() )
299  for (size_t r=0;r<NR;r++)
300  for (size_t c=0;c<NC;c++)
301  outMat.get_unsafe(r,c) = M.get_unsafe(first_row+r,first_col+c);
302  }
303 
304  } // end of detail namespace
305 
306  /** @} */ // end of grouping
307 
308  } // End of math namespace
309 } // End of mrpt namespace
310 
311 
312 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013