matrix_BOOST.cpp
Go to the documentation of this file.
00001 // $Id: matrix_BOOST.cpp 27906 2007-04-27 11:50:53Z wmeeusse $
00002 // Copyright (C) 2002 Klaas Gadeyne <first dot last at gmail dot com>
00003 
00004 //
00005 // This program is free software; you can redistribute it and/or modify
00006 // it under the terms of the GNU Lesser General Public License as published by
00007 // the Free Software Foundation; either version 2.1 of the License, or
00008 // (at your option) any later version.
00009 //
00010 // This program is distributed in the hope that it will be useful,
00011 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013 // GNU Lesser General Public License for more details.
00014 //
00015 // You should have received a copy of the GNU Lesser General Public License
00016 // along with this program; if not, write to the Free Software
00017 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00018 //
00019 
00020 #include "../config.h"
00021 #ifdef __MATRIXWRAPPER_BOOST__
00022 
00023 #include "matrix_BOOST.h"
00024 #include "vector_BOOST.h"
00025 
00026 #include <boost/numeric/ublas/matrix_proxy.hpp>
00027 
00028 using namespace std;
00029 
00030 // Passing the constructor arguments...
00031 MyMatrix::Matrix() : BoostMatrix() {}
00032 MyMatrix::Matrix(int num_rows, int num_cols) : BoostMatrix(num_rows,
00033                                                            num_cols){}
00034 
00035 // Destructor
00036 MyMatrix::~Matrix(){}
00037 
00038 // Copy constructor
00039 MyMatrix::Matrix(const MyMatrix& a) : BoostMatrix(a){}
00040 
00041 // ill-designed
00042 MyMatrix::Matrix(const BoostMatrix & a) : BoostMatrix(a){}
00043 
00044 MyMatrix::Matrix(int num_rows,const RowVector& v):BoostMatrix(num_rows,v.size()){
00045   BoostMatrix & m = *this;
00046   for(unsigned int i=0;i<num_rows;i++)
00047     row(m,i).assign(v);
00048 }
00049 
00050 MyRowVector MyMatrix::operator[](unsigned int i) const{
00051   return this->rowCopy(i);
00052 }
00053 
00054 
00055 // Number of Rows/Cols
00056 unsigned int MyMatrix::rows() const { return this->size1();}
00057 unsigned int MyMatrix::columns() const { return this->size2();}
00058 
00059 // MATRIX - SCALAR operators
00060 MyMatrix& MyMatrix::operator+= (double a)
00061 {
00062   BoostMatrix & op1 = *this;
00063   op1 += boost::numeric::ublas::scalar_matrix<double>(rows(),columns(),a);
00064   return (MyMatrix&)op1;
00065 }
00066 
00067 MyMatrix& MyMatrix::operator-= (double a)
00068 {
00069   BoostMatrix & op1 = (*this);
00070   op1 -= boost::numeric::ublas::scalar_matrix<double>(rows(),columns(),a);
00071   return (MyMatrix&) op1;
00072 }
00073 
00074 MyMatrix& MyMatrix::operator*= (double a)
00075 {
00076   BoostMatrix & op1 = (*this);
00077   op1 *= a;
00078   return *this;
00079 }
00080 
00081 MyMatrix& MyMatrix::operator/= (double a)
00082 {
00083   BoostMatrix & op1 = (*this);
00084   op1 /= a;
00085   return (MyMatrix&) op1;
00086 }
00087 
00088 MyMatrix MyMatrix::operator+ (double a) const
00089 {
00090   return (MyMatrix)(((BoostMatrix)(*this)) + boost::numeric::ublas::scalar_matrix<double>(rows(),columns(),a));
00091 }
00092 
00093 MyMatrix MyMatrix::operator- (double a) const
00094 {
00095   return (MyMatrix)(((BoostMatrix)(*this)) - boost::numeric::ublas::scalar_matrix<double>(rows(),columns(),a));
00096 }
00097 
00098 MyMatrix MyMatrix::operator* (double a) const
00099 {
00100   const BoostMatrix& op1 = (*this);
00101   return (MyMatrix) (op1 *  a);
00102 }
00103 
00104 MyMatrix MyMatrix::operator/ (double a) const
00105 {
00106   const BoostMatrix& op1 = (*this);
00107   return (MyMatrix) (op1 /  a);
00108 }
00109 
00110 MyMatrix&
00111 MyMatrix::operator =(const MySymmetricMatrix& a)
00112 {
00113   *this =(MyMatrix) a;
00114 
00115   return *this;
00116 }
00117 
00118 // MATRIX - MATRIX Operators
00119 MyMatrix MyMatrix::operator- (const MyMatrix& a) const
00120 {
00121   const BoostMatrix& op1 = *this;
00122   const BoostMatrix& op2 = a;
00123 
00124   return (MyMatrix)(op1 - op2);
00125 }
00126 
00127 MyMatrix MyMatrix::operator+ (const MyMatrix& a) const
00128 {
00129   const BoostMatrix& op1 = *this;
00130   const BoostMatrix& op2 = a;
00131 
00132   return (MyMatrix)(op1 + op2);
00133 }
00134 
00135 MyMatrix MyMatrix::operator* (const MyMatrix& a) const
00136 {
00137   const BoostMatrix& op1 = *this;
00138   const BoostMatrix& op2 = a;
00139 
00140   return (MyMatrix) prod(op1,op2);
00141 }
00142 
00143 MyMatrix & MyMatrix::operator+= (const MyMatrix& a)
00144 {
00145   BoostMatrix & op1 = (*this);
00146   const BoostMatrix & op2 = a;
00147   op1 += op2;
00148   return (MyMatrix &) op1;
00149 }
00150 
00151 MyMatrix & MyMatrix::operator-= (const MyMatrix& a)
00152 {
00153   BoostMatrix & op1 = (*this);
00154   const BoostMatrix & op2 = a;
00155   op1 -= op2;
00156   return (MyMatrix &) op1;
00157 }
00158 
00159 
00160 // MATRIX - VECTOR Operators
00161 MyColumnVector MyMatrix::operator* (const MyColumnVector &b) const
00162 {
00163   const BoostMatrix& op1 = (*this);
00164   return (MyColumnVector) prod(op1, ((const BoostColumnVector&)b));
00165 }
00166 
00167 
00168 
00169 double& MyMatrix::operator()(unsigned int a, unsigned int b)
00170 {
00171   BoostMatrix & op1 = (*this);
00172   return op1(a-1,b-1);
00173 }
00174 
00175 double MyMatrix::operator()(unsigned int a, unsigned int b) const
00176 {
00177   BoostMatrix  op1(*this);
00178   return op1(a-1,b-1);
00179 }
00180 
00181 bool MyMatrix::operator==(const MyMatrix& a) const
00182 {
00183   if (this->rows() != a.rows()) return false;
00184   if (this->columns() != a.columns()) return false;
00185   return(norm_inf((BoostMatrix)(*this)-(BoostMatrix)a) == 0);
00186 }
00187 
00188 
00189 // Set all elements equal to a
00190 MyMatrix&
00191  MyMatrix::operator=(double a)
00192 {
00193   *this = (MyMatrix)boost::numeric::ublas::scalar_matrix<double>(rows(),columns(),a);
00194 
00195   return *this;
00196 }
00197 
00198 
00199 MyRowVector MyMatrix::rowCopy(unsigned int r) const
00200 {
00201   unsigned int cols = columns();
00202   BoostRowVector temp(cols);
00203   for (unsigned int i=0; i<cols; i++)
00204     temp(i) = (*this)(r,i+1);
00205   return (MyRowVector) temp;
00206 }
00207 
00208 MyColumnVector MyMatrix::columnCopy(unsigned int c) const
00209 {
00210   unsigned int ro = rows();
00211   BoostColumnVector temp(ro);
00212   for (unsigned int i=0; i<ro; i++)
00213     temp(i) = (*this)(i+1,c);
00214   return (MyColumnVector) temp;
00215 }
00216 
00217 
00218 
00219 
00220 MyMatrix MyMatrix::transpose() const
00221 {
00222   const BoostMatrix &op1 = (*this);
00223   return (MyMatrix) trans(op1);
00224 }
00225 
00226 double MyMatrix::determinant() const
00227 {
00228   unsigned int r = this->rows();
00229   assert(r == this->columns());
00230   double result = 1.0;
00231   const BoostMatrix& A = (*this);
00232   switch (r)
00233   {
00234         case 1:
00235             return A(0,0);
00236         case 2: 
00237             return ( ( A(0,0) * A(1,1)) - ( A(1,0) * A(0,1)) );
00238         default: 
00239             BoostMatrix LU(r,r);
00240             boost::numeric::ublas::permutation_matrix<> ndx(r);
00241             noalias(LU) = A;
00242             int res = lu_factorize(LU,ndx);
00243             assert(res == 0);
00244 
00245             int s = 1;
00246             for (boost::numeric::ublas::matrix<double>::size_type i=0;i<LU.size1();++i) {
00247               result *= LU(i,i);
00248               if (ndx(i)!=i) s = -s;
00249             }
00250             return result*s;
00251   }
00252 }
00253 
00254 
00255 MyMatrix MyMatrix::inverse() const
00256 {
00257   unsigned int r = this->rows();
00258   assert(r == this->columns());
00259   const BoostMatrix& A = (*this);
00260   BoostMatrix Ai(r,r);
00261   switch (r) 
00262   {
00263      case 1:
00264      {
00265         Ai(0,0) = 1/A(0,0);
00266         break;
00267      }
00268      case 2:
00269      {
00270        double det = A(0,0)*A(1,1)-A(0,1)*A(1,0);
00271        Ai(0,0) = A(1,1)/det;
00272        Ai(1,1) = A(0,0)/det;
00273        Ai(0,1) = -A(0,1)/det;
00274        Ai(1,0) = -A(1,0)/det;
00275        break;
00276      }
00277      default:
00278      {
00279        BoostMatrix LU(r,r);
00280        boost::numeric::ublas::permutation_matrix<> ndx(r);
00281        noalias(LU) = A;
00282        int res = lu_factorize(LU,ndx);
00283        assert(res == 0);
00284        noalias(Ai) = boost::numeric::ublas::identity_matrix<double>(r);
00285        lu_substitute(LU,ndx,Ai);
00286        break;
00287      }
00288   }
00289   return Ai;
00290 }
00291 
00292 
00293 int
00294 MyMatrix::convertToSymmetricMatrix(MySymmetricMatrix& sym)
00295 {
00296   // test if matrix is square matrix
00297   assert(this->rows() == this->columns());
00298 
00299   // if necessairy, resize sym
00300   // only check cols or rows. Symmetric matrix is square.
00301   if ( sym.rows() != this->rows() )
00302     sym = MySymmetricMatrix(this->rows());
00303 
00304   // copy elements
00305   for ( unsigned int i=0; i<this->rows(); i++ )
00306     for ( unsigned int j=0; j<=i; j++ )
00307       sym(i+1,j+1) = (*this)(i+1,j+1);
00308   return 0;
00309 }
00310 
00311 void
00312 MyMatrix::resize(unsigned int i, unsigned int j, bool copy, bool initialize)
00313 {
00314   BoostMatrix & temp = (BoostMatrix &) (*this);
00315   temp.resize(i,j,copy);
00316 }
00317 
00318 // get sub matrix
00319 MyMatrix MyMatrix::sub(int i_start, int i_end, int j_start , int j_end) const
00320 {
00321   MyMatrix submatrix(i_end-i_start+1, j_end-j_start+1);
00322   for (int i=i_start; i<=i_end; i++)
00323     for (int j=j_start; j<=j_end; j++)
00324       submatrix(i-i_start+1,j-j_start+1) = (*this)(i,j);
00325 
00326   return submatrix;
00327 }
00328 
00330 // CLASS SYMMETRIC MATRIX  //
00332 
00333 MySymmetricMatrix::SymmetricMatrix() : BoostSymmetricMatrix() {}
00334 MySymmetricMatrix::SymmetricMatrix(int n) : BoostSymmetricMatrix(n) {}
00335 MySymmetricMatrix::SymmetricMatrix(int num_rows,const RowVector& v):BoostSymmetricMatrix(num_rows,v.size()){
00336   BoostSymmetricMatrix & m = *this;
00337   for(unsigned int i=0;i<num_rows;i++)
00338     row(m,i).assign(v);
00339 }
00340 
00341 MyRowVector MySymmetricMatrix::operator[](unsigned int i) const{
00342   return this->rowCopy(i);
00343 }
00344 
00345 
00346 
00347 // Copy constructor
00348 MySymmetricMatrix::SymmetricMatrix(const SymmetricMatrix& a) : BoostSymmetricMatrix(a){}
00349 MySymmetricMatrix::SymmetricMatrix(const BoostSymmetricMatrix & a) : BoostSymmetricMatrix(a){}
00350 
00351 // Destructor
00352 MySymmetricMatrix::~SymmetricMatrix(){}
00353 
00354 // Ask Number of Rows and Columns
00355 unsigned int MySymmetricMatrix::rows() const { return this->size1();}
00356 unsigned int MySymmetricMatrix::columns() const { return this->size2();}
00357 
00358 
00359 MyRowVector MySymmetricMatrix::rowCopy(unsigned int r) const
00360 {
00361   
00362   unsigned int cols = columns();
00363   BoostRowVector temp(cols);
00364   for (unsigned int i=0; i<cols; i++)
00365     temp(i) = (*this)(r,i+1);
00366   return (MyRowVector) temp;
00367 }
00368 
00369 MySymmetricMatrix MySymmetricMatrix::transpose() const {return (*this);}
00370 
00371 MySymmetricMatrix MySymmetricMatrix::inverse() const
00372 {
00373   unsigned int r = this->rows();
00374   assert(r == this->columns());
00375   const BoostMatrix& A = (*this);
00376   BoostSymmetricMatrix Ai(r,r);
00377   switch (r) 
00378   {
00379      case 1:
00380      {
00381         Ai(0,0) = 1/A(0,0);
00382         break;
00383      }
00384      case 2:
00385      {
00386        double det = A(0,0)*A(1,1)-A(0,1)*A(1,0);
00387        Ai(0,0) = A(1,1)/det;
00388        Ai(1,1) = A(0,0)/det;
00389        Ai(0,1) = -A(0,1)/det;
00390        Ai(1,0) = -A(1,0)/det;
00391        break;
00392      }
00393      default:
00394      {
00395        BoostSymmetricMatrix LU(r,r);
00396        boost::numeric::ublas::permutation_matrix<> ndx(r);
00397        noalias(LU) = A;
00398        int res = lu_factorize(LU,ndx);
00399        assert(res == 0);
00400        noalias(Ai) = boost::numeric::ublas::identity_matrix<double>(r);
00401        lu_substitute(LU,ndx,Ai);
00402        break;
00403      }
00404   }
00405   return Ai;
00406 }
00407 
00408 double MySymmetricMatrix::determinant() const
00409 {
00410   unsigned int r = this->rows();
00411   assert(r == this->columns());
00412   const BoostMatrix& A = (*this);
00413   switch (r) 
00414   {
00415      case 1:
00416      {
00417         return A(0,0);
00418      }
00419      case 2:
00420      {
00421        return A(0,0)*A(1,1)-A(0,1)*A(1,0);
00422      }
00423      default:
00424      {
00425         BoostSymmetricMatrix LU(r,r);
00426         boost::numeric::ublas::permutation_matrix<> ndx(r);
00427         noalias(LU) = A;
00428         int res = lu_factorize(LU,ndx);
00429         assert(res == 0);
00430 
00431         double result = 1.0;
00432         int s = 1;
00433         for (boost::numeric::ublas::matrix<double>::size_type i=0;i<LU.size1();++i) {
00434           result *= LU(i,i);
00435           if (ndx(i)!=i) s = -s;
00436         }
00437         return result*s;
00438      }
00439   }
00440 }
00441 
00442 
00443 // Set all elements equal to a
00444 MySymmetricMatrix& MySymmetricMatrix::operator=(const double a)
00445 {
00446   *this = (MySymmetricMatrix)boost::numeric::ublas::scalar_matrix<double>(rows(),columns(),a);
00447 
00448   return *this;
00449 }
00450 
00451 
00452 // SYMMETRICMATRIX - SCALAR operators
00453 MySymmetricMatrix& MySymmetricMatrix::operator +=(double a)
00454 {
00455   BoostSymmetricMatrix & op1 = *this;
00456   op1 += boost::numeric::ublas::scalar_matrix<double>(rows(),columns(),a);
00457   return (MySymmetricMatrix&)op1;
00458 }
00459 
00460 MySymmetricMatrix& MySymmetricMatrix::operator -=(double a)
00461 {
00462   BoostSymmetricMatrix & op1 = *this;
00463   op1 -= boost::numeric::ublas::scalar_matrix<double>(rows(),columns(),a);
00464   return (MySymmetricMatrix&)op1;
00465 }
00466 
00467 MySymmetricMatrix& MySymmetricMatrix::operator *=(double b)
00468 {
00469   BoostSymmetricMatrix & op1 = (*this);
00470   op1 *= b;
00471   return (MySymmetricMatrix&) op1;
00472 }
00473 
00474 MySymmetricMatrix& MySymmetricMatrix::operator /=(double b)
00475 {
00476   BoostSymmetricMatrix & op1 = (*this);
00477   op1 /= b;
00478   return (MySymmetricMatrix&) op1;
00479 }
00480 
00481 MySymmetricMatrix MySymmetricMatrix::operator +(double a) const
00482 {
00483   return (MySymmetricMatrix)(((BoostSymmetricMatrix)(*this)) + boost::numeric::ublas::scalar_matrix<double>(rows(),columns(),a));
00484 }
00485 
00486 MySymmetricMatrix MySymmetricMatrix::operator -(double a) const
00487 {
00488   return (MySymmetricMatrix)(((BoostSymmetricMatrix)(*this)) - boost::numeric::ublas::scalar_matrix<double>(rows(),columns(),a));
00489 }
00490 
00491 MySymmetricMatrix MySymmetricMatrix::operator *(double b) const
00492 {
00493  const BoostSymmetricMatrix& op1 = (*this);
00494   return (MySymmetricMatrix) (op1 *  b);
00495 }
00496 
00497 MySymmetricMatrix MySymmetricMatrix::operator /(double b) const
00498 {
00499   const BoostSymmetricMatrix& op1 = (*this);
00500   return (MySymmetricMatrix) (op1 /  b);
00501 }
00502 
00503 
00504 
00505 
00506 // SYMMETRICMATRIX - MATRIX operators
00507 MyMatrix& MySymmetricMatrix::operator +=(const MyMatrix& a)
00508 {
00509   BoostSymmetricMatrix & op1 = (*this);
00510   op1 += a;
00511   return (MyMatrix &) op1;
00512 }
00513 
00514 MyMatrix& MySymmetricMatrix::operator -=(const MyMatrix& a)
00515 {
00516   BoostSymmetricMatrix & op1 = (*this);
00517   op1 -= a;
00518   return (MyMatrix &) op1;
00519 }
00520 
00521 
00522 MyMatrix MySymmetricMatrix::operator+ (const MyMatrix &a) const
00523 {
00524   const BoostSymmetricMatrix& op1 = *this;
00525   const BoostMatrix& op2 = a;
00526 
00527   return (MyMatrix) (op1 + op2);
00528 }
00529 
00530 MyMatrix MySymmetricMatrix::operator- (const MyMatrix &a) const
00531 {
00532   const BoostSymmetricMatrix& op1 = *this;
00533   const BoostMatrix& op2 = a;
00534 
00535   return (MyMatrix) (op1 - op2);
00536 }
00537 
00538 MyMatrix MySymmetricMatrix::operator* (const MyMatrix &a) const
00539 {
00540   const BoostSymmetricMatrix& op1 = *this;
00541   const BoostMatrix& op2 = a;
00542 
00543   return (MyMatrix) prod(op1, op2);
00544 }
00545 
00546 
00547 
00548 // SYMMETRICMATRIX - SYMMETRICMATRIX operators
00549 MySymmetricMatrix& MySymmetricMatrix::operator +=(const MySymmetricMatrix& a)
00550 {
00551   BoostSymmetricMatrix & op1 = (*this);
00552   const BoostSymmetricMatrix & op2 = a;
00553   op1 += op2;
00554   return (MySymmetricMatrix &) op1;
00555 }
00556 
00557 MySymmetricMatrix& MySymmetricMatrix::operator -=(const MySymmetricMatrix& a)
00558 {
00559   BoostSymmetricMatrix & op1 = (*this);
00560   const BoostSymmetricMatrix & op2 = a;
00561   op1 -= op2;
00562   return (MySymmetricMatrix &) op1;
00563 }
00564 
00565 MySymmetricMatrix MySymmetricMatrix::operator+ (const MySymmetricMatrix &a) const
00566 {
00567   const BoostSymmetricMatrix& op1 = *this;
00568   const BoostSymmetricMatrix& op2 = a;
00569 
00570   return (MySymmetricMatrix) (op1 + op2);
00571 }
00572 
00573 MySymmetricMatrix MySymmetricMatrix::operator- (const MySymmetricMatrix &a) const
00574 {
00575   const BoostSymmetricMatrix& op1 = *this;
00576   const BoostSymmetricMatrix& op2 = a;
00577 
00578   return (MySymmetricMatrix) (op1 - op2);
00579 }
00580 
00581 MyMatrix MySymmetricMatrix::operator* (const MySymmetricMatrix &a) const
00582 {
00583   const BoostSymmetricMatrix& op1 = *this;
00584   const BoostSymmetricMatrix& op2 = a;
00585 
00586   return (MyMatrix) prod(op1, op2);
00587 }
00588 
00589 
00590 
00591 
00592 MyColumnVector MySymmetricMatrix::operator* (const MyColumnVector &b) const
00593 {
00594   const BoostSymmetricMatrix& op1 = (BoostSymmetricMatrix) *this;
00595   return (MyColumnVector) prod(op1, ((const BoostColumnVector&)b));
00596 }
00597 
00598 void MySymmetricMatrix::multiply (const MyColumnVector &b, MyColumnVector &result) const
00599 {
00600   const BoostSymmetricMatrix& op1 = (BoostSymmetricMatrix) *this;
00601   result = (MyColumnVector) prod(op1, ((const BoostColumnVector&)b));
00602 }
00603 
00604 MyMatrix MySymmetricMatrix::sub(int i_start, int i_end, int j_start , int j_end) const
00605 {
00606   MyMatrix submatrix(i_end-i_start+1, j_end-j_start+1);
00607   for (int i=i_start; i<=i_end; i++)
00608     for (int j=j_start; j<=j_end; j++)
00609       submatrix(i-i_start+1,j-j_start+1) = (*this)(i,j);
00610 
00611   return submatrix;
00612 }
00613 
00614 
00615 
00616 double& MySymmetricMatrix::operator()(unsigned int a, unsigned int b)
00617 {
00618   BoostSymmetricMatrix & op1 = (*this);
00619   return op1(a-1,b-1);
00620 }
00621 
00622 double MySymmetricMatrix::operator()(unsigned int a, unsigned int b) const
00623 {
00624   BoostSymmetricMatrix op1(*this);
00625   return op1(a-1,b-1);
00626 }
00627 
00628 bool MySymmetricMatrix::operator==(const MySymmetricMatrix& a) const
00629 {
00630   if (this->rows() != a.rows()) return false;
00631   if (this->columns() != a.columns()) return false;
00632   return(norm_inf((BoostSymmetricMatrix)(*this)-(BoostSymmetricMatrix)a) == 0);
00633 }
00634 
00635 void
00636 MySymmetricMatrix::resize(unsigned int i, bool copy, bool initialize)
00637 {
00638   BoostSymmetricMatrix & temp = (BoostSymmetricMatrix &) (*this);
00639   temp.resize(i, copy);
00640 }
00641 
00642 
00643 #endif


bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Fri Aug 28 2015 10:10:21