CMatrixTemplateNumeric.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2010  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Machine Perception and Intelligent    |
   |      Robotics Lab, University of Malaga (Spain).                          |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#ifndef CMatrixTemplateNumeric_H
#define CMatrixTemplateNumeric_H

#include <mrpt/math/CMatrixTemplate.h>
#include <mrpt/system/os.h>
#include <cmath>
#include <limits>

namespace mrpt
{
        namespace poses
        {
                class CPose2D;
                class CPose3D;
                class CPoint2D;
                class CPoint3D;
        }
                namespace math
                {
                        using namespace mrpt::system;

                        // Forward decls: Classes
                        template <class T> class CVectorTemplate;
                        template <typename T,size_t NROWS,size_t NCOLS> class CMatrixFixedNumeric;
                        template <class T> class MRPTDLLIMPEXP CMatrixTemplateNumeric;
                        // Forward decls: Functions
                        template <typename T,size_t NROWS,size_t NCOLS> void fixedToDynMatrix( const CMatrixFixedNumeric<T,NROWS,NCOLS> &SRC, CMatrixTemplateNumeric<T> &DST);
                        template <typename T,size_t N,size_t M> CMatrixTemplateNumeric<T>& add(CMatrixTemplateNumeric<T> &A, const CMatrixFixedNumeric<T,N,M>& B);
                        template <typename T,size_t N,size_t M> CMatrixTemplateNumeric<T>& substract(CMatrixTemplateNumeric<T> &A, const CMatrixFixedNumeric<T,N,M>& B);
                        template <class MAT_X,class MAT_A,class MAT_OUT> void multiplySubMatrix (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);

                /**  This template class extends the class "CMatrixTemplate" with many common operations with numerical matrixes.
                 *
                 *   The template can be instanced for data types: float, double, long double
                 *
                 *   The following operators have been implemented:
                <P>
                <TABLE cellSpacing=1 cellPadding=1 width="75%" align=center border=1>
                  <TR>
                        <TD align=center width="30%"><b>Implemented Operators</b></TD>
                        <TD width="70%"><center><b>Meaning</b></center></TD></TR>
                  <TR>
                        <TD align=center>x=M(i,j)<br>M(i,j)=x</TD>
                        <TD>This () operator is used to access/change the element at i'th row, j'th column. First index is 0.</TD></TR>
                  <TR>
                        <TD align=center>!M</TD>
                        <TD>The matrix inverse M<sup>-1</sup></TD></TR>
                  <TR>
                        <TD align=center>~M</TD>
                        <TD>The matrix transpose M<sup>T</sup></TD></TR>
                  <TR>
                        <TD align=center>(M^n)</TD>
                        <TD>Power of a matrix: (M*M*M*...M) n times. Use parenthesis with this operator. Use "scalarPow" for the power of individual elements in the matrix. </TD></TR>
                  <TR>
                        <TD align=center> M1 = M2</TD>
                        <TD>Assignment operator: Copy matrix M2 to M1</TD></TR>
                  <TR>
                        <TD align=center>M1 == M2<br>M1 != M2</TD>
                        <TD>Logical comparison: Returns true or false if all elements are identical.</TD></TR>
                  <TR>
                        <TD align=center>x * M</TD>
                        <TD>Scalar multiplication</TD></TR>
                  <TR>
                        <TD align=center>M1 * M2</TD>
                        <TD>Matrix multiplication, with the usual mathematical meaning</TD></TR>
                  <TR>
                        <TD align=center>M1 + M2<br>M1 – M2</TD>
                        <TD>Matrixes addition and substraction.</TD></TR>
                  <TR>
                        <TD align=center>M / x</TD>
                        <TD>Scalar division</TD></TR>
                 <TR>
                        <TD align=center>M1 / M2</TD>
                        <TD>Equivalent to (M1 * M2<sup>-1</sup>)</TD></TR>
                 <TR>
                        <TD align=center>stream &lt;&lt; M;</TD>
                        <TD>Write to a binary CStream, since this class is CSerializable</TD></TR>
                 <TR>
                        <TD align=center>stream &gt;&gt; M;</TD>
                        <TD>Read from a binary CStream, since this class is CSerializable</TD></TR>
                </TABLE>
                </P>
                 *  See also this useful methods:
                 *              - DEBUG_SAVE_MATRIX
                 * \sa CMatrixTemplate
                 */
                template <class T>
                class MRPTDLLIMPEXP CMatrixTemplateNumeric : public CMatrixTemplate<T>
                {
                public:
                        /** Copy constructor from a matrix of any type
                        */
                        template <class R>
                        CMatrixTemplateNumeric(const CMatrixTemplate<R>& m)
                        {
                                CMatrixTemplate<T>::realloc( m.getRowCount(), m.getColCount() );

                                for (size_t i=0; i < CMatrixTemplate<T>::getRowCount(); i++)
                                        for (size_t j=0; j < CMatrixTemplate<T>::getColCount(); j++)
                                                CMatrixTemplate<T>::m_Val[i][j] = static_cast<T> (m.get_unsafe(i,j));
                        }

