Robot Simulator of the Robotics Group for Self-Organization of Control: matrix.h Source File

# matrix.h

Go to the documentation of this file.

    00001 /***************************************************************************
    00002                           matrix.h  -  description
    00003                              -------------------
    00004     email                : georg.martius@web.de
    00005 ***************************************************************************/
    00006 // provides Matrix class with convinient operators
    00007 //  and fast inversion for nonzero square matrixes
    00008 //
    00009 /***************************************************************************/
    00010
    00011 #ifndef MATRIX_H
    00012 #define MATRIX_H
    00013
    00014 #include <string.h>
    00015 #include <assert.h>
    00016 #include <list>
    00017 #include <vector>
    00018 #include <cstdlib>
    00019
    00020 #ifndef AVR
    00021 #include <iostream>
    00022 #endif
    00023
    00024 #include "storeable.h"
    00025
    00026 // TODO: add doxygen section
    00027
    00028 /**
    00029  * namespace for the matrix library
    00030  *@author Georg Martius
    00031  */
    00032 namespace matrix{
    00033
    00034   /// integer constant for use with exp function and (^) operator to transpose the matrix
    00035   extern const int T;
    00036
    00037 /// type for matrix indices
    00038   typedef unsigned int I;
    00039   /// type for matrix elements
    00040   typedef double D;
    00041
    00042   class Matrix;
    00043   typedef std::vector<Matrix> Matrices;
    00044
    00045 #define D_Zero 0
    00046 #define D_One 1
    00047   /** Matrix type. Type D is datatype of matrix elements,
    00048    * which is fixed to double.
    00049    * Type I is the indextype of matrix elements,
    00050    * which is fixed to unsigned int, if AVR is not defined.
    00051    * There are basicly two different types of operation:
    00052    * Inplace operations and copy operations.
    00053    * Please use the latter ones unless you know what you are doing.
    00054    * Just in case of critical performance optimisation use the inplace
    00055    * operations.
    00056    * The most convinient way is to use the overloaded operators
    00057    * (like + * ...).
    00058    * All constructed matrices are initialised with zero elements
    00059    * (unless data is given).
    00060    * All functions perform range checks if in debug mode
    00061    * (NDEBUG is not defined).
    00062    * Please use debug the version (default) for testing
    00063    * @see examples/matrix/matrixexample.cpp
    00064    *
    00065    * @author Georg Martius
    00066    */
    00067   class Matrix : public Storeable {
    00068   public:
    00069     /// default constructor: zero matrix (0x0)
    00070     Matrix()
    00071       : m(0), n(0), buffersize(0), data(0) {};
    00072     /** constucts a matrix with the given size.
    00073         If _data is null then the matrix is filled with zeros.
    00074         otherwise matrix will be filled with _data in a row-wise manner.
    00075         In this case _data must be at least _m*_n elements long
    00076     */
    00077     Matrix(I _m, I _n, const D* _data=0);
    00078     /** constucts a matrix with the given size and fills it with the default value
    00079     */
    00080     Matrix(I _m, I _n, D def);
    00081     /// constucts a instance on the base of a deep copy of the given matrix
    00082     Matrix (const Matrix& c);
    00083     ~Matrix() { if(data) free(data); };
    00084
    00085   public:
    00086     //  /////////////////// Accessors ///////////////////////////////
    00087     /** @return number of rows */
    00088     I getM() const { return m; };
    00089     /** @return number of columns */
    00090     I getN() const { return n; };
    00091     /// @return number number elements in the matrix (getM()*getN())
    00092     I size() const { return n*m; };
    00093
    00094
    00095     /** @return element at position i,j (row, column index) */
    00096     inline D val(I i, I j) const {
    00097       assert( i<m && j<n);
    00098       return data[i*n+j];
    00099     };
    00100     /** @return reference to element at position i,j
    00101         (can be used as left side value) */
    00102     inline D& val(I i, I j) {
    00103       assert( i<m && j<n);
    00104       return data[i*n+j];
    00105     };
    00106
    00107     /** @return element at position i,j (row, column index) and 0 if out of bounds */
    00108     inline D valDef0(I i, I j) const {
    00109       if(i<m && j<n)
    00110         return data[i*n+j];
    00111       else return 0;
    00112     };
    00113
    00114     /** sets the size of the matrix and maybe the data if given (row-wise).
    00115         If data=null then the matrix is set to zero
    00116         @see toZero()
    00117         @see constructor Matrix(m,n,data)
    00118     */
    00119     void set(I _m, I _n, const D* _data=0);
    00120     /** sets the data (row-wise).
    00121         @param _data if null then matrix elements are set to zero
    00122         otherwise the field MUST have the length should be getM()*getN()*/
    00123     void set(const D* _data);
    00124     /** @return row-vector(as 1xN matrix) containing the index'th row */
    00125     Matrix row(I index) const;
    00126     /** @returns submatrix (as KxN matrix)
    00127         containing row from startindex to endindex inclusively (K=stopindex-endindex)
    00128         indices can be out of bounds, they are clipped in any case
    00129     */
    00130     Matrix rows(I startindex, I endindex) const;
    00131     /** @returns column-vector(as Mx1 matrix) containing the index'th column */
    00132     Matrix column(I index) const;
    00133     /** @returns submatrix (as MxK matrix)
    00134         containing column from startindex to endindex inclusively (K=endindex-startindex)
    00135         indices can be out of bounds, they are clipped in any case
    00136     */
    00137     Matrix columns(I startindex, I endindex) const;
    00138
    00139
    00140     /** stores the content of the matrix (row-wise) in the given buffer
    00141         @param buffer Buffer for storing the elements (should have the length given by len)
    00142         @param len Length of the provided buffer.
    00143                In any case only min(len, getM()*getN()) elements are copied.
    00144         @return number of actually written elements
    00145      */
    00146     int convertToBuffer(D* buffer, I len) const;
    00147
    00148     /** @return a list of the content of the matrix (row-wise)
    00149      */
    00150     std::list<D> convertToList() const;
    00151
    00152     /// returns a pointer to the data. UNSAFE!!!
    00153     const double* unsafeGetData() const{return data;}
    00154
    00155     /*       STOREABLE       */
    00156     /** stores the Matrix into the given file stream (binary)
    00157      */
    00158     bool store(FILE* f) const;
    00159
    00160     /** reads a Matrix from the given file stream (binary)
    00161      */
    00162     bool restore(FILE* f);
    00163
    00164     /** writes the Matrix into the given file stream (ascii)
    00165      */
    00166     bool write(FILE* f) const;
    00167
    00168     /** reads a Matrix from the given file stream (ascii)
    00169      */
    00170     bool read(FILE* f);
    00171
    00172
    00173   public:
    00174     // ////////////////////////////////////////////////////////////////////
    00175     // /////////////  operations  /////////////////////////////
    00176     //  (the result of the operation is not stored in one of the operands)
    00177     void add(const Matrix& a, const Matrix& b); ///< addition:    this = a + b
    00178     void add(const Matrix& a, const D& summand); /// add scalar to each element
    00179     void sub(const Matrix& a, const Matrix& b); ///< subtraction: this = a - b
    00180     void mult(const Matrix& a, const Matrix& b);///< multiplication: this = a * b
    00181     void mult(const Matrix& a, const D& fac);///< scaling: this = a * fac
    00182
    00183     void exp(const Matrix& a, int exponent);///< exponent, this = a^i, @see toExp
    00184
    00185     /// returns true if matrix is a 0x0 matrix
    00186     bool isNulltimesNull() const;
    00187
    00188     /// returns true if matrix is a vector
    00189     bool isVector() const;
    00190
    00191     /** bytewise comparison (compares data buffer bytewise, which implies that
    00192         n1*m1 == n2*m2 but not necessarily n1==n2) */
    00193     bool equals(const Matrix& a) const;
    00194
    00195     /** calculates the pseudoinverse, depending on the shape of the matrix
    00196         the left or right pseudoinverse is used.
    00197         If the matrix has more columns than rows then we use
    00198         \f[A^{+} = (A^T A + \lambda \mathbb I)^{-1}A^T\f]
    00199         otherwise
    00200         \f[A^{+} = A^T(A A^T + \lambda \mathbb I)^{-1}\f]
    00201      */
    00202     Matrix pseudoInverse(const D& lambda = 1e-8) const ;
    00203
    00204     /** calculates the secure inverse of a square matrix.
    00205         If singular then the pseudoinverse is used.
    00206      */
    00207     Matrix secureInverse() const;
    00208
    00209     /** returns true if all entries are normal floating point numbers,
    00210         otherwise false (e.g. for nan and inf)*/
    00211     bool hasNormalEntries() const;
    00212
    00213
    00214     /**  maps the matrix to a new matrix
    00215          with all elements mapped with the given function
    00216     */
    00217     Matrix map(D (*fun)(D)) const;
    00218     /**  like map but with additional double parameter for the mapping function
    00219          (first argument of fun is parameter, the second is the value)*/
    00220     Matrix mapP(D param, D (*fun)(D, D)) const;
    00221     /**  like map but with additional arbitrary parameter for the mapping function */
    00222     Matrix mapP(void* param, D (*fun)(void*, D)) const;
    00223
    00224     // Exotic operations ///////////
    00225     /** binary map operator for matrices.
    00226        The resulting matrix consists of the function values applied to the elements of a and b.
    00227        In haskell this would something like: map (uncurry . fun) $ zip a b
    00228     */
    00229     static Matrix map2( D (*fun)(D,D), const Matrix& a, const Matrix& b);
    00230
    00231     /** like map2 but with additional parameter.
    00232         The first argument of fun is the parameter and the second and third
    00233         comes from the matrix elements.
    00234         In haskell this would something like: map (uncurry . (fun p)) $ zip a b
    00235      */
    00236     static Matrix map2P( D param, D (*fun)(D, D,D), const Matrix& a, const Matrix& b);
    00237     /** like map2P but with arbitrary paramters (void*) instead of double
    00238      */
    00239     static Matrix map2P( void* param, D (*fun)(void*, D,D), const Matrix& a, const Matrix& b);
    00240
    00241
    00242
    00243     /** row-wise multiplication
    00244         @param factors column vector (Mx1) of factors, one for each row
    00245     */
    00246     Matrix multrowwise(const Matrix& factors) const;
    00247     /** column-wise multiplication
    00248         @param factors column vector (Mx1) of factors, one for each column
    00249     */
    00250     Matrix multcolwise(const Matrix& factors) const;
    00251
    00252     /// optimised multiplication of Matrix with its transposed: M * M^T
    00253     Matrix multMT() const;
    00254     /// optimised multiplication of transpsoed of Matrix with itself: M^T * M
    00255     Matrix multTM() const;
    00256
    00257     /// returns the product of all elements (\f$ \Pi_{ij} m_{ij} \f$)
    00258     D elementProduct() const;
    00259     /// returns the sum of all elements (\f$ \sum_{ij} m_{ij} \f$)
    00260     D elementSum() const;
    00261
    00262     /** returns the sum of all squares of all elements (\f$ \sum_{ij} m_{ij}^2 \f$)
    00263         this is also known as the square of the Frobenius norm.
    00264      */
    00265     D norm_sqr() const;
    00266
    00267     /// returns a matrix that consists of this matrix above A (number of rows is getM + a.getM())
    00268     Matrix above(const Matrix& a) const ;
    00269     /// returns a matrix that consists of this left beside A  (number of columns is getN + a.getN())
    00270     Matrix beside(const Matrix& a) const ;
    00271
    00272   public:   // normal binary Operators
    00273     /// deep copy
    00274     Matrix& operator = (const Matrix& c) { copy(c); return *this; }
    00275     /// sum of two matrices
    00276     Matrix operator +  (const Matrix& sum) const;
    00277     //    Matrix operator +  (const D& sum) const; /// new operator (guettler)
    00278     /// difference of two matrices
    00279     Matrix operator -  (const Matrix& sum) const;
    00280     /** matrix product*/
    00281     Matrix operator *  (const Matrix& fac) const;
    00282     /** product with scalar (D) (only right side) */
    00283     Matrix operator *  (const D& fac) const;
    00284     /** special matrix potence:
    00285         @param exponent -1 -> inverse;
    00286                        0 -> Identity Matrix;
    00287                     1 -> itself;
    00288                     T -> Transpose
    00289     */
    00290     Matrix operator ^ (int exponent) const;
    00291     /// row-wise multiplication
    00292     Matrix operator & (const Matrix& b) const;
    00293     /// combined assigment operator (higher performance)
    00294     Matrix& operator += (const Matrix& c) {toSum(c);   return *this; }
    00295     /// combined assigment operator (higher performance)
    00296     Matrix& operator -= (const Matrix& c) {toDiff(c);  return *this; }
    00297     /// combined assigment operator (higher performance)
    00298     Matrix& operator *= (const Matrix& c) {
    00299       Matrix result;
    00300       result.mult(*this, c);
    00301       this->copy(result);
    00302       return *this;
    00303     }
    00304     /// combined assigment operator (higher performance)
    00305     Matrix& operator *= (const D& fac) {toMult(fac); return *this; }
    00306     /// combined assigment operator (higher performance)
    00307     Matrix& operator &= (const Matrix& c) {toMultrowwise(c); return *this; }
    00308
    00309 #ifndef AVR
    00310     /// comparison operator (compares elements with tolerance distance of COMPARE_EPS)
    00311     bool operator == (const Matrix& c) const;
    00312     /** printing operator:
    00313         output format: mxn (\n row0\n..rown \n) where rowX is tab seperated list of values
    00314     */
    00315     friend std::ostream& operator<<(std::ostream& , const Matrix&);
    00316 #endif
    00317
    00318   public:
    00319     // /////////////////// inplace Operators ///////////////////////////////
    00320     /** performs a deep copy of the given matrix */
    00321     void copy(const Matrix& c){ // Deep copy
    00322       m=c.m; n=c.n;
    00323       allocate();
    00324       memcpy(data,c.data,m*n*sizeof(D));
    00325     }
    00326
    00327     Matrix& toTranspose();  ///< inplace transpose
    00328     Matrix& toZero();       ///< inplace converts matrix to zero matrix
    00329     Matrix& toId();         ///< inplace converts matrix to identity (use ^0 to get a copy version of it)
    00330     /// inplace addition: this = this + a
    00331     Matrix& toSum(const Matrix& a);
    00332     /// inplace addition: this = this + a, where a is a scalar
    00333     Matrix& toSum(const D& sum);
    00334
    00335     /// inplace subtraction: this = this - a
    00336     Matrix& toDiff(const Matrix& a);
    00337
    00338     /// inplace multiplication: this = this * a
    00339     Matrix& toMult(const Matrix& a);
    00340     /// inplace multiplication with scalar: this = this*fac
    00341     Matrix& toMult(const D& fac);
    00342
    00343     /** special inplace matrix power:
    00344         @param exponent -1 -> inverse; (matrix MUST be SQUARE and NONZERO)
    00345                     0 -> Identity Matrix;
    00346                     1 -> itself;
    00347                     n -> n-th power;
    00348                     T -> Transpose
    00349     */
    00350     Matrix& toExp(int exponent);
    00351     /**  inplace mapping of matrix elements (element-wise application) */
    00352     Matrix& toMap(D (*fun)(D));
    00353     /**  like toMap, but with an extra double parameter for the mapping function. */
    00354     Matrix& toMapP(D param, D (*fun)(D, D));
    00355     /**  like toMap, but with an extra arbitrary parameter for the mapping function. */
    00356     Matrix& toMapP(void* param, D (*fun)(void*, D));
    00357
    00358     /**  like toMap, but with using 2 matrices */
    00359     Matrix& toMap2(D (*fun)(D,D), const Matrix& b);
    00360
    00361     /**  like toMap2, but with additional parameter */
    00362     Matrix& toMap2P( D param, D (*fun)(D, D,D), const Matrix& b);
    00363     /**  like toMap2P, but with arbitrary parameter */
    00364     Matrix& toMap2P( void* param, D (*fun)(void*, D,D), const Matrix& b);
    00365
    00366     // Exotic operations
    00367     /** Inplace row-wise multiplication
    00368         @param factors column vector of factors, one for each row
    00369     */
    00370     Matrix& toMultrowwise(const Matrix& factors);
    00371     /** Inplace column-wise multiplication
    00372         @param factors column vector of factors, one for each column
    00373     */
    00374     Matrix& toMultcolwise(const Matrix& factors);
    00375
    00376     /// sets the matrix a below this matrix
    00377     Matrix& toAbove(const Matrix& a);
    00378     /// sets the matrix a right beside this matrix
    00379     Matrix& toBeside(const Matrix& a);
    00380
    00381     /// sorts the matrix (rowwise)
    00382     Matrix& toSort();
    00383
    00384     /** reshapes the matrix without destroying the data.
    00385         Remember: The data is stored rowwise.
    00386
    00387         Only shrinking is allowed i.e. m*n must be lower or equal to getM()*getN()
    00388     */
    00389     Matrix& reshape(I m, I n);
    00390
    00391     /// adds the given value to the diagonal
    00392     Matrix& pluslambdaI(double lambda = 1e-8);
    00393
    00394     /** adds one or more rows to the existing matrix and fills it with the given data
    00395      *
    00396      * same as toAbove(Matrix(numberRows,getN(),data))
    00397      * @param numberRows number of rows to add (this extends m)
    00398      * @param _data data to add
    00399      * @return the address of the matrix itself
    00400      */
    00401     Matrix& addRows(I numberRows, const D* _data=0);
    00402
    00403     /**
    00404      * same as toAbove(dataMatrix)
    00405      * @param numberRows number of rows to add (unused)
    00406      * @param dataMatrix matrix which contains the data of the new rows
    00407      * @return the address of the matrix itself
    00408      */
    00409     Matrix& addRows(I numberRows, const Matrix& dataMatrix);
    00410
    00411     /** adds one or more columns to the existing matrix
    00412      * same as toBeside(Matrix(getM, numberColumns, _data))
    00413      * @see toBeside()
    00414      * @param numberColumns number of columns to add (this extends n)
    00415      * @param _data data to add
    00416      * @return the address of the matrix itself
    00417      */
    00418      Matrix& addColumns(I numberColumns, const D* _data=0);
    00419
    00420     /**
    00421      * same as toBeside(dataMatrix)
    00422      * @see toBeside()
    00423      * @param numberColumns number of columns to add (unused)
    00424      * @param dataMatrix matrix which contains the data of the new rows
    00425      * @return the address of the matrix itself
    00426      */
    00427     Matrix& addColumns(I numberColumns, const Matrix& dataMatrix);
    00428
    00429
    00430     /** removes one or more rows of the existing matrix,
    00431         same as reshape(getM()-numberRows, getN());
    00432      * @param numberRows number of rows to remove (this reduces m)
    00433      * @return the address of the matrix itself
    00434      */
    00435     Matrix& removeRows(I numberRows);
    00436
    00437     /** removes one or more columns of the existing matrix
    00438      * resets the size of the matrix and deletes the appropiate data.
    00439      * @param numberColumns number of columns to remove (this reduces n)
    00440      * @return the address of the matrix itself
    00441      */
    00442     Matrix& removeColumns(I numberColumns);
    00443
    00444   private:
    00445     // NOTE: buffersize determines available memory storage.
    00446     // m and n define the actual size
    00447     I m, n;
    00448     I buffersize;  // max number if elements
    00449     D* data;      // where the data contents of the matrix are stored
    00450
    00451
    00452   private: // internals
    00453     void allocate();  //internal allocation
    00454     /*inplace matrix invertation:
    00455         Matrix must be SQARE, in addition, all DIAGONAL ELEMENTS MUST BE NONZERO
    00456         (positive definite)
    00457     */
    00458
    00459 #ifndef AVR
    00460     void invertnonzero();
    00461     void invert3x3();
    00462 #endif
    00463     void invert2x2();
    00464   };
    00465
    00466 } // namespace matrix
    00467 #endif