00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef CMatrixTemplate_H
00029 #define CMatrixTemplate_H
00030
00031 #include <mrpt/utils/utils_defs.h>
00032
00033
00034 namespace mrpt
00035 {
00036 namespace math
00037 {
00050 template <class T>
00051 class CMatrixTemplate
00052 {
00053 protected:
00054 T **m_Val;
00055 size_t m_Rows, m_Cols;
00056
00059 void realloc(size_t row, size_t col, bool newElementsToZero = false)
00060 {
00061 if (row!=m_Rows || col!=m_Cols || m_Val==NULL)
00062 {
00063 size_t r;
00064 bool doZeroColumns = newElementsToZero && (col>m_Cols);
00065 size_t sizeZeroColumns = sizeof(T)*(col-m_Cols);
00066
00067
00068 for (r=row;r<m_Rows;r++)
00069 {
00070 free( m_Val[r] );
00071 }
00072
00073
00074 if (!row)
00075 { ::free(m_Val); m_Val=NULL; }
00076 else m_Val = static_cast<T**> (::realloc(m_Val, sizeof(T*) * row ) );
00077
00078
00079 size_t row_size = col * sizeof(T);
00080
00081
00082 for (r=0;r<row;r++)
00083 {
00084 if (r<m_Rows)
00085 {
00086
00087 m_Val[r] = static_cast<T*> (::realloc( m_Val[r], row_size ));
00088
00089 if (doZeroColumns)
00090 {
00091
00092 ::memset(&m_Val[r][m_Cols],0,sizeZeroColumns);
00093 }
00094 }
00095 else
00096 {
00097
00098 m_Val[r] = static_cast<T*> (::calloc( row_size, 1 ));
00099 }
00100 }
00101
00102
00103 m_Rows = row;
00104 m_Cols = col;
00105 }
00106 }
00107
00108 public:
00112 void extractCol(size_t nCol, std::vector<T> &out, int startingRow = 0) const
00113 {
00114 size_t i,n;
00115 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
00116 if (nCol>=m_Cols)
00117 THROW_EXCEPTION("extractCol: Column index out of bounds");
00118 #endif
00119
00120 n = m_Rows - startingRow;
00121 out.resize( n );
00122
00123 for (i=0;i<n;i++)
00124 out[i] = m_Val[i+startingRow][nCol];
00125 }
00126
00130 void extractCol(size_t nCol, CMatrixTemplate<T> &out, int startingRow = 0) const
00131 {
00132 size_t i,n;
00133 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00134 if (nCol>=m_Cols)
00135 THROW_EXCEPTION("extractCol: Column index out of bounds");
00136 #endif
00137
00138 n = m_Rows - startingRow;
00139 out.setSize(n,1);
00140
00141 for (i=0;i<n;i++)
00142 out(i,0) = m_Val[i+startingRow][nCol];
00143 }
00144
00148 template <class F>
00149 void extractRow(size_t nRow, std::vector<F> &out, size_t startingCol = 0) const
00150 {
00151 size_t i,n;
00152 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00153 if (nRow>=m_Rows)
00154 THROW_EXCEPTION("extractRow: Row index out of bounds");
00155 #endif
00156 n = m_Cols - startingCol ;
00157 out.resize( n );
00158
00159 for (i=0;i<n;i++)
00160 out[i] = static_cast<F> ( m_Val[nRow][i+startingCol] );
00161 }
00162
00166 template <class F>
00167 void extractRow(size_t nRow, CMatrixTemplate<F> &out, size_t startingCol = 0) const
00168 {
00169 size_t i,n;
00170 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00171 if (nRow>=m_Rows)
00172 THROW_EXCEPTION("extractRow: Row index out of bounds");
00173 #endif
00174 n = m_Cols - startingCol ;
00175 out.setSize(1,n);
00176
00177 for (i=0;i<n;i++)
00178 out(0,i) = static_cast<F>( m_Val[nRow][i+startingCol] );
00179 }
00180
00183 CMatrixTemplate (const CMatrixTemplate& m) : m_Val(NULL),m_Rows(0),m_Cols(0)
00184 {
00185 (*this) = m;
00186 }
00187
00188 CMatrixTemplate (size_t row = 3, size_t col = 3) : m_Val(NULL),m_Rows(0),m_Cols(0)
00189 {
00190 realloc(row,col);
00191 }
00192
00201 template <typename V, size_t N>
00202 CMatrixTemplate (size_t row, size_t col, V (&theArray)[N] ) : m_Val(NULL),m_Rows(0),m_Cols(0)
00203 {
00204 MRPT_COMPILE_TIME_ASSERT(N!=0)
00205 realloc(row,col);
00206
00207 if (m_Rows*m_Cols != N)
00208 {
00209 THROW_EXCEPTION(format("Mismatch between matrix size %"PRIuPTR"x%"PRIuPTR" and array of length %"PRIuPTR,m_Rows,m_Cols,N))
00210 }
00211 size_t idx=0;
00212 for (size_t i=0; i < m_Rows; i++)
00213 for (size_t j=0; j < m_Cols; j++)
00214 m_Val[i][j] = static_cast<T>(theArray[idx++]);
00215
00216 }
00217
00220 virtual ~CMatrixTemplate()
00221 {
00222 realloc(0,0);
00223 }
00224
00227 CMatrixTemplate& operator = (const CMatrixTemplate& m)
00228 {
00229 realloc( m.m_Rows, m.m_Cols );
00230
00231 for (size_t i=0; i < m_Rows; i++)
00232 for (size_t j=0; j < m_Cols; j++)
00233 m_Val[i][j] = m.m_Val[i][j];
00234
00235 return *this;
00236 }
00237
00248 template <typename V, size_t N>
00249 CMatrixTemplate& operator = (V (&theArray)[N] )
00250 {
00251 MRPT_COMPILE_TIME_ASSERT(N!=0)
00252
00253 if (m_Rows*m_Cols != N)
00254 {
00255 THROW_EXCEPTION(format("Mismatch between matrix size %"PRIuPTR"x%"PRIuPTR" and array of length %"PRIuPTR,m_Rows,m_Cols,N))
00256 }
00257 size_t idx=0;
00258 for (size_t i=0; i < m_Rows; i++)
00259 for (size_t j=0; j < m_Cols; j++)
00260 m_Val[i][j] = static_cast<T>(theArray[idx++]);
00261
00262 return *this;
00263 }
00264
00265
00269 inline size_t getRowCount() const
00270 {
00271 return m_Rows;
00272 }
00273
00277 inline size_t getColCount() const
00278 {
00279 return m_Cols;
00280 }
00281
00285 inline size_t nr() const
00286 {
00287 return m_Rows;
00288 }
00289
00293 inline size_t nc() const
00294 {
00295 return m_Cols;
00296 }
00297
00298
00301 void setSize(size_t row, size_t col)
00302 {
00303 realloc(row,col);
00304 }
00305
00308 bool IsSquare () const
00309 {
00310 return ( m_Rows == m_Cols );
00311 }
00312
00315 inline T& operator () (size_t row, size_t col)
00316 {
00317 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00318 if (row >= m_Rows || col >= m_Cols)
00319 THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(m_Rows),static_cast<unsigned long>(m_Cols)) );
00320 #endif
00321 return m_Val[row][col];
00322 }
00323
00326 inline T operator () (size_t row, size_t col) const
00327 {
00328 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00329 if (row >= m_Rows || col >= m_Cols)
00330 THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(m_Rows),static_cast<unsigned long>(m_Cols)) );
00331 #endif
00332 return m_Val[row][col];
00333 }
00334
00338 inline T& operator () (size_t ith)
00339 {
00340 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00341 ASSERT_(m_Rows==1 || m_Cols==1);
00342 #endif
00343 if (m_Rows==1)
00344 {
00345
00346 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00347 if (ith >= m_Cols)
00348 THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
00349 #endif
00350 return m_Val[0][ith];
00351 }
00352 else
00353 {
00354
00355 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00356 if (ith >= m_Rows)
00357 THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
00358 #endif
00359 return m_Val[ith][0];
00360 }
00361 }
00362
00366 inline T operator () (size_t ith) const
00367 {
00368 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00369 ASSERT_(m_Rows==1 || m_Cols==1);
00370 #endif
00371 if (m_Rows==1)
00372 {
00373
00374 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00375 if (ith >= m_Cols)
00376 THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
00377 #endif
00378 return m_Val[0][ith];
00379 }
00380 else
00381 {
00382
00383 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00384 if (ith >= m_Rows)
00385 THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
00386 #endif
00387 return m_Val[ith][0];
00388 }
00389 }
00390
00393 inline void set_unsafe(size_t row, size_t col,const T &v)
00394 {
00395 m_Val[row][col] = v;
00396 }
00397
00400 inline T get_unsafe(size_t row, size_t col) const
00401 {
00402 return m_Val[row][col];
00403 }
00404
00407 inline T* get_unsafe_row(size_t row) const
00408 {
00409 return m_Val[row];
00410 }
00411
00416 void insertRow(size_t nRow, const std::vector<T> &in)
00417 {
00418 if (nRow>=m_Rows) THROW_EXCEPTION("insertRow: Row index out of bounds");
00419
00420 size_t n = in.size();
00421 ASSERT_(m_Cols>=in.size());
00422
00423 for (size_t i=0;i<n;i++)
00424 m_Val[nRow][i] = in[i];
00425 }
00426
00439 void appendRow(const std::vector<T> &in)
00440 {
00441 size_t i,n, row;
00442
00443 n = m_Cols;
00444 row = m_Rows;
00445
00446 if (m_Cols==0 || m_Rows==0)
00447 {
00448 ASSERT_(!in.empty());
00449 n=m_Cols=in.size();
00450 }
00451 else
00452 {
00453 ASSERT_(in.size()==m_Cols);
00454 }
00455
00456 realloc( row+1,n );
00457
00458 for (i=0;i<n;i++)
00459 m_Val[row][i] = in[i];
00460 }
00461
00466 void insertCol(size_t nCol, const std::vector<T> &in)
00467 {
00468 if (nCol>=m_Cols) THROW_EXCEPTION("insertCol: Row index out of bounds");
00469
00470 size_t n = in.size();
00471 ASSERT_( m_Rows >= in.size() );
00472
00473 for (size_t i=0;i<n;i++)
00474 m_Val[i][nCol] = in[i];
00475 }
00476
00485 template <class R>
00486 void insertMatrix(size_t nRow, size_t nCol, const CMatrixTemplate<R> &in)
00487 {
00488 size_t i,j,ncols,nrows;
00489
00490 nrows = in.m_Rows;
00491 ncols = in.m_Cols;
00492 if ( (nRow+nrows > m_Rows) || (nCol+ncols >m_Cols) )
00493 THROW_EXCEPTION("insertMatrix: Row or Col index out of bounds");
00494
00495 for (i=nRow;i<nRow+nrows;i++)
00496 for(j=nCol;j<nCol+ncols;j++)
00497 set_unsafe(i,j, static_cast<T> (in.get_unsafe(i-nRow,j-nCol) ) );
00498 }
00499
00508 void insertMatrixTranspose(size_t nRow, size_t nCol, const CMatrixTemplate<T> &in)
00509 {
00510 size_t i,j,ncols,nrows;
00511
00512 ncols = in.m_Rows;
00513 nrows = in.m_Cols;
00514 if ( (nRow+nrows > m_Rows) || (nCol+ncols >m_Cols) )
00515 THROW_EXCEPTION("insertMatrix: Row or Col index out of bounds");
00516
00517 for (i=nRow;i<nRow+nrows;i++)
00518 for(j=nCol;j<nCol+ncols;j++)
00519 set_unsafe(i,j, in.get_unsafe(j-nCol,i-nRow) );
00520 }
00521
00522
00529 void insertMatrix(size_t nRow, size_t nCol, const std::vector<T> &in)
00530 {
00531 size_t j,ncols;
00532
00533 ncols = in.size();
00534 if ( (nRow+1 > m_Rows) || (nCol+ncols >m_Cols) )
00535 THROW_EXCEPTION("insertMatrix: Row or Col index out of bounds");
00536
00537 for(j = nCol ; j < nCol + ncols ; j++)
00538 set_unsafe(nRow,j, in[j-nCol] );
00539 }
00540
00545 void joinMatrix(const CMatrixTemplate<T> &left_up, const CMatrixTemplate<T> &right_up,
00546 const CMatrixTemplate<T> &left_down, const CMatrixTemplate<T> &right_down)
00547 {
00548 if ((left_up.getRowCount()!= right_up.getRowCount())||(left_up.getColCount()!=left_down.getColCount())||
00549 (left_down.getRowCount()!=right_down.getRowCount())||(right_up.getColCount()!=right_down.getColCount()))
00550 THROW_EXCEPTION("join_Matrix: Row or Col index out of bounds");
00551 setSize(left_up.getRowCount()+left_down.getRowCount(),left_up.getColCount()+right_up.getColCount());
00552 insertMatrix(0,0,left_up);
00553 insertMatrix(0,left_up.getColCount(),right_up);
00554 insertMatrix(left_up.getRowCount(),0,left_down);
00555 insertMatrix(left_up.getRowCount(),left_up.getColCount(),right_down);
00556 }
00557
00560 void fill( const T &val)
00561 {
00562 for (size_t r=0;r<m_Rows;r++)
00563 for (size_t c=0;c<m_Cols;c++)
00564 m_Val[r][c]= val;
00565 }
00566
00569 void extractSubmatrix(const size_t row1,const size_t row2,const size_t col1,const size_t col2,CMatrixTemplate<T> &out) const {
00570 size_t nrows=row2-row1+1;
00571 size_t ncols=col2-col1+1;
00572 if (nrows<=0||ncols<=0) {
00573 out.realloc(0,0);
00574 return;
00575 }
00576 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00577 if (row1<0||row2>=m_Rows||col1<0||col2>=m_Cols) THROW_EXCEPTION("Indices out of range!");
00578 #endif
00579 out.realloc(nrows,ncols);
00580 for (size_t i=0;i<nrows;i++) for (size_t j=0;j<ncols;j++) out.m_Val[i][j]=m_Val[i+row1][j+col1];
00581 }
00582
00585 inline CMatrixTemplate<T> operator() (const size_t row1,const size_t row2,const size_t col1,const size_t col2) const {
00586 CMatrixTemplate<T> val(0,0);
00587 extractSubmatrix(row1,row2,col1,col2,val);
00588 return val;
00589 }
00590
00593 void exchangeColumns(size_t col1,size_t col2) {
00594 #if defined(_DEBUG)||(MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00595 if (col1<0||col2<0||col1>=m_Cols||col2>=m_Cols) THROW_EXCEPTION("Indices out of range!");
00596 #endif
00597 T tmp;
00598 for (size_t i=0;i<m_Rows;i++) {
00599 tmp=m_Val[i][col1];
00600 m_Val[i][col1]=m_Val[i][col2];
00601 m_Val[i][col2]=tmp;
00602 }
00603 }
00604
00607 void exchangeRows(size_t row1,size_t row2) {
00608 #if defined(_DEBUG)||(MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00609 if (row1<0||row2<0||row1>=m_Rows||row2>=m_Rows) THROW_EXCEPTION("Indices out of range!");
00610 #endif
00611 T tmp;
00612 for (size_t i=0;i<m_Cols;i++) {
00613 tmp=m_Val[row1][i];
00614 m_Val[row1][i]=m_Val[row2][i];
00615 m_Val[row2][i]=tmp;
00616 }
00617 }
00618
00621 void deleteRow(size_t row) {
00622 #if defined(_DEBUG)||(MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00623 if (row<0||row>=m_Rows) THROW_EXCEPTION("Index out of range!");
00624 #endif
00625 for (size_t i=row;i<m_Rows-1;i++) for (size_t j=0;j<m_Cols;j++) m_Val[i][j]=m_Val[i+1][j];
00626 realloc(m_Rows-1,m_Cols);
00627 }
00628
00631 void deleteColumn(size_t col) {
00632 #if defined(_DEBUG)||(MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00633 if (col<0||col>=m_Cols) THROW_EXCEPTION("Index out of range!");
00634 #endif
00635 for (size_t i=0;i<m_Rows;i++) for (size_t j=col;j<m_Cols-1;j++) m_Val[i][j]=m_Val[i][j+1];
00636 realloc(m_Rows,m_Cols-1);
00637 }
00638
00644 template <class R>
00645 void extractMatrix(size_t nRow, size_t nCol, CMatrixTemplate<R> &in) const
00646 {
00647 size_t i,j,ncols,nrows;
00648
00649 nrows = in.getRowCount();
00650 ncols = in.getColCount();
00651 if ( (nRow+nrows > CMatrixTemplate<T>::getRowCount()) || (nCol+ncols >CMatrixTemplate<T>::getColCount()) )
00652 THROW_EXCEPTION("extractMatrix: Row or Col index out of bounds");
00653
00654 for (i=nRow;i<nRow+nrows;i++)
00655 for(j=nCol;j<nCol+ncols;j++)
00656 in.set_unsafe( i-nRow,j-nCol, static_cast<R> ( CMatrixTemplate<T>::m_Val[i][j] ) );
00657 }
00658
00664 void extractMatrix(size_t nRow, size_t nCol,size_t nrows, size_t ncols, CMatrixTemplate<T> &outMat) const
00665 {
00666 size_t i,j;
00667 outMat.setSize(nrows,ncols);
00668
00669 if ( (nRow+nrows > CMatrixTemplate<T>::getRowCount()) || (nCol+ncols >CMatrixTemplate<T>::getColCount()) )
00670 THROW_EXCEPTION("extractMatrix: Row or Col index out of bounds");
00671
00672 for (i=nRow;i<(nRow+nrows);i++)
00673 for(j=nCol;j<(nCol+ncols);j++)
00674 outMat(i-nRow,j-nCol) = CMatrixTemplate<T>::m_Val[i][j];
00675 }
00676
00682 void extractMatrix(size_t nRow, size_t nCol, std::vector<T> &in) const
00683 {
00684 size_t j,ncols,nrows;
00685
00686 ncols = in.size();
00687 nrows = 1;
00688 if ( (nRow+nrows > CMatrixTemplate<T>::getRowCount()) || (nCol+ncols >CMatrixTemplate<T>::getColCount()) )
00689 THROW_EXCEPTION("extractMatrix: Row or Col index out of bounds");
00690
00691 for(j=nCol;j<nCol+ncols;j++)
00692 in[j-nCol] = CMatrixTemplate<T>::m_Val[nRow][j];
00693 }
00694
00700 std::vector<T> extractMatrix(size_t nRow, size_t nCol, size_t ncols) const
00701 {
00702 size_t j;
00703 std::vector<T> out;
00704 out.resize(ncols);
00705
00706 if ( (nRow+1 > CMatrixTemplate<T>::getRowCount()) || (nCol+ncols >CMatrixTemplate<T>::getColCount()) )
00707 THROW_EXCEPTION("extractMatrix: Row or Col index out of bounds");
00708
00709 for(j=nCol;j<nCol+ncols;j++)
00710 out[j-nCol] = CMatrixTemplate<T>::m_Val[nRow][j];
00711 return out;
00712 }
00713
00714
00719 std::string inMatlabFormat() const
00720 {
00721 std::stringstream s;
00722
00723 s << "[";
00724 s << std::scientific;
00725 for (size_t i=0;i<m_Rows;i++)
00726 {
00727 for (size_t j=0;j<m_Cols;j++)
00728 s << m_Val[i][j] << " ";
00729
00730 if (i<m_Rows-1) s << ";";
00731 }
00732 s << "]";
00733
00734 return s.str();
00735 }
00736
00737
00749 bool fromMatlabStringFormat(const std::string &s)
00750 {
00751 this->setSize(0,0);
00752
00753
00754 size_t ini = s.find_first_not_of(" \t");
00755 if (ini==std::string::npos || s[ini]!='[') return false;
00756
00757 size_t end = s.find_last_not_of(" \t");
00758 if (end==std::string::npos || s[end]!=']') return false;
00759
00760 if (ini>end) return false;
00761
00762 std::vector<T> lstElements;
00763
00764 size_t i = ini+1;
00765 size_t nRow = 0;
00766
00767 while (i<end)
00768 {
00769
00770 size_t end_row = s.find_first_of(";]",i);
00771 if (end_row==std::string::npos) { setSize(0,0); return false; }
00772
00773
00774 std::stringstream ss (s.substr(i, end_row-i ));
00775 lstElements.clear();
00776 try
00777 {
00778 while (!ss.eof())
00779 {
00780 T val;
00781 ss >> val;
00782 if (ss.bad() || ss.fail()) break;
00783 lstElements.push_back(val);
00784 }
00785 } catch (...) { }
00786
00787
00788 if (lstElements.empty())
00789 {
00790 if (nRow>0) { setSize(0,0); return false; }
00791
00792 }
00793 else
00794 {
00795 const size_t N = lstElements.size();
00796
00797
00798 if (nRow>0 && m_Cols!=N) { setSize(0,0); return false; }
00799
00800
00801 realloc( nRow+1,N );
00802
00803 for (size_t q=0;q<N;q++)
00804 m_Val[nRow][q] = lstElements[q];
00805
00806
00807 nRow++;
00808 }
00809
00810 i = end_row+1;
00811 }
00812 return true;
00813 }
00814
00815
00816
00817 };
00818
00822 template <class T>
00823 std::ostream& operator << (std::ostream& ostrm, const CMatrixTemplate<T>& m)
00824 {
00825 ostrm << std::setprecision(4);
00826
00827 for (size_t i=0; i < m.getRowCount(); i++)
00828 {
00829 for (size_t j=0; j < m.getColCount(); j++)
00830 {
00831 ostrm << std::setw(13) << m(i,j);
00832 }
00833 ostrm << std::endl;
00834 }
00835 return ostrm;
00836 }
00837
00839 template <class T>
00840 size_t size( const CMatrixTemplate<T>& m, int dim )
00841 {
00842 if (dim==1)
00843 return m.getRowCount();
00844 else if (dim==2)
00845 return m.getColCount();
00846 else THROW_EXCEPTION_CUSTOM_MSG1("size: Matrix dimensions are 1 & 2 only (called with i=%i)",dim);
00847 }
00848
00849
00850 }
00851 }
00852
00853 #endif