                        /** Default constructor, builds a 1x1 matrix */
                        CMatrixTemplateNumeric();

                        /** Constructor */
                        CMatrixTemplateNumeric(size_t row, size_t col);

                        /** Copy & crop constructor, which copies the given matrix but only up to the given size */
                        CMatrixTemplateNumeric(const CMatrixTemplate<T> & m, const size_t cropRowCount, const size_t cropColCount) : CMatrixTemplate<T>(m,cropRowCount,cropColCount)
                        { }

                        /** Constructor from a given size and a C array. The array length must match cols x row.
                          * \code
                          *  const double numbers[] = {
                          *    1,2,3,
                          *    4,5,6 };
                          *      CMatrixDouble   M(3,2, numbers);
                          * \endcode
                          */
                        template <typename V, size_t N>
                        CMatrixTemplateNumeric(size_t row, size_t col, V (&theArray)[N] ) : CMatrixTemplate<T>( row, col, theArray )
                        { }

                        /** Constructor from a given size and a STL container (std::vector, std::list,...) with the initial values. The vector length must match cols x row.
                          */
                        template <typename V>
                        CMatrixTemplateNumeric(size_t row, size_t col, const V &theVector ) : CMatrixTemplate<T>( row, col, theVector )
                        { }

                        /** Destructor
                          */
                        virtual ~CMatrixTemplateNumeric()
                        { }

                        /** Constructor from a mrpt::poses::CPose2D, which generates a 3x1 matrix \f$ [x y \phi]^T \f$
                           */
                        explicit CMatrixTemplateNumeric( const mrpt::poses::CPose2D &p);

                        /** Constructor from a mrpt::poses::CPose6D, which generates a 6x1 matrix \f$ [x y z yaw pitch roll]^T \f$
                           */
                        explicit CMatrixTemplateNumeric( const mrpt::poses::CPose3D &p);

                        /** Constructor from a mrpt::poses::CPoint2D, which generates a 2x1 matrix \f$ [x y]^T \f$
                           */
                        explicit CMatrixTemplateNumeric( const mrpt::poses::CPoint2D &p);

                        /** Constructor from a mrpt::poses::CPoint3D, which generates a 3x1 matrix \f$ [x y z]^T \f$
                           */
                        explicit CMatrixTemplateNumeric( const mrpt::poses::CPoint3D &p);

                        /** Copy constructor from a fixed-size matrix
                          *  \note Method implemented in CMatrixFixedNumeric.h
                          */
                        template <size_t NROWS,size_t NCOLS>
                        explicit CMatrixTemplateNumeric(const CMatrixFixedNumeric<T,NROWS,NCOLS> &M ) {
                                fixedToDynMatrix(M,*this);
                        }

                        /** Copy operator from a fixed-size matrix
                          *  \note Method implemented in CMatrixFixedNumeric.h
                          */
                        template <size_t NROWS,size_t NCOLS>
                        CMatrixTemplateNumeric& operator =(const CMatrixFixedNumeric<T,NROWS,NCOLS> &M ) {
                                fixedToDynMatrix(M,*this);
                                return *this;
                        }

                        /** Assignment operator of other types
                        */
                        template <class R>
                        CMatrixTemplateNumeric<T>& operator = (const CMatrixTemplateNumeric<R>& m)
                        {
                                CMatrixTemplate<T>::realloc( m.getRowCount(), m.getColCount() );

                                for (size_t i=0; i < CMatrixTemplate<T>::getRowCount(); i++)
                                        for (size_t j=0; j < CMatrixTemplate<T>::getColCount(); j++)
                                                CMatrixTemplate<T>::m_Val[i][j] = static_cast<T>(m.get_unsafe(i,j));
                                return *this;
                        }

                        /** Assignment operator for initializing from a C array (The matrix must be set to the correct size before invoking this asignament)
                          * \code
                          *      CMatrixDouble   M(3,2);
                          *  const double numbers[] = {
                          *    1,2,3,
                          *    4,5,6 };
                          *  M = numbers;
                          * \endcode
                          *  Refer also to the constructor with initialization data CMatrixTemplate::CMatrixTemplate
                          */
                        template <typename V, size_t N>
                        CMatrixTemplateNumeric& operator = (V (&theArray)[N] )
                        {
                                CMatrixTemplate<T>::operator = (theArray);
                                return *this;
                        }

