The MRPT project: math_frwds.h Source File

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-2010  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_forwddecls_H
00029 #define mrpt_math_forwddecls_H
00030 
00031 #include <mrpt/config.h>
00032 #include <cmath>
00033 #include <cstdlib>
00034 #include <algorithm>
00035 
00036 /*! \file math_frwds.h
00037   * Forward declarations of all mrpt::math classes related to vectors, arrays and matrices.
00038   * Many of the function implementations are in ops_matrices.h, others in ops_containers.h
00039   */
00040 
00041 namespace mrpt
00042 {
00043         namespace utils
00044         {
00045                 class MRPTDLLIMPEXP CStream;
00046                 template<class T> inline T square(const T& x);
00047         }
00048         namespace poses
00049         {
00050                 class MRPTDLLIMPEXP CPoint2D;
00051                 class MRPTDLLIMPEXP CPoint3D;
00052                 class MRPTDLLIMPEXP CPose2D;
00053                 class MRPTDLLIMPEXP CPose3D;
00054         }
00055 
00056 
00057         namespace math
00058         {
00059                 /*! Selection of the number format in CMatrixTemplate::saveToTextFile
00060                   */
00061                 enum TMatrixTextFileFormat
00062                 {
00063                         MATRIX_FORMAT_ENG = 0,   //!< engineering format '%e'
00064                         MATRIX_FORMAT_FIXED = 1, //!< fixed floating point '%f'
00065                         MATRIX_FORMAT_INT = 2    //!< intergers '%i'
00066                 };
00067 
00068                 /** For usage in one of the constructors of CMatrixFixedNumeric, if it's not required
00069                      to fill it with zeros at the constructor to save time. */
00070                 #define UNINITIALIZED_MATRIX  false,false,false
00071 
00072 
00073                 // TODO: Put many of these functions into "mrpt::math::detail"
00074 
00075                 // ---------------- Forward declarations: Classes ----------------
00076                 template <class T> class CVectorTemplate;
00077 
00078                 template <class T> class CMatrixTemplate;
00079                 template <class T> class MRPTDLLIMPEXP CMatrixTemplateNumeric;
00080                 template <typename T,size_t NROWS,size_t NCOLS> class CMatrixFixedNumeric;
00081                 template<typename MATRIXTYPE> class CArbitrarySubmatrixView;
00082 
00083                 template <typename T, std::size_t N> class CArray;
00084                 template <typename T> class CArray<T,0>;
00085                 template <typename T, std::size_t N> class CArrayPOD;
00086                 template <typename T, std::size_t N> class CArrayNumeric;
00087 
00088                 class MRPTDLLIMPEXP CMatrix;
00089                 class MRPTDLLIMPEXP CMatrixD;
00090 
00091                 // Functions shared by all kind of matrices/vectors/array
00092                 // These are member functions that invoke functions in mrpt::math, most of them in ops_containers.h
00093                 // ------------------------------------------------------------------------------------------------
00094 #define DECLARE_COMMON_CONTAINERS_MEMBERS(NUMTYPE) \
00095                 /*! @name Methods common to all containers  */ \
00096                 /*! Count how many elements in a container are non-zero. */ \
00097                 inline size_t countNonZero() const { return mrpt::math::countNonZero(*this); } \
00098                 /*! Fill all the elements with a given value */ \
00099                 inline void fill(const NUMTYPE &val) { std::fill(mrpt_autotype::begin(),mrpt_autotype::end(),val); } \
00100                 /*! Finds the maximum value (and the corresponding zero-based index). */ \
00101                 inline NUMTYPE maximum(size_t *maxIndex = NULL) const { return mrpt::math::maximum(*this,maxIndex); } \
00102                 /*! Finds the minimum value (and the corresponding zero-based index). */ \
00103                 inline NUMTYPE minimum(size_t *minIndex = NULL) const { return mrpt::math::minimum(*this,minIndex ); } \
00104                 /*! Minimum and maximum of a vector at once */ \
00105                 inline void minimum_maximum(NUMTYPE &out_min, NUMTYPE &out_max, \
00106                         size_t *minIndex = NULL, size_t *maxIndex = NULL) const \
00107                 { mrpt::math::minimum_maximum(*this,out_min,out_max,minIndex,maxIndex); } \
00108                 /*! Compute the norm-infinite of a vector ($f[ ||\mathbf{v}||_\infnty $f]), ie the maximum absolute value of the elements. */ \
00109                 inline NUMTYPE norm_inf(size_t *maxIndex = NULL) const { return mrpt::math::norm_inf(*this,maxIndex); } \
00110                 /*! Sum all the elements  */ \
00111                 inline NUMTYPE sumAll() const { return mrpt::math::sum(*this); } \
00112                 /*! Cumsum of all the elements, saving in an output container */ \
00113                 template <class CONTAINEROUT> inline void cumsum(CONTAINEROUT &out) const { mrpt::math::cumsum(*this,out); } \
00114                 /*! Cumsum of all the elements, returning a container of the given type, for example: CMatrixDouble m = mat.cumsum<CMatrixDouble>(); */ \
00115                 template <class CONTAINEROUT> inline CONTAINEROUT cumsum() const { CONTAINEROUT o; mrpt::math::cumsum(*this,o); return o;} \
00116                 /** Counts the number of elements that appear in both containers (comparison through the == operator). It is assumed that no repeated elements appear within each of the containers.  */ \
00117                 template <class CONTAINER2> inline size_t countCommonElements(const CONTAINER2 &b) const { return mrpt::math::countCommonElements(*this,b); } \
00118                 /** Computes the normalized or normal histogram of a sequence of numbers given the number of bins and the limits. In any case this is a "linear" histogram, i.e. for matrices, all the elements are taken as if they were a plain sequence, not taking into account they were in columns or rows. */ \
00119                 inline std::vector<double> histogram(double limit_min,double limit_max,size_t number_bins,bool do_normalization = false ) const { \
00120                         return mrpt::math::histogram(*this,limit_min,limit_max,number_bins,do_normalization); } \
00121                 /*! Applies a generic operation "T func(T val)" to all the elements. */ \
00122                 template <class F> mrpt_autotype & applyToAllElements( NUMTYPE (*function)(NUMTYPE) ) { \
00123                         std::for_each(this->begin(),this->end(), function); \
00124                         return *this; } \
00125                 inline mrpt_autotype & Sqrt() /*!< Applies sqrt() to all the elements  */ { for (typename mrpt_autotype::iterator it=this->begin();it!=this->end();++it) *it = ::sqrt(*it); return *this; }  \
00126                 inline mrpt_autotype & Abs()  /*!< Applies abs() to all the elements  */ { for (typename mrpt_autotype::iterator it=this->begin();it!=this->end();++it) *it = ::fabs(*it); return *this; }  \
00127                 inline mrpt_autotype & Square()  /*!< Applies square() to all the elements */ { for (typename mrpt_autotype::iterator it=this->begin();it!=this->end();++it) *it = mrpt::utils::square(*it); return *this; }  \
00128                 inline mrpt_autotype & Exp()  /*!< Applies exp() to all the elements */ { for (typename mrpt_autotype::iterator it=this->begin();it!=this->end();++it) *it = ::exp(*it); return *this; }  \
00129                 inline mrpt_autotype & Log()  /*!< Applies log() to all the elements */ { for (typename mrpt_autotype::iterator it=this->begin();it!=this->end();++it) *it = ::log(*it); return *this; }  \
00130                 /*! Return the square norm of the container (for matrices, as if it were a vector with all its elements).  */ \
00131                 inline NUMTYPE squareNorm() const { return mrpt::math::squareNorm(*this); } \
00132                 /*! Return the norm of the container (for matrices, as if it were a vector with all its elements).  */ \
00133                 inline NUMTYPE norm() const { return mrpt::math::norm(*this); } \
00134                 /*! Return the mean of the container (for matrices, as if it were a vector with all its elements).  */ \
00135                 inline NUMTYPE mean() const { return mrpt::math::mean(*this); } \
00136                 inline void adjustRange(const NUMTYPE min_val,const NUMTYPE max_val) /*!< Normalize all the elements such as they are in the given range */ { mrpt::math::adjustRange(*this,min_val,max_val); } \
00137                 inline void normalize(const NUMTYPE min_val=0,const NUMTYPE max_val=1) /*!< Normalize all the elements such as they are in the given range */ { mrpt::math::adjustRange(*this,min_val,max_val); } \
00138                 double std(bool unbiased=true) const /*! Return the standard deviation of all the elements in the container. */ { return mrpt::math::stddev(*this); } \
00139                 void meanAndStd(double &out_mean, double &out_std, bool unbiased = true) /*! Return the mean and standard deviation of all the elements in the container. */ { \
00140                         return mrpt::math::meanAndStd(*this,out_mean,out_std,unbiased); } \
00141 
00142                 //Some operations could be made more efficient if inside each class:
00143                 //operator-() const
00144                 //  Already implemented in ops_containers.h:  +=, -=
00145 #define DECLARE_COMMON_MATRIX_MEMBERS(NUMTYPE) \
00146                 DECLARE_COMMON_CONTAINERS_MEMBERS(NUMTYPE) \
00147                 /*! @name Methods common to all matrices */ \
00148                 /* ========= sum / substract functions: =========  */ \
00149                 inline mrpt_autotype operator+()        {\
00150                         return *this;\
00151                 }\
00152                 inline mrpt_autotype operator-() const  {\
00153                         mrpt_autotype res(*this);\
00154                         for (size_t i=0;i<mrpt_autotype::getRowCount();++i) for (size_t j=0;j<mrpt_autotype::getColCount();++j) res.set_unsafe(i,j,-res.get_unsafe(i,j));\
00155                         return res;\
00156                 }\
00157                 mrpt_autotype &operator+=(const NUMTYPE n) /*!< Sum a scalar */ {\
00158                         for (size_t i=0;i<mrpt_autotype::getRowCount();++i) for (size_t j=0;j<mrpt_autotype::getColCount();++j) mrpt_autotype::get_unsafe(i,j)+=n;\
00159                         return *this;\
00160                 }\
00161                 mrpt_autotype &operator-=(const NUMTYPE n) /*!< Substract a scalar */   {\
00162                         for (size_t i=0;i<mrpt_autotype::getRowCount();++i) for (size_t j=0;j<mrpt_autotype::getColCount();++j) mrpt_autotype::get_unsafe(i,j)-=n;\
00163                         return *this;\
00164                 }\
00165                 mrpt_autotype &operator*=(const NUMTYPE n) /*!< Multiply by scalar */   {\
00166                         for (size_t i=0;i<mrpt_autotype::getRowCount();++i) for (size_t j=0;j<mrpt_autotype::getColCount();++j) mrpt_autotype::get_unsafe(i,j)*=n;\
00167                         return *this;\
00168                 }\
00169                 inline mrpt_autotype& operator/=(const NUMTYPE n) /*!< Divide by scalar */      {\
00170                         return (*this)*=NUMTYPE(1)/n; } \
00171                 template<typename OTHERMATRIX> \
00172                 inline RET_VOID_ASSERT_MRPTMATRIX(OTHERMATRIX) operator*=(const OTHERMATRIX &m) {\
00173                         mrpt::math::detail::multiply_AB(*this,m,*this); } \
00174                 template<typename OTHERMATRIX> mrpt_autotype &add_At(const OTHERMATRIX &m)      {\
00175                         ASSERT_((m.getRowCount()==mrpt_autotype::getColCount())&&(m.getColCount()==mrpt_autotype::getRowCount()));\
00176                         for (size_t i=0;i<mrpt_autotype::getRowCount();++i) for (size_t j=0;j<mrpt_autotype::getColCount();++j) mrpt_autotype::get_unsafe(i,j)+=m.get_unsafe(j,i);\
00177                         return *this;\
00178                 }\
00179                 template<typename OTHERMATRIX> mrpt_autotype &add_Ac(const OTHERMATRIX &m,const NUMTYPE c)      {\
00180                         ASSERT_((m.getRowCount()==mrpt_autotype::getRowCount())&&(m.getColCount()==mrpt_autotype::getColCount()));\
00181                         for (size_t i=0;i<mrpt_autotype::getRowCount();++i) for (size_t j=0;j<mrpt_autotype::getColCount();++j) mrpt_autotype::get_unsafe(i,j)+=c*m.get_unsafe(i,j);\
00182                         return *this;\
00183                 }\
00184                 template<typename OTHERMATRIX> mrpt_autotype &substract_At(const OTHERMATRIX &m)        {\
00185                         ASSERT_((m.getRowCount()==mrpt_autotype::getColCount())&&(m.getColCount()==mrpt_autotype::getRowCount()));\
00186                         for (size_t i=0;i<mrpt_autotype::getRowCount();++i) for (size_t j=0;j<mrpt_autotype::getColCount();++j) mrpt_autotype::get_unsafe(i,j)-=m.get_unsafe(j,i);\
00187                         return *this;\
00188                 } \
00189                 /*! Substract n (integer) times A to this matrix: this -= A * n  */ \
00190                 template<typename OTHERMATRIX> mrpt_autotype &  substract_An(const OTHERMATRIX& m, const size_t n)      {\
00191                         ASSERT_((m.getRowCount()==mrpt_autotype::getRowCount())&&(m.getColCount()==mrpt_autotype::getColCount()));\
00192                         for (size_t i=0;i<mrpt_autotype::getRowCount();++i) for (size_t j=0;j<mrpt_autotype::getColCount();++j) mrpt_autotype::get_unsafe(i,j)-=n*m.get_unsafe(i,j);\
00193                         return *this;\
00194                 }\
00195                 /*! Substract c (scalar) times A to this matrix: this -= A * c  */ \
00196                 template<typename OTHERMATRIX> mrpt_autotype &substract_Ac(const OTHERMATRIX &m,const NUMTYPE c)        {\
00197                         ASSERT_((m.getRowCount()==mrpt_autotype::getRowCount())&&(m.getColCount()==mrpt_autotype::getColCount()));\
00198                         for (size_t i=0;i<mrpt_autotype::getRowCount();++i) for (size_t j=0;j<mrpt_autotype::getColCount();++j) mrpt_autotype::get_unsafe(i,j)-=c*m.get_unsafe(i,j);\
00199                         return *this;\
00200                 }\
00201                 /*! this += A + A<sup>T</sup>  */ \
00202                 template<typename OTHERMATRIX> mrpt_autotype &add_AAt(const OTHERMATRIX &m)     {\
00203                         ASSERT_((mrpt_autotype::getRowCount()==mrpt_autotype::getColCount())&&(mrpt_autotype::getRowCount()==m.getRowCount())&&(mrpt_autotype::getColCount()==m.getColCount()));\
00204                         for (size_t i=0;i<mrpt_autotype::getRowCount();++i) for (size_t j=0;j<mrpt_autotype::getColCount();++j) mrpt_autotype::get_unsafe(i,j)+=m.get_unsafe(i,j)+m.get_unsafe(j,i);\
00205                         return *this;\
00206                 }\
00207                 /*! this -= A + A<sup>T</sup>  */ \
00208                 template<typename OTHERMATRIX> mrpt_autotype &substract_AAt(const OTHERMATRIX &m)       {\
00209                         ASSERT_((mrpt_autotype::getRowCount()==mrpt_autotype::getColCount())&&(mrpt_autotype::getRowCount()==m.getRowCount())&&(mrpt_autotype::getColCount()==m.getColCount()));\
00210                         for (size_t i=0;i<mrpt_autotype::getRowCount();++i) for (size_t j=0;j<mrpt_autotype::getColCount();++j) mrpt_autotype::get_unsafe(i,j)-=m.get_unsafe(i,j)+m.get_unsafe(j,i);\
00211                         return *this;\
00212                 }\
00213                 /* =========  multiply_* functions =========  */ \
00214                 bool empty() const { return this->getColCount()==0 || this->getRowCount()==0; } \
00215                 template <class MATRIX1,class MATRIX2> \
00216                 inline void multiply( const MATRIX1& A, const MATRIX2 &B ) /*!< this = A * B */ { mrpt::math::detail::multiply_AB(A,B,*this); } \
00217                 /*! Computes the vector vOut = this * vIn, where "vIn" is a column vector of the appropriate length. */ \
00218                 template<typename OTHERVECTOR1,typename OTHERVECTOR2> \
00219                 inline void multiply_Ab(const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,bool accumToOutput = false) const {\
00220                         mrpt::math::detail::multiply_Ab(*this,vIn,vOut,accumToOutput); } \
00221                 /*! Computes the vector vOut = this<sup>T</sup> * vIn, where "vIn" is a column vector of the appropriate length. */ \
00222                 template<typename OTHERVECTOR1,typename OTHERVECTOR2> \
00223                 inline void multiply_Atb(const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,bool accumToOutput = false) const {\
00224                         mrpt::math::detail::multiply_Atb(*this,vIn,vOut,accumToOutput); } \
00225                 template <typename MAT_C, typename MAT_R> \
00226                 inline void multiply_HCHt(const MAT_C &C,MAT_R &R,bool accumResultInOutput=false,bool allow_submatrix_mult=false) const /*!< R = this * C * this<sup>T</sup> \sa detail::multiply_HCHt */ { \
00227                         mrpt::math::detail::multiply_HCHt(*this,C,R,accumResultInOutput,allow_submatrix_mult); } \
00228                 /*! R = H * C * H<sup>T</sup> (with a vector H and a symmetric matrix C) In fact when H is a vector, multiply_HCHt_scalar and multiply_HtCH_scalar are exactly equivalent */ \
00229                 template <typename MAT_C> NUMTYPE multiply_HCHt_scalar(const MAT_C &C) const { return mrpt::math::detail::multiply_HCHt_scalar(*this,C); } \
00230                 /*! R = H<sup>T</sup> * C * H (with a vector H and a symmetric matrix C) In fact when H is a vector, multiply_HCHt_scalar and multiply_HtCH_scalar are exactly equivalent */ \
00231                 template <typename MAT_C> NUMTYPE multiply_HtCH_scalar(const MAT_C &C) const { return mrpt::math::detail::multiply_HCHt_scalar(*this,C); } \
00232                 /*! R = H<sup>T</sup> * C * H (with a symmetric matrix C) */ \
00233                 template <typename MAT_C, typename MAT_R> \
00234                 inline void multiply_HtCH(const MAT_C &C,MAT_R &R,bool accumResultInOutput=false,bool allow_submatrix_mult=false) const /*!< R = this<sup>T</sup> * C * this \sa detail::multiply_HtCH */  { \
00235                         mrpt::math::detail::multiply_HtCH(*this,C,R,accumResultInOutput,allow_submatrix_mult); } \
00236                 template <class MAT_A,class MAT_OUT> \
00237                 inline void multiply_subMatrix(const MAT_A &A,MAT_OUT &outResult,const size_t A_cols_offset,const size_t A_rows_offset,const size_t A_col_count) const /*!< outResult = this * A */ { \
00238                         mrpt::math::detail::multiply_subMatrix(*this,A,outResult,A_cols_offset,A_rows_offset,A_col_count); } \
00239                 template <class MAT_A,class MAT_B,class MAT_C> \
00240                 void multiply_ABC(const MAT_A &A, const MAT_B &B, const MAT_C &C) /*!< this = A*B*C  */ { \
00241                         mrpt::math::detail::multiply_ABC(A,B,C, *this); } \
00242                 template <class MAT_A,class MAT_B,class MAT_C> \
00243                 void multiply_ABCt(const MAT_A &A, const MAT_B &B, const MAT_C &C) /*!< this = A*B*(C<sup>T</sup>) */ { \
00244                         mrpt::math::detail::multiply_ABCt(A,B,C, *this); } \
00245                 template <class MAT_A,class MAT_B> \
00246                 inline void multiply_ABt(const MAT_A &A,const MAT_B &B) /*!< this = A * B<sup>T</sup> */ { \
00247                         mrpt::math::detail::multiply_ABt(A,B,*this); } \
00248                 template <class MAT_A> \
00249                 inline void multiply_AAt(const MAT_A &A) /*!< this = A * A<sup>T</sup> */ { \
00250                         mrpt::math::detail::multiply_AAt(A,*this); } \
00251                 template <class MAT_A> \
00252                 inline void multiply_AtA(const MAT_A &A) /*!< this = A<sup>T</sup> * A */ { \
00253                         mrpt::math::detail::multiply_AtA(A,*this); } \
00254                 template <class MAT_A,class MAT_B> \
00255                 inline void multiply_result_is_symmetric(const MAT_A &A,const MAT_B &B) /*!< this = A * B (result is symmetric) */ { \
00256                         mrpt::math::detail::multiply_result_is_symmetric(A,B,*this); } \
00257                 /* =========  misc funcs: =========  */ \
00258                 /*! Alternative access to element (i,j) with first index being (1,1) instead of (0,0) */ \
00259                 inline NUMTYPE _E(const size_t row, const size_t col) const { return mrpt_autotype::get_unsafe(row-1,col-1); } \
00260                 /*! Alternative access to element (i,j) with first index being (1,1) instead of (0,0) */ \
00261                 inline NUMTYPE & _E(const size_t row, const size_t col) { return mrpt_autotype::get_unsafe(row-1,col-1); } \
00262                 inline bool IsSquare() const { return mrpt_autotype::getColCount()==mrpt_autotype::getRowCount(); } \
00263                 inline bool isSquare() const { return mrpt_autotype::getColCount()==mrpt_autotype::getRowCount(); } \
00264                 /*! Checks for the absolute value of the determinant of the matrix being equal or below a given threshold (by default, exactly zero). */ \
00265                 inline bool isSingular(const NUMTYPE epsilon=0) const { \
00266                         if (!isSquare()) return false; else return std::abs(this->det())<=epsilon; } \
00267                 /*! The trace of this square matrix */ \
00268                 inline NUMTYPE trace() const { return mrpt::math::detail::trace(*this); }\
00269                 template <class MAT_OUT> \
00270                 void pseudoInverse( MAT_OUT &out ) const /*!< Pseudo inverse of this matrix M: out = (M<sup>T</sup> * M)^-1 * M<sup>T</sup> */ { \
00271                         mrpt_autotype A, Ainv; A.multiply_AtA(*this); A.inv_fast(Ainv); out.multiply_ABt(Ainv,*this); } \
00272                 template <class MAT_OUT> \
00273                 MAT_OUT pseudoInverse() const /*!< Pseudo inverse of this matrix M: return (M<sup>T</sup> * M)^-1 * M<sup>T</sup>. Invoke with template argument for desired return matrix type: m.pseudoInverse<CMatrixDouble>() */ { \
00274                         mrpt_autotype A, Ainv; A.multiply_AtA(*this); A.inv_fast(Ainv); \
00275                         MAT_OUT out; out.multiply_ABt(Ainv,*this); return out; } \
00276                 inline NUMTYPE det() const /*!< Returns the determinant of the matrix  */ { return mrpt::math::detail::detMatrix(*this); } \
00277                 template <class MATRIXOUT> inline void inv( MATRIXOUT& out ) const /*!< out = inv(this) */ { mrpt::math::detail::invMatrix(*this,out); } \
00278                 inline mrpt_autotype inv() const /*!< mrpt_autotype out = inv(this) */ { mrpt_autotype ret; mrpt::math::detail::invMatrix(*this,ret); return ret; } \
00279                 template <class MATRIXOUT> inline void inv_fast( MATRIXOUT& out ) /*!< out = inv(this), overwriting contents of this */ { mrpt::math::detail::invMatrix_destroySrc(*this,out); } \
00280                 /*! Matrix left divide: RES = A<sup>-1</sup> &#183; this  (A must be a square matrix). \sa rightDivideSquare,fastLeftDivideSquare */ \
00281                 template <class MAT2,class MAT3> inline void leftDivideSquare(const MAT2 &A,MAT3 &RES) const { mrpt::math::detail::leftDivideSquare(*this,A,RES); } \
00282                 /*! Matrix right divide: RES = this &#183; B<sup>-1</sup> (B must be a square matrix). \sa leftDivideSquare,fastRightDivideSquare */ \
00283                 template <class MAT2,class MAT3> inline void rightDivideSquare(const MAT2 &B, MAT3 &RES) const { mrpt::math::detail::rightDivideSquare(*this,B,RES); } \
00284                 /*! Matrix left divide: this = A<sup>-1</sup> &#183; this   (A must be a square matrix, and will be modified on return). \sa leftDivideSquare,fastRightDivideSquare */ \
00285                 template <typename MAT2> inline void fastLeftDivideSquare(MAT2 &A) { mrpt::math::detail::fastLeftDivideSquare(*this,A); } \
00286                 /*! Matrix right divide: this = this &#183; B<sup>-1</sup> (B must be a square matrix, and will be modified on return).  \sa leftDivideSquare,fastRightDivideSquare */ \
00287                 template <typename MAT2> inline void fastRightDivideSquare(MAT2 &B) { mrpt::math::detail::fastRightDivideSquare(*this,B); } \
00288                 /*! Save matrix to a text file, compatible with MATLAB text format. */ \
00289                 inline void  saveToTextFile(const std::string &file,mrpt::math::TMatrixTextFileFormat fileFormat = MATRIX_FORMAT_ENG,bool appendMRPTHeader=false,const std::string &userHeader = std::string("") ) const { \
00290                         mrpt::math::detail::saveMatrixToTextFile(*this, file,fileFormat,appendMRPTHeader,userHeader); } \
00291                         inline int pivot(const size_t row) { return mrpt::math::detail::matrix_pivot(*this,row); } \
00292                 /* =========  Decomposition functions: =========  */ \
00293                 /*! Cholesky factorization: in = out' *  out \return True on success, false on singular matrix \sa mrpt::math::chol */ \
00294                 template <class MATRIXOUT> inline bool chol(MATRIXOUT &out) const { return mrpt::math::detail::chol(*this,out); } \
00295                 /*! Eigenvalues and eigenvector of symmetric matrix: M = eVecs * eVals * eVecs<sup>T</sup>, with eigenvectors in columns in eVecs, eigenvalues in ascending order in diagonal of eVals  \sa eigenVectorsVec, eigenValues */ \
00296                 template <class MATRIX1,class MATRIX2> \
00297                 inline RET_VOID_ASSERT_MRPTMATRICES(MATRIX1,MATRIX2) eigenVectors( MATRIX1& eVecs, MATRIX2& eVals) const { \
00298                         ARRAY_TYPE_SAMESIZE_ROWS_OF(mrpt_autotype) eigVals; \
00299                         eigVals.resize(this->getRowCount()); \
00300                         mrpt::math::detail::eigenVectorsMatrix(*this, &eVecs,eigVals); \
00301                         const size_t n = eigVals.size(); \
00302                         eVals.zeros(n,n); for (size_t i=0;i<n;i++) eVals.get_unsafe(i,i)=eigVals[i]; \
00303                         } \
00304                 /*! Like eigenVectors, but eigenvalues are returned in a vector instead of a matrix's diagonal. \sa eigenVectors,eigenValues */ \
00305                 template <class MATRIX1,class VECTOR1> \
00306                 inline void eigenVectorsVec( MATRIX1& eVecs, VECTOR1& eVals) const { mrpt::math::detail::eigenVectorsMatrix(*this, &eVecs,eVals); } \
00307                 /*! Return the eigenvalues of the matrix in an ordered vector. \sa eigenVectors */ \
00308                 template <class VECTOR1> \
00309                 inline void eigenValues( VECTOR1& eVals) const { mrpt::math::detail::eigenVectorsMatrix(*this,static_cast<const mrpt_autotype*>(NULL),eVals); } \
00310                 inline void setIdentity()       {\
00311                         ASSERT_(isSquare());\
00312                         set_unsafe(0,0,NUMTYPE(1));\
00313                         for (size_t i=1;i<mrpt_autotype::getRowCount();++i)     {\
00314                                 set_unsafe(i,i,NUMTYPE(1));\
00315                                 for (size_t j=0;j<i;++j)        {\
00316                                         set_unsafe(i,j,NUMTYPE(0));\
00317                                         set_unsafe(j,i,NUMTYPE(0));\
00318                                 }\
00319                         }\
00320                 }\
00321                 inline void setIdentity(size_t N)       {\
00322                         setSize(N,N);\
00323                         setIdentity();\
00324                 }\
00325                 template<typename OTHERMATRIX> inline mrpt_autotype &assignMatrix(const OTHERMATRIX &m) {\
00326                         setSize(m.getRowCount(),m.getColCount());\
00327                         for (size_t i=0;i<mrpt_autotype::getRowCount();++i) for (size_t j=0;j<mrpt_autotype::getColCount();++j) mrpt_autotype::set_unsafe(i,j,m.get_unsafe(i,j));\
00328                         return *this;\
00329                 }\
00330                 inline size_t rank(NUMTYPE eps=1e-7) const      {\
00331                         return mrpt::math::detail::rank(*this,eps);\
00332                 }\
00333 
00334 
00335                 // Generic functions in mrpt::math::detail ----------------------------------------------------
00336                 namespace detail
00337                 {
00338                         template<class MATRIX1,class MATRIX2>
00339                                 bool chol(const MATRIX1 &in, MATRIX2 &out);
00340                         template<class MATRIX1>
00341                                 typename MATRIX1::value_type trace(const MATRIX1 &m);
00342 
00343                         template <class MATRIX1,class MATRIX2,class MATRIXRES>
00344                                 void multiply_AB(const MATRIX1& m1,const MATRIX2& m2, MATRIXRES& RESULT );
00345                         template<class MATRIX1, class OTHERVECTOR1,class OTHERVECTOR2>
00346                                 void multiply_Ab(const MATRIX1 &m, const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,bool accumToOutput );
00347                         template<class MATRIX1, class OTHERVECTOR1,class OTHERVECTOR2>
00348                                 void multiply_Atb(const MATRIX1 &m, const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,bool accumToOutput );
00349                         template <class MATRIX1,class MATRIX2>
00350                                 void multiply_AAt( const MATRIX1& m1, MATRIX2& RESULT );
00351                         template <class MATRIX1,class MATRIX2>
00352                                 void multiply_AtA( const MATRIX1& m1, MATRIX2& RESULT );
00353 
00354                         /*! This executes the operation \f$ \mathbf{R} = \mathbf{H} \mathbf{C} \mathbf{H}^t \f$, where 'this' matrix is \f$ \mathbf{H} \f$ and \f$ \mathbf{C} \f$ is symmetric, in an efficient and numerically stable way.
00355                           *  If 'this' matrix is \f$ N \times M \f$, then \f$ \mathbf{C} \f$ must be \f$ M \times M \f$, and the result matrix \f$ R \f$ will be \f$ N \times N \f$.
00356                           * The result from this method is assured to be symmetric (if \f$ \mathbf{C} \f$ is symmetric), whereas executing:
00357                           \code
00358                                  R = H * C * (~H);
00359                           \endcode
00360                           * may lead to non-symmetric matrixes due to numerical rounding errors. In addition, this method is more efficient that the code above (see the MRPT's code examples on matrixes).
00361                           *  If accumResultInOutput=true, the contents of the output matrix will not be cleared, but added to the result of the operations. In this case it must have the correct size
00362                           *   before calling or an exception will be raised.
00363                           * \sa multiply_HCHt_scalar
00364                           */
00365                         template <typename MAT_H, typename MAT_C, typename MAT_R>
00366                                 void multiply_HCHt(const MAT_H &H,const MAT_C &C,MAT_R &R,bool accumResultInOutput=false,bool allow_submatrix_mult=false);
00367                         template <typename MAT_H, typename MAT_C, typename MAT_R>
00368                                 void multiply_HtCH(const MAT_H &H,const MAT_C &C,MAT_R &R,bool accumResultInOutput=false,bool allow_submatrix_mult=false);
00369                         template <typename VECTOR_H, typename MAT_C> typename MAT_C::value_type
00370                                 multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C);
00371 
00372                         template <class MAT_X,class MAT_A,class MAT_OUT>
00373                                 void multiply_subMatrix(const MAT_X &X,const MAT_A &A,MAT_OUT &outResult,const size_t A_cols_offset,const size_t A_rows_offset,const size_t A_col_count);
00374 
00375                         template <class MAT_A,class MAT_B,class MAT_C,class MAT_OUT>
00376                                 void multiply_ABC(const MAT_A &A, const MAT_B &B, const MAT_C &C, MAT_OUT & RES);
00377                         template <class MAT_A,class MAT_B,class MAT_C,class MAT_OUT>
00378                                 void multiply_ABCt(const MAT_A &A, const MAT_B &B, const MAT_C &C, MAT_OUT & RES);
00379 
00380                         template <class MAT_A,class MAT_B,class MAT_OUT>
00381                                 void multiply_ABt(const MAT_A &A,const MAT_B &B, MAT_OUT &out);
00382                         template <class MAT_A,class MAT_OUT>
00383                                 void multiply_AAt(const MAT_A &A, MAT_OUT &out);
00384                         template <class MAT_A,class MAT_OUT>
00385                                 void multiply_AAt(const MAT_A &A,MAT_OUT &out);
00386                         template <class MAT_A,class MAT_B,class MAT_OUT>
00387                                 void multiply_result_is_symmetric(const MAT_A &A,const MAT_B &B, MAT_OUT &out);
00388 
00389 
00390                         template<typename MatrixType> size_t rank(const MatrixType &m,typename MatrixType::value_type eps=1e-7);
00391 
00392                         // -------- MATRIX INVERSES --------
00393                         template <class MATRIXIN,class MATRIXOUT>       inline void  invMatrix( const MATRIXIN &M, MATRIXOUT &out_inv );
00394                         template <class MATRIXIN,class MATRIXOUT>       inline void  invMatrix_destroySrc( MATRIXIN &M, MATRIXOUT &out_inv );
00395                         // Specializations in ops_matrices.h
00396 
00397                         // templates for special cases:
00398                         template <class MATRIXIN,class MATRIXOUT> inline void invMatrix_special_2x2( const MATRIXIN &M, MATRIXOUT &out_inv );
00399                         template <class MATRIXIN,class MATRIXOUT> inline void invMatrix_special_3x3( const MATRIXIN &M, MATRIXOUT &out_inv );
00400 
00401 
00402                         // -------- MATRIX DETERMINANT --------
00403                         template <class MATRIX> RET_ELEMENT_ASSERT_MRPTCONTAINER(MATRIX)
00404                                 detMatrix(const MATRIX& M);
00405                         // Specializations in ops_matrices.h
00406 
00407                         // Special cases:
00408                         template <class MATRIX> inline typename MATRIX::value_type detMatrix_special_2x2(const MATRIX &M);
00409                         template <class MATRIX> inline typename MATRIX::value_type detMatrix_special_3x3(const MATRIX &M);
00410                         template <class MATRIX>        typename MATRIX::value_type detMatrix_special_4x4(const MATRIX &M);
00411 
00412 
00413                         // -------- Eigenvalues --------
00414                         template <class MATRIX1,class MATRIX2,class VECTOR1> void
00415                                 eigenVectorsMatrix(const MATRIX1 &M,MATRIX2 *eVecs,VECTOR1 &eVals );
00416 
00417                         // Special cases
00418                         template <class MATRIX1,class MATRIX2,class VECTOR1> void eigenVectorsMatrix_special_2x2(const MATRIX1 &M,MATRIX2 *eVecs,VECTOR1 &eVals );
00419 
00420                         // specializations that call the above special cases:
00421                         template <> inline void eigenVectorsMatrix<CMatrixFixedNumeric<float,2,2>,CMatrixFixedNumeric<float,2,2>,CArrayNumeric<float,2> >(const CMatrixFixedNumeric<float,2,2> &M,CMatrixFixedNumeric<float,2,2> *eVecs,CArrayNumeric<float,2> &eVals )         { eigenVectorsMatrix_special_2x2(M,eVecs,eVals); }
00422                         template <> inline void eigenVectorsMatrix<CMatrixFixedNumeric<double,2,2>,CMatrixFixedNumeric<double,2,2>,CArrayNumeric<double,2> >(const CMatrixFixedNumeric<double,2,2> &M,CMatrixFixedNumeric<double,2,2> *eVecs,CArrayNumeric<double,2> &eVals )   { eigenVectorsMatrix_special_2x2(M,eVecs,eVals); }
00423 
00424                         // -------- Others --------
00425                         template <class MAT> void
00426                                 saveMatrixToTextFile(const MAT &theMatrix, const std::string &file, TMatrixTextFileFormat fileFormat = MATRIX_FORMAT_ENG, bool appendMRPTHeader = false, const std::string &userHeader = std::string("") );
00427 
00428                         template <typename T,size_t NROWS,size_t NCOLS> void fixedToDynMatrix( const CMatrixFixedNumeric<T,NROWS,NCOLS> &SRC, CMatrixTemplateNumeric<T> &DST);
00429                         template <typename T,size_t NROWS,size_t NCOLS> void insertMatrixFixTransposeIntoDyn(CMatrixTemplate<T> &M,const size_t nRow,const size_t nCol,const CMatrixFixedNumeric<T,NROWS,NCOLS> &in);
00430                         template <typename T,size_t NROWS,size_t NCOLS> void insertMatrixFixIntoDyn(CMatrixTemplate<T> &M,const size_t nRow,const size_t nCol,const CMatrixFixedNumeric<T,NROWS,NCOLS> &in);
00431                         template <typename T,size_t NROWS,size_t NCOLS> void extractFixMatrixFromDynMatrix(const CMatrixTemplate<T> &M,const size_t nRow,const size_t nCol, CMatrixFixedNumeric<T,NROWS,NCOLS> &outMat);
00432 
00433                         template <class MATRIX> int matrix_pivot(MATRIX &M, const size_t row);
00434 
00435                         template <typename MAT1,typename MAT2,typename MAT3> inline void leftDivideSquare(const MAT1 &C,const MAT2 &A,MAT3 &RES);
00436                         template <typename MAT1,typename MAT2,typename MAT3> inline void rightDivideSquare(const MAT1 &C,const MAT2 &B, MAT3 &RES);
00437                         template <typename MAT1,typename MAT2> inline void fastLeftDivideSquare(MAT1 &inout_CB,MAT2 &willBeDestroyed_A);
00438                         template <typename MAT1,typename MAT2> inline void fastRightDivideSquare(MAT1 &inout_CA,MAT2 &willBeDestroyed_B);
00439 
00440 
00441                         /*! An auxiliary function, which return true only for CMatrixTemplate-derived classes */
00442                         template <class MATRIX> inline bool isMatrixTypeResizable(const MATRIX&) { return false; }
00443                         // Specializations are in CMatrixTemplateNumeric.h
00444 
00445 
00446 
00447                 } // end detail
00448 
00449                 // ------- CONTAINERS -----------------
00450                 //  Implementations in ops_contaienrs.h
00451 
00452                 template <class CONTAINER> size_t countNonZero(const CONTAINER &a);
00453                 template <class CONTAINER> typename CONTAINER::value_type maximum(const CONTAINER &v, size_t *maxIndex = NULL);
00454                 template <class CONTAINER> typename CONTAINER::value_type minimum(const CONTAINER &v, size_t *minIndex = NULL);
00455                 template <class CONTAINER> void minimum_maximum(const CONTAINER &v,typename CONTAINER::mrpt_autotype::value_type & out_min,typename CONTAINER::mrpt_autotype::value_type& out_max,size_t *minIndex = static_cast<size_t*>(NULL), size_t *maxIndex = static_cast<size_t*>(NULL));
00456                 template <class CONTAINER> typename CONTAINER::value_type norm_inf(const CONTAINER &v, size_t *maxIndex = NULL);
00457                 template <class CONTAINER> typename CONTAINER::value_type squareNorm(const CONTAINER &v);
00458                 template <class CONTAINER> inline typename CONTAINER::value_type norm(const CONTAINER &v);
00459                 template <class CONTAINER> inline double mean(const CONTAINER &v);
00460                 template <class CONTAINER> inline typename CONTAINER::value_type sum(const CONTAINER &v);
00461                 template <class CONTAINER> void  adjustRange(CONTAINER &m, const typename CONTAINER::value_type minVal,const typename CONTAINER::value_type maxVal);
00462                 template <class CONTAINER1,class CONTAINER2> void cumsum(const CONTAINER1 &in_data, CONTAINER2 &out_cumsum);
00463                 template <class CONTAINER1,class CONTAINER2> size_t  countCommonElements(const CONTAINER1 &a,const CONTAINER2 &b);
00464                 template <class CONTAINER> std::vector<double> histogram(const CONTAINER &v,double limit_min,double limit_max,size_t number_bins,bool do_normalization=false);
00465                 template<class VECTORLIKE> double  stddev(const VECTORLIKE &v, bool unbiased=true);
00466                 template<class VECTORLIKE> void  meanAndStd(const VECTORLIKE &v,double &out_mean, double &out_std, bool unbiased = true);
00467 
00468 
00469                 // Vicinity classes ----------------------------------------------------
00470                 namespace detail        {
00471                         /**
00472                           * The purpose of this class is to model traits for containers, so that they can be used as return values for the function CMatrixTemplate::getVicinity.
00473                           * This class is NOT defined for any base container, because correctness would not be guaranteed. Instead, each class must define its own specialization
00474                           * of the template, containing two functions:
00475                           * - static void initialize(container<T>,size_t N): must reserve space to allow at least the insertion of N*N elements, in a square fashion when appliable.
00476                           * - static void insertInContainer(container<T>,size_t r,size_t c,const T &): must insert the given element in the container. Whenever it's possible, it
00477                           * must insert it in the (r,c) coordinates.
00478                           * For linear containers, the vicinity functions are guaranteed to insert elements in order, i.e., starting from the top and reading from left to right.
00479                           */
00480                         template<typename T> class VicinityTraits;
00481 
00482                         /**
00483                           * This huge template encapsulates a function to get the vicinity of an element, with maximum genericity. Although it's not meant to be called directly,
00484                           * every type defining the ASSERT_ENOUGHROOM assert and the get_unsafe method will work. The assert checks if the boundaries (r-N,r+N,c-N,c+N) fit in
00485                           * the matrix.
00486                           * The template parameters are the following:
00487                           * - MatrixType: the matrix or container base type, from which the vicinity is required.
00488                           * - T: the base type of the matrix or container.
00489                           * - ReturnType: the returning container type. The class VicinityTraits<ReturnType> must be completely defined.
00490                           * - D: the dimension of the vicinity. Current implementations are 4, 5, 8, 9, 12, 13, 20, 21, 24 and 25, although it's easy to implement new variants.
00491                           */
00492                         template<typename MatrixType,typename T,typename ReturnType,size_t D> struct getVicinity;
00493 
00494                 }
00495 
00496 
00497 
00498 
00499         } // End of namespace
00500 } // End of namespace
00501 
00502 #endif