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 poses
00039 {
00040 class CPose2D;
00041 class CPose3D;
00042 class CPoint2D;
00043 class CPoint3D;
00044 }
00045 namespace math
00046 {
00047 using namespace mrpt::system;
00048
00049
00050 template <class T> class CVectorTemplate;
00051 template <typename T,size_t NROWS,size_t NCOLS> class CMatrixFixedNumeric;
00052 template <class T> class MRPTDLLIMPEXP CMatrixTemplateNumeric;
00053 template <typename T,size_t NROWS,size_t NCOLS> void fixedToDynMatrix( const CMatrixFixedNumeric<T,NROWS,NCOLS> &SRC, CMatrixTemplateNumeric<T> &DST);
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111 template <class T>
00112 class MRPTDLLIMPEXP CMatrixTemplateNumeric : public CMatrixTemplate<T>
00113 {
00114 public:
00115
00116
00117 template <class R>
00118 CMatrixTemplateNumeric(const CMatrixTemplate<R>& m)
00119 {
00120 CMatrixTemplate<T>::realloc( m.getRowCount(), m.getColCount() );
00121
00122 for (size_t i=0; i < CMatrixTemplate<T>::getRowCount(); i++)
00123 for (size_t j=0; j < CMatrixTemplate<T>::getColCount(); j++)
00124 CMatrixTemplate<T>::m_Val[i][j] = static_cast<T> (m.get_unsafe(i,j));
00125 }
00126
00127
00128 CMatrixTemplateNumeric();
00129
00130
00131 CMatrixTemplateNumeric(size_t row, size_t col);
00132
00133
00134 CMatrixTemplateNumeric(const CMatrixTemplate<T> & m, const size_t cropRowCount, const size_t cropColCount) : CMatrixTemplate<T>(m,cropRowCount,cropColCount)
00135 { }
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145 template <typename V, size_t N>
00146 CMatrixTemplateNumeric(size_t row, size_t col, V (&theArray)[N] ) : CMatrixTemplate<T>( row, col, theArray )
00147 { }
00148
00149
00150
00151 template <typename V>
00152 CMatrixTemplateNumeric(size_t row, size_t col, const V &theVector ) : CMatrixTemplate<T>( row, col, theVector )
00153 { }
00154
00155
00156
00157 virtual ~CMatrixTemplateNumeric()
00158 { }
00159
00160
00161
00162 explicit CMatrixTemplateNumeric( const mrpt::poses::CPose2D &p);
00163
00164
00165
00166 explicit CMatrixTemplateNumeric( const mrpt::poses::CPose3D &p);
00167
00168
00169
00170 explicit CMatrixTemplateNumeric( const mrpt::poses::CPoint2D &p);
00171
00172
00173
00174 explicit CMatrixTemplateNumeric( const mrpt::poses::CPoint3D &p);
00175
00176
00177
00178
00179 template <size_t NROWS,size_t NCOLS>
00180 explicit CMatrixTemplateNumeric(const CMatrixFixedNumeric<T,NROWS,NCOLS> &M ) {
00181 fixedToDynMatrix(M,*this);
00182 }
00183
00184
00185
00186
00187 template <size_t NROWS,size_t NCOLS>
00188 CMatrixTemplateNumeric& operator =(const CMatrixFixedNumeric<T,NROWS,NCOLS> &M ) {
00189 fixedToDynMatrix(M,*this);
00190 return *this;
00191 }
00192
00193
00194
00195 template <class R>
00196 CMatrixTemplateNumeric<T>& operator = (const CMatrixTemplateNumeric<R>& m)
00197 {
00198 CMatrixTemplate<T>::realloc( m.getRowCount(), m.getColCount() );
00199
00200 for (size_t i=0; i < CMatrixTemplate<T>::getRowCount(); i++)
00201 for (size_t j=0; j < CMatrixTemplate<T>::getColCount(); j++)
00202 CMatrixTemplate<T>::m_Val[i][j] = static_cast<T>(m.get_unsafe(i,j));
00203 return *this;
00204 }
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216 template <typename V, size_t N>
00217 CMatrixTemplateNumeric& operator = (V (&theArray)[N] )
00218 {
00219 CMatrixTemplate<T>::operator = (theArray);
00220 return *this;
00221 }
00222
00223
00224
00225 CMatrixTemplateNumeric<T>& operator = (const CMatrixTemplateNumeric<T>& m);
00226
00227
00228
00229
00230 void setSize(size_t row, size_t col);
00231
00232
00233
00234
00235 void resize(size_t row, size_t col);
00236
00237
00238
00239
00240
00241 void laplacian( CMatrixTemplateNumeric<T> &ret ) const;
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252 void svd(CMatrixTemplateNumeric<T> &U, std::vector<T> &W,CMatrixTemplateNumeric<T> &V) const;
00253
00254
00255
00256
00257
00258
00259
00260 void eigenVectors(CMatrixTemplateNumeric<T> &Z, CMatrixTemplateNumeric<T> &D) const;
00261
00262
00263
00264
00265
00266
00267 CMatrixTemplateNumeric<T> largestEigenvector(
00268 T resolution = 0.01f,
00269 size_t maxIterations = 6,
00270 int *out_Iterations = NULL,
00271 float *out_estimatedResolution = NULL ) const;
00272
00273
00274
00275
00276 CMatrixTemplateNumeric<T>& Sqrt();
00277
00278
00279
00280
00281 CMatrixTemplateNumeric<T>& Abs();
00282
00283
00284
00285
00286 CMatrixTemplateNumeric<T>& Square();
00287
00288
00289
00290
00291
00292 template <class F>
00293 CMatrixTemplateNumeric<T>& applyToAllElements( F function )
00294 {
00295 for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
00296 for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
00297 CMatrixTemplate<T>::m_Val[i][j] = function( CMatrixTemplate<T>::m_Val[i][j] );
00298 return *this;
00299 }
00300
00301
00302
00303 CMatrixTemplateNumeric<T> operator + ();
00304
00305
00306
00307 CMatrixTemplateNumeric<T> operator - ();
00308
00309
00310
00311 CMatrixTemplateNumeric<T>& operator += (const CMatrixTemplateNumeric<T>& m);
00312
00313
00314
00315 CMatrixTemplateNumeric<T>& add_At(const CMatrixTemplateNumeric<T>& m);
00316
00317
00318
00319 CMatrixTemplateNumeric<T>& add_AAt(const CMatrixTemplateNumeric<T>& m);
00320
00321
00322 CMatrixTemplateNumeric<T>& add_Ac(const CMatrixTemplateNumeric<T>& m, const T c);
00323
00324
00325
00326 CMatrixTemplateNumeric<T>& operator -= (const CMatrixTemplateNumeric<T>& m);
00327
00328
00329 CMatrixTemplateNumeric<T>& substract_Ac(const CMatrixTemplateNumeric<T>& m, const T c);
00330
00331
00332
00333 CMatrixTemplateNumeric<T>& operator *= (const T& c);
00334
00335
00336
00337 CMatrixTemplateNumeric<T>& operator *= (const CMatrixTemplateNumeric<T>& m);
00338
00339
00340
00341 void multiply(const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2);
00342
00343
00344
00345 void multiply(const CMatrixTemplateNumeric<T>& m1, const CVectorTemplate<T>& m2);
00346
00347
00348
00349 void multiply_ABt(const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2);
00350
00351
00352
00353 void multiply_AAt( const CMatrixTemplateNumeric<T>& m1 );
00354
00355
00356
00357 void multiply_AtA( const CMatrixTemplateNumeric<T>& m1 );
00358
00359
00360
00361 void multiply_Ab( const std::vector<T>& a, std::vector<T>& out_v );
00362
00363
00364
00365 void multiply_Atb( const std::vector<T>& a, std::vector<T>& out_v );
00366
00367
00368
00369
00370 void multiply_result_is_symmetric(const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2);
00371
00372
00373
00374
00375 void multiplySubMatrix (
00376 const CMatrixTemplateNumeric<T> &A,
00377 CMatrixTemplateNumeric<T> &outResult,
00378 const size_t &A_cols_offset,
00379 const size_t &A_rows_offset,
00380 const size_t &A_col_count );
00381
00382
00383
00384 CMatrixTemplateNumeric<T>& operator /= (const CMatrixTemplateNumeric<T>& m);
00385
00386
00387
00388 CMatrixTemplateNumeric<T>& operator /= (const T& c);
00389
00390
00391
00392 CMatrixTemplateNumeric<T>& operator += (const T& c);
00393
00394
00395
00396 CMatrixTemplateNumeric<T>& operator -= (const T& c);
00397
00398
00399
00400 CMatrixTemplateNumeric<T>& operator ^= (const unsigned int& pow);
00401
00402
00403
00404 void scalarPow(T s);
00405
00406
00407
00408 void zeros(const size_t& row, const size_t& col);
00409
00410
00411
00412 void zeros();
00413
00414
00415
00416 void ones(const size_t& row, const size_t& col);
00417
00418
00419
00420 void ones();
00421
00422
00423
00424 void unit (const size_t& row);
00425
00426
00427
00428 void unit();
00429
00430
00431
00432 CMatrixTemplateNumeric<T> solve (const CMatrixTemplateNumeric<T>& v);
00433
00434
00435
00436 CMatrixTemplateNumeric<T> adj() const;
00437
00438
00439
00440
00441
00442 void inv(CMatrixTemplateNumeric<T> &out_inv) const;
00443
00444
00445
00446
00447
00448 CMatrixTemplateNumeric<T> inv() const {
00449 CMatrixTemplateNumeric<T> inv_res;
00450 this->inv(inv_res);
00451 return inv_res;
00452 }
00453
00454
00455
00456
00457
00458 void inv_fast( CMatrixTemplateNumeric<T> &out_inv );
00459
00460
00461 void pseudoInverse( CMatrixTemplateNumeric<T> &out ) const;
00462
00463
00464 CMatrixTemplateNumeric<T> pseudoInverse() const
00465 {
00466 CMatrixTemplateNumeric<T> M;
00467 pseudoInverse(M);
00468 return M;
00469 }
00470
00471
00472
00473
00474 T det() const;
00475
00476
00477
00478 size_t rank(T eps=0.0) const;
00479
00480
00481
00482 T norm() const;
00483
00484
00485
00486 T cofact (size_t row, size_t col) const;
00487
00488
00489
00490 T cond();
00491
00492
00493
00494 bool isSingular() const;
00495
00496
00497
00498 bool isDiagonal() const;
00499
00500
00501
00502 bool isScalar() const;
00503
00504
00505
00506 bool isUnit() const;
00507
00508
00509
00510 bool isNull() const;
00511
00512
00513
00514 bool isSymmetric() const;
00515
00516
00517
00518 bool isSkewSymmetric() const;
00519
00520
00521
00522 bool isUpperTriangular() const;
00523
00524
00525
00526 bool isLowerTriangular() const;
00527
00528
00529
00530
00531 void matrix_floor();
00532
00533
00534
00535
00536 void matrix_floor(CMatrixTemplateNumeric<T> &out);
00537
00538
00539
00540
00541 void matrix_ceil();
00542
00543
00544
00545
00546 void find_index_max_value(size_t &umax, size_t &vmax, T &max_val) const;
00547
00548
00549
00550 T maximumDiagonal() const;
00551
00552
00553
00554 T maximum() const;
00555
00556
00557
00558 T minimum() const;
00559
00560
00561
00562
00563 void find_index_min_value(size_t &umin, size_t &vmin, T &min_val) const;
00564
00565
00566
00567
00568 void force_symmetry();
00569
00570
00571
00572
00573 void mean( std::vector<T> &outMeanVector ) const;
00574
00575
00576
00577
00578 void meanAndStd(
00579 std::vector<T> &outMeanVector,
00580 std::vector<T> &outStdVector ) const;
00581
00582
00583
00584
00585 void meanAndStdAll(
00586 T &outMean,
00587 T &outStd ) const;
00588
00589 void asCol(CMatrixTemplateNumeric<T> &aux) const;
00590
00591 void asRow(CMatrixTemplateNumeric<T> &aux) const;
00592
00593
00594
00595
00596
00597
00598
00599
00600 void findElementsPassingMahalanobisThreshold(
00601 double stdTimes,
00602 std::vector<size_t> &rowIndexes,
00603 std::vector<size_t> &colIndexes,
00604 bool below = false ) const;
00605
00606
00607
00608
00609 inline void normalize( T minVal=0,T maxVal=1)
00610 {
00611 adjustRange(minVal,maxVal);
00612 }
00613
00614
00615
00616 void adjustRange( T minVal=0,T maxVal=1);
00617
00618
00619
00620
00621 T sumAll() const;
00622
00623
00624
00625
00626
00627 T sum(
00628 size_t firstRow = 0,
00629 size_t firstCol = 0,
00630 size_t lastRow = std::numeric_limits<size_t>::max(),
00631 size_t lastCol = std::numeric_limits<size_t>::max() ) const;
00632
00633
00634
00635
00636 void multiplyByMatrixAndByTransposeNonSymmetric(
00637 const CMatrixTemplateNumeric<T> &C,
00638 CMatrixTemplateNumeric<T> &R,
00639 bool accumOnOutput = false,
00640 bool substractInsteadOfSum = false
00641 ) const;
00642
00643
00644
00645
00646
00647 void multiply_ABC(
00648 const CMatrixTemplateNumeric<T> &A,
00649 const CMatrixTemplateNumeric<T> &B,
00650 const CMatrixTemplateNumeric<T> &C);
00651
00652
00653
00654
00655 void multiply_ABCt(
00656 const CMatrixTemplateNumeric<T> &A,
00657 const CMatrixTemplateNumeric<T> &B,
00658 const CMatrixTemplateNumeric<T> &C);
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676 void multiply_HCHt(
00677 const CMatrixTemplateNumeric<T> &C,
00678 CMatrixTemplateNumeric<T> &R,
00679 bool allowSubMatrixMultiplication = false,
00680 size_t subMatrixOffset = 0,
00681 bool accumResultInOutput = false ) const;
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691 T multiply_HCHt_scalar(
00692 const CMatrixTemplateNumeric<T> &C ) const;
00693
00694 private:
00695
00696 int pivot(size_t row);
00697
00698 };
00699
00700
00701
00702
00703 template <class T>
00704 bool operator == (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
00705 {
00706 if (m1.getRowCount() != m2.getRowCount() || m1.getColCount() != m2.getColCount())
00707 return false;
00708
00709 for (size_t i=0; i < m1.getRowCount(); i++)
00710 for (size_t j=0; j < m1.getColCount(); j++)
00711 if (m1(i,j) != m2(i,j))
00712 return false;
00713
00714 return true;
00715 }
00716
00717
00718
00719 template <class T>
00720 bool operator != (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
00721 {
00722 return !(m1 == m2);
00723 }
00724
00725
00726
00727 template <class T>
00728 CMatrixTemplateNumeric<T> operator + (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
00729 {
00730 CMatrixTemplateNumeric<T> temp(m1);
00731 temp += m2;
00732 return temp;
00733 }
00734
00735
00736
00737 template <class T>
00738 CMatrixTemplateNumeric<T> operator - (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
00739 {
00740 CMatrixTemplateNumeric<T> temp(m1);
00741 temp -= m2;
00742 return temp;
00743 }
00744
00745
00746
00747 template <class T>
00748 CMatrixTemplateNumeric<T> operator * (const CMatrixTemplateNumeric<T>& m, const T& no)
00749 {
00750 CMatrixTemplateNumeric<T> temp(m);
00751 temp*=no;
00752 return temp;
00753 }
00754
00755
00756
00757 template <class T>
00758 CMatrixTemplateNumeric<T> operator * (const T& no, const CMatrixTemplateNumeric<T>& m)
00759 {
00760 CMatrixTemplateNumeric<T> temp(m);
00761 temp*=no;
00762 return temp;
00763 }
00764
00765
00766
00767 template <class T>
00768 CMatrixTemplateNumeric<T> operator * (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
00769 {
00770 CMatrixTemplateNumeric<T> res(m1);
00771 res*=m2;
00772 return res;
00773 }
00774
00775
00776
00777 template <class T>
00778 CMatrixTemplateNumeric<T> operator * (const CMatrixTemplateNumeric<T>& m1, const CVectorTemplate<T>& m2)
00779 {
00780 CMatrixTemplateNumeric<T> res(m1.getRowCount(),1);
00781 res.multiply(m1,m2);
00782 return res;
00783 }
00784
00785
00786
00787 template <class T>
00788 CMatrixTemplateNumeric<T> operator / (const CMatrixTemplateNumeric<T>& m, const T& no)
00789 {
00790 return (m * (T(1) / no));
00791 }
00792
00793
00794
00795 template <class T>
00796 CMatrixTemplateNumeric<T> operator / (const T& no, const CMatrixTemplateNumeric<T>& m)
00797 {
00798 return (!m * no);
00799 }
00800
00801
00802
00803 template <class T>
00804 CMatrixTemplateNumeric<T> operator / (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
00805 {
00806 return (m1 * !m2);
00807 }
00808
00809
00810
00811 template <class T>
00812 CMatrixTemplateNumeric<T> operator ^ (const CMatrixTemplateNumeric<T>& m, const unsigned int& pow)
00813 {
00814 CMatrixTemplateNumeric<T> temp(m);
00815 temp ^= pow;
00816 return temp;
00817 }
00818
00819
00820
00821 template <class T>
00822 CMatrixTemplateNumeric<T> operator ~ (const CMatrixTemplateNumeric<T>& m)
00823 {
00824 CMatrixTemplateNumeric<T> temp(m.getColCount(),m.getRowCount());
00825
00826 for (size_t i=0; i < m.getRowCount(); i++)
00827 for (size_t j=0; j < m.getColCount(); j++)
00828 {
00829 T x = m(i,j);
00830 temp(j,i) = x;
00831 }
00832 return temp;
00833 }
00834
00835
00836
00837 template <class T>
00838 CMatrixTemplateNumeric<T> operator ! (const CMatrixTemplateNumeric<T> &m)
00839 {
00840 return m.inv();
00841 }
00842
00843
00844
00845
00846 #define DEBUG_SAVE_MATRIX(M) \
00847 { \
00848 char auxStr[100]; \
00849 os::sprintf(auxStr,99,"%s.txt",#M); \
00850 M.saveToTextFile(auxStr); \
00851 } \
00852
00853
00854
00855
00856
00857
00858 typedef CMatrixTemplateNumeric<float> CMatrixFloat;
00859
00860
00861
00862
00863
00864 typedef CMatrixTemplateNumeric<double> CMatrixDouble;
00865
00866
00867
00868
00869 typedef CMatrixTemplateNumeric<unsigned int> CMatrixUInt;
00870
00871
00872
00873
00874 typedef CMatrixTemplate<bool> CMatrixBool;
00875
00876 #ifdef HAVE_LONG_DOUBLE
00877
00878
00879
00880 typedef CMatrixTemplateNumeric<long double> CMatrixLongDouble;
00881 #else
00882
00883
00884
00885 typedef CMatrixTemplateNumeric<double> CMatrixLongDouble;
00886 #endif
00887
00888 }
00889 }
00890
00891 #endif