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 CMatrixTemplateNumeric_H
00029 #define CMatrixTemplateNumeric_H
00030
00031 #include <mrpt/math/CMatrixTemplate.h>
00032 #include <mrpt/system/os.h>
00033 #include <cmath>
00034 #include <limits>
00035
00036 namespace mrpt
00037 {
00038 namespace math
00039 {
00040 using namespace mrpt::system;
00041 template <class T> class CVectorTemplate;
00042
00096 template <class T>
00097 class CMatrixTemplateNumeric : public CMatrixTemplate<T>
00098 {
00099 public:
00102 template <class R>
00103 CMatrixTemplateNumeric(const CMatrixTemplate<R>& m)
00104 {
00105 CMatrixTemplate<T>::realloc( m.getRowCount(), m.getColCount() );
00106
00107 for (size_t i=0; i < CMatrixTemplate<T>::getRowCount(); i++)
00108 for (size_t j=0; j < CMatrixTemplate<T>::getColCount(); j++)
00109 CMatrixTemplate<T>::m_Val[i][j] = static_cast<T> (m.get_unsafe(i,j));
00110 }
00111
00114 CMatrixTemplateNumeric(size_t row = 1, size_t col = 1) : CMatrixTemplate<T>( row, col )
00115 { }
00116
00125 template <typename V, size_t N>
00126 CMatrixTemplateNumeric(size_t row, size_t col, V (&theArray)[N] ) : CMatrixTemplate<T>( row, col, theArray )
00127 { }
00128
00131 virtual ~CMatrixTemplateNumeric()
00132 { }
00133
00136 template <class R>
00137 CMatrixTemplateNumeric<T>& operator = (const CMatrixTemplateNumeric<R>& m)
00138 {
00139 CMatrixTemplate<T>::realloc( m.getRowCount(), m.getColCount() );
00140
00141 for (size_t i=0; i < CMatrixTemplate<T>::getRowCount(); i++)
00142 for (size_t j=0; j < CMatrixTemplate<T>::getColCount(); j++)
00143 CMatrixTemplate<T>::m_Val[i][j] = static_cast<T>(m.get_unsafe(i,j));
00144 return *this;
00145 }
00146
00157 template <typename V, size_t N>
00158 CMatrixTemplateNumeric& operator = (V (&theArray)[N] )
00159 {
00160 CMatrixTemplate<T>::operator = (theArray);
00161 return *this;
00162 }
00163
00166 CMatrixTemplateNumeric<T>& operator = (const CMatrixTemplateNumeric<T>& m)
00167 {
00168 CMatrixTemplate<T>::realloc( m.getRowCount(), m.getColCount() );
00169
00170 for (size_t i=0; i < CMatrixTemplate<T>::getRowCount(); i++)
00171 for (size_t j=0; j < CMatrixTemplate<T>::getColCount(); j++)
00172 CMatrixTemplate<T>::m_Val[i][j] = m.m_Val[i][j];
00173 return *this;
00174 }
00175
00178 void setSize(size_t row, size_t col)
00179 {
00180 CMatrixTemplate<T>::realloc(row,col,true);
00181 }
00182
00187 void saveToTextFile(
00188 const std::string &file,
00189 int fileFormat = 0) const
00190 {
00191 MRPT_TRY_START;
00192
00193 FILE *f=os::fopen(file.c_str(),"wt");
00194 if (!f)
00195 THROW_EXCEPTION("saveToTextFile: Error opening file for writing matrix as text!");
00196
00197 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
00198 {
00199 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
00200 {
00201 switch(fileFormat)
00202 {
00203 case 0: os::fprintf(f,"%.16e",static_cast<double>(this->operator() (i,j)) ); break;
00204 case 1: os::fprintf(f,"%f",static_cast<double>(this->operator() (i,j)) ); break;
00205 case 2: os::fprintf(f,"%i",static_cast<int>(this->operator() (i,j)) ); break;
00206 default:
00207 THROW_EXCEPTION("Unsupported value for the parameter 'fileFormat'!");
00208 };
00209
00210 if (j<(CMatrixTemplate<T>::m_Cols-1)) os::fprintf(f," ");
00211 }
00212 os::fprintf(f,"\n");
00213 }
00214 os::fclose(f);
00215
00216 MRPT_TRY_END;
00217 }
00218
00219
00224 void loadFromTextFile(const std::string &file)
00225 {
00226 std::ifstream f(file.c_str());
00227 if (f.fail()) THROW_EXCEPTION_CUSTOM_MSG1("loadFromTextFile: can't open file:'%s'",file.c_str());
00228
00229 std::string str;
00230 std::vector<double> fil(512);
00231
00232 const char *ptr;
00233 char *ptrEnd;
00234 size_t i,j;
00235 size_t nCols = std::numeric_limits<size_t>::max();
00236 size_t nRows = 0;
00237
00238 CMatrixTemplate<T>::realloc(0,0);
00239
00240 while ( !f.eof() )
00241 {
00242 std::getline(f,str);
00243
00244 if (str.size() && str[0]!='#' && str[0]!='%')
00245 {
00246
00247 ptr = str.c_str();
00248
00249 ptrEnd = NULL;
00250 i=0;
00251
00252
00253 while ( ptr[0] && ptr!=ptrEnd )
00254 {
00255
00256 while (ptr[0] && (ptr[0]==' ' || ptr[0]=='\t' || ptr[0]=='\r' || ptr[0]=='\n'))
00257 ptr++;
00258
00259 if (fil.size()<=i) fil.resize(fil.size()+512);
00260
00261
00262 fil[i] = strtod(ptr,&ptrEnd);
00263
00264
00265 if (ptr!=ptrEnd)
00266 {
00267 i++;
00268 ptr = ptrEnd;
00269 ptrEnd = NULL;
00270 }
00271 };
00272
00273 if (nCols==std::numeric_limits<size_t>::max())
00274 {
00275
00276 nCols = i;
00277 CMatrixTemplate<T>::realloc(nCols,nCols);
00278 }
00279 else
00280 {
00281
00282 if (CMatrixTemplate<T>::getColCount()!=i )
00283 THROW_EXCEPTION("The matrix in the text file must have the same number of elements in each row!");
00284 }
00285
00286
00287 for (j=0;j<nCols;j++)
00288 CMatrixTemplate<T>::m_Val[nRows][j] = static_cast<T>(fil[j]);
00289
00290 nRows++;
00291 if (nRows >= CMatrixTemplate<T>::getRowCount() )
00292 CMatrixTemplate<T>::realloc( nRows+10, nCols );
00293
00294 }
00295
00296 }
00297
00298 if (nRows && nCols)
00299 CMatrixTemplate<T>::realloc( nRows, nCols );
00300
00301
00302 if ( !CMatrixTemplate<T>::getRowCount() || !CMatrixTemplate<T>::getColCount() )
00303 THROW_EXCEPTION("loadFromTextFile: Error loading from text file");
00304 }
00305
00306
00309 void laplacian( CMatrixTemplateNumeric<T> &ret ) const
00310 {
00311 if ( CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols ) THROW_EXCEPTION( "laplacian: Defined for square matrixes only!");
00312
00313 size_t i,j,size;
00314
00315 size = CMatrixTemplate<T>::m_Rows;
00316
00317
00318
00319 std::vector<T> deg(size);
00320
00321 for (i=0;i<size;i++)
00322 {
00323 deg[i] = 0;
00324 for (j=0;j<size;j++)
00325 deg[i] += CMatrixTemplate<T>::m_Val[j][i];
00326 }
00327
00328
00329
00330
00331 ret.realloc(size,size);
00332
00333 for(i=0;i<size;i++)
00334 {
00335 ret(i,i) = deg[i] - CMatrixTemplate<T>::m_Val[i][i];
00336
00337 for(j=i+1;j<size;j++)
00338 {
00339 ret(i,j) =
00340 ret(j,i) = -CMatrixTemplate<T>::m_Val[i][j];
00341 }
00342 }
00343 }
00344
00354 void svd(CMatrixTemplateNumeric<T> &U, std::vector<T> &W,CMatrixTemplateNumeric<T> &V) const
00355 {
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366 size_t i,j;
00367 T **a, **v;
00368 T *w;
00369 size_t m = CMatrixTemplate<T>::getRowCount(),n=CMatrixTemplate<T>::getColCount();
00370
00371
00372
00373 typedef T* matrix_type_ptr;
00374
00375 w = new T[n];
00376 a = new matrix_type_ptr[m];
00377 v = new matrix_type_ptr[n];
00378 for (i=0;i<m;i++) a[i] = new T[n];
00379 for (i=0;i<n;i++) v[i] = new T[n];
00380
00381
00382 for (i=0;i<m;i++)
00383 for (j=0;j<n;j++)
00384 a[i][j] = (*this)(i,j);
00385
00386
00387
00388 svdcmp(a,m,n,w,v);
00389
00390
00391
00392 U.setSize( m,n );
00393 W.resize( n );
00394 V.setSize(n,n);
00395
00396 for (i=0;i<m;i++)
00397 for (j=0;j<n;j++)
00398 U(i,j)= a[i][j];
00399
00400 for (i=0;i<n;i++)
00401 W[i] = w[i];
00402
00403 for (i=0;i<n;i++)
00404 for (j=0;j<n;j++)
00405 V(i,j)=v[i][j];
00406
00407
00408
00409 for (i=0;i<m;i++) delete[] a[i];
00410 for (i=0;i<n;i++) delete[] v[i];
00411 delete[] a;
00412 delete[] v;
00413 delete[] w;
00414 }
00415
00422 void eigenVectors(CMatrixTemplateNumeric<T> &Z, CMatrixTemplateNumeric<T> &D) const
00423 {
00424 if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
00425 THROW_EXCEPTION( "eigenVectors: Only for square matrixes!");
00426
00427 std::vector<unsigned int> indxs;
00428 std::vector<bool> already;
00429
00430 CMatrixTemplateNumeric<T> save( *this );
00431 CMatrixTemplateNumeric<T> E;
00432 size_t i,j;
00433 const size_t n = CMatrixTemplate<T>::m_Cols;
00434 T **a;
00435 T *d,*e;
00436
00437 MRPT_TRY_START;
00438
00439
00440
00441
00442
00443
00444 for (i=0;i<n;i++)
00445 for (j=i;j<n;j++)
00446 if (CMatrixTemplate<T>::m_Val[i][j]!=CMatrixTemplate<T>::m_Val[j][i])
00447 {
00448 THROW_EXCEPTION(format("eigenVectors: The matrix is not symmetric! m(%lu,%lu)=%.16e != m(%lu,%lu)=%.16e\n",
00449 static_cast<unsigned long>(i),static_cast<unsigned long>(j), static_cast<double> ( CMatrixTemplate<T>::m_Val[i][j] ),
00450 static_cast<unsigned long>(j),static_cast<unsigned long>(i), static_cast<double> ( CMatrixTemplate<T>::m_Val[j][i]) ) )
00451 }
00452
00453
00454
00455 typedef T* matrix_type_ptr;
00456
00457 a = new matrix_type_ptr[n+1];
00458 for (i=1;i<=n;i++) a[i] = new T[n+1];
00459 d = new T[n+1];
00460 e = new T[n+1];
00461
00462 for (i=1;i<=n;i++)
00463 for (j=1;j<=n;j++)
00464 a[i][j] = CMatrixTemplate<T>::m_Val[i-1][j-1];
00465
00466
00467
00468 tred2( a, n, d, e);
00469
00470 tqli(d,e,n,a);
00471
00472
00473
00474
00475
00476
00477
00478 indxs.resize(n+1);
00479 already.resize(n+1, false);
00480
00481 for (i=1;i<=n;i++)
00482 {
00483 size_t minIndx = std::numeric_limits<size_t>::max();
00484 for (j=1;j<=n;j++)
00485 if (!already[j])
00486 {
00487 if (minIndx==std::numeric_limits<size_t>::max()) minIndx = j;
00488 else
00489 if (d[j]<d[minIndx]) minIndx = j;
00490 }
00491
00492
00493 indxs[i] = static_cast<unsigned int> ( minIndx );
00494 already[minIndx] = true;
00495 }
00496
00497 for (i=1;i<=n;i++)
00498 ASSERT_(already[i]);
00499
00500
00501
00502 Z.setSize(n,n);
00503 D.setSize(n,n);
00504 for (i=1;i<=n;i++)
00505 for (j=1;j<=n;j++)
00506 {
00507 Z(i-1,j-1) = a[i][indxs[j]];
00508 if (i==j)
00509 {
00510 if (d[indxs[j]]<0)
00511 D(i-1,i-1) = -d[indxs[j]];
00512 else D(i-1,i-1) = d[indxs[j]];
00513 }
00514 else D(i-1,j-1) = 0;
00515
00516
00517 ASSERT_( !(system::isNaN( Z(i-1,j-1) )));
00518 ASSERT_( !(system::isNaN( D(i-1,i-1) )));
00519 }
00520
00521
00522
00523 for (i=1;i<=n;i++) delete[] a[i];
00524 delete[] a;
00525 delete[] d;
00526 delete[] e;
00527
00528
00529 for (i=0;i<n;i++)
00530 for (j=i;j<n;j++)
00531 CMatrixTemplate<T>::m_Val[i][j] = CMatrixTemplate<T>::m_Val[j][i] = save.get_unsafe(i,j);
00532
00533 MRPT_TRY_END_WITH_CLEAN_UP( \
00534 std::cout << "[eigenVectors] The matrix leading to exception is:" << std::endl << save << std::endl; \
00535 for (i=0;i<n;i++) \
00536 for (j=i;j<n;j++) \
00537 CMatrixTemplate<T>::m_Val[i][j] = CMatrixTemplate<T>::m_Val[j][i] = save.get_unsafe(i,j); \
00538 );
00539
00540 }
00541
00547 CMatrixTemplateNumeric<T> largestEigenvector(
00548 T resolution = 0.01f,
00549 size_t maxIterations = 6,
00550 int *out_Iterations = NULL,
00551 float *out_estimatedResolution = NULL ) const
00552 {
00553
00554
00555 size_t i, iter=0, n = CMatrixTemplate<T>::m_Rows;
00556 CMatrixTemplateNumeric<T> x,xx;
00557 T norm,dif;
00558
00559
00560 x.ones(n,1);
00561
00562
00563 do
00564 {
00565 xx = (*this) * x;
00566
00567
00568 norm = 0;
00569 for (i=0;i<n;i++)
00570 norm+= mrpt::utils::square(xx(i,0));
00571 xx *= static_cast<T>((1.0/::sqrt(norm)));
00572
00573
00574 dif = 0;
00575 for (i=0;i<n;i++)
00576 dif+=static_cast<T>(( mrpt::utils::square(fabs(xx(i,0)-x(i,0)))));
00577 dif=static_cast<T>(::sqrt(dif));
00578
00579
00580 x = xx;
00581
00582
00583 iter++;
00584
00585 } while (iter<maxIterations && dif>resolution);
00586
00587 if (out_Iterations) *out_Iterations=static_cast<int>(iter);
00588 if (out_estimatedResolution) *out_estimatedResolution=dif;
00589
00590
00591 return x;
00592 }
00593
00596 void Sqrt()
00597 {
00598 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
00599 {
00600 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
00601 {
00602 if (CMatrixTemplate<T>::m_Val[i][j]>0)
00603 CMatrixTemplate<T>::m_Val[i][j] = static_cast<T>( ::sqrt(CMatrixTemplate<T>::m_Val[i][j]));
00604 else CMatrixTemplate<T>::m_Val[i][j] = 0;
00605 }
00606 }
00607 return;
00608 }
00609
00612 void Abs()
00613 {
00614 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
00615 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
00616 CMatrixTemplate<T>::m_Val[i][j] = static_cast<T>( ::fabs(CMatrixTemplate<T>::m_Val[i][j]));
00617 return;
00618 }
00619
00622 CMatrixTemplateNumeric<T> operator + ()
00623 {
00624 return (*this);
00625 }
00626
00629 CMatrixTemplateNumeric<T> operator - ()
00630 {
00631 CMatrixTemplateNumeric<T> temp(CMatrixTemplate<T>::m_Rows,CMatrixTemplate<T>::m_Cols);
00632
00633 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
00634 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
00635 temp.m_Val[i][j] = - CMatrixTemplate<T>::m_Val[i][j];
00636
00637 return temp;
00638 }
00639
00642 CMatrixTemplateNumeric<T>& operator += (const CMatrixTemplateNumeric<T>& m)
00643 {
00644 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00645 if (CMatrixTemplate<T>::m_Rows != m.m_Rows || CMatrixTemplate<T>::m_Cols != m.m_Cols)
00646 THROW_EXCEPTION( "operator+= : Inconsistent matrix sizes in addition!");
00647 #endif
00648
00649 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
00650 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
00651 CMatrixTemplate<T>::m_Val[i][j] += m.m_Val[i][j];
00652 return *this;
00653 }
00654
00657 CMatrixTemplateNumeric<T>& addAt(const CMatrixTemplateNumeric<T>& m)
00658 {
00659 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00660 if (CMatrixTemplate<T>::m_Rows != m.m_Cols || CMatrixTemplate<T>::m_Cols != m.m_Rows)
00661 THROW_EXCEPTION( "Inconsistent matrix sizes in addition!");
00662 #endif
00663
00664 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
00665 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
00666 CMatrixTemplate<T>::m_Val[i][j] += m.m_Val[j][i];
00667 return *this;
00668 }
00669
00672 CMatrixTemplateNumeric<T>& addAAt(const CMatrixTemplateNumeric<T>& m)
00673 {
00674 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00675 if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols || m.m_Cols!=m.m_Rows || CMatrixTemplate<T>::m_Cols != m.m_Rows)
00676 THROW_EXCEPTION( "Inconsistent matrix sizes in addition!");
00677 #endif
00678 const size_t N = CMatrixTemplate<T>::m_Rows;
00679
00680 for (size_t i=0; i < N; i++)
00681 for (size_t j=i; j < N; j++)
00682 CMatrixTemplate<T>::m_Val[i][j] += m.m_Val[j][i] + m.m_Val[i][j];
00683 return *this;
00684 }
00685
00686
00689 CMatrixTemplateNumeric<T>& operator -= (const CMatrixTemplateNumeric<T>& m)
00690 {
00691 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00692 if (CMatrixTemplate<T>::m_Rows != m.m_Rows || CMatrixTemplate<T>::m_Cols != m.m_Cols)
00693 THROW_EXCEPTION( "operator+= : Inconsistent matrix sizes in addition!");
00694 #endif
00695 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
00696 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
00697 CMatrixTemplate<T>::m_Val[i][j] -= m.m_Val[i][j];
00698 return *this;
00699 }
00700
00701
00704 CMatrixTemplateNumeric<T>& operator *= (const T& c)
00705 {
00706 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
00707 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
00708 CMatrixTemplate<T>::m_Val[i][j] *= c;
00709 return *this;
00710 }
00711
00714 CMatrixTemplateNumeric<T>& operator *= (const CMatrixTemplateNumeric<T>& m)
00715 {
00716 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00717 if (CMatrixTemplate<T>::m_Cols != m.m_Rows)
00718 THROW_EXCEPTION( "operator*= : Inconsistent matrix sizes in multiplication!");
00719 #endif
00720 CMatrixTemplateNumeric<T> temp(CMatrixTemplate<T>::m_Rows,m.m_Cols);
00721
00722 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
00723 for (size_t j=0; j < m.m_Cols; j++)
00724 {
00725 temp.m_Val[i][j] = 0;
00726 for (size_t k=0; k < CMatrixTemplate<T>::m_Cols; k++)
00727 temp.m_Val[i][j] += CMatrixTemplate<T>::m_Val[i][k] * m.m_Val[k][j];
00728 }
00729 *this = temp;
00730 return *this;
00731 }
00732
00735 void multiply(const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
00736 {
00737 MRPT_TRY_START
00738
00739 size_t M1R = m1.getRowCount();
00740 size_t M1C = m1.getColCount();
00741 size_t M2C = m2.getColCount();
00742
00743 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00744 if (m1.getColCount() != m2.getRowCount())
00745 THROW_EXCEPTION( "multiply: Inconsistent matrix sizes in multiplication!");
00746 #endif
00747
00748 if (&m1==this || &m2==this)
00749 {
00750
00751 T *temp= new T[M1R*M2C];
00752 size_t i;
00753
00754 T *ptr = temp;
00755 for (i=0; i < M1R; i++)
00756 {
00757 for (size_t j=0; j < M2C; j++)
00758 {
00759 T accum = 0;
00760 for (size_t k=0; k < M1C; k++)
00761 accum += m1.get_unsafe(i,k) * m2.get_unsafe(k,j);
00762 *(ptr++) = accum;
00763 }
00764 }
00765
00766
00767
00768 setSize(M1R,M2C);
00769 ptr = temp;
00770 for (i=0; i < M1R; i++)
00771 for (size_t j=0; j < M2C; j++)
00772 set_unsafe(i,j, *(ptr++) );
00773
00774 delete[] temp;
00775 }
00776 else
00777 {
00778
00779 setSize( M1R,M2C );
00780
00781 for (size_t i=0; i < M1R; i++)
00782 {
00783 for (size_t j=0; j < M2C; j++)
00784 {
00785 T accum = 0;
00786 for (size_t k=0; k < M1C; k++)
00787 accum += m1.get_unsafe(i,k) * m2.get_unsafe(k,j);
00788 set_unsafe(i,j,accum);
00789 }
00790 }
00791
00792 }
00793
00794
00795 MRPT_TRY_END
00796 }
00797
00798
00801 void multiply(const CMatrixTemplateNumeric<T>& m1, const CVectorTemplate<T>& m2)
00802 {
00803 MRPT_TRY_START
00804
00805 size_t M1R = m1.getRowCount();
00806 size_t M1C = m1.getColCount();
00807
00808 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00809 if (m1.getColCount() != m2.size())
00810 THROW_EXCEPTION( "multiply: Inconsistent matrix sizes in multiplication!");
00811 #endif
00812
00813 if (&m1==this)
00814 {
00815
00816 T *temp= new T[M1R];
00817
00818 T *ptr = temp;
00819 size_t i;
00820 for (i=0; i < M1R; i++)
00821 {
00822 T accum = 0;
00823 for (size_t k=0; k < M1C; k++)
00824 accum += m1.get_unsafe(i,k) * m2[k];
00825 *(ptr++) = accum;
00826 }
00827
00828
00829
00830 setSize(M1R,1);
00831 ptr = temp;
00832 for (i=0; i < M1R; i++)
00833 set_unsafe(i,0, *(ptr++) );
00834 delete[] temp;
00835 }
00836 else
00837 {
00838
00839 setSize( M1R,1 );
00840
00841 for (size_t i=0; i < M1R; i++)
00842 {
00843 T accum = 0;
00844 for (size_t k=0; k < M1C; k++)
00845 accum += m1.get_unsafe(i,k) * m2[k];
00846 set_unsafe(i,0,accum);
00847 }
00848 }
00849
00850 MRPT_TRY_END
00851 }
00852
00855 void multiply_ABt(const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
00856 {
00857 MRPT_TRY_START
00858
00859 size_t M1R = m1.getRowCount();
00860 size_t M1C = m1.getColCount();
00861 size_t M2C = m2.getRowCount();
00862
00863 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00864 if (m1.getColCount() != m2.getColCount())
00865 THROW_EXCEPTION( "multiply: Inconsistent matrix sizes in multiplication!");
00866 #endif
00867
00868 if (&m1==this || &m2==this)
00869 {
00870
00871 T *temp= new T[M1R*M2C];
00872
00873 T *ptr = temp;
00874 size_t i;
00875 for (i=0; i < M1R; i++)
00876 {
00877 for (size_t j=0; j < M2C; j++)
00878 {
00879 T accum = 0;
00880 for (size_t k=0; k < M1C; k++)
00881 accum += m1.get_unsafe(i,k) * m2.get_unsafe(j,k);
00882 *(ptr++) = accum;
00883 }
00884 }
00885
00886
00887
00888 setSize(M1R,M2C);
00889 ptr = temp;
00890 for (i=0; i < M1R; i++)
00891 for (size_t j=0; j < M2C; j++)
00892 set_unsafe(i,j, *(ptr++) );
00893
00894 delete[] temp;
00895 }
00896 else
00897 {
00898
00899 setSize( M1R,M2C );
00900
00901 for (size_t i=0; i < M1R; i++)
00902 {
00903 for (size_t j=0; j < M2C; j++)
00904 {
00905 T accum = 0;
00906 for (size_t k=0; k < M1C; k++)
00907 accum += m1.get_unsafe(i,k) * m2.get_unsafe(j,k);
00908 set_unsafe(i,j,accum);
00909 }
00910 }
00911 }
00912
00913 MRPT_TRY_END
00914 }
00915
00918 void multiply_AAt( const CMatrixTemplateNumeric<T>& m1 )
00919 {
00920 MRPT_TRY_START
00921
00922 const size_t M1R = m1.getRowCount();
00923 const size_t M1C = m1.getColCount();
00924 const size_t M2C = m1.getRowCount();
00925
00926
00927 if (&m1==this)
00928 {
00929
00930 T *temp= new T[M1R*M2C];
00931
00932 T *ptr = temp;
00933 size_t i;
00934 for (i=0; i < M1R; i++)
00935 {
00936 for (size_t j=i; j < M2C; j++)
00937 {
00938 T accum = 0;
00939 for (size_t k=0; k < M1C; k++)
00940 accum += m1.get_unsafe(i,k) * m1.get_unsafe(j,k);
00941 *(ptr++) = accum;
00942 }
00943 }
00944
00945
00946 setSize(M1R,M2C);
00947 ptr = temp;
00948 for (i=0; i < M1R; i++)
00949 for (size_t j=i; j < M2C; j++)
00950 {
00951 set_unsafe(i,j, * ptr );
00952 set_unsafe(j,i, *(ptr++) );
00953 }
00954 delete[] temp;
00955 }
00956 else
00957 {
00958
00959 setSize( M1R,M2C );
00960
00961 for (size_t i=0; i < M1R; i++)
00962 {
00963 for (size_t j=i; j < M2C; j++)
00964 {
00965 T accum = 0;
00966 for (size_t k=0; k < M1C; k++)
00967 accum += m1.get_unsafe(i,k) * m1.get_unsafe(j,k);
00968 set_unsafe(i,j,accum);
00969 set_unsafe(j,i,accum);
00970 }
00971 }
00972 }
00973
00974 MRPT_TRY_END
00975 }
00976
00979 void multiply_AtA( const CMatrixTemplateNumeric<T>& m1 )
00980 {
00981 MRPT_TRY_START
00982
00983 const size_t M1R = m1.getColCount();
00984 const size_t M1C = m1.getRowCount();
00985 const size_t M2C = m1.getColCount();
00986
00987
00988 if (&m1==this)
00989 {
00990
00991 T *temp= new T[M1R*M2C];
00992
00993 T *ptr = temp;
00994 size_t i;
00995 for (i=0; i < M1R; i++)
00996 {
00997 for (size_t j=i; j < M2C; j++)
00998 {
00999 T accum = 0;
01000 for (size_t k=0; k < M1C; k++)
01001 accum += m1.get_unsafe(k,i) * m1.get_unsafe(k,j);
01002 *(ptr++) = accum;
01003 }
01004 }
01005
01006
01007 setSize(M1R,M2C);
01008 ptr = temp;
01009 for (i=0; i < M1R; i++)
01010 for (size_t j=i; j < M2C; j++)
01011 {
01012 set_unsafe(i,j, * ptr );
01013 set_unsafe(j,i, *(ptr++) );
01014 }
01015 delete[] temp;
01016 }
01017 else
01018 {
01019
01020 setSize( M1R,M2C );
01021
01022 for (size_t i=0; i < M1R; i++)
01023 {
01024 for (size_t j=i; j < M2C; j++)
01025 {
01026 T accum = 0;
01027 for (size_t k=0; k < M1C; k++)
01028 accum += m1.get_unsafe(k,i) * m1.get_unsafe(k,j);
01029 set_unsafe(i,j,accum);
01030 set_unsafe(j,i,accum);
01031 }
01032 }
01033 }
01034
01035 MRPT_TRY_END
01036 }
01037
01040 void multiply_Ab( const std::vector<T>& a, std::vector<T>& out_v )
01041 {
01042 MRPT_TRY_START
01043
01044
01045
01046 const size_t N = CMatrixTemplate<T>::getRowCount();
01047 const size_t M = CMatrixTemplate<T>::getColCount();
01048
01049 ASSERT_( a.size() == M )
01050 out_v.resize(N);
01051
01052 T accum;
01053 typename std::vector<T>::const_iterator a_it;
01054 typename std::vector<T>::iterator v_it;
01055 size_t i,j;
01056
01057 for (i=0, v_it=out_v.begin(); i < N; i++)
01058 {
01059 accum = 0;
01060 for (j=0, a_it=a.begin(); j < M; j++)
01061 accum += *a_it++ * CMatrixTemplate<T>::get_unsafe(i,j);
01062
01063 *v_it++ = accum;
01064 }
01065
01066 MRPT_TRY_END
01067 }
01068
01071 void multiply_Atb( const std::vector<T>& a, std::vector<T>& out_v )
01072 {
01073 MRPT_TRY_START
01074
01075
01076
01077 const size_t N = CMatrixTemplate<T>::getRowCount();
01078 const size_t M = CMatrixTemplate<T>::getColCount();
01079
01080 ASSERT_( a.size() == N )
01081 out_v.resize(M);
01082
01083 T accum;
01084 typename std::vector<T>::const_iterator a_it;
01085 typename std::vector<T>::iterator v_it;
01086 size_t i,j;
01087
01088 for (i=0, v_it=out_v.begin(); i < M; i++)
01089 {
01090 accum = 0;
01091 for (j=0, a_it=a.begin(); j < N; j++)
01092 accum += *a_it++ * CMatrixTemplate<T>::get_unsafe(j,i);
01093
01094 *v_it++ = accum;
01095 }
01096
01097 MRPT_TRY_END
01098 }
01099
01100
01101
01104 void multiply_result_is_symmetric(const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
01105 {
01106 MRPT_TRY_START
01107
01108 size_t M1R = m1.getRowCount();
01109 size_t M1C = m1.getColCount();
01110 size_t M2C = m2.getColCount();
01111
01112 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
01113 if (m1.getColCount() != m2.getRowCount())
01114 THROW_EXCEPTION( "multiply: Inconsistent matrix sizes in multiplication!");
01115 #endif
01116
01117 if (&m1==this || &m2==this)
01118 {
01119
01120 T *temp= new T[M1R*M2C];
01121
01122 T *ptr = temp;
01123 size_t i;
01124 for (i=0; i < M1R; i++)
01125 {
01126 for (size_t j=i; j < M2C; j++)
01127 {
01128 T accum = 0;
01129 for (size_t k=0; k < M1C; k++)
01130 accum += m1.get_unsafe(i,k) * m2.get_unsafe(k,j);
01131 *(ptr++) = accum;
01132 }
01133 }
01134
01135
01136
01137 setSize(M1R,M2C);
01138 ptr = temp;
01139 for (i=0; i < M1R; i++)
01140 for (size_t j=i; j < M1C; j++)
01141 {
01142 set_unsafe(i,j, *ptr );
01143 set_unsafe(j,i, *ptr );
01144 ptr++;
01145 }
01146
01147 delete[] temp;
01148 }
01149 else
01150 {
01151
01152 setSize( M1R,M2C );
01153
01154 size_t i;
01155
01156 for (i=0; i < M1R; i++)
01157 {
01158 for (size_t j=i; j < M2C; j++)
01159 {
01160 T accum = 0;
01161 for (size_t k=0; k < M1C; k++)
01162 accum += m1.get_unsafe(i,k) * m2.get_unsafe(k,j);
01163 set_unsafe(i,j,accum);
01164 }
01165 }
01166 for (i=0; i < M1R; i++)
01167 for (size_t j=0; j<i; j++)
01168 CMatrixTemplate<T>::set_unsafe(j,i, CMatrixTemplate<T>::get_unsafe(i,j));
01169 }
01170
01171 MRPT_TRY_END
01172 }
01173
01174
01178 void multiplySubMatrix (
01179 const CMatrixTemplateNumeric<T> &A,
01180 CMatrixTemplateNumeric<T> &outResult,
01181 const size_t &A_cols_offset,
01182 const size_t &A_rows_offset,
01183 const size_t &A_col_count )
01184 {
01185 MRPT_TRY_START
01186
01187
01188 size_t N = CMatrixTemplate<T>::m_Rows;
01189 size_t M = A_col_count;
01190
01191 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
01192 ASSERT_( A.m_Cols >= A_col_count + A_cols_offset );
01193 ASSERT_( A.m_Rows >= N + A_rows_offset );
01194 #endif
01195 outResult.setSize(N,M);
01196
01197 for (size_t i=0; i < N; i++)
01198 {
01199 for (size_t j=0; j < M; j++)
01200 {
01201 T tmp = 0;
01202 for (size_t k=0; k < CMatrixTemplate<T>::m_Cols; k++)
01203 tmp += CMatrixTemplate<T>::m_Val[i][k] * A.m_Val[k+A_rows_offset][j+A_cols_offset];
01204 outResult.m_Val[i][j] = tmp;
01205 }
01206 }
01207 MRPT_TRY_END
01208 }
01209
01212 CMatrixTemplateNumeric<T>& operator /= (const CMatrixTemplateNumeric<T>& m)
01213 {
01214 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
01215 if (CMatrixTemplate<T>::m_Cols != m.m_Cols || CMatrixTemplate<T>::m_Rows != m.m_Rows )
01216 THROW_EXCEPTION( "operator/= : Matrixes are not of the same dimensions!");
01217 #endif
01218 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
01219 for (size_t j=0; j < m.m_Cols; j++)
01220 if (m.m_Val[i][j]!=0)
01221 {
01222 CMatrixTemplate<T>::m_Val[i][j] /= m.m_Val[i][j];
01223 }
01224 return *this;
01225 }
01226
01227
01230 CMatrixTemplateNumeric<T>& operator /= (const T& c)
01231 {
01232 MRPT_TRY_START;
01233 if (c==0) THROW_EXCEPTION("Aborted! Trying to divide by zero!");
01234
01235 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
01236 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
01237 CMatrixTemplate<T>::m_Val[i][j] /= c;
01238 return *this;
01239
01240 MRPT_TRY_END;
01241 }
01242
01245 CMatrixTemplateNumeric<T>& operator += (const T& c)
01246 {
01247 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
01248 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
01249 CMatrixTemplate<T>::m_Val[i][j] += c;
01250 return *this;
01251 }
01252
01255 CMatrixTemplateNumeric<T>& operator -= (const T& c)
01256 {
01257 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
01258 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
01259 CMatrixTemplate<T>::m_Val[i][j] -= c;
01260 return *this;
01261 }
01262
01265 CMatrixTemplateNumeric<T>& operator ^= (const unsigned int& pow)
01266 {
01267 CMatrixTemplateNumeric<T> temp(*this);
01268
01269 for (size_t i=2; i <= pow; i++)
01270 *this = *this * temp;
01271
01272 return *this;
01273 }
01274
01277 void scalarPow(T s)
01278 {
01279 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
01280 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
01281 CMatrixTemplate<T>::m_Val[i][j] = pow(CMatrixTemplate<T>::m_Val[i][j],s);
01282 }
01283
01286 void zeros(const size_t& row, const size_t& col)
01287 {
01288 setSize(row,col);
01289 zeros();
01290 }
01291
01294 void zeros()
01295 {
01296 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
01297 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
01298 CMatrixTemplate<T>::m_Val[i][j] = 0;
01299 }
01300
01303 void ones(const size_t& row, const size_t& col)
01304 {
01305 setSize(row,col);
01306 ones();
01307 }
01308
01311 void ones()
01312 {
01313 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
01314 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
01315 CMatrixTemplate<T>::m_Val[i][j] = 1;
01316 }
01317
01320 void unit (const size_t& row)
01321 {
01322 setSize(row,row);
01323 unit();
01324 }
01325
01328 void unit()
01329 {
01330 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
01331 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
01332 CMatrixTemplate<T>::m_Val[i][j] = (i==j) ? 1 : 0;
01333 }
01334
01337 CMatrixTemplateNumeric<T> solve (const CMatrixTemplateNumeric<T>& v)
01338 {
01339 size_t i,j,k;
01340 T a1;
01341
01342 if (!(CMatrixTemplate<T>::m_Rows == CMatrixTemplate<T>::m_Cols && CMatrixTemplate<T>::m_Cols == v.m_Rows))
01343 THROW_EXCEPTION( "solve:Inconsistent matrices!");
01344
01345 CMatrixTemplateNumeric<T> temp(CMatrixTemplate<T>::m_Rows,CMatrixTemplate<T>::m_Cols+v.m_Cols);
01346 for (i=0; i < CMatrixTemplate<T>::m_Rows; i++)
01347 {
01348 for (j=0; j < CMatrixTemplate<T>::m_Cols; j++)
01349 temp.m_Val[i][j] = CMatrixTemplate<T>::m_Val[i][j];
01350 for (k=0; k < v.m_Cols; k++)
01351 temp.m_Val[i][CMatrixTemplate<T>::m_Cols+k] = v.m_Val[i][k];
01352 }
01353 for (k=0; k < CMatrixTemplate<T>::m_Rows; k++)
01354 {
01355 int indx = temp.pivot(k);
01356 if (indx == -1)
01357 {
01358 std::cout << "[solve] Matrix that leaded to error is:" << std::endl << (*this) << std::endl;
01359 THROW_EXCEPTION( "solve: Singular matrix!");
01360 }
01361
01362 a1 = temp.m_Val[k][k];
01363 for (j=k; j < temp.m_Cols; j++)
01364 temp.m_Val[k][j] /= a1;
01365
01366 for (i=k+1; i < CMatrixTemplate<T>::m_Rows; i++)
01367 {
01368 a1 = temp.m_Val[i][k];
01369 for (j=k; j < temp.m_Cols; j++)
01370 temp.m_Val[i][j] -= a1 * temp.m_Val[k][j];
01371 }
01372 }
01373
01374 CMatrixTemplateNumeric<T> s(v.m_Rows,v.m_Cols);
01375 for (k=0; k < v.m_Cols; k++)
01376 for (int m=int(CMatrixTemplate<T>::m_Rows)-1; m >= 0; m--)
01377 {
01378 s.m_Val[m][k] = temp.m_Val[m][CMatrixTemplate<T>::m_Cols+k];
01379 for (j=m+1; j < CMatrixTemplate<T>::m_Cols; j++)
01380 s.m_Val[m][k] -= temp.m_Val[m][j] * s.m_Val[j][k];
01381 }
01382 return s;
01383 }
01384
01387 CMatrixTemplateNumeric<T> adj() const
01388 {
01389 if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
01390 THROW_EXCEPTION( "adj: Adjoin of a non-square matrix.");
01391
01392 CMatrixTemplateNumeric<T> temp(CMatrixTemplate<T>::m_Rows,CMatrixTemplate<T>::m_Cols);
01393
01394 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
01395 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
01396 temp.m_Val[j][i] = cofact(i,j);
01397 return temp;
01398 }
01399
01403 CMatrixTemplateNumeric<T> inv() const
01404 {
01405 size_t i,j,k;
01406 T a1,a2,*rowptr;
01407
01408 if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
01409 THROW_EXCEPTION( "operator!: Inversion of a non-square matrix");
01410
01411 CMatrixTemplateNumeric<T> temp(CMatrixTemplate<T>::m_Rows,CMatrixTemplate<T>::m_Cols);
01412 CMatrixTemplateNumeric<T> temp2( *this );
01413
01414 temp.unit();
01415 for (k=0; k < CMatrixTemplate<T>::m_Rows; k++)
01416 {
01417 int indx = temp2.pivot(k);
01418 if (indx == -1)
01419 {
01420 std::cerr << "[inv] Matrix that leaded to error is (also in 'err_mat.txt'):" << std::endl << (*this) << std::endl;
01421 saveToTextFile("err_mat.txt");
01422 THROW_EXCEPTION( "operator!: Inversion of a singular matrix");
01423 }
01424
01425 if (indx != 0)
01426 {
01427 rowptr = temp.m_Val[k];
01428 temp.m_Val[k] = temp.m_Val[indx];
01429 temp.m_Val[indx] = rowptr;
01430 }
01431 a1 = temp2.m_Val[k][k];
01432 for (j=0; j < temp2.m_Rows; j++)
01433 {
01434 temp2.m_Val[k][j] /= a1;
01435 temp.m_Val[k][j] /= a1;
01436 }
01437 for (i=0; i < temp2.m_Rows; i++)
01438 {
01439 if (i != k)
01440 {
01441 a2 = temp2.m_Val[i][k];
01442 for (j=0; j < temp2.m_Rows; j++)
01443 {
01444 temp2.m_Val[i][j] -= a2 * temp2.m_Val[k][j];
01445 temp.m_Val[i][j] -= a2 * temp.m_Val[k][j];
01446 }
01447 }
01448 }
01449 }
01450 return temp;
01451 }
01452
01456 void inv_fast( CMatrixTemplateNumeric<T> &out_inv )
01457 {
01458 size_t i,j,k;
01459 T a1,a2,*rowptr;
01460
01461 if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
01462 THROW_EXCEPTION( "operator!: Inversion of a non-square matrix");
01463
01464 out_inv.setSize(CMatrixTemplate<T>::m_Rows,CMatrixTemplate<T>::m_Cols);
01465 out_inv.unit();
01466 for (k=0; k < CMatrixTemplate<T>::m_Rows; k++)
01467 {
01468 int indx = (*this).pivot(k);
01469 if (indx == -1)
01470 {
01471 std::cerr << "[inv] Matrix that leaded to error is:" << std::endl << (*this) << std::endl;
01472 THROW_EXCEPTION( "operator!: Inversion of a singular matrix");
01473 }
01474
01475 if (indx != 0)
01476 {
01477 rowptr = out_inv.m_Val[k];
01478 out_inv.m_Val[k] = out_inv.m_Val[indx];
01479 out_inv.m_Val[indx] = rowptr;
01480 }
01481 a1 = CMatrixTemplate<T>::m_Val[k][k];
01482 for (j=0; j < CMatrixTemplate<T>::m_Rows; j++)
01483 {
01484 CMatrixTemplate<T>::m_Val[k][j] /= a1;
01485 out_inv.m_Val[k][j] /= a1;
01486 }
01487 for (i=0; i < CMatrixTemplate<T>::m_Rows; i++)
01488 {
01489 if (i != k)
01490 {
01491 a2 = CMatrixTemplate<T>::m_Val[i][k];
01492 for (j=0; j < CMatrixTemplate<T>::m_Rows; j++)
01493 {
01494 CMatrixTemplate<T>::m_Val[i][j] -= a2 * CMatrixTemplate<T>::m_Val[k][j];
01495 out_inv.m_Val[i][j] -= a2 * out_inv.m_Val[k][j];
01496 }
01497 }
01498 }
01499 }
01500 }
01501
01504 T det() const
01505 {
01506 size_t i,j,k;
01507 T piv,detVal = T(1);
01508
01509 if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
01510 THROW_EXCEPTION( "det: Determinant a non-square matrix!");
01511
01512 CMatrixTemplateNumeric<T> temp(*this);
01513
01514 for (k=0; k < CMatrixTemplate<T>::m_Rows; k++)
01515 {
01516 int indx = temp.pivot(k);
01517 if (indx == -1)
01518 return 0;
01519 if (indx != 0)
01520 detVal = - detVal;
01521 detVal = detVal * temp.m_Val[k][k];
01522
01523 for (i=k+1; i < CMatrixTemplate<T>::m_Rows; i++)
01524 {
01525 piv = temp.m_Val[i][k] / temp.m_Val[k][k];
01526 for (j=k+1; j < CMatrixTemplate<T>::m_Rows; j++)
01527 temp.m_Val[i][j] -= piv * temp.m_Val[k][j];
01528 }
01529 }
01530 return detVal;
01531 }
01532
01535 T norm() const
01536 {
01537 T retVal = 0;
01538
01539 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
01540 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
01541 retVal += CMatrixTemplate<T>::m_Val[i][j] * CMatrixTemplate<T>::m_Val[i][j];
01542 retVal = ::sqrt( retVal );
01543 return retVal;
01544 }
01545
01548 T cofact (size_t row, size_t col) const
01549 {
01550 size_t i,i1,j,j1;
01551
01552 if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
01553 THROW_EXCEPTION( "cofact: Cofactor of a non-square matrix!");
01554
01555 if (row > CMatrixTemplate<T>::m_Rows || col > CMatrixTemplate<T>::m_Cols)
01556 THROW_EXCEPTION( "cofact: Index out of range!");
01557
01558 CMatrixTemplateNumeric<T> temp (CMatrixTemplate<T>::m_Rows-1,CMatrixTemplate<T>::m_Cols-1);
01559
01560 for (i=i1=0; i < CMatrixTemplate<T>::m_Rows; i++)
01561 {
01562 if (i == row)
01563 continue;
01564 for (j=j1=0; j < CMatrixTemplate<T>::m_Cols; j++)
01565 {
01566 if (j == col)
01567 continue;
01568 temp.m_Val[i1][j1] = CMatrixTemplate<T>::m_Val[i][j];
01569 j1++;
01570 }
01571 i1++;
01572 }
01573 T cof = temp.det();
01574 if ((row+col)%2 == 1)
01575 cof = -cof;
01576
01577 return cof;
01578 }
01579
01582 T cond()
01583 {
01584 CMatrixTemplateNumeric<T> the_inv = inv();
01585 return norm() * the_inv.norm();
01586 }
01587
01590 bool isSingular() const
01591 {
01592 if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
01593 return false;
01594 return (det() == T(0));
01595 }
01596
01599 bool isDiagonal() const
01600 {
01601 if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
01602 return false;
01603 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
01604 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
01605 if (i != j && CMatrixTemplate<T>::m_Val[i][j] != T(0))
01606 return false;
01607 return true;
01608 }
01609
01612 bool isScalar() const
01613 {
01614 if (!isDiagonal())
01615 return false;
01616 T v = CMatrixTemplate<T>::m_Val[0][0];
01617 for (size_t i=1; i < CMatrixTemplate<T>::m_Rows; i++)
01618 if (CMatrixTemplate<T>::m_Val[i][i] != v)
01619 return false;
01620 return true;
01621 }
01622
01625 bool isUnit() const
01626 {
01627 return (isScalar() && CMatrixTemplate<T>::m_Val[0][0] == T(1));
01628 }
01629
01632 bool isNull() const
01633 {
01634 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
01635 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
01636 if (CMatrixTemplate<T>::m_Val[i][j] != T(0))
01637 return false;
01638 return true;
01639 }
01640
01643 bool isSymmetric() const
01644 {
01645 if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
01646 return false;
01647 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
01648 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
01649 if (CMatrixTemplate<T>::m_Val[i][j] != CMatrixTemplate<T>::m_Val[j][i])
01650 return false;
01651 return true;
01652 }
01653
01656 bool isSkewSymmetric() const
01657 {
01658 if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
01659 return false;
01660 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
01661 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
01662 if (CMatrixTemplate<T>::m_Val[i][j] != -CMatrixTemplate<T>::m_Val[j][i])
01663 return false;
01664 return true;
01665 }
01666
01669 bool isUpperTriangular() const
01670 {
01671 if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
01672 return false;
01673 for (size_t i=1; i < CMatrixTemplate<T>::m_Rows; i++)
01674 for (size_t j=0; j < i-1; j++)
01675 if (CMatrixTemplate<T>::m_Val[i][j] != T(0))
01676 return false;
01677 return true;
01678 }
01679
01682 bool isLowerTriangular() const
01683 {
01684 if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
01685 return false;
01686
01687 for (size_t j=1; j < CMatrixTemplate<T>::m_Cols; j++)
01688 for (size_t i=0; i < j-1; i++)
01689 if (CMatrixTemplate<T>::m_Val[i][j] != T(0))
01690 return false;
01691
01692 return true;
01693 }
01694
01697 std::string inMatlabFormat() const
01698 {
01699 std::string s;
01700 char str[100];
01701 s = "[";
01702 for (size_t i=0;i<CMatrixTemplate<T>::m_Rows;i++)
01703 {
01704 for (size_t j=0;j<CMatrixTemplate<T>::m_Cols;j++)
01705 {
01706 os::sprintf(str,100,"%e ",static_cast<double> (CMatrixTemplate<T>::m_Val[i][j] ) );
01707 s=s+ std::string(str);
01708 }
01709 if (i<CMatrixTemplate<T>::m_Rows-1) s=s+";";
01710 }
01711 s=s+"];";
01712 return s;
01713 }
01717 void matrix_floor()
01718 {
01719 for (size_t i=0;i<CMatrixTemplate<T>::m_Rows;i++)
01720 for (size_t j=0;j<CMatrixTemplate<T>::m_Cols;j++)
01721 CMatrixTemplate<T>::m_Val[i][j] = floor(CMatrixTemplate<T>::m_Val[i][j]);
01722 }
01723
01727 void matrix_floor(CMatrixTemplateNumeric<T> &out)
01728 {
01729 out.setSize(CMatrixTemplate<T>::m_Rows,CMatrixTemplate<T>::m_Cols);
01730 for (size_t i=0;i<CMatrixTemplate<T>::m_Rows;i++)
01731 for (size_t j=0;j<CMatrixTemplate<T>::m_Cols;j++)
01732 out(i,j) = floor(CMatrixTemplate<T>::m_Val[i][j]);
01733 }
01734
01738 void matrix_ceil()
01739 {
01740 for (size_t i=0;i<CMatrixTemplate<T>::m_Rows;i++)
01741 for (size_t j=0;j<CMatrixTemplate<T>::m_Cols;j++)
01742 CMatrixTemplate<T>::m_Val[i][j] = ceil(CMatrixTemplate<T>::m_Val[i][j]);
01743 }
01744
01748 void find_index_max_value(size_t &umax, size_t &vmax, T &max_val) const
01749 {
01750 max_val = CMatrixTemplate<T>::get_unsafe(0,0);
01751 umax = vmax = 0;
01752 for (size_t i=0;i<CMatrixTemplate<T>::getRowCount();i++)
01753 {
01754 for(size_t j=0;j<CMatrixTemplate<T>::getColCount();j++)
01755 {
01756 if (max_val<CMatrixTemplate<T>::get_unsafe(i,j))
01757 {
01758 max_val=CMatrixTemplate<T>::get_unsafe(i,j);
01759 umax=j;
01760 vmax=i;
01761 }
01762 }
01763 }
01764 }
01765
01768 T maximumDiagonal() const
01769 {
01770 if (!CMatrixTemplate<T>::getRowCount() || !CMatrixTemplate<T>::getColCount()) return static_cast<T>(0);
01771 ASSERT_( CMatrixTemplate<T>::getRowCount() == CMatrixTemplate<T>::getColCount() );
01772 T max_val = (*this)(0,0);
01773 for (size_t i=0;i<CMatrixTemplate<T>::getRowCount();i++)
01774 if (max_val<CMatrixTemplate<T>::get_unsafe(i,i))
01775 max_val=CMatrixTemplate<T>::get_unsafe(i,i);
01776
01777 return max_val;
01778 }
01779
01782 T maximum() const
01783 {
01784 if (!CMatrixTemplate<T>::getRowCount() || !CMatrixTemplate<T>::getColCount()) return static_cast<T>(0);
01785 T max_val = (*this)(0,0);
01786 for (size_t i=0;i<CMatrixTemplate<T>::getRowCount();i++)
01787 for(size_t j=0;j<CMatrixTemplate<T>::getColCount();j++)
01788 if (max_val<CMatrixTemplate<T>::get_unsafe(i,j))
01789 max_val=CMatrixTemplate<T>::get_unsafe(i,j);
01790
01791 return max_val;
01792 }
01793
01796 T minimum() const
01797 {
01798 if (!CMatrixTemplate<T>::getRowCount() || !CMatrixTemplate<T>::getColCount()) return static_cast<T>(0);
01799 T min_val = (*this)(0,0);
01800 for (size_t i=0;i<CMatrixTemplate<T>::getRowCount();i++)
01801 for(size_t j=0;j<CMatrixTemplate<T>::getColCount();j++)
01802 if (min_val>CMatrixTemplate<T>::get_unsafe(i,j))
01803 min_val =CMatrixTemplate<T>::get_unsafe(i,j);
01804 return min_val;
01805 }
01806
01810 void find_index_min_value(size_t &umin, size_t &vmin, T &min_val) const
01811 {
01812 ASSERT_(CMatrixTemplate<T>::getRowCount()>0 && CMatrixTemplate<T>::getColCount()>0);
01813 min_val = (*this)(0,0);
01814 for (size_t i=0;i<CMatrixTemplate<T>::getRowCount();i++)
01815 {
01816 for(size_t j=0;j<CMatrixTemplate<T>::getColCount();j++)
01817 {
01818 if (min_val>(*this)(i,j))
01819 {
01820 min_val=(*this)(i,j);
01821 umin=j;
01822 vmin=i;
01823 }
01824 }
01825 }
01826 }
01827
01831 void force_symmetry()
01832 {
01833 if (CMatrixTemplate<T>::m_Rows!=CMatrixTemplate<T>::m_Cols)
01834 THROW_EXCEPTION("Error in force_symmetry. The matrix is not square");
01835
01836 for (size_t i=0;i<CMatrixTemplate<T>::m_Rows-1;i++)
01837 for(size_t j=i+1;j<CMatrixTemplate<T>::m_Cols;j++)
01838 CMatrixTemplate<T>::set_unsafe(j,i, CMatrixTemplate<T>::get_unsafe(i,j) );
01839 }
01840
01841
01845 void mean( std::vector<T> &outMeanVector ) const
01846 {
01847 MRPT_TRY_START;
01848
01849 size_t nCols = CMatrixTemplate<T>::getColCount();
01850 size_t nRows = CMatrixTemplate<T>::getRowCount();
01851 ASSERT_(nCols!=0);
01852 ASSERT_(nRows!=0);
01853
01854
01855 outMeanVector.resize(nCols);
01856
01857 for (size_t c=0;c<nCols;c++)
01858 {
01859 T accum = 0;
01860 for (size_t r=0;r<nRows;r++) accum += (*this)(r,c);
01861 outMeanVector[c] = accum / nRows;
01862 }
01863
01864 MRPT_TRY_END;
01865 }
01866
01870 void meanAndStd(
01871 std::vector<T> &outMeanVector,
01872 std::vector<T> &outStdVector ) const
01873 {
01874 MRPT_TRY_START;
01875
01876 size_t nCols = CMatrixTemplate<T>::getColCount();
01877 size_t nRows = CMatrixTemplate<T>::getRowCount();
01878 ASSERT_(nCols>=1);
01879 ASSERT_(nRows>=1);
01880
01881
01882 outMeanVector.resize(nCols);
01883
01884 for (size_t c=0;c<nCols;c++)
01885 {
01886 T accum = 0;
01887 for (size_t r=0;r<nRows;r++)
01888 accum += (*this)(r,c);
01889 outMeanVector[c] = accum / nRows;
01890 }
01891
01892
01893 outStdVector.resize(nCols);
01894 for (size_t c=0;c<nCols;c++)
01895 {
01896 T accum = 0, thisMean = outMeanVector[c];
01897 for (size_t r=0;r<nRows;r++)
01898 accum += mrpt::utils::square( (*this)(r,c) - thisMean );
01899 if (nRows>1)
01900 outStdVector[c] = ::sqrt(accum / (nRows-1));
01901 else outStdVector[c] = ::sqrt(accum / nRows);
01902 }
01903
01904 MRPT_TRY_END;
01905 }
01906
01910 void meanAndStdAll(
01911 T &outMean,
01912 T &outStd ) const
01913 {
01914 MRPT_TRY_START;
01915
01916 size_t nCols = CMatrixTemplate<T>::getColCount();
01917 size_t nRows = CMatrixTemplate<T>::getRowCount();
01918 size_t c;
01919 ASSERT_(nCols!=0);
01920 ASSERT_(nRows!=0);
01921
01922
01923 outMean=0;
01924
01925 for (c=0;c<nCols;c++)
01926 for (size_t r=0;r<nRows;r++)
01927 outMean += (*this)(r,c);
01928 outMean /= nRows*nCols;
01929
01930
01931 outStd=0;
01932 for (c=0;c<nCols;c++)
01933 for (size_t r=0;r<nRows;r++)
01934 outStd += mrpt::utils::square( (*this)(r,c) - outMean );
01935 outStd = ::sqrt(outStd / (nRows*nCols) );
01936
01937 MRPT_TRY_END;
01938 }
01939
01945 template <class R>
01946 void extractMatrix(size_t nRow, size_t nCol, CMatrixTemplateNumeric<R> &in) const
01947 {
01948 size_t i,j,ncols,nrows;
01949
01950 nrows = in.getRowCount();
01951 ncols = in.getColCount();
01952 if ( (nRow+nrows > CMatrixTemplate<T>::getRowCount()) || (nCol+ncols >CMatrixTemplate<T>::getColCount()) )
01953 THROW_EXCEPTION("extractMatrix: Row or Col index out of bounds");
01954
01955 for (i=nRow;i<nRow+nrows;i++)
01956 for(j=nCol;j<nCol+ncols;j++)
01957 in.set_unsafe( i-nRow,j-nCol, static_cast<R> ( CMatrixTemplate<T>::m_Val[i][j] ) );
01958 }
01959
01965 void extractMatrix(size_t nRow, size_t nCol,size_t nrows, size_t ncols, CMatrixTemplateNumeric<T> &outMat) const
01966 {
01967 size_t i,j;
01968 outMat.setSize(nrows,ncols);
01969
01970 if ( (nRow+nrows > CMatrixTemplate<T>::getRowCount()) || (nCol+ncols >CMatrixTemplate<T>::getColCount()) )
01971 THROW_EXCEPTION("extractMatrix: Row or Col index out of bounds");
01972
01973 for (i=nRow;i<(nRow+nrows);i++)
01974 for(j=nCol;j<(nCol+ncols);j++)
01975 outMat(i-nRow,j-nCol) = CMatrixTemplate<T>::m_Val[i][j];
01976 }
01977
01983 void extractMatrix(size_t nRow, size_t nCol, std::vector<T> &in) const
01984 {
01985 size_t j,ncols,nrows;
01986
01987 ncols = in.size();
01988 nrows = 1;
01989 if ( (nRow+nrows > CMatrixTemplate<T>::getRowCount()) || (nCol+ncols >CMatrixTemplate<T>::getColCount()) )
01990 THROW_EXCEPTION("extractMatrix: Row or Col index out of bounds");
01991
01992 for(j=nCol;j<nCol+ncols;j++)
01993 in[j-nCol] = CMatrixTemplate<T>::m_Val[nRow][j];
01994 }
01995
02001 std::vector<T> extractMatrix(size_t nRow, size_t nCol, size_t ncols) const
02002 {
02003 size_t j;
02004 std::vector<T> out;
02005 out.resize(ncols);
02006
02007 if ( (nRow+1 > CMatrixTemplate<T>::getRowCount()) || (nCol+ncols >CMatrixTemplate<T>::getColCount()) )
02008 THROW_EXCEPTION("extractMatrix: Row or Col index out of bounds");
02009
02010 for(j=nCol;j<nCol+ncols;j++)
02011 out[j-nCol] = CMatrixTemplate<T>::m_Val[nRow][j];
02012 return out;
02013 }
02014
02015 void asCol()
02016 {
02017 CMatrixTemplateNumeric<T> aux;
02018 aux.setSize(CMatrixTemplate<T>::m_Cols*CMatrixTemplate<T>::m_Rows,1);
02019
02020 for (size_t i=0;i<CMatrixTemplate<T>::m_Rows;i++)
02021 for (size_t j=0;j<CMatrixTemplate<T>::m_Cols;j++)
02022 aux(j+i*CMatrixTemplate<T>::m_Cols,0)=CMatrixTemplate<T>::m_Val[i][j];
02023 (*this) = aux;
02024 }
02025
02026
02027 void asRow()
02028 {
02029 CMatrixTemplateNumeric<T> aux;
02030 aux.setSize(1,CMatrixTemplate<T>::m_Cols*CMatrixTemplate<T>::m_Rows);
02031
02032 for (size_t i=0;i<CMatrixTemplate<T>::m_Rows;i++)
02033 for (size_t j=0;j<CMatrixTemplate<T>::m_Rows;j++)
02034 aux(0,j+i*CMatrixTemplate<T>::m_Cols)=CMatrixTemplate<T>::m_Val[i][j];
02035 (*this) = aux;
02036 }
02037
02038 void asCol(CMatrixTemplateNumeric<T> &aux) const
02039 {
02040 aux.setSize(CMatrixTemplate<T>::m_Cols*CMatrixTemplate<T>::m_Rows,1);
02041
02042 for (size_t i=0;i<CMatrixTemplate<T>::m_Rows;i++)
02043 for (size_t j=0;j<CMatrixTemplate<T>::m_Cols;j++)
02044 aux(j+i*CMatrixTemplate<T>::m_Cols,0)=CMatrixTemplate<T>::m_Val[i][j];
02045 }
02046
02047
02048 void asRow(CMatrixTemplateNumeric<T> &aux) const
02049 {
02050 aux.setSize(1,CMatrixTemplate<T>::m_Cols*CMatrixTemplate<T>::m_Rows);
02051
02052 for (size_t i=0;i<CMatrixTemplate<T>::m_Rows;i++)
02053 for (size_t j=0;j<CMatrixTemplate<T>::m_Rows;j++)
02054 aux(0,j+i*CMatrixTemplate<T>::m_Cols)=CMatrixTemplate<T>::m_Val[i][j];
02055 }
02056
02057
02065 void findElementsPassingMahalanobisThreshold(
02066 double stdTimes,
02067 std::vector<size_t> &rowIndexes,
02068 std::vector<size_t> &colIndexes,
02069 bool below = false ) const
02070 {
02071 MRPT_TRY_START;
02072
02073 size_t nCols = CMatrixTemplate<T>::getColCount();
02074 size_t nRows = CMatrixTemplate<T>::getRowCount();
02075
02076 rowIndexes.clear();
02077 colIndexes.clear();
02078
02079
02080 T mean,std;
02081 meanAndStdAll(mean,std);
02082
02083
02084 double thres = mean + stdTimes * std * (below ? (-1):1);
02085
02086 if (below)
02087 {
02088 for (size_t c=0;c<nCols;c++)
02089 for (size_t r=0;r<nRows;r++)
02090 if ( (*this)(r,c) < thres )
02091 {
02092 rowIndexes.push_back(r);
02093 colIndexes.push_back(c);
02094 }
02095 }
02096 else
02097 {
02098 for (size_t c=0;c<nCols;c++)
02099 for (size_t r=0;r<nRows;r++)
02100 if ( (*this)(r,c) > thres )
02101 {
02102 rowIndexes.push_back(r);
02103 colIndexes.push_back(c);
02104 }
02105 }
02106
02107 MRPT_TRY_END;
02108 }
02109
02113 inline void normalize( T minVal=0,T maxVal=1)
02114 {
02115 adjustRange(minVal,maxVal);
02116 }
02117
02120 void adjustRange( T minVal=0,T maxVal=1)
02121 {
02122 MRPT_TRY_START;
02123 if (!CMatrixTemplate<T>::getRowCount() || !CMatrixTemplate<T>::getColCount()) return;
02124 T curMin = minimum();
02125 T curMax = maximum();
02126 T curRan = curMax-curMin;
02127 (*this) -= (curMin+minVal);
02128 if (curRan!=0) (*this) *= (maxVal-minVal)/curRan;
02129 MRPT_TRY_END;
02130 }
02131
02135 T sumAll() const
02136 {
02137 MRPT_TRY_START;
02138 size_t nCols = CMatrixTemplate<T>::getColCount();
02139 size_t nRows = CMatrixTemplate<T>::getRowCount();
02140 T accum=0;
02141
02142 for (size_t c=0;c<nCols;c++)
02143 for (size_t r=0;r<nRows;r++)
02144 accum += CMatrixTemplate<T>::m_Val[r][c];
02145
02146 return accum;
02147 MRPT_TRY_END;
02148 }
02149
02154 T sum(
02155 size_t firstRow = 0,
02156 size_t firstCol = 0,
02157 size_t lastRow = std::numeric_limits<size_t>::max(),
02158 size_t lastCol = std::numeric_limits<size_t>::max() ) const
02159 {
02160 MRPT_TRY_START;
02161 size_t col_1 = CMatrixTemplate<T>::getColCount();
02162 size_t row_1 = CMatrixTemplate<T>::getRowCount();
02163
02164 if (col_1==0 || row_1==0) return 0;
02165
02166 col_1--; row_1--;
02167
02168 if (lastCol!=std::numeric_limits<size_t>::max())
02169 {
02170 ASSERT_(lastCol>= firstCol);
02171 col_1=lastCol;
02172 }
02173 if (lastRow!=std::numeric_limits<size_t>::max())
02174 {
02175 ASSERT_(lastRow>= firstRow );
02176 row_1=lastRow;
02177 }
02178
02179 ASSERT_( row_1 < CMatrixTemplate<T>::getRowCount() );
02180 ASSERT_( col_1 < CMatrixTemplate<T>::getColCount() );
02181
02182 T accum=0;
02183
02184 for (size_t c=firstCol;c<=col_1;c++)
02185 for (size_t r=firstRow;r<=row_1;r++)
02186 accum += CMatrixTemplate<T>::m_Val[r][c];
02187
02188 return accum;
02189 MRPT_TRY_END;
02190 }
02191
02195 void multiplyByMatrixAndByTransposeNonSymmetric(
02196 const CMatrixTemplateNumeric<T> &C,
02197 CMatrixTemplateNumeric<T> &R,
02198 bool accumOnOutput = false,
02199 bool substractInsteadOfSum = false
02200 ) const
02201 {
02202 MRPT_TRY_START;
02203 ASSERT_( (C.m_Rows == C.m_Cols) && (C.m_Rows == this->CMatrixTemplate<T>::m_Cols) );
02204 ASSERT_( &C != this );
02205 ASSERT_( &R != this );
02206 size_t N = CMatrixTemplate<T>::m_Rows;
02207 size_t M = CMatrixTemplate<T>::m_Cols;
02208 size_t i,j,k,l;
02209 T sumAccumInner;
02210
02211 if (accumOnOutput)
02212 R.setSize(N,N);
02213 else R.zeros(N,N);
02214
02215 if (substractInsteadOfSum)
02216 {
02217 for (i=0;i<N;i++)
02218 {
02219 for (l=0;l<M;l++)
02220 {
02221 sumAccumInner = 0;
02222 for (k=0;k<M;k++)
02223 sumAccumInner += CMatrixTemplate<T>::m_Val[i][k] * C.m_Val[k][l];
02224 for (j=0;j<N;j++)
02225 R.m_Val[i][j] -= sumAccumInner * CMatrixTemplate<T>::m_Val[j][l];
02226 }
02227 }
02228 }
02229 else
02230 {
02231 for (i=0;i<N;i++)
02232 {
02233 for (l=0;l<M;l++)
02234 {
02235 sumAccumInner = 0;
02236 for (k=0;k<M;k++)
02237 sumAccumInner += CMatrixTemplate<T>::m_Val[i][k] * C.m_Val[k][l];
02238 for (j=0;j<N;j++)
02239 R.m_Val[i][j] += sumAccumInner * CMatrixTemplate<T>::m_Val[j][l];
02240 }
02241 }
02242 }
02243
02244 MRPT_TRY_END;
02245 }
02246
02251 void multiplyABC(
02252 const CMatrixTemplateNumeric<T> &A,
02253 const CMatrixTemplateNumeric<T> &B,
02254 const CMatrixTemplateNumeric<T> &C)
02255 {
02256 MRPT_TRY_START;
02257 if ((A.m_Cols != B.m_Rows)||(B.m_Cols != C.m_Rows))
02258 THROW_EXCEPTION("Wrong Matrix sizes");
02259
02260 size_t i,j,k,l;
02261 T sumAccumInner;
02262
02263 (*this).zeros(A.m_Rows,C.m_Cols);
02264
02265 for (i=0;i<A.m_Rows;i++)
02266 for (l=0;l<B.m_Cols;l++)
02267 {
02268 sumAccumInner = 0;
02269 for (k=0;k<A.m_Cols;k++)
02270 sumAccumInner += A.m_Val[i][k] * B.m_Val[k][l];
02271
02272 for (j=0;j<C.m_Cols;j++)
02273 CMatrixTemplate<T>::m_Val[i][j] += sumAccumInner * C.m_Val[l][j];
02274 }
02275
02276 MRPT_TRY_END;
02277 }
02278
02282 void multiplyABCt(
02283 const CMatrixTemplateNumeric<T> &A,
02284 const CMatrixTemplateNumeric<T> &B,
02285 const CMatrixTemplateNumeric<T> &C)
02286 {
02287 MRPT_TRY_START;
02288 if ((A.m_Cols != B.m_Rows)||(B.m_Cols != C.m_Cols))
02289 THROW_EXCEPTION("Wrong Matrix sizes");
02290
02291 size_t i,j,k,l;
02292 T sumAccumInner;
02293
02294 zeros(A.m_Rows,C.m_Rows);
02295
02296 for (i=0;i<A.m_Rows;i++)
02297 for (l=0;l<B.m_Cols;l++)
02298 {
02299 sumAccumInner = 0;
02300 for (k=0;k<A.m_Cols;k++)
02301 sumAccumInner += A.m_Val[i][k] * B.m_Val[k][l];
02302
02303 for (j=0;j<C.m_Rows;j++)
02304 CMatrixTemplate<T>::m_Val[i][j] += sumAccumInner * C.m_Val[j][l];
02305 }
02306
02307 MRPT_TRY_END;
02308 }
02309
02326 void multiplyByMatrixAndByTranspose(
02327 const CMatrixTemplateNumeric<T> &C,
02328 CMatrixTemplateNumeric<T> &R,
02329 bool allowSubMatrixMultiplication = false,
02330 size_t subMatrixOffset = 0,
02331 bool accumResultInOutput = false ) const
02332 {
02333 MRPT_TRY_START;
02334
02335 size_t C_start = allowSubMatrixMultiplication ? subMatrixOffset : 0;
02336
02337 ASSERT_( C.m_Rows == C.m_Cols);
02338
02339 if (!allowSubMatrixMultiplication )
02340 {
02341 ASSERT_( C.m_Rows == this->CMatrixTemplate<T>::m_Cols );
02342 }
02343 else
02344 {
02345 ASSERT_( C_start + this->CMatrixTemplate<T>::m_Cols <= C.m_Rows );
02346 }
02347
02348 ASSERT_( &C != this );
02349 ASSERT_( &R != this );
02350 size_t N = CMatrixTemplate<T>::m_Rows;
02351 size_t M = CMatrixTemplate<T>::m_Cols;
02352 size_t i,j,k,l;
02353 T sumAccum,sumAccumInner;
02354
02355 if ( accumResultInOutput )
02356 {
02357 ASSERT_( R.getColCount()==N && R.getRowCount()==N )
02358 }
02359 else
02360 {
02361 R.zeros(N,N);
02362 }
02363
02364 if (N*M>100)
02365 {
02366 CMatrixTemplateNumeric<T> R_(N,M);
02367
02368
02369 for (i=0;i<N;i++)
02370 for (j=0;j<M;j++)
02371 {
02372 sumAccum = 0;
02373 for (l=0;l<M;l++)
02374 sumAccum += CMatrixTemplate<T>::m_Val[i][l]*C.m_Val[l+C_start][j+C_start];
02375 R_.m_Val[i][j] = sumAccum;
02376 }
02377
02378
02379 for (i=0;i<N;i++)
02380 for (j=i;j<N;j++)
02381 {
02382 sumAccum = R.m_Val[i][j];
02383 for (l=0;l<M;l++)
02384 sumAccum += R_.m_Val[i][l] * CMatrixTemplate<T>::m_Val[j][l];
02385 R.m_Val[i][j] = R.m_Val[j][i] = sumAccum;
02386 }
02387 }
02388 else
02389 {
02390
02391 for (i=0;i<N;i++)
02392 {
02393 for (j=i;j<N;j++)
02394 {
02395 sumAccum = R.m_Val[i][j];
02396 for (l=0;l<M;l++)
02397 {
02398 sumAccumInner = 0;
02399 for (k=0;k<M;k++)
02400 sumAccumInner += CMatrixTemplate<T>::m_Val[i][k] * C.m_Val[k+C_start][l+C_start];
02401 sumAccum += sumAccumInner * CMatrixTemplate<T>::m_Val[j][l];
02402 }
02403 R.m_Val[i][j] = R.m_Val[j][i] = sumAccum;
02404 }
02405 }
02406 }
02407
02408 MRPT_TRY_END;
02409 }
02410
02419 T multiplyByMatrixAndByTransposeScalar(
02420 const CMatrixTemplateNumeric<T> &C ) const
02421 {
02422 MRPT_TRY_START;
02423 ASSERT_( (C.m_Rows == C.m_Cols) );
02424 ASSERT_( &C != this );
02425 size_t N = CMatrixTemplate<T>::m_Rows;
02426 size_t M = CMatrixTemplate<T>::m_Cols;
02427 size_t k,l;
02428 T sumAccum,sumAccumInner;
02429
02430 if (N==1)
02431 {
02432
02433 ASSERT_( C.m_Rows == M );
02434
02435 sumAccum = 0;
02436 for (l=0;l<M;l++)
02437 {
02438 sumAccumInner = 0;
02439 for (k=0;k<M;k++)
02440 sumAccumInner += CMatrixTemplate<T>::m_Val[0][k] * C.m_Val[k][l];
02441 sumAccum += sumAccumInner * CMatrixTemplate<T>::m_Val[0][l];
02442 }
02443 return sumAccum;
02444
02445 }
02446 else
02447 if (M==1)
02448 {
02449
02450
02451 ASSERT_( C.m_Rows == N );
02452
02453 sumAccum = 0;
02454 for (l=0;l<N;l++)
02455 {
02456 sumAccumInner = 0;
02457 for (k=0;k<N;k++)
02458 sumAccumInner += CMatrixTemplate<T>::m_Val[k][0] * C.m_Val[k][l];
02459 sumAccum += sumAccumInner * CMatrixTemplate<T>::m_Val[l][0];
02460 }
02461 return sumAccum;
02462 }
02463 else THROW_EXCEPTION("This matrix must be a one-column or a one-row matrix!");
02464
02465
02466 MRPT_TRY_END;
02467 }
02468
02469
02470 private:
02473 int pivot(size_t row)
02474 {
02475 size_t k = row;
02476 double amax,temp;
02477
02478 amax = -1;
02479 for (size_t i=row; i < CMatrixTemplate<T>::m_Rows; i++)
02480 if ( (temp = fabs( CMatrixTemplate<T>::m_Val[i][row])) > amax && temp != 0)
02481 {
02482 amax = temp;
02483 k = i;
02484 }
02485 if (CMatrixTemplate<T>::m_Val[k][row] == T(0))
02486 return -1;
02487 if (k != row)
02488 {
02489 T* rowptr = CMatrixTemplate<T>::m_Val[k];
02490 CMatrixTemplate<T>::m_Val[k] = CMatrixTemplate<T>::m_Val[row];
02491 CMatrixTemplate<T>::m_Val[row] = rowptr;
02492 return static_cast<int>( k );
02493 }
02494 return 0;
02495 }
02496
02504 static void tred2(T **a, size_t nn, T d[], T e[])
02505 {
02506 int l,k,j,i;
02507 int n = static_cast<int> (nn);
02508 T scale,hh,h,g,f;
02509
02510 for (i=n;i>=2;i--)
02511 {
02512 l=i-1;
02513 h=scale=0.0;
02514 if (l > 1)
02515 {
02516 for (k=1;k<=l;k++)
02517 scale += fabs(a[i][k]);
02518 if (scale == 0)
02519 e[i]=a[i][l];
02520 else
02521 {
02522 for (k=1;k<=l;k++)
02523 {
02524 a[i][k] /= scale;
02525 h += a[i][k]*a[i][k];
02526 }
02527 f=a[i][l];
02528 g=(f >= 0 ? -::sqrt(h) : (::sqrt(h)));
02529 e[i]=scale*g;
02530 h -= f*g;
02531 a[i][l]=f-g;
02532 f=0;
02533 for (j=1;j<=l;j++)
02534 {
02535 a[j][i]=a[i][j]/h;
02536 g=0.0;
02537 for (k=1;k<=j;k++)
02538 g += a[j][k]*a[i][k];
02539 for (k=j+1;k<=l;k++)
02540 g += a[k][j]*a[i][k];
02541 e[j]=g/h;
02542 f += e[j]*a[i][j];
02543 }
02544 hh=f/(h+h);
02545 for (j=1;j<=l;j++)
02546 {
02547 f=a[i][j];
02548 e[j]=g=e[j]-hh*f;
02549 for (k=1;k<=j;k++)
02550 a[j][k] -= (f*e[k]+g*a[i][k]);
02551 }
02552 }
02553 } else
02554 e[i]=a[i][l];
02555 d[i]=h;
02556 }
02557
02558
02559 d[1]=0;
02560 e[1]=0;
02561
02562
02563
02564 for (i=1;i<=n;i++)
02565 {
02566 l= i-1;
02567 if (d[i])
02568 for (j=1;j<=l;j++)
02569 {
02570 g=0.0;
02571 for (k=1;k<=l;k++)
02572 g += a[i][k]*a[k][j];
02573 for (k=1;k<=l;k++)
02574 a[k][j] -= g*a[k][i];
02575 }
02576 d[i]=a[i][i];
02577 a[i][i]=1;
02578 for (j=1;j<=l;j++)
02579 a[j][i]=a[i][j]= 0;
02580 }
02581 }
02582
02593 static void tqli(T d[], T e[], size_t nn, T **z)
02594 {
02595 int m,l,iter,i,k;
02596 int n = static_cast<int> (nn);
02597 T s,r,p,g,f,dd,c,b;
02598
02599 const T EPS= std::numeric_limits<T>::epsilon();
02600
02601 for (i=2;i<=n;i++)
02602 e[i-1]=e[i];
02603
02604 e[n]=0.0;
02605 for (l=1;l<=n;l++)
02606 {
02607 iter=0;
02608 do
02609 {
02610 for (m=l;m<=(n-1);m++)
02611 {
02612 dd=static_cast<T>(( fabs(d[m])+fabs(d[m+1]) ) );
02613
02614
02615
02616
02617
02618
02619
02620
02621 if (fabs(e[m]) <= EPS*dd) break;
02622 }
02623 if (m != l)
02624 {
02625 if (iter++ == 60) THROW_EXCEPTION("tqli: Too many iterations in tqli!")
02626
02627 g=static_cast<T>(( (d[l+1]-d[l])/(2.0*e[l])));
02628 r=pythag(g,1.0);
02629 g=d[m]-d[l]+e[l]/(g+SIGN(r,g));
02630 s = c = 1;
02631 p = 0;
02632 for (i=m-1;i>=l;i--)
02633 {
02634 f=s*e[i];
02635 b=c*e[i];
02636 e[i+1]=(r=pythag(f,g));
02637 if (r == 0.0)
02638 {
02639 d[i+1] -= p;
02640 e[m]=0.0;
02641 break;
02642 }
02643 s=f/r;
02644 c=g/r;
02645 g=d[i+1]-p;
02646 r=(d[i]-g)*s+2.0f*c*b;
02647 d[i+1]=g+(p=s*r);
02648 g=c*r-b;
02649
02650 for (k=1;k<=n;k++)
02651 {
02652 f=z[k][i+1];
02653 z[k][i+1]=s*z[k][i]+c*f;
02654 z[k][i]=c*z[k][i]-s*f;
02655 }
02656 }
02657
02658 if (r == 0.0 && i >= l) continue;
02659 d[l] -= p;
02660 e[l]=g;
02661 e[m]=0.0;
02662 }
02663 } while (m != l);
02664 }
02665 }
02666
02669 static T pythag(T a, T b)
02670 {
02671 static T at, bt, ct;
02672 return static_cast<T>(( ((at = fabs(a)) > (bt = fabs(b)) ?
02673 (ct = bt/at, at*(::sqrt(1.0+ct*ct))) :
02674 (bt ? (ct = at/bt, bt*(::sqrt(1.0+ct*ct))): 0)) ) );
02675 }
02676
02679 static T SIGN(T a,T b)
02680 {
02681 return static_cast<T>(( ((b) >= 0 ? fabs(a) : -fabs(a)) ));
02682 }
02683
02687 static void svdcmp(T* a[], int m, int n, T w[], T* v[])
02688 {
02689
02690
02691
02692
02693
02694
02695
02696 int flag, i, its, j, jj, k, l, nm;
02697 double c, f, h, s, x, y, z;
02698 double anorm = 0.0, g = 0.0, scale = 0.0;
02699
02700 if (m < n) THROW_EXCEPTION("svdcmp(): Matrix is not augmented with extra rows of zeros");
02701 std::vector<double> rv1(n);
02702 const T EPS= std::numeric_limits<T>::epsilon();
02703
02704
02705
02706 l = 0;
02707 nm = 0;
02708 for (i = 0; i < n; i++)
02709 {
02710 l = i + 1;
02711 rv1[i] = scale*g;
02712 g = s = scale = 0.0;
02713 if (i < m) {
02714 for (k = i; k < m; k++) scale += fabs(a[k][i]);
02715 if (scale) {
02716 for (k = i; k < m; k++) {
02717 a[k][i] /= static_cast<T>(scale);
02718 s += a[k][i]*a[k][i];
02719 }
02720 f = a[i][i];
02721 g = -SIGN(::sqrt(static_cast<T>(s)),static_cast<T>(f));
02722 h = f*g - s;
02723 a[i][i] = static_cast<T>((f - g));
02724 if (i != n - 1) {
02725 for (j = l; j < n; j++) {
02726 for (s = 0.0, k = i; k < m; k++) s += a[k][i]*a[k][j];
02727 f = s/h;
02728 for ( k = i; k < m; k++) a[k][j] += static_cast<T>((f*a[k][i]));
02729 }
02730 }
02731 for (k = i; k < m; k++) a[k][i] *= static_cast<T>(scale);
02732 }
02733 }
02734 w[i] = static_cast<T>((scale*g));
02735 g = s= scale = 0.0;
02736 if (i < m && i != n - 1) {
02737 for (k = l; k < n; k++) scale += fabs(a[i][k]);
02738 if (scale) {
02739 for (k = l; k < n; k++) {
02740 a[i][k] /= static_cast<T>(scale);
02741 s += a[i][k]*a[i][k];
02742 }
02743 f = a[i][l];
02744 g = -SIGN(::sqrt(static_cast<T>(s)), static_cast<T>(f));
02745 h = f*g - s;
02746 a[i][l] = static_cast<T>((f - g));
02747 for (k = l; k < n; k++) rv1[k] = a[i][k]/h;
02748 if (i != m - 1) {
02749 for (j = l; j < m; j++) {
02750 for (s = 0.0, k = l; k < n; k++) s += a[j][k]*a[i][k];
02751 for (k = l; k < n; k++) a[j][k] += static_cast<T>((s*rv1[k]));
02752 }
02753 }
02754 for (k = l; k < n; k++) a[i][k] *= static_cast<T>(scale);
02755 }
02756 }
02757 anorm = std::max(anorm, (fabs(w[i]) + fabs(rv1[i])));
02758 }
02759
02760 for (i = n - 1; 0 <= i; i--) {
02761 if (i < n - 1) {
02762 if (g) {
02763 for (j = l; j < n; j++) v[j][i] = static_cast<T>(((a[i][j]/a[i][l])/g));
02764
02765 for (j = l; j < n; j++) {
02766 for (s = 0.0, k = l; k < n; k++) s += a[i][k]*v[k][j];
02767 for (k = l; k < n; k++) v[k][j] += static_cast<T>((s*v[k][i]));
02768 }
02769 }
02770 for (j = l; j < n; j++) v[i][j] = v[j][i] = 0.0;
02771 }
02772 v[i][i] = 1.0;
02773 g = rv1[i];
02774 l = i;
02775 }
02776
02777 for (i = n - 1; 0 <= i; i--) {
02778 l = i + 1;
02779 g = w[i];
02780 if (i < n - 1) for (j = l; j < n; j++) a[i][j] = 0.0;
02781 if (g) {
02782 g = 1.0/g;
02783 if (i != n - 1) {
02784 for (j = l; j < n; j++) {
02785 for (s = 0.0, k = l; k < m; k++) s += a[k][i]*a[k][j];
02786 f = (s/a[i][i])*g;
02787 for (k = i; k < m; k++) a[k][j] += static_cast<T>((f*a[k][i]));
02788 }
02789 }
02790 for (j = i; j < m; j++) a[j][i] *= static_cast<T>(g);
02791 }
02792 else {
02793 for (j = i; j < m; j++) a[j][i] = 0.0;
02794 }
02795 a[i][i] += 1.0;
02796 }
02797
02798 for (k = n - 1; 0 <= k; k--) {
02799 for (its = 0; its < 30; its++) {
02800 flag = 1;
02801 for (l = k; 0 <= l; l--) {
02802 nm = l - 1;
02803
02804
02805 if (fabs(rv1[l]) <= EPS*anorm)
02806 {
02807 flag = 0;
02808 break;
02809 }
02810 if ( fabs(w[nm]) + anorm == anorm) break;
02811 }
02812 if (flag) {
02813 c = 0.0;
02814 s = 1.0;
02815 for (i = l; i <= k; i++) {
02816 f = s*rv1[i];
02817 if (fabs(f) + anorm != anorm) {
02818 g = w[i];
02819 h = pythag(static_cast<T>(f), static_cast<T>(g));
02820 w[i] = static_cast<T>(h);
02821 h = static_cast<T>((1.0/h));
02822 c = static_cast<T>((g*h));
02823 s = (-f*h);
02824 for (j = 0; j < m; j++) {
02825 y = a[j][nm];
02826 z = a[j][i];
02827 a[j][nm] = static_cast<T>((y*c + z*s));
02828 a[j][i] = static_cast<T>((z*c - y*s));
02829 }
02830 }
02831 }
02832 }
02833 z = w[k];
02834 if (l == k) {
02835 if (z < 0.0) {
02836 w[k] = static_cast<T>(-z);
02837 for (j = 0; j < n; j++) v[j][k] = (-v[j][k]);
02838 }
02839 break;
02840 }
02841 if (its == 29)
02842 THROW_EXCEPTION("svdcmp(): Not convergence in 30 SVDCMP iterations!");
02843
02844 x = w[l];
02845 nm = k - 1;
02846 y = w[nm];
02847 g = rv1[nm];
02848 h = rv1[k];
02849 f = ((y - z)*(y + z) + (g - h)*(g + h))/(2.0*h*y);
02850 g = pythag(static_cast<T>(f), static_cast<T>(1.0));
02851 f = ((x - z)*(x + z) + h*((y/(f + SIGN(static_cast<T>(g), static_cast<T>(f)))) - h))/x;
02852
02853 c = s = 1.0;
02854 for (j = l; j <= nm; j++) {
02855 i = j + 1;
02856 g = rv1[i];
02857 y = w[i];
02858 h = s*g;
02859 g = c*g;
02860 z = pythag(static_cast<T>(f), static_cast<T>(h));
02861 rv1[j] = z;
02862 c = f/z;
02863 s = h/z;
02864 f = x*c + g*s;
02865 g = g*c - x*s;
02866 h = y*s;
02867 y = y*c;
02868 for (jj = 0; jj < n; jj++) {
02869 x = v[jj][j];
02870 z = v[jj][i];
02871 v[jj][j] = static_cast<T>(x*c + z*s);
02872 v[jj][i] = static_cast<T>(z*c - x*s);
02873 }
02874 z = pythag(static_cast<T>(f), static_cast<T>(h));
02875 w[j] = static_cast<T>(z);
02876 if (z) {
02877 z = 1.0/z;
02878 c = f*z;
02879 s = h*z;
02880 }
02881 f = (c*g) + (s*y);
02882 x = (c*y) - (s*g);
02883 for (jj = 0; jj < m; jj++) {
02884 y = a[jj][j];
02885 z = a[jj][i];
02886 a[jj][j] = static_cast<T>(y*c + z*s);
02887 a[jj][i] = static_cast<T>(z*c - y*s);
02888 }
02889 }
02890 rv1[l] = 0.0;
02891 rv1[k] = f;
02892 w[k] = static_cast<T>(x);
02893 }
02894 }
02895 }
02896
02897
02898 };
02899
02900
02903 template <class T>
02904 bool operator == (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
02905 {
02906 if (m1.getRowCount() != m2.getRowCount() || m1.getColCount() != m2.getColCount())
02907 return false;
02908
02909 for (size_t i=0; i < m1.getRowCount(); i++)
02910 for (size_t j=0; j < m1.getColCount(); j++)
02911 if (m1(i,j) != m2(i,j))
02912 return false;
02913
02914 return true;
02915 }
02916
02919 template <class T>
02920 bool operator != (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
02921 {
02922 return !(m1 == m2);
02923 }
02924
02927 template <class T>
02928 CMatrixTemplateNumeric<T> operator + (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
02929 {
02930 CMatrixTemplateNumeric<T> temp(m1);
02931 temp += m2;
02932 return temp;
02933 }
02934
02937 template <class T>
02938 CMatrixTemplateNumeric<T> operator - (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
02939 {
02940 CMatrixTemplateNumeric<T> temp(m1);
02941 temp -= m2;
02942 return temp;
02943 }
02944
02947 template <class T>
02948 CMatrixTemplateNumeric<T> operator * (const CMatrixTemplateNumeric<T>& m, const T& no)
02949 {
02950 CMatrixTemplateNumeric<T> temp(m);
02951 temp*=no;
02952 return temp;
02953 }
02954
02957 template <class T>
02958 CMatrixTemplateNumeric<T> operator * (const T& no, const CMatrixTemplateNumeric<T>& m)
02959 {
02960 CMatrixTemplateNumeric<T> temp(m);
02961 temp*=no;
02962 return temp;
02963 }
02964
02967 template <class T>
02968 CMatrixTemplateNumeric<T> operator * (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
02969 {
02970 CMatrixTemplateNumeric<T> res(m1);
02971 res*=m2;
02972 return res;
02973 }
02974
02977 template <class T>
02978 CMatrixTemplateNumeric<T> operator * (const CMatrixTemplateNumeric<T>& m1, const CVectorTemplate<T>& m2)
02979 {
02980 CMatrixTemplateNumeric<T> res(m1.getRowCount(),1);
02981 res.multiply(m1,m2);
02982 return res;
02983 }
02984
02987 template <class T>
02988 CMatrixTemplateNumeric<T> operator / (const CMatrixTemplateNumeric<T>& m, const T& no)
02989 {
02990 return (m * (T(1) / no));
02991 }
02992
02995 template <class T>
02996 CMatrixTemplateNumeric<T> operator / (const T& no, const CMatrixTemplateNumeric<T>& m)
02997 {
02998 return (!m * no);
02999 }
03000
03003 template <class T>
03004 CMatrixTemplateNumeric<T> operator / (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
03005 {
03006 return (m1 * !m2);
03007 }
03008
03011 template <class T>
03012 CMatrixTemplateNumeric<T> operator ^ (const CMatrixTemplateNumeric<T>& m, const unsigned int& pow)
03013 {
03014 CMatrixTemplateNumeric<T> temp(m);
03015 temp ^= pow;
03016 return temp;
03017 }
03018
03021 template <class T>
03022 CMatrixTemplateNumeric<T> operator ~ (const CMatrixTemplateNumeric<T>& m)
03023 {
03024 CMatrixTemplateNumeric<T> temp(m.getColCount(),m.getRowCount());
03025
03026 for (size_t i=0; i < m.getRowCount(); i++)
03027 for (size_t j=0; j < m.getColCount(); j++)
03028 {
03029 T x = m(i,j);
03030 temp(j,i) = x;
03031 }
03032 return temp;
03033 }
03034
03037 template <class T>
03038 CMatrixTemplateNumeric<T> operator ! (const CMatrixTemplateNumeric<T> &m)
03039 {
03040 return m.inv();
03041 }
03042
03045 #define DEBUG_SAVE_MATRIX(M) \
03046 { \
03047 char auxStr[100]; \
03048 os::sprintf(auxStr,99,"%s.txt",#M); \
03049 M.saveToTextFile(auxStr); \
03050 } \
03051
03052
03057 typedef CMatrixTemplateNumeric<float> CMatrixFloat;
03058
03063 typedef CMatrixTemplateNumeric<double> CMatrixDouble;
03064
03068 typedef CMatrixTemplateNumeric<unsigned int> CMatrixUInt;
03069
03073 typedef CMatrixTemplate<bool> CMatrixBool;
03074
03075
03076 }
03077 }
03078
03079 #endif