                        /** Assignment operator for the same type
                        */
                        CMatrixTemplateNumeric<T>& operator = (const CMatrixTemplateNumeric<T>& m);

                        /** Changes the size of matrix, maintaining its previous content as possible and padding with zeros where applicable.
                          *  setSize and resize are exactly equivalent methods.
                          */
                        void setSize(size_t row, size_t col);

                        /** Changes the size of matrix, maintaining its previous content as possible and padding with zeros where applicable.
                          *  setSize and resize are exactly equivalent methods.
                          */
                        void resize(size_t row, size_t col);



                        /** Computes the laplacian of the matrix, useful for graph matrixes.
                         */
                        void laplacian( CMatrixTemplateNumeric<T> &ret ) const;

                        /** Computes the SVD (Singular Value Decomposition) of the matrix.
                          *  If "this" matrix is named A with dimensions M x N, this method computes: <br>
                          *                     A = U * W * V' <br>
                          * <br>
                          *  , where U is a M x N column orthogonal matrix, W is a diagonal matrix
                          *  containing the singular values, and V is a NxN matrix. <br>
                          * This method returns the U matrix, the N elements in the diagonal of W as a vector,
                          *  and the matrix V, NOT TRANSPOSED.
                          */
                        void  svd(CMatrixTemplateNumeric<T> &U, std::vector<T> &W,CMatrixTemplateNumeric<T> &V) const;

                        /** Computes the eigenvalues/eigenvector decomposition of a symmetric matrix.
                         *    The decomposition is: M = Z · D · Z<sup>T</sup>, where columns in Z are the
                         *        eigenvectors and the diagonal matrix D contains the eigenvalues
                         *    as diagonal elements, sorted in <i>ascending</i> order.
                         *    The algorithm is taken from "Numerical recipes in C", freely available online.
                         */
                        void  eigenVectors(CMatrixTemplateNumeric<T> &Z, CMatrixTemplateNumeric<T> &D) const;

                        /** Efficiently computes only the biggest eigenvector of the matrix using the Power Method, and returns it as a column vector.
                          *  The computation time for this method, in a Pentium 4 1.4Ghz is:<br>
                          *         T = 7.0867e-008*n<sup>2</sup> + 1.9191e-005*n + 0.0017494 seconds  <br>
                          *  where N is the matrix size.
                          */
                        CMatrixTemplateNumeric<T>  largestEigenvector(
                                T                       resolution = 0.01f,
                                size_t                  maxIterations = 6,
                                int                     *out_Iterations = NULL,
                                float           *out_estimatedResolution = NULL ) const;

                        /** Computes the sqrt of each element in the matrix, replacing current values;
                          * \return A reference to THIS object.
                                */
                        CMatrixTemplateNumeric<T>& Sqrt();

                        /** Computes the absolute value of each element in the matrix, replacing current values.
                          * \return A reference to THIS object.
                          */
                        CMatrixTemplateNumeric<T>& Abs();

                        /** Computes the square of each element in the matrix, replacing current values.
                          * \return A reference to THIS object.
                          */
                        CMatrixTemplateNumeric<T>& Square();

                        /** Applies a generic operation to all the elements of the matrix.
                          *   The passed functor object must implement the "T operator(T val)" operator.
                          * \return A reference to THIS object.
                          */
                        template <class F>
                        CMatrixTemplateNumeric<T>& applyToAllElements( F function )
                        {
                                for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
                                        for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
                                                CMatrixTemplate<T>::m_Val[i][j] = function( CMatrixTemplate<T>::m_Val[i][j] );
                                return *this;
                        }

                   /** Unary operator */
                   CMatrixTemplateNumeric<T>  operator + ();

                   /** Unary operator */
                        CMatrixTemplateNumeric<T>  operator - ();

                        /** combined addition and assignment operator */
                        CMatrixTemplateNumeric<T>&  operator += (const CMatrixTemplateNumeric<T>& m);

                        /** combined addition and assignment operator \note Implemented in CMatrixFixedNumeric.h */
                        template <size_t N,size_t M>
                        CMatrixTemplateNumeric<T>&  operator += (const CMatrixFixedNumeric<T,N,M>& m) { return add(*this,m); }

                        /** Add to this matrix the transpose of A.
                        */
                        CMatrixTemplateNumeric<T>&  add_At(const CMatrixTemplateNumeric<T>& m);

                        /** Add to this matrix A and its transpose (this = this + A + At)
                        */
                        CMatrixTemplateNumeric<T>&  add_AAt(const CMatrixTemplateNumeric<T>& m);

                        /** Add A times a scalar c to this matrix: this = this + A * c  */
                        CMatrixTemplateNumeric<T>&  add_Ac(const CMatrixTemplateNumeric<T>& m, const T c);

