The MRPT project: math_frwds.h Source 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 <mrpt/base/link_pragmas.h>
00033 #include <mrpt/utils/types.h>
00034 #include <cmath>
00035 #include <cstdlib>
00036 #include <algorithm>
00037 
00038 /*! \file math_frwds.h
00039   * Forward declarations of all mrpt::math classes related to vectors, arrays and matrices.
00040   * Many of the function implementations are in ops_matrices.h, others in ops_containers.h
00041   */
00042 
00043 namespace mrpt
00044 {
00045         namespace utils
00046         {
00047                 class BASE_IMPEXP CStream;
00048                 template<class T> inline T square(const T& x);
00049         }
00050 
00051         namespace poses
00052         {
00053                 class CPoint2D;
00054                 class CPoint3D;
00055                 class CPose2D;
00056                 class CPose3D;
00057                 class CPose3DQuat;
00058         }
00059 
00060         namespace math
00061         {
00062                 struct TPoint2D;
00063                 struct TPoint3D;
00064                 struct TPose2D;
00065                 struct TPose3D;
00066                 struct TPose3DQuat;
00067 
00068                 /*! Selection of the number format in CMatrixTemplate::saveToTextFile
00069                   */
00070                 enum TMatrixTextFileFormat
00071                 {
00072                         MATRIX_FORMAT_ENG = 0,   //!< engineering format '%e'
00073                         MATRIX_FORMAT_FIXED = 1, //!< fixed floating point '%f'
00074                         MATRIX_FORMAT_INT = 2    //!< intergers '%i'
00075                 };
00076 
00077                 /** For usage in one of the constructors of CMatrixFixedNumeric or CMatrixTemplate (and derived classes), if it's not required
00078                      to fill it with zeros at the constructor to save time. */
00079                 #define UNINITIALIZED_MATRIX  false,false,false
00080 
00081 
00082                 // ---------------- Forward declarations: Classes ----------------
00083                 template <class T> class CVectorTemplate;
00084 
00085                 template <class T> class CMatrixTemplate;
00086                 template <class T> class BASE_IMPEXP CMatrixTemplateNumeric;
00087                 template <typename T,size_t NROWS,size_t NCOLS> class CMatrixFixedNumeric;
00088                 template<typename MATRIXTYPE> class CArbitrarySubmatrixView;
00089 
00090                 template <typename T, std::size_t N> class CArray;
00091                 template <typename T> class CArray<T,0>;
00092                 template <typename T, std::size_t N> class CArrayPOD;
00093                 template <typename T, std::size_t N> class CArrayNumeric;
00094 
00095                 class BASE_IMPEXP CMatrix;
00096                 class BASE_IMPEXP CMatrixD;
00097 
00098                 // Functions shared by all kind of matrices/vectors/array
00099                 // These are member functions that invoke functions in mrpt::math, most of them in ops_containers.h
00100                 // ------------------------------------------------------------------------------------------------
00101 #define DECLARE_COMMON_CONTAINERS_MEMBERS(NUMTYPE) \
00102                 /*! @name Methods common to all containers  */ \
00103                 /*! Count how many elements in a container are non-zero. */ \
00104                 inline size_t countNonZero() const { return mrpt::math::countNonZero(*this); } \
00105                 /*! Fill all the elements with a given value */ \
00106                 inline void fill(const NUMTYPE &val) { std::fill(mrpt_autotype::begin(),mrpt_autotype::end(),val); } \
00107                 /*! Finds the maximum value (and the corresponding zero-based index). */ \
00108                 inline NUMTYPE maximum(size_t *maxIndex = NULL) const { return mrpt::math::maximum(*this,maxIndex); } \
00109                 /*! Finds the minimum value (and the corresponding zero-based index). */ \
00110                 inline NUMTYPE minimum(size_t *minIndex = NULL) const { return mrpt::math::minimum(*this,minIndex ); } \
00111                 /*! Minimum and maximum of a vector at once */ \
00112                 inline void minimum_maximum(NUMTYPE &out_min, NUMTYPE &out_max, \
00113                         size_t *minIndex = NULL, size_t *maxIndex = NULL) const \
00114                 { mrpt::math::minimum_maximum(*this,out_min,out_max,minIndex,maxIndex); } \
00115                 /*! Compute the norm-infinite of a vector ($f[ ||\mathbf{v}||_\infnty $f]), ie the maximum absolute value of the elements. */ \
00116                 inline NUMTYPE norm_inf(size_t *maxIndex = NULL) const { return mrpt::math::norm_inf(*this,maxIndex); } \
00117                 /*! Sum all the elements, returning a value of the same type than the container  */ \
00118                 inline NUMTYPE sumAll() const { return mrpt::math::sum(*this); } \
00119                 /*! Sum all the elements, returning a value of a custom type (this could be used to avoid overflow with containers of integers) */ \
00120                 template <typename RET_TYPE> inline RET_TYPE sumAllRetType() const { return mrpt::math::sumRetType<mrpt_autotype,NUMTYPE>(*this); } \
00121                 /*! Cumsum of all the elements, saving in an output container */ \
00122                 template <class CONTAINEROUT> inline void cumsum(CONTAINEROUT &out) const { mrpt::math::cumsum(*this,out); } \
00123                 /*! Cumsum of all the elements, returning a container of the given type, for example: CMatrixDouble m = mat.cumsum<CMatrixDouble>(); */ \
00124                 template <class CONTAINEROUT> inline CONTAINEROUT cumsum() const { CONTAINEROUT o; mrpt::math::cumsum(*this,o); return o;} \
00125                 /** 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.  */ \
00126                 template <class CONTAINER2> inline size_t countCommonElements(const CONTAINER2 &b) const { return mrpt::math::countCommonElements(*this,b); } \
00127                 /** 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. */ \
00128                 inline std::vector<double> histogram(double limit_min,double limit_max,size_t number_bins,bool do_normalization = false ) const { \
00129                         return mrpt::math::histogram(*this,limit_min,limit_max,number_bins,do_normalization); } \
00130                 /*! Applies a generic operation "T func(T val)" to all the elements. */ \
00131                 template <class F> mrpt_autotype & applyToAllElements( NUMTYPE (*function)(NUMTYPE) ) { \
00132                         std::for_each(this->begin(),this->end(), function); \
00133                         return *this; } \
00134                 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; }  \
00135                 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; }  \
00136                 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; }  \
00137                 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; }  \
00138                 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; }  \
00139                 /*! Return the square norm of the container (for matrices, as if it were a vector with all its elements).  */ \
00140                 inline NUMTYPE squareNorm() const { return mrpt::math::squareNorm(*this); } \
00141                 /*! Return the norm of the container (for matrices, as if it were a vector with all its elements).  */ \
00142                 inline NUMTYPE norm() const { return mrpt::math::norm(*this); } \
00143                 /*! Return the mean of the container (for matrices, as if it were a vector with all its elements).  */ \
00144                 inline double mean() const { return mrpt::math::mean(*this); } \
00145                 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); } \
00146                 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); } \
00147                 inline double std(bool unbiased=true) const /*! Return the standard deviation of all the elements in the container. */ { return mrpt::math::stddev(*this); } \
00148                 void meanAndStd(double &out_mean, double &out_std, bool unbiased = true) /*! Return the mean and standard deviation of all the elements in the container. */ { \
00149                         return mrpt::math::meanAndStd(*this,out_mean,out_std,unbiased); } \
00150 
00151                 //Some operations could be made more efficient if inside each class:
00152                 //operator-() const
00153                 //  Already implemented in ops_containers.h:  +=, -=
00154 #define DECLARE_COMMON_MATRIX_MEMBERS(NUMTYPE) \
00155                 DECLARE_COMMON_CONTAINERS_MEMBERS(NUMTYPE) \
00156                 /*! @name Methods common to all matrices */ \
00157                 /* ========= sum / substract functions: =========  */ \
00158                 inline mrpt_autotype operator+()        {\
00159                         return *this;\
00160                 }\
00161                 inline mrpt_autotype operator-() const  {\
00162                         mrpt_autotype res(*this);\
00163                         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));\
00164                         return res;\
00165                 }\
00166                 mrpt_autotype &operator+=(const NUMTYPE n) /*!< Sum a scalar */ {\
00167                         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;\
00168                         return *this;\
00169                 }\
00170                 mrpt_autotype &operator-=(const NUMTYPE n) /*!< Substract a scalar */   {\
00171                         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;\
00172                         return *this;\
00173                 }\
00174                 mrpt_autotype &operator*=(const NUMTYPE n) /*!< Multiply by scalar */   {\
00175                         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;\
00176                         return *this;\
00177                 }\
00178                 inline mrpt_autotype& operator/=(const NUMTYPE n) /*!< Divide by scalar */      {\
00179                         return (*this)*=NUMTYPE(1)/n; } \
00180                 template<typename OTHERMATRIX> \
00181                 inline RET_VOID_ASSERT_MRPTMATRIX(OTHERMATRIX) operator*=(const OTHERMATRIX &m) {\
00182                         mrpt::math::detail::multiply_AB(*this,m,*this); } \
00183                 template<typename OTHERMATRIX> mrpt_autotype &add_At(const OTHERMATRIX &m)      {\
00184                         ASSERT_((m.getRowCount()==mrpt_autotype::getColCount())&&(m.getColCount()==mrpt_autotype::getRowCount()));\
00185                         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);\
00186                         return *this;\
00187                 }\
00188                 template<typename OTHERMATRIX> mrpt_autotype &add_Ac(const OTHERMATRIX &m,const NUMTYPE c)      {\
00189                         ASSERT_((m.getRowCount()==mrpt_autotype::getRowCount())&&(m.getColCount()==mrpt_autotype::getColCount()));\
00190                         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);\
00191                         return *this;\
00192                 }\
00193                 template<typename OTHERMATRIX> mrpt_autotype &substract_At(const OTHERMATRIX &m)        {\
00194                         ASSERT_((m.getRowCount()==mrpt_autotype::getColCount())&&(m.getColCount()==mrpt_autotype::getRowCount()));\
00195                         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);\
00196                         return *this;\
00197                 } \
00198                 /*! Substract n (integer) times A to this matrix: this -= A * n  */ \
00199                 template<typename OTHERMATRIX> mrpt_autotype &  substract_An(const OTHERMATRIX& m, const size_t n)      {\
00200                         ASSERT_((m.getRowCount()==mrpt_autotype::getRowCount())&&(m.getColCount()==mrpt_autotype::getColCount()));\
00201                         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);\
00202                         return *this;\
00203                 }\
00204                 /*! Substract c (scalar) times A to this matrix: this -= A * c  */ \
00205                 template<typename OTHERMATRIX> mrpt_autotype &substract_Ac(const OTHERMATRIX &m,const NUMTYPE c)        {\
00206                         ASSERT_((m.getRowCount()==mrpt_autotype::getRowCount())&&(m.getColCount()==mrpt_autotype::getColCount()));\
00207                         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);\
00208                         return *this;\
00209                 }\
00210                 /*! this += A + A<sup>T</sup>  */ \
00211                 template<typename OTHERMATRIX> mrpt_autotype &add_AAt(const OTHERMATRIX &m)     {\
00212                         ASSERT_((mrpt_autotype::getRowCount()==mrpt_autotype::getColCount())&&(mrpt_autotype::getRowCount()==m.getRowCount())&&(mrpt_autotype::getColCount()==m.getColCount()));\
00213                         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);\
00214                         return *this;\
00215                 }\
00216                 /*! this -= A + A<sup>T</sup>  */ \
00217                 template<typename OTHERMATRIX> mrpt_autotype &substract_AAt(const OTHERMATRIX &m)       {\
00218                         ASSERT_((mrpt_autotype::getRowCount()==mrpt_autotype::getColCount())&&(mrpt_autotype::getRowCount()==m.getRowCount())&&(mrpt_autotype::getColCount()==m.getColCount()));\
00219                         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);\
00220                         return *this;\
00221                 }\
00222                 /* =========  multiply_* functions =========  */ \
00223                 bool empty() const { return this->getColCount()==0 || this->getRowCount()==0; } \
00224                 template <class MATRIX1,class MATRIX2> \
00225                 inline void multiply( const MATRIX1& A, const MATRIX2 &B ) /*!< this = A * B */ { mrpt::math::detail::multiply_AB(A,B,*this); } \
00226                 template <class MATRIX1,class MATRIX2> \
00227                 inline void multiply_AB( const MATRIX1& A, const MATRIX2 &B ) /*!< this = A * B */ { mrpt::math::detail::multiply_AB(A,B,*this); } \
00228                 /*! Computes the vector vOut = this * vIn, where "vIn" is a column vector of the appropriate length. */ \
00229                 template<typename OTHERVECTOR1,typename OTHERVECTOR2> \
00230                 inline void multiply_Ab(const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,bool accumToOutput = false) const {\
00231                         mrpt::math::detail::multiply_Ab(*this,vIn,vOut,accumToOutput); } \
00232                 /*! Computes the vector vOut = this<sup>T</sup> * vIn, where "vIn" is a column vector of the appropriate length. */ \
00233                 template<typename OTHERVECTOR1,typename OTHERVECTOR2> \
00234                 inline void multiply_Atb(const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,bool accumToOutput = false) const {\
00235                         mrpt::math::detail::multiply_Atb(*this,vIn,vOut,accumToOutput); } \
00236                 template <typename MAT_C, typename MAT_R> \
00237                 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 */ { \
00238                         mrpt::math::detail::multiply_HCHt(*this,C,R,accumResultInOutput,allow_submatrix_mult); } \
00239                 /*! 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 */ \
00240                 template <typename MAT_C> NUMTYPE multiply_HCHt_scalar(const MAT_C &C) const { return mrpt::math::detail::multiply_HCHt_scalar(*this,C); } \
00241                 /*! 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 */ \
00242                 template <typename MAT_C> NUMTYPE multiply_HtCH_scalar(const MAT_C &C) const { return mrpt::math::detail::multiply_HCHt_scalar(*this,C); } \
00243                 /*! R = H<sup>T</sup> * C * H (with a symmetric matrix C) */ \
00244                 template <typename MAT_C, typename MAT_R> \
00245                 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 */  { \
00246                         mrpt::math::detail::multiply_HtCH(*this,C,R,accumResultInOutput,allow_submatrix_mult); } \
00247                 template <class MAT_A,class MAT_OUT> \
00248                 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 */ { \
00249                         mrpt::math::detail::multiply_subMatrix(*this,A,outResult,A_cols_offset,A_rows_offset,A_col_count); } \
00250                 template <class MAT_A,class MAT_B,class MAT_C> \
00251                 void multiply_ABC(const MAT_A &A, const MAT_B &B, const MAT_C &C) /*!< this = A*B*C  */ { \
00252                         mrpt::math::detail::multiply_ABC(A,B,C, *this); } \
00253                 template <class MAT_A,class MAT_B,class MAT_C> \
00254                 void multiply_ABCt(const MAT_A &A, const MAT_B &B, const MAT_C &C) /*!< this = A*B*(C<sup>T</sup>) */ { \
00255                         mrpt::math::detail::multiply_ABCt(A,B,C, *this); } \
00256                 template <class MAT_A,class MAT_B> \
00257                 inline void multiply_ABt(const MAT_A &A,const MAT_B &B) /*!< this = A * B<sup>T</sup> */ { \
00258                         mrpt::math::detail::multiply_ABt(A,B,*this); } \
00259                 template <class MAT_A> \
00260                 inline void multiply_AAt(const MAT_A &A) /*!< this = A * A<sup>T</sup> */ { \
00261                         mrpt::math::detail::multiply_AAt(A,*this); } \
00262                 template <class MAT_A> \
00263                 inline void multiply_AtA(const MAT_A &A) /*!< this = A<sup>T</sup> * A */ { \
00264                         mrpt::math::detail::multiply_AtA(A,*this); } \
00265                 template <class MAT_A,class MAT_B> \
00266                 inline void multiply_result_is_symmetric(const MAT_A &A,const MAT_B &B) /*!< this = A * B (result is symmetric) */ { \
00267                         mrpt::math::detail::multiply_result_is_symmetric(A,B,*this); } \
00268                 /* =========  Assignment functions : =========  */ \
00269                 /*! Assignment of a matrix from any other kind of matrix */ \
00270                 template<typename OTHERMATRIX> inline mrpt_autotype &assignMatrix(const OTHERMATRIX &m) {\
00271                         setSize(m.getRowCount(),m.getColCount());\
00272                         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));\
00273                         return *this;\
00274                 }\
00275                 /*! Assignment of a matrix to another one (of any type!), transposed. */\
00276                 template<typename OTHERMATRIX> inline mrpt_autotype &assignMatrixTransposed(const OTHERMATRIX &m)       {\
00277                         setSize(m.getColCount(),m.getRowCount());\
00278                         for (size_t i=0;i<m.getRowCount();++i) for (size_t j=0;j<m.getColCount();++j) set_unsafe(j,i,m.get_unsafe(i,j));\
00279                         return *this;\
00280                 }\
00281                 /*! Assignment of a matrix from any other kind of matrix */ \
00282                 /*template<typename MATRIX> inline RET_TYPE_ASSERT_MRPTMATRIX(MATRIX,mrpt_autotype)& operator=(const MATRIX &m) { return assignMatrix(m); } */ \
00283                 inline mrpt_autotype & operator=( const TPose2D &p)  { this->setSize(3,1); mrpt::math::containerFromPoseOrPoint(*this,p); return *this; } \
00284                 inline mrpt_autotype & operator=( const TPose3D &p)  { this->setSize(6,1); mrpt::math::containerFromPoseOrPoint(*this,p); return *this;} \
00285                 inline mrpt_autotype & operator=( const TPose3DQuat &p)  { this->setSize(7,1); mrpt::math::containerFromPoseOrPoint(*this,p); return *this;} \
00286                 inline mrpt_autotype & operator=( const TPoint2D &p) { this->setSize(2,1); mrpt::math::containerFromPoseOrPoint(*this,p); return *this;} \
00287                 inline mrpt_autotype & operator=( const TPoint3D &p) { this->setSize(3,1); mrpt::math::containerFromPoseOrPoint(*this,p); return *this;} \
00288                 inline mrpt_autotype & operator=( const mrpt::poses::CPose2D &p)  { this->setSize(3,1); mrpt::math::containerFromPoseOrPoint(*this,p); return *this;} \
00289                 inline mrpt_autotype & operator=( const mrpt::poses::CPose3D &p)  { this->setSize(6,1); mrpt::math::containerFromPoseOrPoint(*this,p); return *this;} \
00290                 inline mrpt_autotype & operator=( const mrpt::poses::CPose3DQuat &p)  { this->setSize(7,1); mrpt::math::containerFromPoseOrPoint(*this,p); return *this;} \
00291                 inline mrpt_autotype & operator=( const mrpt::poses::CPoint2D &p) { this->setSize(2,1); mrpt::math::containerFromPoseOrPoint(*this,p); return *this;} \
00292                 inline mrpt_autotype & operator=( const mrpt::poses::CPoint3D &p) { this->setSize(3,1); mrpt::math::containerFromPoseOrPoint(*this,p); return *this;} \
00293                 /* =========  misc funcs: =========  */ \
00294                 /*! Alternative access to element (i,j) with first index being (1,1) instead of (0,0) */ \
00295                 inline NUMTYPE _E(const size_t row, const size_t col) const { return mrpt_autotype::get_unsafe(row-1,col-1); } \
00296                 /*! Alternative access to element (i,j) with first index being (1,1) instead of (0,0) */ \
00297                 inline NUMTYPE & _E(const size_t row, const size_t col) { return mrpt_autotype::get_unsafe(row-1,col-1); } \
00298                 /*! Access the n'th element viewing the matrix as an array, with n=row*COLS+col */ \
00299                 inline NUMTYPE _A(const size_t n) const { return mrpt_autotype::get_unsafe(n/mrpt_autotype::getColCount(),n%mrpt_autotype::getColCount()); } \
00300                 /*! Access the n'th element viewing the matrix as an array, with n=row*COLS+col */ \
00301                 inline NUMTYPE &_A(const size_t n) { return mrpt_autotype::get_unsafe(n/mrpt_autotype::getColCount(),n%mrpt_autotype::getColCount()); } \
00302                 inline bool IsSquare() const { return mrpt_autotype::getColCount()==mrpt_autotype::getRowCount(); } \
00303                 inline bool isSquare() const { return mrpt_autotype::getColCount()==mrpt_autotype::getRowCount(); } \
00304                 /*! Checks for the absolute value of the determinant of the matrix being equal or below a given threshold (by default, exactly zero). */ \
00305                 inline bool isSingular(const NUMTYPE epsilon=0) const { \
00306                         if (!isSquare()) return false; else return std::abs(this->det())<=epsilon; } \
00307                 /*! The trace of this square matrix */ \
00308                 inline NUMTYPE trace() const { return mrpt::math::detail::trace(*this); }\
00309                 template <class MAT_OUT> \
00310                 void pseudoInverse( MAT_OUT &out ) const /*!< Pseudo inverse of this matrix M: out = (M<sup>T</sup> * M)^-1 * M<sup>T</sup> */ { \
00311                         mrpt_autotype A, Ainv; A.multiply_AtA(*this); A.inv_fast(Ainv); out.multiply_ABt(Ainv,*this); } \
00312                 template <class MAT_OUT> \
00313                 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>() */ { \
00314                         mrpt_autotype A, Ainv; A.multiply_AtA(*this); A.inv_fast(Ainv); \
00315                         MAT_OUT out; out.multiply_ABt(Ainv,*this); return out; } \
00316                 inline NUMTYPE det() const /*!< Returns the determinant of the matrix  */ { return mrpt::math::detail::detMatrix(*this); } \
00317                 template <class MATRIXOUT> inline void inv( MATRIXOUT& out ) const /*!< out = inv(this) */ { mrpt::math::detail::invMatrix(*this,out); } \
00318                 inline mrpt_autotype inv() const /*!< mrpt_autotype out = inv(this) */ { mrpt_autotype ret; mrpt::math::detail::invMatrix(*this,ret); return ret; } \
00319                 template <class MATRIXOUT> inline void inv_fast( MATRIXOUT& out ) /*!< out = inv(this), overwriting contents of this */ { mrpt::math::detail::invMatrix_destroySrc(*this,out); } \
00320                 /*! Matrix left divide: RES = A<sup>-1</sup> &#183; this  (A must be a square matrix). \sa rightDivideSquare,fastLeftDivideSquare */ \
00321                 template <class MAT2,class MAT3> inline void leftDivideSquare(const MAT2 &A,MAT3 &RES) const { mrpt::math::detail::leftDivideSquare(*this,A,RES); } \
00322                 /*! Matrix right divide: RES = this &#183; B<sup>-1</sup> (B must be a square matrix). \sa leftDivideSquare,fastRightDivideSquare */ \
00323                 template <class MAT2,class MAT3> inline void rightDivideSquare(const MAT2 &B, MAT3 &RES) const { mrpt::math::detail::rightDivideSquare(*this,B,RES); } \
00324                 /*! 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 */ \
00325                 template <typename MAT2> inline void fastLeftDivideSquare(MAT2 &A) { mrpt::math::detail::fastLeftDivideSquare(*this,A); } \
00326                 /*! 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 */ \
00327                 template <typename MAT2> inline void fastRightDivideSquare(MAT2 &B) { mrpt::math::detail::fastRightDivideSquare(*this,B); } \
00328                 /*! Save matrix to a text file, compatible with MATLAB text format. */ \
00329                 inline void  saveToTextFile(const std::string &file,mrpt::math::TMatrixTextFileFormat fileFormat = MATRIX_FORMAT_ENG,bool appendMRPTHeader=false,const std::string &userHeader = std::string("") ) const { \
00330                         mrpt::math::detail::saveMatrixToTextFile(*this, file,fileFormat,appendMRPTHeader,userHeader); } \
00331                 /*! Return a string representation of the matrix in the form: "[m11 m12 m13...;m21 m22 m23...]", compatible with MATLAB matrices. */ \
00332                 inline std::string inMatlabFormat(const size_t decimal_digits=6) const { return detail::matrix_inMatlabFormat(*this,decimal_digits); } \
00333                 inline int pivot(const size_t row) { return mrpt::math::detail::matrix_pivot(*this,row); } \
00334                 /* =========  Decomposition functions: =========  */ \
00335                 /*! Cholesky factorization: in = out' *  out \return True on success, false on singular matrix \sa mrpt::math::chol */ \
00336                 template <class MATRIXOUT> inline bool chol(MATRIXOUT &out) const { return mrpt::math::detail::chol(*this,out); } \
00337                 /*! 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 */ \
00338                 template <class MATRIX1,class MATRIX2> \
00339                 inline RET_VOID_ASSERT_MRPTMATRICES(MATRIX1,MATRIX2) eigenVectors( MATRIX1& eVecs, MATRIX2& eVals) const { \
00340                         ARRAY_TYPE_SAMESIZE_ROWS_OF(mrpt_autotype) eigVals; \
00341                         eigVals.resize(this->getRowCount()); \
00342                         mrpt::math::detail::eigenVectorsMatrix(*this, &eVecs,eigVals); \
00343                         const size_t n = eigVals.size(); \
00344                         eVals.zeros(n,n); for (size_t i=0;i<n;i++) eVals.get_unsafe(i,i)=eigVals[i]; \
00345                         } \
00346                 /*! Like eigenVectors, but eigenvalues are returned in a vector instead of a matrix's diagonal. \sa eigenVectors,eigenValues */ \
00347                 template <class MATRIX1,class VECTOR1> \
00348                 inline void eigenVectorsVec( MATRIX1& eVecs, VECTOR1& eVals) const { mrpt::math::detail::eigenVectorsMatrix(*this, &eVecs,eVals); } \
00349                 /*! Return the eigenvalues of the matrix in an ordered vector. \sa eigenVectors */ \
00350                 template <class VECTOR1> \
00351                 inline void eigenValues( VECTOR1& eVals) const { mrpt::math::detail::eigenVectorsMatrix(*this,static_cast<const mrpt_autotype*>(NULL),eVals); } \
00352                 inline void setIdentity()       {\
00353                         ASSERT_(isSquare());\
00354                         set_unsafe(0,0,NUMTYPE(1));\
00355                         for (size_t i=1;i<mrpt_autotype::getRowCount();++i)     {\
00356                                 set_unsafe(i,i,NUMTYPE(1));\
00357                                 for (size_t j=0;j<i;++j)        {\
00358                                         set_unsafe(i,j,NUMTYPE(0));\
00359                                         set_unsafe(j,i,NUMTYPE(0));\
00360                                 }\
00361                         }\
00362                 }\
00363                 inline void setIdentity(size_t N)       {\
00364                         setSize(N,N);\
00365                         setIdentity();\
00366                 }\
00367                 inline size_t rank(NUMTYPE eps=1e-7) const      {\
00368                         return mrpt::math::detail::rank(*this,eps);\
00369                 }\
00370                 /*! Inserts a matrix into this matrix, at the position (nRow,nCol). An exception is raised in the inserted matrix does not fit. */ \
00371                 template <class MATRIXLIKE> inline void  insertMatrix(const size_t nRow, const size_t nCol, const MATRIXLIKE &in) \
00372                 { detail::insertMatrixInto(*this,nRow,nCol,in); } \
00373                 /*! Inserts the transpose of a matrix into this matrix, at the position (nRow,nCol). An exception is raised in the inserted matrix does not fit. */ \
00374                 template <class MATRIXLIKE> inline void  insertMatrixTranspose(const size_t nRow, const size_t nCol, const MATRIXLIKE &in) \
00375                 { detail::insertMatrixTransposeInto(*this,nRow,nCol,in); } \
00376                 /*! Extract a sub matrix from this matrix (the size of the output matrix upon call determines the size of the submatrix to extract). */ \
00377                 template <class MATOUT> inline void extractMatrix(const size_t first_row, const size_t first_col, MATOUT &outMat) const { detail::extractMatrix(*this,first_row,first_col,outMat); } \
00378                 /*! Extract a sub matrix of the given size from this matrix. */ \
00379                 template <class MATOUT> inline void extractMatrix(const size_t first_row, const size_t first_col, const size_t nRows, const size_t nCols, MATOUT &outMat) const { outMat.setSize(nRows,nCols); detail::extractMatrix(*this,first_row,first_col,outMat); } \
00380                 template <class VECLIKE> inline void extractRow(const size_t nRow, VECLIKE &out, const size_t startingCol = 0) const { detail::extractRowFromMatrix(*this,nRow,out,startingCol); } \
00381                 template <class VECLIKE> inline void extractCol(const size_t nCol, VECLIKE &out, const size_t startingRow = 0) const { detail::extractColFromMatrix(*this,nCol,out,startingRow); } \
00382                 template <class VECLIKE> inline void insertRow(const size_t nRow, VECLIKE &in, const size_t startingCol = 0) { detail::insertRowToMatrix(*this,nRow,in,startingCol); } \
00383                 template <class VECLIKE> inline void insertCol(const size_t nCol, VECLIKE &in, const size_t startingRow = 0) { detail::insertColToMatrix(*this,nCol,in,startingRow); } \
00384 
00385 
00386 
00387                 // Generic functions in mrpt::math::detail ----------------------------------------------------
00388                 namespace detail
00389                 {
00390                         template<class MATRIX1,class MATRIX2>
00391                                 bool chol(const MATRIX1 &in, MATRIX2 &out);
00392                         template<class MATRIX1>
00393                                 typename MATRIX1::value_type trace(const MATRIX1 &m);
00394 
00395                         template <class MATRIX1,class MATRIX2,class MATRIXRES>
00396                                 void multiply_AB(const MATRIX1& m1,const MATRIX2& m2, MATRIXRES& RESULT );
00397                         template<class MATRIX1, class OTHERVECTOR1,class OTHERVECTOR2>
00398                                 void multiply_Ab(const MATRIX1 &m, const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,bool accumToOutput );
00399                         template<class MATRIX1, class OTHERVECTOR1,class OTHERVECTOR2>
00400                                 void multiply_Atb(const MATRIX1 &m, const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,bool accumToOutput );
00401                         template <class MATRIX1,class MATRIX2>
00402                                 void multiply_AAt( const MATRIX1& m1, MATRIX2& RESULT );
00403                         template <class MATRIX1,class MATRIX2>
00404                                 void multiply_AtA( const MATRIX1& m1, MATRIX2& RESULT );
00405 
00406                         /*! 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.
00407                           *  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$.
00408                           * The result from this method is assured to be symmetric (if \f$ \mathbf{C} \f$ is symmetric), whereas executing:
00409                           \code
00410                                  R = H * C * (~H);
00411                           \endcode
00412                           * 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).
00413                           *  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
00414                           *   before calling or an exception will be raised.
00415                           * \sa multiply_HCHt_scalar
00416                           */
00417                         template <typename MAT_H, typename MAT_C, typename MAT_R>
00418                                 void multiply_HCHt(const MAT_H &H,const MAT_C &C,MAT_R &R,bool accumResultInOutput=false,bool allow_submatrix_mult=false);
00419                         template <typename MAT_H, typename MAT_C, typename MAT_R>
00420                                 void multiply_HtCH(const MAT_H &H,const MAT_C &C,MAT_R &R,bool accumResultInOutput=false,bool allow_submatrix_mult=false);
00421                         template <typename VECTOR_H, typename MAT_C> typename MAT_C::value_type
00422                                 multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C);
00423 
00424                         template <class MAT_X,class MAT_A,class MAT_OUT>
00425                                 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);
00426 
00427                         template <class MAT_A,class MAT_B,class MAT_C,class MAT_OUT>
00428                                 void multiply_ABC(const MAT_A &A, const MAT_B &B, const MAT_C &C, MAT_OUT & RES);
00429                         template <class MAT_A,class MAT_B,class MAT_C,class MAT_OUT>
00430                                 void multiply_ABCt(const MAT_A &A, const MAT_B &B, const MAT_C &C, MAT_OUT & RES);
00431 
00432                         template <class MAT_A,class MAT_B,class MAT_OUT>
00433                                 void multiply_ABt(const MAT_A &A,const MAT_B &B, MAT_OUT &out);
00434                         template <class MAT_A,class MAT_OUT>
00435                                 void multiply_AAt(const MAT_A &A, MAT_OUT &out);
00436                         template <class MAT_A,class MAT_OUT>
00437                                 void multiply_AAt(const MAT_A &A,MAT_OUT &out);
00438                         template <class MAT_A,class MAT_B,class MAT_OUT>
00439                                 void multiply_result_is_symmetric(const MAT_A &A,const MAT_B &B, MAT_OUT &out);
00440 
00441 
00442                         template<typename MatrixType> size_t rank(const MatrixType &m,typename MatrixType::value_type eps=1e-7);
00443 
00444                         // -------- MATRIX INVERSES --------
00445                         template <class MATRIXIN,class MATRIXOUT>       inline void  invMatrix( const MATRIXIN &M, MATRIXOUT &out_inv );
00446                         template <class MATRIXIN,class MATRIXOUT>       inline void  invMatrix_destroySrc( MATRIXIN &M, MATRIXOUT &out_inv );
00447                         // Specializations in ops_matrices.h
00448 
00449                         // templates for special cases:
00450                         template <class MATRIXIN,class MATRIXOUT> inline void invMatrix_special_2x2( const MATRIXIN &M, MATRIXOUT &out_inv );
00451                         template <class MATRIXIN,class MATRIXOUT> inline void invMatrix_special_3x3( const MATRIXIN &M, MATRIXOUT &out_inv );
00452 
00453 
00454                         // -------- MATRIX DETERMINANT --------
00455                         template <class MATRIX> RET_ELEMENT_ASSERT_MRPTCONTAINER(MATRIX)
00456                                 detMatrix(const MATRIX& M);
00457                         // Specializations in ops_matrices.h
00458 
00459                         // Special cases:
00460                         template <class MATRIX> inline typename MATRIX::value_type detMatrix_special_2x2(const MATRIX &M);
00461                         template <class MATRIX> inline typename MATRIX::value_type detMatrix_special_3x3(const MATRIX &M);
00462                         template <class MATRIX>        typename MATRIX::value_type detMatrix_special_4x4(const MATRIX &M);
00463 
00464 
00465                         // -------- Eigenvalues --------
00466                         template <class MATRIX1,class MATRIX2,class VECTOR1> void
00467                                 eigenVectorsMatrix(const MATRIX1 &M,MATRIX2 *eVecs,VECTOR1 &eVals );
00468 
00469                         // Special cases
00470                         template <class MATRIX1,class MATRIX2,class VECTOR1> void eigenVectorsMatrix_special_2x2(const MATRIX1 &M,MATRIX2 *eVecs,VECTOR1 &eVals );
00471 
00472                         // specializations that call the above special cases:
00473                         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); }
00474                         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); }
00475 
00476                         // -------- Others --------
00477                         template <class MAT> void
00478                                 saveMatrixToTextFile(const MAT &theMatrix, const std::string &file, TMatrixTextFileFormat fileFormat = MATRIX_FORMAT_ENG, bool appendMRPTHeader = false, const std::string &userHeader = std::string("") );
00479                         template <class MATRIX>
00480                                 std::string  matrix_inMatlabFormat(const MATRIX &m,const size_t decimal_digits);
00481 
00482                         template <typename T,size_t NROWS,size_t NCOLS> void fixedToDynMatrix( const CMatrixFixedNumeric<T,NROWS,NCOLS> &SRC, CMatrixTemplateNumeric<T> &DST);
00483                         template <class MAT1,class MAT2> void insertMatrixInto( MAT1 &M,const size_t nRow,const size_t nCol,const MAT2 &in);
00484                         template <class MAT1,class MAT2> void insertMatrixTransposeInto( MAT1 &M,const size_t nRow,const size_t nCol,const MAT2 &in);
00485 
00486                         template <class MATORG, class MATDEST> void extractMatrix(const MATORG &origin,const size_t first_row,const size_t first_col, MATDEST &out_subMatrix);
00487                         template <class MAT,class VEC> void  extractRowFromMatrix(const MAT &m, size_t nRow, VEC &out, const size_t startingCol);
00488                         template <class MAT,class VEC> void  extractColFromMatrix(const MAT &m, size_t nCol, VEC &out, const size_t startingRow);
00489                         template <class MAT,class VEC> void  insertRowToMatrix(MAT &m, size_t nRow, const VEC &in, const size_t startingCol);
00490                         template <class MAT,class VEC> void  insertColToMatrix(MAT &m, size_t nCol, const VEC &in, const size_t startingRow);
00491 
00492                         template <class MATRIX> int matrix_pivot(MATRIX &M, const size_t row);
00493 
00494                         template <typename MAT1,typename MAT2,typename MAT3> inline void leftDivideSquare(const MAT1 &C,const MAT2 &A,MAT3 &RES);
00495                         template <typename MAT1,typename MAT2,typename MAT3> inline void rightDivideSquare(const MAT1 &C,const MAT2 &B, MAT3 &RES);
00496                         template <typename MAT1,typename MAT2> inline void fastLeftDivideSquare(MAT1 &inout_CB,MAT2 &willBeDestroyed_A);
00497                         template <typename MAT1,typename MAT2> inline void fastRightDivideSquare(MAT1 &inout_CA,MAT2 &willBeDestroyed_B);
00498 
00499 
00500                         /*! An auxiliary function, which return true only for CMatrixTemplate-derived classes */
00501                         template <class MATRIX> inline bool isMatrixTypeResizable(const MATRIX&) { return false; }
00502                         // Specializations are in CMatrixTemplateNumeric.h
00503 
00504 
00505                         // Implemented in "lightweight_geom_data.cpp"
00506                         TPoint2D BASE_IMPEXP lightFromPose(const mrpt::poses::CPoint2D &p);     //!< Convert a pose into a light-weight structure (functional form, needed for forward declarations)
00507                         TPoint3D BASE_IMPEXP lightFromPose(const mrpt::poses::CPoint3D &p);     //!< Convert a pose into a light-weight structure (functional form, needed for forward declarations)
00508                         TPose2D  BASE_IMPEXP lightFromPose(const mrpt::poses::CPose2D &p);      //!< Convert a pose into a light-weight structure (functional form, needed for forward declarations)
00509                         TPose3D  BASE_IMPEXP lightFromPose(const mrpt::poses::CPose3D &p);      //!< Convert a pose into a light-weight structure (functional form, needed for forward declarations)
00510                         TPose3DQuat BASE_IMPEXP lightFromPose(const mrpt::poses::CPose3DQuat &p);       //!< Convert a pose into a light-weight structure (functional form, needed for forward declarations)
00511 
00512                 } // end detail
00513 
00514                 // ------- CONTAINERS -----------------
00515                 //  Implementations in ops_contaienrs.h
00516 
00517                 template <class CONTAINER> size_t countNonZero(const CONTAINER &a);
00518                 template <class CONTAINER> typename CONTAINER::value_type maximum(const CONTAINER &v, size_t *maxIndex = NULL);
00519                 template <class CONTAINER> typename CONTAINER::value_type minimum(const CONTAINER &v, size_t *minIndex = NULL);
00520                 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));
00521                 template <class CONTAINER> typename CONTAINER::value_type norm_inf(const CONTAINER &v, size_t *maxIndex = NULL);
00522                 template <class CONTAINER> typename CONTAINER::value_type squareNorm(const CONTAINER &v);
00523                 template <class CONTAINER> inline typename CONTAINER::value_type norm(const CONTAINER &v);
00524                 template <class CONTAINER> inline double mean(const CONTAINER &v);
00525                 template <class CONTAINER> inline typename CONTAINER::value_type sum(const CONTAINER &v);
00526                 template <class CONTAINER,typename RET> inline RET sumRetType(const CONTAINER &v);
00527                 template <class CONTAINER> void  adjustRange(CONTAINER &m, const typename CONTAINER::value_type minVal,const typename CONTAINER::value_type maxVal);
00528                 template <class CONTAINER1,class CONTAINER2> void cumsum(const CONTAINER1 &in_data, CONTAINER2 &out_cumsum);
00529                 template <class CONTAINER1,class CONTAINER2> size_t  countCommonElements(const CONTAINER1 &a,const CONTAINER2 &b);
00530                 template <class CONTAINER> std::vector<double> histogram(const CONTAINER &v,double limit_min,double limit_max,size_t number_bins,bool do_normalization=false, std::vector<double> *out_bin_centers=NULL);
00531                 template<class VECTORLIKE> double  stddev(const VECTORLIKE &v, bool unbiased=true);
00532                 template<class VECTORLIKE> void  meanAndStd(const VECTORLIKE &v,double &out_mean, double &out_std, bool unbiased = true);
00533 
00534                 /** Conversion of poses to MRPT containers (vector/matrix) */
00535                 template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPoint2D &p);
00536                 template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPoint3D &p);
00537                 template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPose2D &p);
00538                 template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPose3D &p);
00539                 template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPose3DQuat &p);
00540 
00541                 template <class CONTAINER> inline CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const mrpt::poses::CPoint2D &p) { return containerFromPoseOrPoint(C, mrpt::math::detail::lightFromPose(p)); }
00542                 template <class CONTAINER> inline CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const mrpt::poses::CPoint3D &p) { return containerFromPoseOrPoint(C, mrpt::math::detail::lightFromPose(p)); }
00543                 template <class CONTAINER> inline CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const mrpt::poses::CPose2D &p)  { return containerFromPoseOrPoint(C, mrpt::math::detail::lightFromPose(p)); }
00544                 template <class CONTAINER> inline CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const mrpt::poses::CPose3D &p)  { return containerFromPoseOrPoint(C, mrpt::math::detail::lightFromPose(p)); }
00545                 template <class CONTAINER> inline CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const mrpt::poses::CPose3DQuat &p)  { return containerFromPoseOrPoint(C, mrpt::math::detail::lightFromPose(p)); }
00546 
00547 
00548                 // Vicinity classes ----------------------------------------------------
00549                 namespace detail        {
00550                         /**
00551                           * 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.
00552                           * This class is NOT defined for any base container, because correctness would not be guaranteed. Instead, each class must define its own specialization
00553                           * of the template, containing two functions:
00554                           * - 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.
00555                           * - 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
00556                           * must insert it in the (r,c) coordinates.
00557                           * 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.
00558                           */
00559                         template<typename T> class VicinityTraits;
00560 
00561                         /**
00562                           * 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,
00563                           * 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
00564                           * the matrix.
00565                           * The template parameters are the following:
00566                           * - MatrixType: the matrix or container base type, from which the vicinity is required.
00567                           * - T: the base type of the matrix or container.
00568                           * - ReturnType: the returning container type. The class VicinityTraits<ReturnType> must be completely defined.
00569                           * - 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.
00570                           */
00571                         template<typename MatrixType,typename T,typename ReturnType,size_t D> struct getVicinity;
00572 
00573                 }
00574 
00575 
00576                 // Other forward decls:
00577                 template <class T> T wrapTo2Pi(T a);
00578 
00579 
00580         } // End of namespace
00581 } // End of namespace
00582 
00583 #endif