Main MRPT website > C++ reference
MRPT logo

ops_matrices.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef  mrpt_math_matrix_ops_H
00029 #define  mrpt_math_matrix_ops_H
00030 
00031 #include <mrpt/math/math_frwds.h>  // Fordward declarations
00032 
00033 #include <mrpt/math/CMatrix.h>
00034 #include <mrpt/math/CMatrixD.h>
00035 #include <mrpt/utils/CStream.h>
00036 
00037 #include <mrpt/math/CMatrixTemplateNumeric.h>
00038 #include <mrpt/math/CMatrixFixedNumeric.h>
00039 
00040 #include <mrpt/math/ops_containers.h>           // Many generic operations
00041 
00042 #include <mrpt/utils/metaprogramming.h>  // for copy_container_typecasting()
00043 
00044 
00045 /** \file ops_matrices.h
00046   * This file implements miscelaneous matrix and matrix/vector operations, plus internal functions in mrpt::math::detail
00047   *
00048   */
00049 
00050 namespace mrpt
00051 {
00052         namespace math
00053         {
00054                 /** @name Operators for binary streaming of MRPT matrices
00055                     @{ */
00056 
00057                 /** Read operator from a CStream. The format is compatible with that of CMatrix & CMatrixD */
00058                 template <size_t NROWS,size_t NCOLS>
00059                 mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in, CMatrixFixedNumeric<float,NROWS,NCOLS> & M) {
00060                         CMatrix  aux;
00061                         in.ReadObject(&aux);
00062                         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))
00063                         M = aux;
00064                         return in;
00065                 }
00066                 /** Read operator from a CStream. The format is compatible with that of CMatrix & CMatrixD */
00067                 template <size_t NROWS,size_t NCOLS>
00068                 mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in, CMatrixFixedNumeric<double,NROWS,NCOLS> & M) {
00069                         CMatrixD  aux;
00070                         in.ReadObject(&aux);
00071                         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))
00072                         M = aux;
00073                         return in;
00074                 }
00075 
00076                 /** Write operator for writing into a CStream. The format is compatible with that of CMatrix & CMatrixD */
00077                 template <size_t NROWS,size_t NCOLS>
00078                 mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const CMatrixFixedNumeric<float,NROWS,NCOLS> & M) {
00079                         CMatrix aux = CMatrixFloat(M);
00080                         out.WriteObject(&aux);
00081                         return out;
00082                 }
00083                 /** Write operator for writing into a CStream. The format is compatible with that of CMatrix & CMatrixD */
00084                 template <size_t NROWS,size_t NCOLS>
00085                 mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const CMatrixFixedNumeric<double,NROWS,NCOLS> & M) {
00086                         CMatrixD aux = CMatrixDouble(M);
00087                         out.WriteObject(&aux);
00088                         return out;
00089                 }
00090 
00091                 /** @} */  // end MRPT matrices stream operators
00092 
00093 
00094                 /** @name Operators for text streaming of MRPT matrices
00095                     @{ */
00096 
00097 
00098                 /** Dumps the matrix to a text ostream, adding a final "\n" to Eigen's default output. */
00099                 template <typename T,size_t NROWS,size_t NCOLS>
00100                 inline std::ostream & operator << (std::ostream & s, const CMatrixFixedNumeric<T,NROWS,NCOLS>& m)
00101                 {
00102                         Eigen::IOFormat  fmt; fmt.matSuffix="\n";
00103                         return s << m.format(fmt);
00104                 }
00105 
00106                 /** Dumps the matrix to a text ostream, adding a final "\n" to Eigen's default output. */
00107                 template<typename T>
00108                 inline std::ostream & operator << (std::ostream & s, const CMatrixTemplateNumeric<T>& m)
00109                 {
00110                         Eigen::IOFormat  fmt; fmt.matSuffix="\n";
00111                         return s << m.format(fmt);
00112                 }
00113 
00114                 /** Dumps the vector as a row to a text ostream, with the format: "[v1 v2 v3... vN]" */
00115                 template<typename T>
00116                 inline std::ostream & operator << (std::ostream & s, const mrpt::dynamicsize_vector<T>& m)
00117                 {
00118                         Eigen::IOFormat  fmt; fmt.rowSeparator=" "; fmt.matPrefix="["; fmt.matSuffix="]";
00119                         return s << m.format(fmt);
00120                 }
00121 
00122                 /** @} */  // end MRPT matrices stream operators
00123 
00124 
00125                 /** Transpose operator for matrices */
00126                 template <class Derived>
00127                 inline const typename Eigen::MatrixBase<Derived>::AdjointReturnType operator ~(const Eigen::MatrixBase<Derived> &m) {
00128                         return m.adjoint();
00129                 }
00130 
00131                 /** Unary inversion operator. */
00132                 template <class Derived>
00133                 inline typename Eigen::MatrixBase<Derived>::PlainObject operator !(const Eigen::MatrixBase<Derived> &m) {
00134                         return m.inv();
00135                 }
00136 
00137                 /** @} */  // end MRPT matrices operators
00138 
00139 
00140                 /** R = H * C * H^t (with C symmetric) */
00141                 template <typename MAT_H, typename MAT_C, typename MAT_R>
00142                 inline void multiply_HCHt(
00143                         const MAT_H &H,
00144                         const MAT_C &C,
00145                         MAT_R &R,
00146                         bool accumResultInOutput ) //   bool allow_submatrix_mult)
00147                 {
00148                         if (accumResultInOutput)
00149                              R += ( (H * C.template selfadjointView<Eigen::Lower>()).eval() * H.adjoint()).eval().template selfadjointView<Eigen::Lower>();
00150                         else
00151                              R  = ( (H * C.template selfadjointView<Eigen::Lower>()).eval() * H.adjoint()).eval().template selfadjointView<Eigen::Lower>();
00152                 }
00153 
00154                 /** r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C) */
00155                 template <typename VECTOR_H, typename MAT_C>
00156                 typename MAT_C::value_type
00157                 multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
00158                 {
00159                         return (H.matrix().adjoint() * C * H.matrix()).eval()(0,0);
00160                 }
00161 
00162                 /** R = H^t * C * H  (with C symmetric) */
00163                 template <typename MAT_H, typename MAT_C, typename MAT_R>
00164                 void multiply_HtCH(
00165                         const MAT_H &H,
00166                         const MAT_C &C,
00167                         MAT_R &R,
00168                         bool accumResultInOutput) // bool allow_submatrix_mult)
00169                 {
00170                         if (accumResultInOutput)
00171                              R += ( (H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() * H).eval().template selfadjointView<Eigen::Lower>();
00172                         else
00173                              R  = ( (H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() * H).eval().template selfadjointView<Eigen::Lower>();
00174                 }
00175 
00176 
00177                 /** 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.
00178                   * \param v The set of data as a NxM matrix, of types: CMatrixTemplateNumeric or CMatrixFixedNumeric
00179                   * \param out_mean The output M-vector for the estimated mean.
00180                   * \param out_cov The output MxM matrix for the estimated covariance matrix, this can also be either a fixed-size of dynamic size matrix.
00181                   * \sa mrpt::math::meanAndCovVec, math::mean,math::stddev, math::cov
00182                   */
00183                 template<class MAT_IN,class VECTOR, class MAT_OUT>
00184                 void meanAndCovMat(
00185                         const MAT_IN & v,
00186                         VECTOR       & out_mean,
00187                         MAT_OUT      & out_cov
00188                         )
00189                 {
00190                         const size_t N = v.rows();
00191                         ASSERTMSG_(N>0,"The input matrix contains no elements");
00192                         const double N_inv = 1.0/N;
00193 
00194                         const size_t M = v.cols();
00195                         ASSERTMSG_(M>0,"The input matrix contains rows of length 0");
00196 
00197                         // First: Compute the mean
00198                         out_mean.assign(M,0);
00199                         for (size_t i=0;i<N;i++)
00200                                 for (size_t j=0;j<M;j++)
00201                                         out_mean[j]+=v.coeff(i,j);
00202                         out_mean*=N_inv;
00203 
00204                         // Second: Compute the covariance
00205                         //  Save only the above-diagonal part, then after averaging
00206                         //  duplicate that part to the other half.
00207                         out_cov.zeros(M,M);
00208                         for (size_t i=0;i<N;i++)
00209                         {
00210                                 for (size_t j=0;j<M;j++)
00211                                         out_cov.get_unsafe(j,j)+=square(v.get_unsafe(i,j)-out_mean[j]);
00212 
00213                                 for (size_t j=0;j<M;j++)
00214                                         for (size_t k=j+1;k<M;k++)
00215                                                 out_cov.get_unsafe(j,k)+=(v.get_unsafe(i,j)-out_mean[j])*(v.get_unsafe(i,k)-out_mean[k]);
00216                         }
00217                         for (size_t j=0;j<M;j++)
00218                                 for (size_t k=j+1;k<M;k++)
00219                                         out_cov.get_unsafe(k,j) = out_cov.get_unsafe(j,k);
00220                         out_cov*=N_inv;
00221                 }
00222 
00223                 /** Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample, so the covariance is MxM.
00224                   * \param v The set of data, as a NxM matrix.
00225                   * \param out_cov The output MxM matrix for the estimated covariance matrix.
00226                   * \sa math::mean,math::stddev, math::cov
00227                   */
00228                 template<class MATRIX>
00229                 inline Eigen::Matrix<typename MATRIX::Scalar,MATRIX::ColsAtCompileTime,MATRIX::ColsAtCompileTime>
00230                         cov( const MATRIX &v )
00231                 {
00232                         Eigen::Matrix<double,MATRIX::ColsAtCompileTime,1> m;
00233                         Eigen::Matrix<typename MATRIX::Scalar,MATRIX::ColsAtCompileTime,MATRIX::ColsAtCompileTime>  C;
00234                         meanAndCovMat(v,m,C);
00235                         return C;
00236                 }
00237 
00238                 /** A useful macro for saving matrixes to a file while debugging. */
00239                 #define SAVE_MATRIX(M) M.saveToTextFile(#M ".txt");
00240 
00241 
00242                 /** 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)
00243                   */
00244                 template <class MAT_A,class SKEW_3VECTOR,class MAT_OUT>
00245                 void multiply_A_skew3(const MAT_A &A,const SKEW_3VECTOR &v, MAT_OUT &out)
00246                 {
00247                         MRPT_START
00248                         ASSERT_EQUAL_(size(A,2),3)
00249                         ASSERT_EQUAL_(v.size(),3)
00250                         const size_t N = size(A,1);
00251                         out.setSize(N,3);
00252                         for (size_t i=0;i<N;i++)
00253                         {
00254                                 out.set_unsafe(i,0, A.get_unsafe(i,1)*v[2]-A.get_unsafe(i,2)*v[1] );
00255                                 out.set_unsafe(i,1,-A.get_unsafe(i,0)*v[2]+A.get_unsafe(i,2)*v[0] );
00256                                 out.set_unsafe(i,2, A.get_unsafe(i,0)*v[1]-A.get_unsafe(i,1)*v[0] );
00257                         }
00258                         MRPT_END
00259                 }
00260 
00261                 /** 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)
00262                   */
00263                 template <class SKEW_3VECTOR,class MAT_A,class MAT_OUT>
00264                 void multiply_skew3_A(const SKEW_3VECTOR &v, const MAT_A &A,MAT_OUT &out)
00265                 {
00266                         MRPT_START
00267                         ASSERT_EQUAL_(size(A,1),3)
00268                         ASSERT_EQUAL_(v.size(),3)
00269                         const size_t N = size(A,2);
00270                         out.setSize(3,N);
00271                         for (size_t i=0;i<N;i++)
00272                         {
00273                                 out.set_unsafe(0,i,-A.get_unsafe(1,i)*v[2]+A.get_unsafe(2,i)*v[1] );
00274                                 out.set_unsafe(1,i, A.get_unsafe(0,i)*v[2]-A.get_unsafe(2,i)*v[0] );
00275                                 out.set_unsafe(2,i,-A.get_unsafe(0,i)*v[1]+A.get_unsafe(1,i)*v[0] );
00276                         }
00277                         MRPT_END
00278                 }
00279 
00280 
00281         // ------ Implementatin of detail functions -------------
00282         namespace detail
00283         {
00284                 /** Extract a submatrix - The output matrix must be set to the required size before call. */
00285                 template <class MATORG, class MATDEST>
00286                 void extractMatrix(
00287                         const MATORG &M,
00288                         const size_t first_row,
00289                         const size_t first_col,
00290                         MATDEST &outMat)
00291                 {
00292                         const size_t NR = outMat.getRowCount();
00293                         const size_t NC = outMat.getColCount();
00294                         ASSERT_BELOWEQ_( first_row+NR, M.getRowCount() )
00295                         ASSERT_BELOWEQ_( first_col+NC, M.getColCount() )
00296                         for (size_t r=0;r<NR;r++)
00297                                 for (size_t c=0;c<NC;c++)
00298                                         outMat.get_unsafe(r,c) = M.get_unsafe(first_row+r,first_col+c);
00299                 }
00300 
00301         } // end of detail namespace
00302 
00303         } // End of math namespace
00304 } // End of mrpt namespace
00305 
00306 
00307 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011