                        /** combined subtraction and assignment operator
                        */
                        CMatrixTemplateNumeric<T>&  operator -= (const CMatrixTemplateNumeric<T>& m);

                        /** combined substraction and assignment operator \note Implemented in CMatrixFixedNumeric.h */
                        template <size_t N,size_t M>
                        CMatrixTemplateNumeric<T>&  operator -= (const CMatrixFixedNumeric<T,N,M>& m) { return substract(*this,m); }

                        /** Substract A times a scalar c to this matrix: this = this - A * c  */
                        CMatrixTemplateNumeric<T>&  substract_Ac(const CMatrixTemplateNumeric<T>& m, const T c);

                        /** Substract A times an integer n to this matrix: this = this - A * n  */
                        CMatrixTemplateNumeric<T>&  substract_An(const CMatrixTemplateNumeric<T>& m, const size_t n)
                        {
                                #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
                                        if (CMatrixTemplate<T>::m_Rows != m.m_Rows || CMatrixTemplate<T>::m_Cols != m.m_Cols)
                                                THROW_EXCEPTION( "Inconsistent matrix sizes");
                                #endif

                                        for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
                                                for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
                                                        for (size_t k=0;k<n;k++)
                                                                CMatrixTemplate<T>::m_Val[i][j] -= m.m_Val[i][j];
                                        return *this;
                        }

                        /** combined scalar multiplication and assignment operator
                        */
                        CMatrixTemplateNumeric<T>&  operator *= (const T& c);

                        /** combined matrix multiplication and assignment operator
                        */
                        CMatrixTemplateNumeric<T>&  operator *= (const CMatrixTemplateNumeric<T>& m);

                        /** Multiply 2 matrices and save the result in "this" object.
                        */
                        void multiply(const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2);

                        /** Multiply one matrix by a column vector and save the result in "this" object.
                        */
                        void multiply(const CMatrixTemplateNumeric<T>& m1, const CVectorTemplate<T>& m2);

                        /** Makes this = M1 * M2^T
                        */
                        void multiply_ABt(const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2);

                        /** Makes this = M1 * M1^T, taking into account that the result is symmetric and only half the computations must be done.
                          */
                        void multiply_AAt( const CMatrixTemplateNumeric<T>& m1 );

                        /** Makes this = M1^T * M1, taking into account that the result is symmetric and only half the computations must be done.
                          */
                        void multiply_AtA( const CMatrixTemplateNumeric<T>& m1 );

                        /** Computes the vector v = this * a, where "a" is a column vector of the appropriate length.
                          */
                        void multiply_Ab( const std::vector<T>& a, std::vector<T>& out_v, bool accum_to_output = false );

                        /** Computes the vector v = this^T * a, where "a" is a column vector of the appropriate length.
                          */
                        void multiply_Atb( const std::vector<T>& a, std::vector<T>& out_v, bool accum_to_output = false );


                        /** Multiply 2 matrices and save the result in "this" object, for the cases in which we know in advance that the result will be a symmetrical matrix (DO NOT USE OTHERWISE!!).
                        */
                        void multiply_result_is_symmetric(const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2);

                        /** Matrix multiplication of this matrix with a submatrix of 'A', saving the result in a third matrix.
                          *   OUT = THIS * A
                          */
                        template <class MAT_A,class MAT_OUT>
                        void multiplySubMatrix (
                                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 )
                        {
                                multiplySubMatrix(*this,A,outResult,A_cols_offset,A_rows_offset,A_col_count);
                        }

                        /** Dot division by another matrix (element by element division)
                          */
                        CMatrixTemplateNumeric<T>&  operator /= (const CMatrixTemplateNumeric<T>& m);

                        /** combined scalar division and assignment operator
                        */
                        CMatrixTemplateNumeric<T>&  operator /= (const T& c);

                        /** Combined scalar addition and assignment operator
                        */
                        CMatrixTemplateNumeric<T>&  operator += (const T& c);

                        /** Combined scalar substraction and assignment operator
                        */
                        CMatrixTemplateNumeric<T>&  operator -= (const T& c);

                        /** combined power and assignment operator
                        */
                        CMatrixTemplateNumeric<T>&  operator ^= (const unsigned int& pow);

                        /** Scalar power of all elements to a given power, this is diferent of ^ operator.
                                */
                        void  scalarPow(T s);

                   /** Set all elements to zero
                        */
                   void  zeros(const size_t& row, const size_t& col);

                   /** Set all elements to zero
                        */
                   void  zeros();

                   /** Set all elements to one
                        */
                   void  ones(const size_t& row, const size_t& col);

