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 {
00038
00039 template <typename U> U myStaticCast(double val) { return static_cast<U>(val); }
00040 template <> bool myStaticCast(double val);
00041
00042
00043
00044 enum TMatrixTextFileFormat
00045 {
00046 MATRIX_FORMAT_ENG = 0,
00047 MATRIX_FORMAT_FIXED = 1,
00048 MATRIX_FORMAT_INT = 2
00049 };
00050
00051
00052 template <class T> class CMatrixTemplate;
00053 template <typename T,size_t NROWS,size_t NCOLS> class CMatrixFixedNumeric;
00054 template <class MAT> void saveMatrixToTextFile(const MAT &theMatrix, const std::string &file, TMatrixTextFileFormat fileFormat = MATRIX_FORMAT_ENG, bool appendMRPTHeader = false, const std::string &userHeader = std::string("") );
00055 template <typename T,size_t NROWS,size_t NCOLS> void insertMatrixFixTransposeIntoDyn(CMatrixTemplate<T> &M,const size_t nRow,const size_t nCol,const CMatrixFixedNumeric<T,NROWS,NCOLS> &in);
00056 template <typename T,size_t NROWS,size_t NCOLS> void insertMatrixFixIntoDyn(CMatrixTemplate<T> &M,const size_t nRow,const size_t nCol,const CMatrixFixedNumeric<T,NROWS,NCOLS> &in);
00057 template <typename T,size_t NROWS,size_t NCOLS> void extractFixMatrixFromDynMatrix(const CMatrixTemplate<T> &M,const size_t nRow,const size_t nCol, CMatrixFixedNumeric<T,NROWS,NCOLS> &outMat);
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073 template <class T>
00074 class CMatrixTemplate
00075 {
00076 public:
00077 typedef T value_type;
00078
00079 protected:
00080 T **m_Val;
00081 size_t m_Rows, m_Cols;
00082
00083
00084
00085 void realloc(size_t row, size_t col, bool newElementsToZero = false)
00086 {
00087 if (row!=m_Rows || col!=m_Cols || m_Val==NULL)
00088 {
00089 size_t r;
00090 bool doZeroColumns = newElementsToZero && (col>m_Cols);
00091 size_t sizeZeroColumns = sizeof(T)*(col-m_Cols);
00092
00093
00094 for (r=row;r<m_Rows;r++)
00095 {
00096 mrpt::system::os::aligned_free( m_Val[r] );
00097 }
00098
00099
00100 if (!row)
00101 { mrpt::system::os::aligned_free(m_Val); m_Val=NULL; }
00102 else m_Val = static_cast<T**> (mrpt::system::os::aligned_realloc(m_Val, sizeof(T*) * row, 16 ) );
00103
00104
00105 size_t row_size = col * sizeof(T);
00106
00107
00108 for (r=0;r<row;r++)
00109 {
00110 if (r<m_Rows)
00111 {
00112
00113 m_Val[r] = static_cast<T*> (mrpt::system::os::aligned_realloc( m_Val[r], row_size, 16));
00114
00115 if (doZeroColumns)
00116 {
00117
00118 ::memset(&m_Val[r][m_Cols],0,sizeZeroColumns);
00119 }
00120 }
00121 else
00122 {
00123
00124 m_Val[r] = static_cast<T*> ( mrpt::system::os::aligned_calloc( row_size, 16 ));
00125 }
00126 }
00127
00128
00129 m_Rows = row;
00130 m_Cols = col;
00131 }
00132 }
00133
00134 public:
00135
00136
00137
00138 void extractCol(size_t nCol, std::vector<T> &out, int startingRow = 0) const
00139 {
00140 size_t i,n;
00141 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00142 if (nCol>=m_Cols)
00143 THROW_EXCEPTION("extractCol: Column index out of bounds");
00144 #endif
00145
00146 n = m_Rows - startingRow;
00147 out.resize( n );
00148
00149 for (i=0;i<n;i++)
00150 out[i] = m_Val[i+startingRow][nCol];
00151 }
00152
00153
00154
00155
00156 void extractCol(size_t nCol, CMatrixTemplate<T> &out, int startingRow = 0) const
00157 {
00158 size_t i,n;
00159 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00160 if (nCol>=m_Cols)
00161 THROW_EXCEPTION("extractCol: Column index out of bounds");
00162 #endif
00163
00164 n = m_Rows - startingRow;
00165 out.setSize(n,1);
00166
00167 for (i=0;i<n;i++)
00168 out(i,0) = m_Val[i+startingRow][nCol];
00169 }
00170
00171
00172
00173
00174 void swapCols(size_t nCol1, size_t nCol2)
00175 {
00176 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00177 if (nCol1>=m_Cols || nCol2>=m_Cols)
00178 THROW_EXCEPTION("swapCols: Column index out of bounds");
00179 #endif
00180 const size_t n = m_Rows;
00181 for (size_t i=0;i<n;i++)
00182 std::swap( m_Val[i][nCol1], m_Val[i][nCol2] );
00183 }
00184
00185
00186
00187
00188 void swapRows(size_t nRow1, size_t nRow2)
00189 {
00190 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00191 if (nRow1>=m_Rows || nRow2>=m_Rows)
00192 THROW_EXCEPTION("swapCols: Row index out of bounds");
00193 #endif
00194 const size_t n = m_Cols;
00195 for (size_t i=0;i<n;i++)
00196 std::swap( m_Val[nRow1][i], m_Val[nRow2][i] );
00197 }
00198
00199
00200
00201
00202 template <class F>
00203 void extractRow(size_t nRow, std::vector<F> &out, size_t startingCol = 0) const
00204 {
00205 size_t i,n;
00206 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00207 if (nRow>=m_Rows)
00208 THROW_EXCEPTION("extractRow: Row index out of bounds");
00209 #endif
00210 n = m_Cols - startingCol ;
00211 out.resize( n );
00212
00213 for (i=0;i<n;i++)
00214 out[i] = static_cast<F> ( m_Val[nRow][i+startingCol] );
00215 }
00216
00217
00218
00219
00220 template <class F>
00221 void extractRow(size_t nRow, CMatrixTemplate<F> &out, size_t startingCol = 0) const
00222 {
00223 size_t i,n;
00224 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00225 if (nRow>=m_Rows)
00226 THROW_EXCEPTION("extractRow: Row index out of bounds");
00227 #endif
00228 n = m_Cols - startingCol ;
00229 out.setSize(1,n);
00230
00231 for (i=0;i<n;i++)
00232 out(0,i) = static_cast<F>( m_Val[nRow][i+startingCol] );
00233 }
00234
00235
00236
00237 CMatrixTemplate (const CMatrixTemplate& m) : m_Val(NULL),m_Rows(0),m_Cols(0)
00238 {
00239 (*this) = m;
00240 }
00241
00242 CMatrixTemplate (size_t row = 3, size_t col = 3) : m_Val(NULL),m_Rows(0),m_Cols(0)
00243 {
00244 realloc(row,col);
00245 }
00246
00247
00248
00249 CMatrixTemplate (const CMatrixTemplate& m, const size_t cropRowCount, const size_t cropColCount) : m_Val(NULL),m_Rows(0),m_Cols(0)
00250 {
00251 ASSERT_(m.m_Rows>=cropRowCount)
00252 ASSERT_(m.m_Cols>=cropColCount)
00253
00254 realloc( cropRowCount, cropColCount );
00255
00256 for (size_t i=0; i < m_Rows; i++)
00257 for (size_t j=0; j < m_Cols; j++)
00258 m_Val[i][j] = m.m_Val[i][j];
00259 }
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269 template <typename V, size_t N>
00270 CMatrixTemplate (size_t row, size_t col, V (&theArray)[N] ) : m_Val(NULL),m_Rows(0),m_Cols(0)
00271 {
00272 MRPT_COMPILE_TIME_ASSERT(N!=0)
00273 realloc(row,col);
00274
00275 if (m_Rows*m_Cols != N)
00276 {
00277 THROW_EXCEPTION(format("Mismatch between matrix size %"PRIuPTR"x%"PRIuPTR" and array of length %"PRIuPTR,m_Rows,m_Cols,N))
00278 }
00279 size_t idx=0;
00280 for (size_t i=0; i < m_Rows; i++)
00281 for (size_t j=0; j < m_Cols; j++)
00282 m_Val[i][j] = static_cast<T>(theArray[idx++]);
00283
00284 }
00285
00286
00287
00288 template <typename V>
00289 CMatrixTemplate(size_t row, size_t col, const V &theVector ) : m_Val(NULL),m_Rows(0),m_Cols(0)
00290 {
00291 const size_t N = theVector.size();
00292 realloc(row,col);
00293
00294 if (m_Rows*m_Cols != N)
00295 {
00296 THROW_EXCEPTION(format("Mismatch between matrix size %"PRIuPTR"x%"PRIuPTR" and container of length %"PRIuPTR,m_Rows,m_Cols,N))
00297 }
00298 typename V::const_iterator it = theVector.begin();
00299 for (size_t i=0; i < m_Rows; i++)
00300 for (size_t j=0; j < m_Cols; j++)
00301 m_Val[i][j] = static_cast<T>( *(it++) );
00302
00303 }
00304
00305
00306
00307 virtual ~CMatrixTemplate()
00308 {
00309 realloc(0,0);
00310 }
00311
00312
00313
00314 CMatrixTemplate& operator = (const CMatrixTemplate& m)
00315 {
00316 realloc( m.m_Rows, m.m_Cols );
00317
00318 for (size_t i=0; i < m_Rows; i++)
00319 for (size_t j=0; j < m_Cols; j++)
00320 m_Val[i][j] = m.m_Val[i][j];
00321
00322 return *this;
00323 }
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335 template <typename V, size_t N>
00336 CMatrixTemplate& operator = (V (&theArray)[N] )
00337 {
00338 MRPT_COMPILE_TIME_ASSERT(N!=0)
00339
00340 if (m_Rows*m_Cols != N)
00341 {
00342 THROW_EXCEPTION(format("Mismatch between matrix size %"PRIuPTR"x%"PRIuPTR" and array of length %"PRIuPTR,m_Rows,m_Cols,N))
00343 }
00344 size_t idx=0;
00345 for (size_t i=0; i < m_Rows; i++)
00346 for (size_t j=0; j < m_Cols; j++)
00347 m_Val[i][j] = static_cast<T>(theArray[idx++]);
00348
00349 return *this;
00350 }
00351
00352
00353
00354
00355
00356 inline size_t getRowCount() const
00357 {
00358 return m_Rows;
00359 }
00360
00361
00362
00363
00364 inline size_t getColCount() const
00365 {
00366 return m_Cols;
00367 }
00368
00369
00370
00371 void setSize(size_t row, size_t col)
00372 {
00373 realloc(row,col);
00374 }
00375
00376
00377
00378 inline bool IsSquare () const
00379 {
00380 return ( m_Rows == m_Cols );
00381 }
00382
00383
00384
00385 inline T& operator () (size_t row, size_t col)
00386 {
00387 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00388 if (row >= m_Rows || col >= m_Cols)
00389 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)) );
00390 #endif
00391 return m_Val[row][col];
00392 }
00393
00394
00395
00396 inline T operator () (size_t row, size_t col) const
00397 {
00398 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00399 if (row >= m_Rows || col >= m_Cols)
00400 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)) );
00401 #endif
00402 return m_Val[row][col];
00403 }
00404
00405
00406
00407
00408 inline T& operator () (size_t ith)
00409 {
00410 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00411 ASSERT_(m_Rows==1 || m_Cols==1);
00412 #endif
00413 if (m_Rows==1)
00414 {
00415
00416 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00417 if (ith >= m_Cols)
00418 THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
00419 #endif
00420 return m_Val[0][ith];
00421 }
00422 else
00423 {
00424
00425 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00426 if (ith >= m_Rows)
00427 THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
00428 #endif
00429 return m_Val[ith][0];
00430 }
00431 }
00432
00433
00434
00435
00436 inline T operator () (size_t ith) const
00437 {
00438 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00439 ASSERT_(m_Rows==1 || m_Cols==1);
00440 #endif
00441 if (m_Rows==1)
00442 {
00443
00444 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00445 if (ith >= m_Cols)
00446 THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
00447 #endif
00448 return m_Val[0][ith];
00449 }
00450 else
00451 {
00452
00453 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00454 if (ith >= m_Rows)
00455 THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
00456 #endif
00457 return m_Val[ith][0];
00458 }
00459 }
00460
00461
00462
00463 inline void set_unsafe(size_t row, size_t col,const T &v)
00464 {
00465 m_Val[row][col] = v;
00466 }
00467
00468
00469
00470 inline T get_unsafe(size_t row, size_t col) const
00471 {
00472 return m_Val[row][col];
00473 }
00474
00475
00476
00477 inline T &get_unsafe(size_t row,size_t col) {
00478 return m_Val[row][col];
00479 }
00480
00481
00482
00483 inline T* get_unsafe_row(size_t row) const
00484 {
00485 return m_Val[row];
00486 }
00487
00488
00489
00490
00491
00492
00493 inline void extractRows(size_t firstRow,size_t lastRow,CMatrixTemplate<T> &out) const {
00494 extractMatrix(firstRow,0,lastRow-firstRow+1,m_Cols,out);
00495 }
00496
00497
00498
00499
00500
00501
00502 inline void extractColumns(size_t firstCol,size_t lastCol,CMatrixTemplate<T> &out) const {
00503 extractMatrix(0,firstCol,m_Rows,lastCol-firstCol+1,out);
00504 }
00505
00506
00507
00508
00509
00510 void insertRow(size_t nRow, const std::vector<T> &in)
00511 {
00512 if (nRow>=m_Rows) THROW_EXCEPTION("insertRow: Row index out of bounds");
00513
00514 size_t n = in.size();
00515 ASSERT_(m_Cols>=in.size());
00516
00517 for (size_t i=0;i<n;i++)
00518 m_Val[nRow][i] = in[i];
00519 }
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534 void appendRow(const std::vector<T> &in)
00535 {
00536 size_t i,n, row;
00537
00538 n = m_Cols;
00539 row = m_Rows;
00540
00541 if (m_Cols==0 || m_Rows==0)
00542 {
00543 ASSERT_(!in.empty());
00544 n=m_Cols=in.size();
00545 }
00546 else
00547 {
00548 ASSERT_(in.size()==m_Cols);
00549 }
00550
00551 realloc( row+1,n );
00552
00553 for (i=0;i<n;i++)
00554 m_Val[row][i] = in[i];
00555 }
00556
00557
00558
00559
00560
00561
00562
00563 void appendCol(const std::vector<T> &in) {
00564 size_t r=m_Rows,c=m_Cols;
00565 if (m_Cols==0||m_Rows==0) {
00566 ASSERT_(!in.empty());
00567 r=in.size();
00568 c=0;
00569 } else ASSERT_(in.size()==m_Rows);
00570 realloc(r,c+1);
00571 for (size_t i=0;i<m_Rows;i++) m_Val[i][m_Cols-1]=in[i];
00572 }
00573
00574
00575
00576
00577
00578 void insertCol(size_t nCol, const std::vector<T> &in)
00579 {
00580 if (nCol>=m_Cols) THROW_EXCEPTION("insertCol: Row index out of bounds");
00581
00582 size_t n = in.size();
00583 ASSERT_( m_Rows >= in.size() );
00584
00585 for (size_t i=0;i<n;i++)
00586 m_Val[i][nCol] = in[i];
00587 }
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597 template <class R>
00598 void insertMatrix(const size_t nRow,const size_t nCol, const CMatrixTemplate<R> &in)
00599 {
00600 size_t i,j,ncols,nrows;
00601
00602 nrows = in.m_Rows;
00603 ncols = in.m_Cols;
00604 if ( (nRow+nrows > m_Rows) || (nCol+ncols >m_Cols) )
00605 THROW_EXCEPTION("insertMatrix: Row or Col index out of bounds");
00606
00607 for (i=nRow;i<nRow+nrows;i++)
00608 for(j=nCol;j<nCol+ncols;j++)
00609 set_unsafe(i,j, static_cast<T> (in.get_unsafe(i-nRow,j-nCol) ) );
00610 }
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620 void insertMatrixTranspose(const size_t nRow,const size_t nCol, const CMatrixTemplate<T> &in)
00621 {
00622 size_t i,j,ncols,nrows;
00623
00624 ncols = in.m_Rows;
00625 nrows = in.m_Cols;
00626 if ( (nRow+nrows > m_Rows) || (nCol+ncols >m_Cols) )
00627 THROW_EXCEPTION("insertMatrix: Row or Col index out of bounds");
00628
00629 for (i=nRow;i<nRow+nrows;i++)
00630 for(j=nCol;j<nCol+ncols;j++)
00631 set_unsafe(i,j, in.get_unsafe(j-nCol,i-nRow) );
00632 }
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642 template <size_t NROWS,size_t NCOLS>
00643 void insertMatrix(const size_t nRow, const size_t nCol, const CMatrixFixedNumeric<T,NROWS,NCOLS> &in) {
00644 mrpt::math::insertMatrixFixIntoDyn(*this,nRow,nCol,in);
00645 }
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655 template <size_t NROWS,size_t NCOLS>
00656 void insertMatrixTranspose(const size_t nRow,const size_t nCol, const CMatrixFixedNumeric<T,NROWS,NCOLS> &in) {
00657 mrpt::math::insertMatrixFixTransposeIntoDyn(*this,nRow,nCol,in);
00658 }
00659
00660
00661
00662
00663
00664
00665
00666 void insertMatrix(size_t nRow, size_t nCol, const std::vector<T> &in)
00667 {
00668 size_t j,ncols;
00669
00670 ncols = in.size();
00671 if ( (nRow+1 > m_Rows) || (nCol+ncols >m_Cols) )
00672 THROW_EXCEPTION("insertMatrix: Row or Col index out of bounds");
00673
00674 for(j = nCol ; j < nCol + ncols ; j++)
00675 set_unsafe(nRow,j, in[j-nCol] );
00676 }
00677
00678
00679
00680
00681
00682 void joinMatrix(const CMatrixTemplate<T> &left_up, const CMatrixTemplate<T> &right_up,
00683 const CMatrixTemplate<T> &left_down, const CMatrixTemplate<T> &right_down)
00684 {
00685 if ((left_up.getRowCount()!= right_up.getRowCount())||(left_up.getColCount()!=left_down.getColCount())||
00686 (left_down.getRowCount()!=right_down.getRowCount())||(right_up.getColCount()!=right_down.getColCount()))
00687 THROW_EXCEPTION("join_Matrix: Row or Col index out of bounds");
00688 setSize(left_up.getRowCount()+left_down.getRowCount(),left_up.getColCount()+right_up.getColCount());
00689 insertMatrix(0,0,left_up);
00690 insertMatrix(0,left_up.getColCount(),right_up);
00691 insertMatrix(left_up.getRowCount(),0,left_down);
00692 insertMatrix(left_up.getRowCount(),left_up.getColCount(),right_down);
00693 }
00694
00695
00696
00697 void fill( const T &val)
00698 {
00699 for (size_t r=0;r<m_Rows;r++)
00700 for (size_t c=0;c<m_Cols;c++)
00701 m_Val[r][c]= val;
00702 }
00703
00704
00705
00706 void extractSubmatrix(const size_t row1,const size_t row2,const size_t col1,const size_t col2,CMatrixTemplate<T> &out) const {
00707 size_t nrows=row2-row1+1;
00708 size_t ncols=col2-col1+1;
00709 if (nrows<=0||ncols<=0) {
00710 out.realloc(0,0);
00711 return;
00712 }
00713 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00714 if (row1<0||row2>=m_Rows||col1<0||col2>=m_Cols) THROW_EXCEPTION("Indices out of range!");
00715 #endif
00716 out.realloc(nrows,ncols);
00717 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];
00718 }
00719
00720
00721
00722 inline CMatrixTemplate<T> operator() (const size_t row1,const size_t row2,const size_t col1,const size_t col2) const {
00723 CMatrixTemplate<T> val(0,0);
00724 extractSubmatrix(row1,row2,col1,col2,val);
00725 return val;
00726 }
00727
00728
00729
00730 void exchangeColumns(size_t col1,size_t col2) {
00731 #if defined(_DEBUG)||(MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00732 if (col1<0||col2<0||col1>=m_Cols||col2>=m_Cols) THROW_EXCEPTION("Indices out of range!");
00733 #endif
00734 T tmp;
00735 for (size_t i=0;i<m_Rows;i++) {
00736 tmp=m_Val[i][col1];
00737 m_Val[i][col1]=m_Val[i][col2];
00738 m_Val[i][col2]=tmp;
00739 }
00740 }
00741
00742
00743
00744 void exchangeRows(size_t row1,size_t row2) {
00745 #if defined(_DEBUG)||(MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00746 if (row1<0||row2<0||row1>=m_Rows||row2>=m_Rows) THROW_EXCEPTION("Indices out of range!");
00747 #endif
00748 T tmp;
00749 for (size_t i=0;i<m_Cols;i++) {
00750 tmp=m_Val[row1][i];
00751 m_Val[row1][i]=m_Val[row2][i];
00752 m_Val[row2][i]=tmp;
00753 }
00754 }
00755
00756
00757
00758 void deleteRow(size_t row) {
00759 #if defined(_DEBUG)||(MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00760 if (row<0||row>=m_Rows) THROW_EXCEPTION("Index out of range!");
00761 #endif
00762 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];
00763 realloc(m_Rows-1,m_Cols);
00764 }
00765
00766
00767
00768 void deleteColumn(size_t col) {
00769 #if defined(_DEBUG)||(MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00770 if (col<0||col>=m_Cols) THROW_EXCEPTION("Index out of range!");
00771 #endif
00772 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];
00773 realloc(m_Rows,m_Cols-1);
00774 }
00775
00776
00777
00778
00779
00780
00781 template <class R>
00782 void extractMatrix(size_t nRow, size_t nCol, CMatrixTemplate<R> &in) const
00783 {
00784 size_t i,j,ncols,nrows;
00785
00786 nrows = in.getRowCount();
00787 ncols = in.getColCount();
00788 if ( (nRow+nrows > CMatrixTemplate<T>::getRowCount()) || (nCol+ncols >CMatrixTemplate<T>::getColCount()) )
00789 THROW_EXCEPTION("extractMatrix: Row or Col index out of bounds");
00790
00791 for (i=nRow;i<nRow+nrows;i++)
00792 for(j=nCol;j<nCol+ncols;j++)
00793 in.set_unsafe( i-nRow,j-nCol, static_cast<R> ( CMatrixTemplate<T>::m_Val[i][j] ) );
00794 }
00795
00796
00797
00798
00799
00800
00801 void extractMatrix(size_t nRow, size_t nCol,size_t nrows, size_t ncols, CMatrixTemplate<T> &outMat) const
00802 {
00803 size_t i,j;
00804 outMat.setSize(nrows,ncols);
00805
00806 if ( (nRow+nrows > CMatrixTemplate<T>::getRowCount()) || (nCol+ncols >CMatrixTemplate<T>::getColCount()) )
00807 THROW_EXCEPTION("extractMatrix: Row or Col index out of bounds");
00808
00809 for (i=nRow;i<(nRow+nrows);i++)
00810 for(j=nCol;j<(nCol+ncols);j++)
00811 outMat(i-nRow,j-nCol) = CMatrixTemplate<T>::m_Val[i][j];
00812 }
00813
00814
00815
00816
00817
00818
00819 void extractMatrix(size_t nRow, size_t nCol, std::vector<T> &in) const
00820 {
00821 size_t j,ncols,nrows;
00822
00823 ncols = in.size();
00824 nrows = 1;
00825 if ( (nRow+nrows > CMatrixTemplate<T>::getRowCount()) || (nCol+ncols >CMatrixTemplate<T>::getColCount()) )
00826 THROW_EXCEPTION("extractMatrix: Row or Col index out of bounds");
00827
00828 for(j=nCol;j<nCol+ncols;j++)
00829 in[j-nCol] = CMatrixTemplate<T>::m_Val[nRow][j];
00830 }
00831
00832
00833
00834
00835
00836
00837 template <size_t NROWS,size_t NCOLS>
00838 void extractMatrix(const size_t nRow, const size_t nCol, CMatrixFixedNumeric<T,NROWS,NCOLS> &outMat) const {
00839 mrpt::math::extractFixMatrixFromDynMatrix(*this,nRow,nCol,outMat);
00840 }
00841
00842
00843
00844
00845
00846
00847 std::vector<T> extractMatrix(size_t nRow, size_t nCol, size_t ncols) const
00848 {
00849 size_t j;
00850 std::vector<T> out;
00851 out.resize(ncols);
00852
00853 if ( (nRow+1 > CMatrixTemplate<T>::getRowCount()) || (nCol+ncols >CMatrixTemplate<T>::getColCount()) )
00854 THROW_EXCEPTION("extractMatrix: Row or Col index out of bounds");
00855
00856 for(j=nCol;j<nCol+ncols;j++)
00857 out[j-nCol] = CMatrixTemplate<T>::m_Val[nRow][j];
00858 return out;
00859 }
00860
00861
00862
00863
00864
00865
00866 std::string inMatlabFormat() const
00867 {
00868 std::stringstream s;
00869
00870 s << "[";
00871 s << std::scientific;
00872 for (size_t i=0;i<m_Rows;i++)
00873 {
00874 for (size_t j=0;j<m_Cols;j++)
00875 s << m_Val[i][j] << " ";
00876
00877 if (i<m_Rows-1) s << ";";
00878 }
00879 s << "]";
00880
00881 return s.str();
00882 }
00883
00884
00885
00886
00887
00888
00889
00890
00891
00892
00893
00894
00895
00896 bool fromMatlabStringFormat(const std::string &s)
00897 {
00898 this->setSize(0,0);
00899
00900
00901 size_t ini = s.find_first_not_of(" \t");
00902 if (ini==std::string::npos || s[ini]!='[') return false;
00903
00904 size_t end = s.find_last_not_of(" \t");
00905 if (end==std::string::npos || s[end]!=']') return false;
00906
00907 if (ini>end) return false;
00908
00909 std::vector<T> lstElements;
00910
00911 size_t i = ini+1;
00912 size_t nRow = 0;
00913
00914 while (i<end)
00915 {
00916
00917 size_t end_row = s.find_first_of(";]",i);
00918 if (end_row==std::string::npos) { setSize(0,0); return false; }
00919
00920
00921 std::stringstream ss (s.substr(i, end_row-i ));
00922 lstElements.clear();
00923 try
00924 {
00925 while (!ss.eof())
00926 {
00927 T val;
00928 ss >> val;
00929 if (ss.bad() || ss.fail()) break;
00930 lstElements.push_back(val);
00931 }
00932 } catch (...) { }
00933
00934
00935 if (lstElements.empty())
00936 {
00937 if (nRow>0) { setSize(0,0); return false; }
00938
00939 }
00940 else
00941 {
00942 const size_t N = lstElements.size();
00943
00944
00945 if (nRow>0 && m_Cols!=N) { setSize(0,0); return false; }
00946
00947
00948 realloc( nRow+1,N );
00949
00950 for (size_t q=0;q<N;q++)
00951 m_Val[nRow][q] = lstElements[q];
00952
00953
00954 nRow++;
00955 }
00956
00957 i = end_row+1;
00958 }
00959 return true;
00960 }
00961
00962
00963
00964
00965
00966
00967
00968
00969
00970
00971
00972 void saveToTextFile(
00973 const std::string &file,
00974 TMatrixTextFileFormat fileFormat = MATRIX_FORMAT_ENG,
00975 bool appendMRPTHeader = false,
00976 const std::string &userHeader = std::string("")
00977 ) const
00978 {
00979 mrpt::math::saveMatrixToTextFile(*this, file,fileFormat,appendMRPTHeader,userHeader);
00980 }
00981
00982
00983
00984
00985
00986 void loadFromTextFile(const std::string &file)
00987 {
00988 std::ifstream f(file.c_str());
00989 if (f.fail()) THROW_EXCEPTION_CUSTOM_MSG1("loadFromTextFile: can't open file:'%s'",file.c_str());
00990
00991 std::string str;
00992 std::vector<double> fil(512);
00993
00994 const char *ptr;
00995 char *ptrEnd;
00996 size_t i,j;
00997 size_t nCols = std::numeric_limits<size_t>::max();
00998 size_t nRows = 0;
00999
01000 CMatrixTemplate<T>::realloc(0,0);
01001
01002 while ( !f.eof() )
01003 {
01004 std::getline(f,str);
01005
01006 if (str.size() && str[0]!='#' && str[0]!='%')
01007 {
01008
01009 ptr = str.c_str();
01010
01011 ptrEnd = NULL;
01012 i=0;
01013
01014
01015 while ( ptr[0] && ptr!=ptrEnd )
01016 {
01017
01018 while (ptr[0] && (ptr[0]==' ' || ptr[0]=='\t' || ptr[0]=='\r' || ptr[0]=='\n'))
01019 ptr++;
01020
01021 if (fil.size()<=i) fil.resize(fil.size()+512);
01022
01023
01024 fil[i] = strtod(ptr,&ptrEnd);
01025
01026
01027 if (ptr!=ptrEnd)
01028 {
01029 i++;
01030 ptr = ptrEnd;
01031 ptrEnd = NULL;
01032 }
01033 };
01034
01035 if (nCols==std::numeric_limits<size_t>::max())
01036 {
01037
01038 nCols = i;
01039 CMatrixTemplate<T>::realloc(nCols,nCols);
01040 }
01041 else
01042 {
01043
01044 if (CMatrixTemplate<T>::getColCount()!=i )
01045 THROW_EXCEPTION("The matrix in the text file must have the same number of elements in each row!");
01046 }
01047
01048
01049 for (j=0;j<nCols;j++)
01050 CMatrixTemplate<T>::m_Val[nRows][j] = myStaticCast<T>(fil[j]);
01051
01052 nRows++;
01053 if (nRows >= CMatrixTemplate<T>::getRowCount() )
01054 CMatrixTemplate<T>::realloc( nRows+10, nCols );
01055
01056 }
01057
01058 }
01059
01060 if (nRows && nCols)
01061 CMatrixTemplate<T>::realloc( nRows, nCols );
01062
01063
01064 if ( !CMatrixTemplate<T>::getRowCount() || !CMatrixTemplate<T>::getColCount() )
01065 THROW_EXCEPTION("loadFromTextFile: Error loading from text file");
01066 }
01067
01068
01069
01070
01071
01072
01073
01074
01075 void removeColumns( const mrpt::vector_size_t &idxsToRemove, bool vectorIsAlreadySorted = false)
01076 {
01077 MRPT_TRY_START
01078
01079 if (idxsToRemove.empty()) return;
01080 ASSERT_(idxsToRemove.size()<=m_Cols);
01081 if (idxsToRemove.size()==m_Cols)
01082 {
01083 this->setSize(m_Rows,0);
01084 return;
01085 }
01086
01087 const vector_size_t *idxs = &idxsToRemove;
01088 vector_size_t auxSortedIdxs;
01089
01090 if (!vectorIsAlreadySorted)
01091 {
01092 auxSortedIdxs = idxsToRemove;
01093 std::sort(auxSortedIdxs.begin(),auxSortedIdxs.end());
01094 idxs = &idxsToRemove;
01095 }
01096
01097 size_t newColCount = m_Cols;
01098
01099 for (vector_size_t::const_reverse_iterator i=idxs->rbegin();i!=idxs->rend();++i)
01100 {
01101 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
01102 if (*i>=m_Cols) THROW_EXCEPTION("removeColumns: Column index out of bounds");
01103 #endif
01104 for (size_t row=0;row<m_Rows;row++)
01105 {
01106
01107 for (size_t c=*i;c<newColCount;c++)
01108 m_Val[row][c] = m_Val[row][c+1];
01109 }
01110 newColCount--;
01111 }
01112
01113 m_Cols = newColCount;
01114
01115 MRPT_TRY_END
01116 }
01117
01118
01119
01120 void getAsVector(std::vector<T> &out) const {
01121 out.clear();
01122 out.reserve(m_Rows*m_Cols);
01123 for (size_t i=0;i<m_Rows;i++) out.insert(out.end(),&(m_Val[i][0]),&(m_Val[i][m_Cols]));
01124 }
01125
01126 };
01127
01128
01129
01130
01131 template <class T>
01132 std::ostream& operator << (std::ostream& ostrm, const CMatrixTemplate<T>& m)
01133 {
01134 ostrm << std::setprecision(4);
01135
01136 for (size_t i=0; i < m.getRowCount(); i++)
01137 {
01138 for (size_t j=0; j < m.getColCount(); j++)
01139 {
01140 ostrm << std::setw(13) << m(i,j);
01141 }
01142 ostrm << std::endl;
01143 }
01144 return ostrm;
01145 }
01146
01147
01148 template <class T>
01149 size_t size( const CMatrixTemplate<T>& m, int dim )
01150 {
01151 if (dim==1)
01152 return m.getRowCount();
01153 else if (dim==2)
01154 return m.getColCount();
01155 else THROW_EXCEPTION_CUSTOM_MSG1("size: Matrix dimensions are 1 & 2 only (called with i=%i)",dim);
01156 }
01157
01158
01159
01160
01161
01162
01163
01164
01165
01166 template <class MAT>
01167 void saveMatrixToTextFile(
01168 const MAT &theMatrix,
01169 const std::string &file,
01170 TMatrixTextFileFormat fileFormat,
01171 bool appendMRPTHeader,
01172 const std::string &userHeader
01173 )
01174 {
01175 using namespace mrpt::system;
01176
01177 MRPT_TRY_START;
01178
01179 FILE *f=os::fopen(file.c_str(),"wt");
01180 if (!f)
01181 THROW_EXCEPTION_CUSTOM_MSG1("saveToTextFile: Error opening file '%s' for writing a matrix as text.", file.c_str());
01182
01183 if (!userHeader.empty())
01184 fprintf(f,"%s",userHeader.c_str() );
01185
01186 if (appendMRPTHeader)
01187 fprintf(f,"%% File generated with MRPT %s at %s\n%%-----------------------------------------------------------------\n",
01188 mrpt::system::MRPT_getVersion().c_str(),
01189 mrpt::system::dateTimeLocalToString( mrpt::system::now() ).c_str() );
01190
01191 for (size_t i=0; i < theMatrix.getRowCount(); i++)
01192 {
01193 for (size_t j=0; j < theMatrix.getColCount(); j++)
01194 {
01195 switch(fileFormat)
01196 {
01197 case MATRIX_FORMAT_ENG: os::fprintf(f,"%.16e",static_cast<double>(theMatrix(i,j))); break;
01198 case MATRIX_FORMAT_FIXED: os::fprintf(f,"%.16f",static_cast<double>(theMatrix(i,j))); break;
01199 case MATRIX_FORMAT_INT: os::fprintf(f,"%i",static_cast<int>(theMatrix(i,j))); break;
01200 default:
01201 THROW_EXCEPTION("Unsupported value for the parameter 'fileFormat'!");
01202 };
01203
01204 if (j<(theMatrix.getColCount()-1)) os::fprintf(f," ");
01205 }
01206 os::fprintf(f,"\n");
01207 }
01208 os::fclose(f);
01209 MRPT_TRY_END;
01210 }
01211
01212
01213
01214
01215
01216 template <typename T>
01217 class CMatrixColumnAccessor
01218 {
01219 protected:
01220 CMatrixTemplate<T> &m_mat;
01221 size_t m_colInd;
01222 public:
01223 CMatrixColumnAccessor(CMatrixTemplate<T>& mat, size_t colIdx) : m_mat(mat), m_colInd(colIdx) { ASSERT_(colIdx<m_mat.getColCount()) }
01224 inline T & operator[](const size_t i) { return m_mat(i,m_colInd); }
01225 inline const T & operator[](const size_t i) const { return m_mat(i,m_colInd); }
01226 };
01227
01228
01229
01230
01231 template<typename T>
01232 class CMatrixColumnAccessorExtended {
01233 protected:
01234 CMatrixTemplate<T> &m_mat;
01235 size_t m_colInd;
01236 size_t m_rowOffset;
01237 size_t m_elementsSpace;
01238 public:
01239 CMatrixColumnAccessorExtended(CMatrixTemplate<T> &mat,size_t col,size_t offset,size_t space):m_mat(mat),m_colInd(col),m_rowOffset(offset),m_elementsSpace(space) {
01240 ASSERT_(col<m_mat.getColCount());
01241 }
01242 inline T &operator[](size_t i) {
01243 return m_mat(m_rowOffset+(i*m_elementsSpace),m_colInd);
01244 }
01245 inline const T &operator[](size_t i) const {
01246 return m_mat(m_rowOffset+(i*m_elementsSpace),m_colInd);
01247 }
01248 };
01249
01250
01251
01252
01253 template<class T>
01254 class CConstMatrixColumnAccessor {
01255 protected:
01256 const CMatrixTemplate<T> &m_mat;
01257 size_t m_colInd;
01258 public:
01259 CConstMatrixColumnAccessor(const CMatrixTemplate<T> &mat,size_t colIdx):m_mat(mat),m_colInd(colIdx) {
01260 ASSERT_(colIdx<m_mat.getColCount());
01261 }
01262 inline const T &operator[](size_t i) const {
01263 return m_mat(i,m_colInd);
01264 }
01265 };
01266
01267
01268
01269
01270 template<typename T>
01271 class CConstMatrixColumnAccessorExtended {
01272 protected:
01273 const CMatrixTemplate<T> &m_mat;
01274 size_t m_colInd;
01275 size_t m_rowOffset;
01276 size_t m_elementsSpace;
01277 public:
01278 CConstMatrixColumnAccessorExtended(const CMatrixTemplate<T> &mat,size_t col,size_t offset,size_t space):m_mat(mat),m_colInd(col),m_rowOffset(offset),m_elementsSpace(space) {
01279 ASSERT_(col<m_mat.getColCount());
01280 }
01281 inline const T &operator[](size_t i) const {
01282 return m_mat(m_rowOffset+(i*m_elementsSpace),m_colInd);
01283 }
01284 };
01285
01286
01287
01288
01289 template <typename T>
01290 class CMatrixRowAccessor
01291 {
01292 protected:
01293 CMatrixTemplate<T> &m_mat;
01294 size_t m_rowInd;
01295 public:
01296 CMatrixRowAccessor(CMatrixTemplate<T>& mat, size_t rowIdx) : m_mat(mat), m_rowInd(rowIdx) { ASSERT_(rowIdx<m_mat.getRowCount()) }
01297 inline T & operator[](const size_t i) { return m_mat(m_rowInd,i); }
01298 inline const T & operator[](const size_t i) const { return m_mat(m_rowInd,i); }
01299 };
01300
01301
01302
01303
01304 template<class T>
01305 class CMatrixRowAccessorExtended {
01306 protected:
01307 CMatrixTemplate<T> &m_mat;
01308 size_t m_rowInd;
01309 size_t m_colOffset;
01310 size_t m_elementsSpace;
01311 public:
01312 CMatrixRowAccessorExtended(CMatrixTemplate<T> &mat,size_t row,size_t offset,size_t space):m_mat(mat),m_rowInd(row),m_colOffset(offset),m_elementsSpace(space) {
01313 ASSERT_(row<m_mat.getRowCount());
01314 }
01315 inline T &operator[](size_t i) {
01316 return m_mat(m_rowInd,m_colOffset+(i*m_elementsSpace));
01317 }
01318 inline const T &operator[](size_t i) const {
01319 return m_mat(m_rowInd,m_colOffset+(i*m_elementsSpace));
01320 }
01321 };
01322
01323
01324
01325
01326 template<class T>
01327 class CConstMatrixRowAccessor {
01328 protected:
01329 const CMatrixTemplate<T> &m_mat;
01330 size_t m_rowInd;
01331 public:
01332 CConstMatrixRowAccessor(const CMatrixTemplate<T> &mat,size_t row):m_mat(mat),m_rowInd(row) {
01333 ASSERT_(row<m_mat.getRowCount());
01334 }
01335 inline const T &operator[](size_t i) {
01336 return m_mat(m_rowInd,i);
01337 }
01338 };
01339
01340
01341
01342
01343 template<class T>
01344 class CConstMatrixRowAccessorExtended {
01345 protected:
01346 const CMatrixTemplate<T> &m_mat;
01347 size_t m_rowInd;
01348 size_t m_colOffset;
01349 size_t m_elementsSpace;
01350 public:
01351 CConstMatrixRowAccessorExtended(const CMatrixTemplate<T> &mat,size_t row,size_t offset,size_t space):m_mat(mat),m_rowInd(row),m_colOffset(offset),m_elementsSpace(space) {
01352 ASSERT_(row<m_mat.getRowCount());
01353 }
01354 inline const T &operator[](size_t i) {
01355 return m_mat(m_rowInd,m_colOffset+(i*m_elementsSpace));
01356 }
01357 };
01358
01359 }
01360 }
01361
01362 #endif