Robotsystem from Robot Group Leipzig: matrix.cpp Source File

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

- File List
- File Members

# matrix.cpp

Go to the documentation of this file.

    00001 /***************************************************************************
    00002                           matrix.cpp  -  description
    00003                              -------------------
    00004     email                : georg.martius@web.de
    00005 ***************************************************************************/
    00006 //
    00007 // $Log: matrix.cpp,v $
    00008 // Revision 1.23.6.1  2006/03/29 15:12:46  martius
    00009 // column accessor function added
    00010 //
    00011 // Revision 1.23  2005/10/21 11:58:25  martius
    00012 // map2 (similar to map but for 2 matrices)
    00013 // changed naming of functions to be more consistent.
    00014 //  Functions with "to" in front indicate the change of this. (Still not consistent with add, mult ...)
    00015 //
    00016 // Revision 1.22  2005/10/06 17:10:06  martius
    00017 // convertToList
    00018 // above and toAbove
    00019 //
    00020 // Revision 1.21  2005/09/21 08:42:53  martius
    00021 // convertToBuffer is const
    00022 //
    00023 // Revision 1.20  2005/08/06 20:47:36  martius
    00024 // Commented
    00025 //
    00026 // Revision 1.19  2005/07/21 15:13:36  martius
    00027 // mapP addid (mapping with additional parameter)
    00028 //
    00029 // Revision 1.18  2005/07/07 10:26:59  martius
    00030 // exp(1x1) matrix implemented
    00031 //
    00032 // Revision 1.17  2005/06/21 15:35:21  martius
    00033 // hide invert3x3 and invert_nonzero for AVR to minimize binary size
    00034 //
    00035 // Revision 1.16  2005/06/19 23:00:30  martius
    00036 // AVR
    00037 //
    00038 // Revision 1.15  2005/06/17 15:19:03  martius
    00039 // version workes with avr
    00040 //
    00041 // Revision 1.14  2005/06/10 08:21:59  martius
    00042 // mult???wise are copy operations now!
    00043 // toMult???wise are inplace instead
    00044 //
    00045 // Revision 1.13  2005/06/10 07:50:47  martius
    00046 // multMT and multTM are const!
    00047 //
    00048 // Revision 1.12  2005/06/09 11:52:03  martius
    00049 // multMT (M * M^T) and multTM (M^T * M)
    00050 //
    00051 // Revision 1.11  2005/06/02 22:48:48  martius
    00052 // copy is inline and works correct now
    00053 //
    00054 // Revision 1.10  2005/06/02 08:49:08  martius
    00055 // mult_row/column_wise
    00056 // convertToBuffer
    00057 // Matrix comparison uses epsilon instead of plain ==
    00058 //
    00059 // Revision 1.9  2005/05/30 22:40:56  martius
    00060 // map becomes toMap and the new map returns a new matrix
    00061 // exp becomes toExp
    00062 //
    00063 // Revision 1.7  2005/05/30 17:21:41  martius
    00064 // added zero
    00065 // set() and constructor(m,n,0) initialise with zero
    00066 // id returns void (more consistent)
    00067 //
    00068 // Revision 1.6  2005/05/30 16:43:12  martius
    00069 // map function included (component-wise function application)
    00070 //
    00071 // Revision 1.5  2005/05/30 10:15:36  martius
    00072 // proper log entry in header
    00073 //
    00074 /***************************************************************************/
    00075
    00076 #include "matrix.h"
    00077 #include <string.h>
    00078 #include <math.h>
    00079
    00080 namespace matrix {
    00081
    00082 #define COMPARE_EPS 1e-12
    00083 const int T=0xFF;
    00084
    00085   Matrix::Matrix(const Matrix& c)
    00086     : m(0), n(0), buffersize(0), data(0){
    00087     copy(c);
    00088   }
    00089
    00090   Matrix::Matrix(unsigned short _m, unsigned short _n, const D* _data /*=0*/)
    00091     : m(_m), n(_n), buffersize(0), data(0) {
    00092     allocate();
    00093     set(_data);
    00094   };
    00095
    00096   // internal allocation
    00097   void Matrix::allocate() {
    00098     if((unsigned)m*n > buffersize){
    00099       buffersize=m*n;
    00100       if(data) { delete[] data; }
    00101       data = new D[buffersize];
    00102       assert(data);
    00103     }
    00104   }
    00105
    00106
    00107 ////////////////////////////////////////////////////////////////////////////////
    00108 ////// ACCESSORS ///////////////////////////////////////////////////////////////
    00109   Matrix Matrix::row(unsigned short index) const{
    00110     assert(index < m);
    00111     Matrix result(1,n,data+(index*n));
    00112     return result;
    00113   }
    00114
    00115   Matrix Matrix::column(unsigned short index) const {
    00116     assert(index < n);
    00117     Matrix result(m,1);
    00118     for(int i=0; i<m; i++){
    00119       result.val(i,0) = val(i,index);
    00120     }
    00121     return result;
    00122   }
    00123
    00124
    00125   void Matrix::set(unsigned short _m, unsigned short _n, const D* _data /*=0*/){
    00126     m=_m;
    00127     n=_n;
    00128     allocate();
    00129     set(_data);
    00130   }
    00131
    00132   void Matrix::set(const D* _data){
    00133     if (_data)
    00134       memcpy(data,_data, m*n*sizeof(D));
    00135     else toZero();
    00136   }
    00137
    00138   int Matrix::convertToBuffer(D* buffer, unsigned int len) const {
    00139     if(buffer && data){
    00140       unsigned int minlen = (len < (unsigned)m*n) ? len : m*n;
    00141       memcpy(buffer, data, sizeof(D) * minlen);
    00142       return minlen;
    00143     }
    00144     return 0;
    00145   }
    00146
    00147   list<D> Matrix::convertToList() const {
    00148     list<D> l;
    00149     if(data){
    00150       for(int i=0; i < m*n; i++){
    00151         l.push_back(data[i]);
    00152       }
    00153     }
    00154     return l;
    00155   }
    00156
    00157 ////////////////////////////////////////////////////////////////////////////////
    00158 ////// OPERATORS ///////////////////////////////////////////////////////////////
    00159 //  INPLACE:
    00160 //________________________________________________________________
    00161
    00162   void Matrix::toTranspose(){
    00163     assert(buffersize > 0);
    00164     if(m!=1 && n!=1){ // if m or n == 1 then no copying is necessary!
    00165       double* newdata = new D[buffersize];
    00166       for(unsigned short i=0; i < m; i++){
    00167         for(unsigned short j=0; j < n; j++){
    00168           newdata[j*m+i]=data[i*n+j];
    00169         }
    00170       }
    00171       delete[] data;
    00172       data=newdata;
    00173     }
    00174     // swap n and m
    00175     unsigned short t = m;
    00176     m=n;
    00177     n=t;
    00178   }
    00179
    00180   void Matrix::toZero(){
    00181     memset(data, D_Zero, m*n*sizeof(D));
    00182   }
    00183
    00184   void Matrix::toId(){
    00185     toZero();
    00186     unsigned short smallerdim = m < n ? m : n;
    00187     for(unsigned short i=0; i < smallerdim; i++){
    00188       val(i,i)=D_One;
    00189     }
    00190   }
    00191
    00192   void Matrix::add(const Matrix& a,const Matrix& b){
    00193     assert(a.m==b.m && a.n==b.n);
    00194     copy(a);
    00195     toSum(b);
    00196   }
    00197
    00198   void Matrix::sub(const Matrix& a,const Matrix& b){
    00199     assert(a.m==b.m && a.n==b.n);
    00200     copy(a);
    00201     toDiff(b);
    00202   }
    00203
    00204   void Matrix::mult(const Matrix& a,const Matrix& b){
    00205     assert(a.n==b.m);
    00206     m=a.m;
    00207     n=b.n;
    00208     unsigned short interdim = a.n;
    00209     allocate();
    00210     D d;
    00211     for(unsigned short i=0; i < m; i++){
    00212       for(unsigned short j=0; j < n; j++){
    00213         d=0;
    00214         for(unsigned short k=0; k < interdim; k++){
    00215           d+=a.val(i,k)*b.val(k,j);
    00216         }
    00217         val(i,j)=d;
    00218       }
    00219     }
    00220   }
    00221
    00222   void Matrix::mult(const Matrix& a, const D& fac){
    00223     m=a.m;
    00224     n=a.n;
    00225     allocate();
    00226     for(unsigned short i=0; i<m*n; i++){
    00227       data[i]=a.data[i]*fac;
    00228     }
    00229   }
    00230
    00231   void Matrix::toMult(const D& fac){
    00232     for(unsigned short i=0; i<m*n; i++){
    00233       data[i]*=fac;
    00234     }
    00235   }
    00236
    00237   /** special inplace matrix potence:
    00238       @param: exp -1 -> inverse; 0 -> Identity Matrix;
    00239       1 -> itself;
    00240       T -> Transpose
    00241   */
    00242   void Matrix::toExp (int exponent) {
    00243     switch(exponent){
    00244     case -1:
    00245       if(m==1) val(0,0) = 1/val(0,0);
    00246       else if(m==2) invert2x2();
    00247 #ifndef AVR
    00248       else if(m==3) invert3x3();
    00249       else invertnonzero();
    00250 #endif
    00251       break;
    00252     case 0:
    00253       toId();
    00254       break;
    00255     case 1: // do nothing
    00256       break;
    00257     case T:
    00258       toTranspose();
    00259       break;
    00260     default:
    00261       assert("Should not be reached" == 0);
    00262       break;
    00263     }
    00264   }
    00265
    00266   void Matrix::toMap(D (*fun)(D)){
    00267     unsigned int len = m*n;
    00268     for(unsigned short i=0; i < len; i++){
    00269       data[i]=fun(data[i]);
    00270     }
    00271   }
    00272
    00273   Matrix Matrix::map(D (*fun)(D)) const {
    00274     Matrix result(*this);
    00275     result.toMap(fun);
    00276     return result;
    00277   }
    00278
    00279   void Matrix::toMapP(void* param, D (*fun)(void*, D)){
    00280     unsigned int len = m*n;
    00281     for(unsigned short i=0; i < len; i++){
    00282       data[i]=fun(param, data[i]);
    00283     }
    00284   }
    00285
    00286   Matrix Matrix::mapP(void* param, D (*fun)(void*, D)) const {
    00287     Matrix result(*this);
    00288     result.toMapP(param,fun);
    00289     return result;
    00290   }
    00291
    00292   void Matrix::toMultrowwise(const Matrix& factors){
    00293     assert(m == factors.m && factors.n == 1);
    00294     for(unsigned int i = 0; i < m; i++){
    00295       for (unsigned int j = 0; j < n; j++){
    00296         val(i,j) *= factors.val(i,0);
    00297       }
    00298     }
    00299   }
    00300
    00301   void Matrix::toMultcolwise(const Matrix& factors){
    00302     assert(n == factors.m && factors.n == 1);
    00303     for(unsigned int i = 0; i < m; i++){
    00304       for (unsigned int j = 0; j < n; j++){
    00305         val(i,j) *= factors.val(j,0);
    00306       }
    00307     }
    00308   }
    00309
    00310   void Matrix::toAbove(const Matrix& a){
    00311     assert(a.n == this->n);
    00312     data = (D*)realloc(data, sizeof(D) * (this->m * this->n + a.n * a.m));
    00313     memcpy(data+this->m * this->n, a.data, sizeof(D) * (a.n * a.m));
    00314     this->m+=a.m;
    00315   }
    00316
    00317
    00318   Matrix Matrix::multrowwise(const Matrix& factors) const {
    00319     Matrix result(*this);
    00320     result.toMultrowwise(factors);
    00321     return result;
    00322   }
    00323
    00324   Matrix Matrix::multcolwise(const Matrix& factors) const{
    00325     Matrix result(*this);
    00326     result.toMultcolwise(factors);
    00327     return result;
    00328   }
    00329
    00330   // multiply Matrix with its transposed: M * M^T
    00331   Matrix Matrix::multMT() const {
    00332     assert(m != 0 && n != 0);
    00333     Matrix result(m,m);
    00334     D d;
    00335     for(unsigned short i=0; i < m; i++){
    00336       for(unsigned short j=0; j < m; j++){
    00337         d=0;
    00338         for(unsigned short k=0; k < n; k++){
    00339           d+=val(i,k) * val(j,k);
    00340         }
    00341         result.val(i,j)=d;
    00342       }
    00343     }
    00344     return result;
    00345   }
    00346
    00347   // multiply transpsoed of Matrix with itself: M^T * M
    00348   Matrix Matrix::multTM() const {
    00349     assert(m != 0 && n != 0);
    00350     Matrix result(n,n);
    00351     D d;
    00352     for(unsigned short i=0; i < n; i++){
    00353       for(unsigned short j=0; j < n; j++){
    00354         d=0;
    00355         for(unsigned short k=0; k < m; k++){
    00356           d+=val(k,i) * val(k,j);
    00357         }
    00358         result.val(i,j)=d;
    00359       }
    00360     }
    00361     return result;
    00362   }
    00363
    00364   /// returns the product of all elements
    00365   D Matrix::elementProduct() const {
    00366     D rv=1;
    00367     for(unsigned short i=0; i<m*n; i++){
    00368       rv*=data[i];
    00369     }
    00370     return rv;
    00371   }
    00372
    00373   /// returns the sum of all elements
    00374   D Matrix::elementSum() const {
    00375     D rv=0;
    00376     for(unsigned short i=0; i<m*n; i++){
    00377       rv+=data[i];
    00378     }
    00379     return rv;
    00380   }
    00381
    00382   /// returns a matrix that consists of b below this
    00383   Matrix Matrix::above(const Matrix& b) const {
    00384     Matrix r(*this);
    00385     r.toAbove(b);
    00386     return r;
    00387   }
    00388
    00389
    00390 #ifndef AVR
    00391   /* inplace matrix invertation:
    00392       Matrix must be SQARE, in addition, all DIAGONAL ELEMENTS MUST BE NONZERO  */
    00393   void Matrix::invertnonzero()  {
    00394     assert(m==n && m>1); // must be of dimension >= 2
    00395
    00396     for (unsigned int i=1; i < m; i++) data[i] /= data[0]; // normalize row 0
    00397     for (unsigned int i=1; i < m; i++)  {
    00398       for (unsigned int j=i; j < m; j++)  { // do a column of L
    00399         D sum = 0.0;
    00400         for (unsigned int k = 0; k < i; k++)
    00401           sum += val(j,k) * val(k,i);
    00402         val(j,i) -= sum;
    00403       }
    00404       if (i == (unsigned) m-1) continue;
    00405       for (unsigned int j=i+1; j < m; j++)  {  // do a row of U
    00406         D sum = 0.0;
    00407         for (unsigned int k = 0; k < i; k++)
    00408           sum += val(i,k)*val(k,j);
    00409         val(i,j) = (val(i,j)-sum) / val(i,i);
    00410       }
    00411     }
    00412     for (unsigned int i = 0; i < m; i++ )  // invert L
    00413       for (unsigned int j = i; j < m; j++ )  {
    00414         D x = 1.0;
    00415         if ( i != j ) {
    00416           x = 0.0;
    00417           for (unsigned int k = i; k < j; k++ )
    00418             x -= val(j,k)*val(k,i);
    00419         }
    00420         val(j,i) = x / val(j,j);
    00421       }
    00422     for (unsigned int i = 0; i < m; i++ )   // invert U
    00423       for (unsigned int j = i; j < m; j++ )  {
    00424         if ( i == j ) continue;
    00425         D sum = 0.0;
    00426         for (unsigned int k = i; k < j; k++ )
    00427           sum += val(k,j)*( (i==k) ? 1.0 : val(i,k) );
    00428         val(i,j) = -sum;
    00429       }
    00430     for (unsigned int i = 0; i < m; i++ )   // final inversion
    00431       for (unsigned int j = 0; j < m; j++ )  {
    00432         D sum = 0.0;
    00433         for (unsigned int k = ((i>j)?i:j); k < m; k++ )
    00434           sum += ((j==k)?1.0:val(j,k))*val(k,i);
    00435         val(j,i) = sum;
    00436       }
    00437   };
    00438 #endif
    00439
    00440
    00441   void Matrix::invert2x2(){
    00442     assert(m==n && m==2);
    00443     // high speed version
    00444     D detQ = data[0] * data[3] - data[1] * data[2];
    00445     D tmp = data[0];
    00446     data[0] = data[3] / detQ;
    00447     data[3] = tmp / detQ;
    00448     data[1] /= -detQ;
    00449     data[2] /= -detQ;
    00450   }
    00451
    00452 #ifndef AVR
    00453   void Matrix::invert3x3(){
    00454     assert(m==n && m==3);
    00455     D Q_adjoint[3][3];
    00456     D detQ=0;
    00457     //calculate the inverse of Q
    00458     Q_adjoint[0][0]=val(1,1)*val(2,2)-val(1,2)*val(2,1) ;
    00459     Q_adjoint[0][1]=(val(1,2)*val(2,0)-val(1,0)*val(2,2)) ;
    00460     Q_adjoint[0][2]=val(1,0)*val(2,1)-val(1,1)*val(2,0) ;
    00461     Q_adjoint[1][0]=(val(2,1)*val(0,2)-val(0,1)*val(2,2)) ;
    00462     Q_adjoint[1][1]=val(0,0)*val(2,2)-val(0,2)*val(2,0) ;
    00463     Q_adjoint[1][2]=(val(0,1)*val(2,0)-val(0,0)*val(2,1)) ;
    00464     Q_adjoint[2][0]=val(0,1)*val(1,2)-val(1,1)*val(0,2) ;
    00465     Q_adjoint[2][1]=(val(1,0)*val(0,2)-val(0,0)*val(1,2)) ;
    00466     Q_adjoint[2][2]=val(0,0)*val(1,1)-val(0,1)*val(1,0) ;
    00467     detQ=val(0,0)*Q_adjoint[0][0]+val(0,1)*Q_adjoint[0][1]+val(0,2)*Q_adjoint[0][2] ;
    00468     for(int i=0; i<3; i++){
    00469       for(int j=0; j<3; j++) {
    00470         val(i,j)=(Q_adjoint[j][i])/detQ  ;
    00471       }
    00472     }
    00473   }
    00474 #endif
    00475
    00476   ////////////////////////////////////////////////////////////////////////////////
    00477   // normal binary operators
    00478
    00479   Matrix Matrix::operator +  (const Matrix& sum) const {
    00480     Matrix result;
    00481     result.add(*this, sum);
    00482     return result;
    00483   }
    00484   Matrix Matrix::operator -  (const Matrix& sum) const {
    00485     Matrix result;
    00486     result.sub(*this, sum);
    00487     return result;
    00488   }
    00489   /** matrix product*/
    00490   Matrix Matrix::operator *  (const Matrix& fac) const {
    00491     Matrix result;
    00492     result.mult(*this, fac);
    00493     return result;
    00494   }
    00495   /** product with scalar (double)*/
    00496   Matrix Matrix::operator *  (const D& scalar) const {
    00497     Matrix result;
    00498     result.mult(*this, scalar);
    00499     return result;
    00500   }
    00501   /** special matrix potence:
    00502       @param: exp -1 -> inverse; 0 -> Identity Matrix;
    00503       1 -> itself; 2-> Matrix*Matrix^T
    00504       T -> Transpose
    00505   */
    00506   Matrix Matrix::operator ^ (int exponent) const {
    00507     Matrix result(*this);
    00508     result.toExp(exponent);
    00509     return result;
    00510   }
    00511
    00512 #ifndef AVR
    00513   bool Matrix::operator == (const Matrix& c) const {
    00514     if(m!=c.m || n!=c.n) return false;
    00515     D* p1=data;
    00516     D* p2=c.data;
    00517     unsigned int len = m*n;
    00518     for(unsigned int i = 0; i < len; i++){
    00519       if(fabs(*p1 - *p2) > COMPARE_EPS) {
    00520         return false;
    00521       }
    00522       p1++; p2++;
    00523     }
    00524     return true;
    00525   }
    00526
    00527   ostream& operator<<(ostream& str, const Matrix& mat){
    00528     if (mat.m==0 || mat.n==0) return str << "0";
    00529     else {
    00530       str << mat.m << "x" << mat.n << " (\n";
    00531       for(int i=0; i < mat.m; i++){
    00532         for(int j=0; j < mat.n; j++){
    00533           str << mat.val(i,j) << "\t";
    00534         }
    00535         str << endl;
    00536       }
    00537     }
    00538     return str << ")\n";
    00539   }
    00540 #endif
    00541
    00542 }
    00543
    00544 #include "matrix.tests.cpp"