                   /** Set all elements to one
                        */
                   void  ones();

                   /** Build an unit matrix.
                        */
                   void  unit (const size_t& row);

                   /** Build an unit matrix.
                        */
                   void  unit();

                        /** Solve the matrix as linear equations system.
                        */
                        CMatrixTemplateNumeric<T>  solve (const CMatrixTemplateNumeric<T>& v) const;

                        /** Computes the adjunt of matrix.
                        */
                        CMatrixTemplateNumeric<T>  adj() const;

                        /** Computes the Inverse of matrix.
                        *       Return the inverse of a matrix without modifying the original matrix
                        * \note Efficient hard-coded solutions are implemented for 2x2 and 3x3 matrices.
                        */
                        void inv(CMatrixTemplateNumeric<T> &out_inv) const;

                        /** Computes the Inverse of matrix.
                        *       Return the inverse of a matrix without modifying the original matrix
                        * \note Efficient hard-coded solutions are implemented for 2x2 and 3x3 matrices.
                        */
                        CMatrixTemplateNumeric<T> inv() const {
                                CMatrixTemplateNumeric<T> inv_res;
                                this->inv(inv_res);
                                return inv_res;
                        }

                        /** Computes the Inverse of matrix, DESTROYING the current matrix and returning the inverse in an user-supplied matrix.
                          * \note Efficient hard-coded solutions are implemented for 2x2 and 3x3 matrices.
                        *  By AJOGD/JLBC
                        */
                        void  inv_fast( CMatrixTemplateNumeric<T> &out_inv );

                        /** Compute the pseudo-inverse of this matrix M, given by \f$ ( M^\top M )^{-1} X^\top \f$  */
                        void pseudoInverse( CMatrixTemplateNumeric<T> &out ) const;

                        /** Compute the pseudo-inverse of this matrix M, given by \f$ ( M^\top M )^{-1} X^\top \f$  */
                        CMatrixTemplateNumeric<T> pseudoInverse()  const
                        {
                                CMatrixTemplateNumeric<T> M;
                                pseudoInverse(M);
                                return M;
                        }

                        /** Computes the determinant of matrix.
                          * \note Efficient hard-coded solutions are implemented for 2x2 and 3x3 matrices.
                          */
                        T  det() const;

                        /** Computes the rank of the matrix using a slight variation of Gauss method.
                          */
                        size_t rank(T eps=0.0) const;

                        /** Computes the norm of matrix.
                        */
                        T  norm() const;

                        /** Computes the cofact.
                        */
                        T  cofact (size_t row, size_t col) const;

                        /** Computes the cond.
                        */
                        T        cond();

                        /** Checks for matrix type
                          */
                        bool  isSingular() const;

                        /** Checks for matrix type
                          */
                        bool  isDiagonal() const;

                        /** Checks for matrix type
                          */
                        bool  isScalar() const;

                        /** Checks for matrix type
                          */
                        bool  isUnit() const;

                        /** Checks for matrix type
                          */
                        bool  isNull() const;

                        /** Checks for matrix type
                          */
                        bool  isSymmetric() const;

                        /** Checks for matrix type
                          */
                        bool  isSkewSymmetric() const;

                        /** Checks for matrix type
                          */
                        bool  isUpperTriangular() const;

                        /** Checks for matrix type
                          */
                        bool  isLowerTriangular() const;

                        /** Round towards minus infinity modifying the matrix
                          * (by AJOGD @ JAN-2007)
                          */
                        void  matrix_floor();

                        /** Round towards minus infinity
                          * (by AJOGD @ JAN-2007)
                          */
                        void  matrix_floor(CMatrixTemplateNumeric<T> &out);

                        /** Round towards plus infinity
                          * (by AJOGD @ JAN-2007)
                          */
                        void  matrix_ceil();

                        /** Finds the maximum value in the matrix, and returns its position.
                          * (by AJOGD @ JAN-2007)
                          */
                        void  find_index_max_value(size_t &umax, size_t &vmax, T &max_val) const;

                        /** Finds the maximum value in the diagonal of the matrix.
                          */
                        T  maximumDiagonal() const;

                        /** Finds the maximum value in the matrix.
                          */
                        T  maximum() const;

                        /** Finds the minimum value in the matrix.
                          */
                        T  minimum() const;

                        /** Finds the minimum value in the matrix, and returns its position.
                          * (by AJOGD @ JAN-2007)
                          */
                        void  find_index_min_value(size_t  &umin, size_t  &vmin, T &min_val) const;

                        /** Force symmetry in the matrix
                          * (by AJOGD @ JAN-2007)
                          */
                        void  force_symmetry();

                        /** Computes a row with the mean values of each column in the matrix.
                          * \sa meanAndStdAll
                          */
                        void  mean( std::vector<T> &outMeanVector ) const;

