Robotsystem from Robot Group Leipzig: matrix.h Source File

- Main Page
- Modules
- Namespaces
- Classes
- Files
- Directories
- Examples

- File List
- File Members

# 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 // $Log: matrix.h,v $
    00010 // Revision 1.20.6.4  2006/03/30 23:08:53  martius
    00011 // comments
    00012 //
    00013 // Revision 1.20.6.3  2006/03/30 12:34:45  martius
    00014 // documentation updated
    00015 //
    00016 // Revision 1.20.6.2  2006/03/29 15:12:46  martius
    00017 // column accessor function added
    00018 //
    00019 // Revision 1.20.6.1  2005/12/06 17:38:10  martius
    00020 // *** empty log message ***
    00021 //
    00022 // Revision 1.20  2005/10/21 11:58:25  martius
    00023 // map2 (similar to map but for 2 matrices)
    00024 // changed naming of functions to be more consistent.
    00025 //  Functions with "to" in front indicate the change of this. (Still not consistent with add, mult ...)
    00026 //
    00027 // Revision 1.19  2005/10/06 17:10:06  martius
    00028 // convertToList
    00029 // above and toAbove
    00030 //
    00031 // Revision 1.18  2005/09/21 08:43:01  martius
    00032 // convertToBuffer is const
    00033 //
    00034 // Revision 1.17  2005/08/06 20:47:36  martius
    00035 // Commented
    00036 //
    00037 // Revision 1.16  2005/07/21 15:13:36  martius
    00038 // mapP addid (mapping with additional parameter)
    00039 //
    00040 // Revision 1.15  2005/06/21 15:35:21  martius
    00041 // hide invert3x3 and invert_nonzero for AVR to minimize binary size
    00042 //
    00043 // Revision 1.14  2005/06/17 15:19:03  martius
    00044 // version workes with avr
    00045 //
    00046 // Revision 1.13  2005/06/10 08:23:15  martius
    00047 // mult???wise are copy operations now!
    00048 // toMult???wise are the inplace variant.
    00049 //
    00050 // Revision 1.11  2005/06/09 11:52:03  martius
    00051 // multMT (M * M^T) and multTM (M^T * M)
    00052 //
    00053 // Revision 1.10  2005/06/02 22:48:54  martius
    00054 // copy is inline and works correct now
    00055 //
    00056 // Revision 1.9  2005/06/02 08:48:31  martius
    00057 // mult_row/column_wise
    00058 // convertToBuffer
    00059 //
    00060 // Revision 1.8  2005/05/30 22:40:56  martius
    00061 // map becomes toMap and the new map returns a new matrix
    00062 // exp becomes toExp
    00063 //
    00064 // Revision 1.7  2005/05/30 21:46:54  martius
    00065 // *** empty log message ***
    00066 //
    00067 // Revision 1.6  2005/05/30 17:21:26  martius
    00068 // added zero
    00069 // set() and constructor(m,n,0) initialise with zero
    00070 // id returns void (more consistent)
    00071 //
    00072 // Revision 1.5  2005/05/30 16:42:56  martius
    00073 // map function included (component-wise function application)
    00074 //
    00075 // Revision 1.4  2005/05/30 11:28:09  martius
    00076 // marked "row" as untested
    00077 //
    00078 // Revision 1.3  2005/05/30 10:14:54  martius
    00079 // 3x3 explicit matrix inversion
    00080 //
    00081 // Revision 1.2  2005/05/30 09:45:46  martius
    00082 // Working. Interface not tested in pratice
    00083 //
    00084 // Revision 1.1  2005/05/29 22:43:08  martius
    00085 // proper Makefile with dependencies and tag generator
    00086 //
    00087 /***************************************************************************/
    00088
    00089 #ifndef MATRIX_H
    00090 #define MATRIX_H
    00091
    00092 #include <string.h>
    00093 #include <assert.h>
    00094 #include <list>
    00095
    00096 #ifndef AVR
    00097 #include <iostream>
    00098 #endif
    00099 using namespace std;
    00100
    00101 // TODO: add doxygen section
    00102
    00103 /**
    00104  * namespace for matrix library
    00105  *@author Georg Martius
    00106  */
    00107 namespace matrix{
    00108
    00109   /// integer constant for use with exp function and (^) operator to transpose the matrix
    00110   extern const int T;
    00111
    00112   /// type for matrix elements
    00113   typedef double D;
    00114 #define D_Zero 0
    00115 #define D_One 1
    00116   /** Matrix type. Type D is datatype of matrix elements, which is fixed to double.
    00117       There are basicly two different types of operation:
    00118       Inplace operations and copy operations.
    00119       Please use the latter ones unless you know what you are doing.
    00120       Just in case of critical performance optimisation use the inplace operations.
    00121       The most convinient way is to use the overloaded operators (like + * ...).
    00122       All constructed matrices are initialised with zero elements (unless data is given).
    00123       All functions perform range checks if in debug mode (NDEBUG is not defined).
    00124       Please use -lmatrix_debug for testing.
    00125       @author Georg Martius
    00126   */
    00127   class Matrix{
    00128   public:
    00129     /// default constructor: zero matrix (0x0)
    00130     Matrix()
    00131       : m(0), n(0), buffersize(0), data(0) {};
    00132     /** constucts a matrix with the given size.
    00133         If _data is null then the matrix is filled with zeros.
    00134         otherwise matrix will be filled with _data in a row-wise manner.
    00135         In this case _data must be at least _m*_n elements long
    00136     */
    00137     Matrix(unsigned short _m, unsigned short _n, const D* _data=0);
    00138     /// constucts a instance on the base of a deep copy of the given matrix
    00139     Matrix (const Matrix& c);
    00140     ~Matrix() { if(data) delete[] data; };
    00141
    00142   public:
    00143     //  /////////////////// Accessors ///////////////////////////////
    00144     /** @return number of rows */
    00145     unsigned short getM() const { return m; };
    00146     /** @return number of columns */
    00147     unsigned short getN() const { return n; };
    00148     /** @return element at position i,j (row, column index) */
    00149     D val(unsigned short i, unsigned short j) const {
    00150       assert( i<m && j<n);
    00151       return data[i*n+j];
    00152     };
    00153     /** @return reference to element at position i,j
    00154         (can be used as left side value) */
    00155     D& val(unsigned short i, unsigned short j) {
    00156       assert( i<m && j<n);
    00157       return data[i*n+j];
    00158     };
    00159     /** sets the size of the matrix and maybe the data if given (row-wise).
    00160         If data=null then the matrix is set to zero
    00161         @see toZero()
    00162         @see constructor Matrix(m,n,data)
    00163     */
    00164     void set(unsigned short _m, unsigned short _n, const D* _data=0);
    00165     /** sets the data (row-wise).
    00166         @param _data if null then matrix elements are set to zero
    00167         otherwise the field MUST have the length should be getM()*getN()*/
    00168     void set(const D* _data);
    00169     /** @return row-vector(as 1xN matrix) containing the index'th row */
    00170     Matrix row(unsigned short index) const;
    00171     /** @returns column-vector(as Nx1 matrix) containing the index'th column */
    00172     Matrix column(unsigned short index) const;
    00173
    00174     /** stores the content of the matrix (row-wise) in the given buffer
    00175         @param buffer Buffer for storing the elements (should have the length given by len)
    00176         @param len Length of the provided buffer.
    00177                In any case only min(len, getM()*getN()) elements are copied.
    00178         @return number of actually written elements
    00179      */
    00180     int convertToBuffer(D* buffer, unsigned int len) const;
    00181     /** @return a list of the content of the matrix (row-wise)
    00182      */
    00183     list<D> convertToList() const;
    00184
    00185   public:
    00186     // ////////////////////////////////////////////////////////////////////
    00187     // /////////////  operations  /////////////////////////////
    00188     //  (the result of the operation is not stored in one of the operands)
    00189     void add(const Matrix& a, const Matrix& b); ///< addition:    this = a + b
    00190     void sub(const Matrix& a, const Matrix& b); ///< subtraction: this = a - b
    00191     void mult(const Matrix& a, const Matrix& b);///< multiplication: this = a * b
    00192     void mult(const Matrix& a, const D& fac);///< scaling: this = a * fac
    00193
    00194     /**  maps the matrix to a new matrix
    00195          with all elements mapped with the given function
    00196     */
    00197     Matrix map(D (*fun)(D)) const;
    00198     /**  like map but with additional parameter for the mapping function
    00199     */
    00200     Matrix mapP(void* param, D (*fun)(void*, D)) const;
    00201
    00202     // Exotic operations ///////////
    00203     /** binary map operator for matrices.
    00204        The resulting matrix consists of the function values applied to the elements of a and b.
    00205        In haskell this would something like: map (uncurry . fun) $ zip a b
    00206     */
    00207     static Matrix map2( D (*fun)(D,D), const Matrix& a, const Matrix& b) {
    00208       assert(a.m == b.m && a.n == b.n);
    00209       Matrix result(a);
    00210       unsigned int len = a.m*a.n;
    00211       for(unsigned short i=0; i < len; i++){
    00212         result.data[i] = fun(a.data[i], b.data[i]);
    00213       }
    00214       return result;
    00215     }
    00216
    00217     /** row-wise multiplication
    00218         @param factors column vector (Mx1) of factors, one for each row
    00219     */
    00220     Matrix multrowwise(const Matrix& factors) const;
    00221     /** column-wise multiplication
    00222         @param factors column vector (Mx1) of factors, one for each column
    00223     */
    00224     Matrix multcolwise(const Matrix& factors) const;
    00225
    00226     /// optimised multiplication of Matrix with its transposed: M * M^T
    00227     Matrix multMT() const;
    00228     /// optimised multiplication of transpsoed of Matrix with itself: M^T * M
    00229     Matrix multTM() const;
    00230
    00231     /// returns the product of all elements (\Pi_{ij} m_{ij})
    00232     D elementProduct() const;
    00233     /// returns the sum of all elements (\Sum_{ij} m_{ij})
    00234     D elementSum() const;
    00235
    00236     /// returns a matrix that consists of this above A
    00237     Matrix above(const Matrix& a) const ;
    00238
    00239   public:   /// normal binary Operators
    00240     /// deep copy
    00241     Matrix& operator = (const Matrix& c) { copy(c); return *this; }
    00242     Matrix operator +  (const Matrix& sum) const;
    00243     Matrix operator -  (const Matrix& sum) const;
    00244     /** matrix product*/
    00245     Matrix operator *  (const Matrix& fac) const;
    00246     /** product with scalar (D) */
    00247     Matrix operator *  (const D& fac) const;
    00248     /** special matrix potence:
    00249         @param exponent -1 -> inverse;
    00250                        0 -> Identity Matrix;
    00251                     1 -> itself;
    00252                     T -> Transpose
    00253     */
    00254     Matrix operator ^ (int exponent) const;
    00255     /// performant combined assigment operators
    00256     Matrix& operator += (const Matrix& c) {toSum(c);   return *this; }
    00257     Matrix& operator -= (const Matrix& c) {toDiff(c);  return *this; }
    00258     Matrix& operator *= (const Matrix& c) {
    00259       Matrix result;
    00260       result.mult(*this, c);
    00261       return *this;
    00262     }
    00263     Matrix& operator *= (const D& fac) {toMult(fac); return *this; }
    00264
    00265 #ifndef AVR
    00266     /// comparison operator (compares elements with tolerance distance of COMPARE_EPS)
    00267     bool operator == (const Matrix& c) const;
    00268     /** printing operator:
    00269         output format: mxn (\n row0\n..rown \n) where rowX is tab seperated list of values
    00270     */
    00271     friend ostream& operator<<(ostream& , const Matrix&);
    00272 #endif
    00273
    00274   public:
    00275     // /////////////////// inplace Operators ///////////////////////////////
    00276     /** performs a deep copy of the given matrix */
    00277     void copy(const Matrix& c){ // Deep copy
    00278       m=c.m; n=c.n;
    00279       allocate();
    00280       memcpy(data,c.data,m*n*sizeof(D));
    00281     }
    00282
    00283     void toTranspose();  ///< inplace transpose
    00284     void toZero();       ///< inplace converts matrix to zero matrix
    00285     void toId();         ///< inplace converts matrix to identity (use ^0 to get a copy version of it)
    00286     /// inplace addition: this = this + a
    00287     void toSum(const Matrix& a) {
    00288       assert(a.m==m && a.n==n);
    00289       for(unsigned short i=0; i<m*n; i++){
    00290         data[i]+=a.data[i];
    00291       }
    00292     }
    00293     /// inplace subtraction: this = this - a
    00294     void toDiff(const Matrix& a){
    00295       assert(a.m==m && a.n==n);
    00296       for(unsigned short i=0; i<m*n; i++){
    00297         data[i]-=a.data[i];
    00298       }
    00299     }
    00300
    00301     /// inplace multiplication with scalar: this = this*fac
    00302     void toMult(const D& fac);
    00303
    00304     /** special inplace matrix potence:
    00305         @param exponent -1 -> inverse; (matrix MUST be SQUARE and NONZERO)
    00306                     0 -> Identity Matrix;
    00307                     1 -> itself;
    00308                     T -> Transpose
    00309     */
    00310     void toExp(int exponent);
    00311     /**  inplace mapping of matrix elements (element-wise application) */
    00312     void toMap(D (*fun)(D));
    00313     /**  like toMap, but with an extra parameter for the mapping function. */
    00314     void toMapP(void* param, D (*fun)(void*, D));
    00315     // Exotic operations
    00316     /** Inplace row-wise multiplication
    00317         @param factors column vector of factors, one for each row
    00318     */
    00319     void toMultrowwise(const Matrix& factors);
    00320     /** Inplace column-wise multiplication
    00321         @param factors column vector of factors, one for each column
    00322     */
    00323     void toMultcolwise(const Matrix& factors);
    00324
    00325     /// sets the matrix a below (this) matrix
    00326     void toAbove(const Matrix& a);
    00327
    00328
    00329   private:
    00330     // NOTE: buffersize determines available memory storage.
    00331     // m and n define the actual size
    00332     unsigned short m, n;
    00333     unsigned int buffersize;  // max number if elements
    00334     D* data;      // where the data contents of the matrix are stored
    00335
    00336
    00337   private: // internals
    00338     void allocate();  //internal allocation
    00339     /*inplace matrix invertation:
    00340         Matrix must be SQARE, in addition, all DIAGONAL ELEMENTS MUST BE NONZERO
    00341         (positive definite)
    00342     */
    00343 #ifndef AVR
    00344     void invertnonzero();
    00345     void invert3x3();
    00346 #endif
    00347     void invert2x2();
    00348   };
    00349
    00350 } // namespace matrix
    00351 #endif