                        /** Computes a row with the mean values of each column in the matrix and the associated vector with the standard deviation of each column.
                          * \sa mean,meanAndStdAll
                          */
                        void  meanAndStd(
                                std::vector<T> &outMeanVector,
                                std::vector<T> &outStdVector ) const;

                        /** Computes the mean and standard deviation of all the elements in the matrix as a whole.
                          * \sa mean,meanAndStd
                          */
                        void  meanAndStdAll(
                                T &outMean,
                                T &outStd )  const;

                        void  asCol(CMatrixTemplateNumeric<T>   &aux) const;

                        void  asRow(CMatrixTemplateNumeric<T>   &aux) const;

                        /** Finds elements whose values are a given number of times above (or below) the mean, in 1D Mahalanobis distance.
                          *  This returns two lists with the "row" and "column" indexes (i,j) of those elements m[i][j] such as:
                          *    m[i][j] > mean(matrix) + stdTimes·std(matrix)
                          *  The elements below the threshold
                          *    mean(matrix) - stdTimes·std(matrix)
                          *  can also be obtained setting "below" to "true".
                          */
                        void  findElementsPassingMahalanobisThreshold(
                                double                                  stdTimes,
                                std::vector<size_t>             &rowIndexes,
                                std::vector<size_t>             &colIndexes,
                                bool                                    below = false ) const;

                        /** Adjusts the range of elements in the matrix such as the minimum and maximum values being those supplied by the user.
                          *  This method is just a shortcut for adjustRange.
                          */
                        inline void  normalize( T minVal=0,T maxVal=1)
                        {
                                adjustRange(minVal,maxVal);
                        }

                        /** Adjusts the range of elements in the matrix such as the minimum and maximum values being those supplied by the user.
                          */
                        void  adjustRange( T minVal=0,T maxVal=1);

                        /** Returns the sum of all the elements in the matrix
                          * \sa sum
                          */
                        T  sumAll() const;

                        /** Returns the sum of a given part of the matrix.
                          *  The default value (std::numeric_limits<size_t>::max()) for the last column/row means to sum up to the last column/row.
                          * \sa sumAll
                          */
                        T  sum(
                                size_t firstRow = 0,
                                size_t firstCol = 0,
                                size_t lastRow  = std::numeric_limits<size_t>::max(),
                                size_t lastCol  = std::numeric_limits<size_t>::max() ) const;

                        /** Computes:  R = H * C * H^t , where H is this matrix.
                          *
                          */
                        void  multiplyByMatrixAndByTransposeNonSymmetric(
                                const CMatrixTemplateNumeric<T>         &C,
                                CMatrixTemplateNumeric<T>                       &R,
                                bool                                                            accumOnOutput = false,
                                bool                                                            substractInsteadOfSum = false
                                ) const;

                        /**     Calculate the operation S = ABC where S is this object
                        * \sa multiply_ABCt
                        * By AJOGD
                        */
                        void  multiply_ABC(
                                const CMatrixTemplateNumeric<T>         &A,
                                const CMatrixTemplateNumeric<T>         &B,
                                const CMatrixTemplateNumeric<T>         &C);

                        /**     Calculate the operation S = ABCt where S is this object
                        * \sa multiply_ABC
                        */
                        void  multiply_ABCt(
                                const CMatrixTemplateNumeric<T>         &A,
                                const CMatrixTemplateNumeric<T>         &B,
                                const CMatrixTemplateNumeric<T>         &C);

                        /** 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.
                          *  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$.
                          * The result from this method is assured to be symmetric (if \f$ \mathbf{C} \f$ is symmetric), whereas executing:
                          \code
                                 R = H * C * (~H);
                          \endcode
                          * 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).
                          *
                          *  If allowSubMatrixMultiplication=true, the multiplication will be performed with a \f$ M \times M \f$ submatrix of C only, with M being the number of columns of H, and C being possibly larger. This is useful in some SLAM problems.
                          *  In this case, an optional offset 'subMatrixOffset' can be supplied such as a submatrix from the diagonal of C is used, starting at a given column and row index (first=0).

                          *  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
                          *   before calling or an exception will be raised since this probably is a bug.
                          *
                          * \sa multiply_HCHt_scalar
                          */
                        void  multiply_HCHt(
                                const CMatrixTemplateNumeric<T>         &C,
                                CMatrixTemplateNumeric<T>                       &R,
                                bool                                 allowSubMatrixMultiplication = false,
                                size_t                               subMatrixOffset = 0,
                                bool                                 accumResultInOutput = false  ) const;

                        /** An special case of multiply_HCHt for the case of the resulting matrix being a scalar (that is, a 1x1 matrix) - This method directly returns this as a scalar avoiding the construction of a 1x1 matrix.
                          *  This matrix (H) must be \f$ 1 \times N \f$ or a \f$ N \times 1 \f$ matrix and C must by \f$ N \times N \f$, or an exception will be raised.
                          *  Refer to multiply_HCHt for more information.
                          \code
                                 return = ( H * C * (~H) ) (0,0);
                          \endcode
                          * \sa multiply_HCHt, mrpt::math::multiply_HCHt_scalar
                          */
                        T  multiply_HCHt_scalar(
                                const CMatrixTemplateNumeric<T>         &C ) const;

                        /**
                          * Left divide. If the equation is A·B=C, call C.leftDivideSquare(A,B) to obtain B.
                          * A must be a square matrix.
                          * \sa rightDivideSquare,fastLeftDivideSquare
                          */
                        void leftDivideSquare(const CMatrixTemplateNumeric<T> &A,CMatrixTemplateNumeric<T> &B) const;

                        /**
                          * Right divide. If the equation is A·B=C, call C.rightDivideSquare(B,A) to obtain A.
                          * B must be a square matrix.
                          * \sa leftDivideSquare,fastRightDivideSquare
                          */
                        void rightDivideSquare(const CMatrixTemplateNumeric<T> &B,CMatrixTemplateNumeric<T> &A) const;

                        /**
                          * Left divide. If the equation is A·B=C, call C.fastLeftDivideSquare(A,B) to obtain B.
                          * However, A is destroyed. A must be a square matrix.
                          * \sa leftDivideSquare,fastRightDivideSquare
                          */
                        void fastLeftDivideSquare(CMatrixTemplateNumeric<T> &A,CMatrixTemplateNumeric<T> &B) const;

                        /**
                          * Right divide. If the equation is A·B=C, call C.fastRightDivideSquare(B,A) to obtain A.
                          * However, B is destroyed. B must be a square matrix.
                          * \sa leftDivideSquare,fastRightDivideSquare
                          */
                        void fastRightDivideSquare(CMatrixTemplateNumeric<T> &B,CMatrixTemplateNumeric<T> &A) const;

                private:
                        /** Internal use.  */
                        int pivot(size_t row);

                }; // end of class definition


                /** Logical equal-to operator
                 */
                template <class T>
                bool  operator == (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
                {
                   if (m1.getRowCount() != m2.getRowCount() || m1.getColCount() != m2.getColCount())
                          return false;

                   for (size_t i=0; i < m1.getRowCount(); i++)
                          for (size_t j=0; j < m1.getColCount(); j++)
                                  if (m1(i,j) != m2(i,j))
                                         return false;

                   return true;
                }

                /** logical no-equal-to operator
                 */
                template <class T>
                bool  operator != (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
                {
                        return !(m1 == m2);
                }

                /** binary addition operator
                 */
                template <class T>
                CMatrixTemplateNumeric<T>  operator + (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
                {
                   CMatrixTemplateNumeric<T>    temp(m1);
                   temp += m2;
                   return temp;
                }

                /** binary subtraction operator
                 */
                template <class T>
                 CMatrixTemplateNumeric<T>  operator - (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
                {
                   CMatrixTemplateNumeric<T>    temp(m1);
                   temp -= m2;
                   return temp;
                }

                /** binary scalar multiplication operator
                 */
                template <class T>
                CMatrixTemplateNumeric<T>  operator * (const CMatrixTemplateNumeric<T>& m, const T& no)
                {
                        CMatrixTemplateNumeric<T>       temp(m);
                        temp*=no;
                        return temp;
                }

                /** binary scalar multiplication operator
                 */
                template <class T>
                CMatrixTemplateNumeric<T>  operator * (const T& no, const CMatrixTemplateNumeric<T>& m)
                {
                        CMatrixTemplateNumeric<T>       temp(m);
                        temp*=no;
                        return temp;
                }

                /** binary matrix multiplication operator
                 */
                template <class T>
                CMatrixTemplateNumeric<T>  operator * (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
                {
                        CMatrixTemplateNumeric<T>       res(m1);
                        res*=m2;
                        return res;
                }

                /** Binary matrix multiplication operator, with one matrix being NxM and the second being a column matrix Mx1.
                 */
                template <class T>
                CMatrixTemplateNumeric<T>  operator * (const CMatrixTemplateNumeric<T>& m1, const CVectorTemplate<T>& m2)
                {
                        CMatrixTemplateNumeric<T>       res(m1.getRowCount(),1);
                        res.multiply(m1,m2);
                        return res;
                }

                /** binary scalar division operator
                 */
                template <class T>
                 CMatrixTemplateNumeric<T>  operator / (const CMatrixTemplateNumeric<T>& m, const T& no)
                {
                        return (m * (T(1) / no));
                }

                /** binary scalar division operator
                 */
                template <class T>
                 CMatrixTemplateNumeric<T>  operator / (const T& no, const CMatrixTemplateNumeric<T>& m)
                {
                        return (!m * no);
                }

                /** binary matrix division operator
                 */
                template <class T>
                 CMatrixTemplateNumeric<T>  operator / (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
                {
                        return (m1 * !m2);
                }

                /** binary power operator
                 */
                template <class T>
                 CMatrixTemplateNumeric<T>  operator ^ (const CMatrixTemplateNumeric<T>& m, const unsigned int& pow)
                {
                        CMatrixTemplateNumeric<T>       temp(m);
                        temp ^= pow;
                        return temp;
                }

                /** unary transpose operator
                 */
                template <class T>
                CMatrixTemplateNumeric<T>  operator ~ (const CMatrixTemplateNumeric<T>& m)
                {
                        CMatrixTemplateNumeric<T>       temp(m.getColCount(),m.getRowCount());

                        for (size_t i=0; i < m.getRowCount(); i++)
                                for (size_t j=0; j < m.getColCount(); j++)
                                {
                                        T       x = m(i,j);
                                        temp(j,i) = x;
                                }
                        return temp;
                }

                /** Unary inversion operator.
                 */
                template <class T>
                CMatrixTemplateNumeric<T>  operator ! (const CMatrixTemplateNumeric<T> &m)
                {
                        return m.inv();
                }


                /** A useful macro for saving matrixes to a file while debugging.
                  */
                #define DEBUG_SAVE_MATRIX(M) \
                        { \
                                char auxStr[100]; \
                                os::sprintf(auxStr,99,"%s.txt",#M); \
                                M.saveToTextFile(auxStr); \
                        } \

                /** An special case of multiply_HCHt for the case of the resulting matrix being a scalar (that is, a 1x1 matrix) - This method directly returns this as a scalar avoiding the construction of a 1x1 matrix.
                  \code
                         return = ( H * C * (~H) ) (0,0);
                  \endcode
                  */
                template<class T>
                T MRPTDLLIMPEXP multiply_HCHt_scalar(
                        const std::vector<T> &H,
                        const CMatrixTemplateNumeric<T> &C );

                /** Declares a matrix of float numbers (non serializable).
                  *  For a serializable version, use math::CMatrix
                  *  \sa CMatrixDouble, CMatrix, CMatrixD
                  */
                typedef CMatrixTemplateNumeric<float> CMatrixFloat;

                /** Declares a matrix of double numbers (non serializable).
                  *  For a serializable version, use math::CMatrixD
                  *  \sa CMatrixFloat, CMatrix, CMatrixD
                  */
                typedef CMatrixTemplateNumeric<double> CMatrixDouble;

                /** Declares a matrix of unsigned ints (non serializable).
                  *  \sa CMatrixDouble, CMatrixFloat
                  */
                typedef CMatrixTemplateNumeric<unsigned int> CMatrixUInt;

                /** Declares a matrix of booleans (non serializable).
                  *  \sa CMatrixDouble, CMatrixFloat, CMatrixB
                  */
                typedef CMatrixTemplate<bool> CMatrixBool;

#ifdef HAVE_LONG_DOUBLE
                /** Declares a matrix of "long doubles" (non serializable), or of "doubles" if the compiler does not support "long double".
                  *  \sa CMatrixDouble, CMatrixFloat
                  */
                typedef CMatrixTemplateNumeric<long double> CMatrixLongDouble;
#else
                /** Declares a matrix of "long doubles" (non serializable), or of "doubles" if the compiler does not support "long double".
                  *  \sa CMatrixDouble, CMatrixFloat
                  */
                typedef CMatrixTemplateNumeric<double> CMatrixLongDouble;
#endif

        namespace detail        {
                /**
                  * Vicinity traits class specialization for fixed size matrices.
                  */
                template<typename T> class VicinityTraits<CMatrixTemplateNumeric<T> >   {
                public:
                        inline static void initialize(CMatrixTemplateNumeric<T> &mat,size_t N)  {
                                mat.setSize(N,N);
                                mat.fill(0);
                        }
                        inline static void insertInContainer(CMatrixTemplateNumeric<T> &mat,size_t r,size_t c,const T &t)       {
                                mat.get_unsafe(r,c)=t;
                        }
                };
        }       //End of detail namespace.

        } // End of namespace
} // End of namespace

#endif

---

Page generated by Doxygen 1.6.2 for MRPT 0.8.1 SVN:exported at Mon Feb 15 22:01:07 UTC 2010