Image Component Library (ICL)
|
00001 /******************************************************************** 00002 ** Image Component Library (ICL) ** 00003 ** ** 00004 ** Copyright (C) 2006-2013 CITEC, University of Bielefeld ** 00005 ** Neuroinformatics Group ** 00006 ** Website: www.iclcv.org and ** 00007 ** http://opensource.cit-ec.de/projects/icl ** 00008 ** ** 00009 ** File : ICLMath/src/ICLMath/FixedMatrix.h ** 00010 ** Module : ICLMath ** 00011 ** Authors: Christof Elbrechter ** 00012 ** ** 00013 ** ** 00014 ** GNU LESSER GENERAL PUBLIC LICENSE ** 00015 ** This file may be used under the terms of the GNU Lesser General ** 00016 ** Public License version 3.0 as published by the ** 00017 ** ** 00018 ** Free Software Foundation and appearing in the file LICENSE.LGPL ** 00019 ** included in the packaging of this file. Please review the ** 00020 ** following information to ensure the license requirements will ** 00021 ** be met: http://www.gnu.org/licenses/lgpl-3.0.txt ** 00022 ** ** 00023 ** The development of this software was supported by the ** 00024 ** Excellence Cluster EXC 277 Cognitive Interaction Technology. ** 00025 ** The Excellence Cluster EXC 277 is a grant of the Deutsche ** 00026 ** Forschungsgemeinschaft (DFG) in the context of the German ** 00027 ** Excellence Initiative. ** 00028 ** ** 00029 ********************************************************************/ 00030 00031 #pragma once 00032 00033 #include <ICLUtils/CompatMacros.h> 00034 #include <ICLUtils/Exception.h> 00035 #include <ICLUtils/ClippedCast.h> 00036 #include <ICLUtils/FixedArray.h> 00037 #include <ICLMath/DynMatrix.h> 00038 #include <ICLMath/MatrixSubRectIterator.h> 00039 00040 #include <iterator> 00041 #include <algorithm> 00042 #include <numeric> 00043 #include <functional> 00044 #include <iostream> 00045 #include <vector> 00046 #include <cmath> 00047 00048 #ifdef ICL_HAVE_IPP 00049 #include <ippm.h> 00050 #endif 00051 00052 namespace icl{ 00053 namespace math{ 00054 00056 struct FixedMatrixBase{ 00058 template<class SrcIterator, class DstIterator, unsigned int N> 00059 static void optimized_copy(SrcIterator srcBegin, SrcIterator srcEnd, DstIterator dstBegin){ 00060 if(N>30){ 00061 std::copy(srcBegin,srcEnd,dstBegin); 00062 }else{ 00063 while(srcBegin != srcEnd){ 00064 // *dstBegin++ = *srcBegin++; 00065 *dstBegin = 00066 *srcBegin; 00067 00068 dstBegin++; 00069 srcBegin++; 00070 } 00071 } 00072 } 00073 }; 00074 00076 00077 template<class T, unsigned int COLS, unsigned int ROWS> class FixedMatrix; 00081 00082 00091 template<class T,unsigned int N, class Iterator> 00092 class FixedMatrixPart : public FixedMatrixBase{ 00093 00094 public: 00096 Iterator begin; 00097 00099 Iterator end; 00100 00102 FixedMatrixPart(Iterator begin, Iterator end):begin(begin),end(end){} 00103 00105 FixedMatrixPart& operator=(const T &value){ 00106 std::fill(begin,end,value); 00107 return *this; 00108 } 00110 00111 FixedMatrixPart& operator=(const FixedMatrixPart &other){ 00112 FixedMatrixBase::optimized_copy<Iterator,Iterator,N>(other.begin,other.end,begin); 00113 return *this; 00114 } 00115 00117 00119 template<class OtherIterator, class OtherT> 00120 FixedMatrixPart& operator=(const FixedMatrixPart<OtherT,N,OtherIterator> &other){ 00121 std::transform(other.begin,other.end,begin,utils::clipped_cast<OtherT,T>); 00122 return *this; 00123 } 00124 00126 00127 template<unsigned int COLS> 00128 FixedMatrixPart& operator=(const FixedMatrix<T,COLS,N/COLS> &m); 00129 00131 00132 template<class T2, unsigned int COLS> 00133 FixedMatrixPart& operator=(const FixedMatrix<T2,COLS,N/COLS> &m); 00134 00135 }; 00136 00137 00138 00140 00171 template<class T,unsigned int COLS,unsigned int ROWS> 00172 class FixedMatrix : public utils::FixedArray<T, COLS*ROWS>, public FixedMatrixBase{ 00173 public: 00174 00176 00177 DynMatrix<T> dyn() { return DynMatrix<T>(COLS,ROWS,begin(),false); } 00178 00180 const DynMatrix<T> dyn() const { return DynMatrix<T>(COLS,ROWS,const_cast<T*>(begin()),false); } 00181 00183 static const FixedMatrix &null(){ 00184 static FixedMatrix null_matrix(T(0)); 00185 return null_matrix; 00186 } 00187 00189 static const unsigned int DIM = COLS*ROWS; 00190 00191 public: 00192 00194 00195 FixedMatrix(){} 00196 00198 FixedMatrix(const T &initValue){ 00199 std::fill(begin(),end(),initValue); 00200 } 00201 00203 00207 FixedMatrix(const T *srcdata){ 00208 FixedMatrixBase::optimized_copy<const T*,T*,DIM>(srcdata,srcdata+dim(),begin()); 00209 //std::copy(srcdata,srcdata+dim(),begin()); 00210 } 00211 00213 00216 FixedMatrix(const T& v0,const T& v1,const T& v2=0,const T& v3=0, 00217 const T& v4=0,const T& v5=0,const T& v6=0,const T& v7=0, 00218 const T& v8=0,const T& v9=0,const T& v10=0,const T& v11=0, 00219 const T& v12=0,const T& v13=0,const T& v14=0,const T& v15=0){ 00220 #define C1(N) if(DIM>N) utils::FixedArray<T,COLS*ROWS>::m_data[N]=v##N 00221 #define C4(A,B,C,D) C1(A);C1(B);C1(C);C1(D) 00222 C4(0,1,2,3);C4(4,5,6,7);C4(8,9,10,11);C4(12,13,14,15); 00223 #undef C1 00224 #undef C2 00225 } 00226 00228 00229 template<class OtherIterator> 00230 FixedMatrix(OtherIterator begin, OtherIterator end){ 00231 FixedMatrixBase::optimized_copy<OtherIterator,T*,DIM>(begin,end,this->begin()); 00232 // std::copy(begin,end,begin()); 00233 } 00234 00235 // Explicit Copy template based constructor (deep copy) 00236 FixedMatrix(const FixedMatrix &other){ 00237 FixedMatrixBase::optimized_copy<const T*,T*,DIM>(other.begin(),other.end(),begin()); 00238 //std::copy(other.begin(),other.end(),begin()); 00239 } 00240 00241 // Explicit Copy template based constructor (deep copy) 00242 template<class otherT> 00243 FixedMatrix(const FixedMatrix<otherT,COLS,ROWS> &other){ 00244 std::transform(other.begin(),other.end(),begin(),utils::clipped_cast<otherT,T>); 00245 } 00246 00248 template<class Iterator> 00249 FixedMatrix (const FixedMatrixPart<T,DIM,Iterator> &r){ 00250 FixedMatrixBase::optimized_copy<Iterator,T*,DIM>(r.begin,r.end,begin()); 00251 } 00252 00254 template<class otherT, class Iterator> 00255 FixedMatrix(const FixedMatrixPart<otherT,DIM,Iterator> &r){ 00256 std::transform(r.begin,r.end,begin(),utils::clipped_cast<otherT,T>); 00257 } 00258 00260 FixedMatrix &operator=(const FixedMatrix &other){ 00261 if(this == &other) return *this; 00262 FixedMatrixBase::optimized_copy<const T*,T*,DIM>(other.begin(),other.end(),begin()); 00263 //std::copy(other.begin(),other.end(),begin()); 00264 return *this; 00265 } 00266 00268 00269 template<class otherT> 00270 FixedMatrix &operator=(const FixedMatrix<otherT,COLS,ROWS> &other){ 00271 if(this == &other) return *this; 00272 std::transform(other.begin(),other.end(),begin(),utils::clipped_cast<otherT,T>); 00273 return *this; 00274 } 00275 00277 FixedMatrix &operator=(const T &t){ 00278 std::fill(begin(),end(),t); 00279 return *this; 00280 } 00281 00283 template<class Iterator> 00284 FixedMatrix &operator=(const FixedMatrixPart<T,DIM,Iterator> &r){ 00285 // std::copy(r.begin,r.end,begin()); 00286 FixedMatrixBase::optimized_copy<Iterator,T*,DIM>(r.begin,r.end,begin()); 00287 return *this; 00288 } 00289 00291 template<class otherT, class Iterator> 00292 FixedMatrix &operator=(const FixedMatrixPart<otherT,DIM,Iterator> &r){ 00293 std::transform(r.begin,r.end,begin(),utils::clipped_cast<otherT,T>); 00294 return *this; 00295 } 00296 00298 00302 FixedMatrix operator/(const FixedMatrix &m) const 00303 throw (IncompatibleMatrixDimensionException, 00304 InvalidMatrixDimensionException, 00305 SingularMatrixException){ 00306 return this->operator*(m.inv()); 00307 } 00308 00310 FixedMatrix &operator/=(const FixedMatrix &m) 00311 throw (IncompatibleMatrixDimensionException, 00312 InvalidMatrixDimensionException, 00313 SingularMatrixException){ 00314 return *this = this->operator*(m.inv()); 00315 } 00316 00318 FixedMatrix operator*(T f) const{ 00319 FixedMatrix d; 00320 std::transform(begin(),end(),d.begin(),std::bind2nd(std::multiplies<T>(),f)); 00321 return d; 00322 } 00323 00325 FixedMatrix &operator*=(T f){ 00326 std::transform(begin(),end(),begin(),std::bind2nd(std::multiplies<T>(),f)); 00327 return *this; 00328 } 00329 00331 FixedMatrix operator/(T f) const{ 00332 FixedMatrix d; 00333 std::transform(begin(),end(),d.begin(),std::bind2nd(std::divides<T>(),f)); 00334 return d; 00335 } 00336 00338 FixedMatrix &operator/=(T f){ 00339 std::transform(begin(),end(),begin(),std::bind2nd(std::divides<T>(),f)); 00340 return *this; 00341 } 00342 00344 FixedMatrix operator+(const T &t) const{ 00345 FixedMatrix d; 00346 std::transform(begin(),end(),d.begin(),std::bind2nd(std::plus<T>(),t)); 00347 return d; 00348 } 00349 00351 FixedMatrix &operator+=(const T &t){ 00352 std::transform(begin(),end(),begin(),std::bind2nd(std::plus<T>(),t)); 00353 return *this; 00354 } 00355 00356 00358 FixedMatrix operator-(const T &t) const{ 00359 FixedMatrix d; 00360 std::transform(begin(),end(),d.begin(),std::bind2nd(std::minus<T>(),t)); 00361 return d; 00362 } 00363 00365 FixedMatrix &operator-=(const T &t){ 00366 std::transform(begin(),end(),begin(),std::bind2nd(std::minus<T>(),t)); 00367 return *this; 00368 } 00369 00371 FixedMatrix operator+(const FixedMatrix &m) const{ 00372 FixedMatrix d; 00373 std::transform(begin(),end(),m.begin(),d.begin(),std::plus<T>()); 00374 return d; 00375 } 00376 00378 FixedMatrix &operator+=(const FixedMatrix &m){ 00379 std::transform(begin(),end(),m.begin(),begin(),std::plus<T>()); 00380 return *this; 00381 } 00382 00384 FixedMatrix operator-(const FixedMatrix &m) const{ 00385 FixedMatrix d; 00386 std::transform(begin(),end(),m.begin(),d.begin(),std::minus<T>()); 00387 return d; 00388 } 00390 FixedMatrix &operator-=(const FixedMatrix &m){ 00391 std::transform(begin(),end(),m.begin(),begin(),std::minus<T>()); 00392 return *this; 00393 } 00394 00396 00398 FixedMatrix operator-() const { 00399 FixedMatrix cpy(*this); 00400 std::transform(cpy.begin(),cpy.end(),cpy.begin(),std::negate<T>()); 00401 return cpy; 00402 } 00403 00404 00406 T &operator()(unsigned int col,unsigned int row){ 00407 return begin()[col+cols()*row]; 00408 } 00409 00411 const T &operator() (unsigned int col,unsigned int row) const{ 00412 return begin()[col+cols()*row]; 00413 } 00414 00416 T &at(unsigned int col,unsigned int row) throw (InvalidIndexException){ 00417 if(col>=cols() || row >= rows()) throw InvalidIndexException("row or col index too large"); 00418 return begin()[col+cols()*row]; 00419 } 00420 00422 const T &at(unsigned int col,unsigned int row) const throw (InvalidIndexException){ 00423 return const_cast<FixedMatrix*>(this)->at(col,row); 00424 } 00425 00427 00429 T &operator[](unsigned int idx){ 00430 return begin()[idx]; 00431 } 00433 00435 const T &operator[](unsigned int idx) const{ 00436 return begin()[idx]; 00437 } 00438 00439 00441 typedef T* iterator; 00442 00444 typedef const T* const_iterator; 00445 00447 typedef T* row_iterator; 00448 00450 typedef const T* const_row_iterator; 00451 00453 static unsigned int rows(){ return ROWS; } 00454 00456 static unsigned int cols(){ return COLS; } 00457 00459 T *data() { return utils::FixedArray<T,COLS*ROWS>::m_data; } 00460 00462 const T *data() const { return utils::FixedArray<T,COLS*ROWS>::m_data; } 00463 00465 static unsigned int dim() { return DIM; } 00466 00468 struct col_iterator : public std::iterator<std::random_access_iterator_tag,T>{ 00469 00471 typedef unsigned int difference_type; 00472 00474 mutable T *p; 00475 00477 static const unsigned int STRIDE = COLS; 00478 00480 col_iterator(T *col_begin):p(col_begin){} 00481 00483 col_iterator &operator++(){ 00484 p+=STRIDE; 00485 return *this; 00486 } 00488 const col_iterator &operator++() const{ 00489 p+=STRIDE; 00490 return *this; 00491 } 00493 col_iterator operator++(int){ 00494 col_iterator tmp = *this; 00495 ++(*this); 00496 return tmp; 00497 } 00499 const col_iterator operator++(int) const{ 00500 col_iterator tmp = *this; 00501 ++(*this); 00502 return tmp; 00503 } 00504 00506 col_iterator &operator--(){ 00507 p-=STRIDE; 00508 return *this; 00509 } 00510 00512 const col_iterator &operator--() const{ 00513 p-=STRIDE; 00514 return *this; 00515 } 00516 00518 col_iterator operator--(int){ 00519 col_iterator tmp = *this; 00520 --(*this); 00521 return tmp; 00522 } 00523 00525 const col_iterator operator--(int) const{ 00526 col_iterator tmp = *this; 00527 --(*this); 00528 return tmp; 00529 } 00530 00532 col_iterator &operator+=(difference_type n){ 00533 p += n * STRIDE; 00534 return *this; 00535 } 00536 00538 const col_iterator &operator+=(difference_type n) const{ 00539 p += n * STRIDE; 00540 return *this; 00541 } 00542 00543 00545 col_iterator &operator-=(difference_type n){ 00546 p -= n * STRIDE; 00547 return *this; 00548 } 00549 00551 const col_iterator &operator-=(difference_type n) const{ 00552 p -= n * STRIDE; 00553 return *this; 00554 } 00555 00556 00558 col_iterator operator+(difference_type n) { 00559 col_iterator tmp = *this; 00560 tmp+=n; 00561 return tmp; 00562 } 00563 00565 const col_iterator operator+(difference_type n) const{ 00566 col_iterator tmp = *this; 00567 tmp+=n; 00568 return tmp; 00569 } 00570 00572 col_iterator operator-(difference_type n) { 00573 col_iterator tmp = *this; 00574 tmp-=n; 00575 return tmp; 00576 } 00577 00579 const col_iterator operator-(difference_type n) const { 00580 col_iterator tmp = *this; 00581 tmp-=n; 00582 return tmp; 00583 } 00584 00586 difference_type operator-(const col_iterator &i) const{ 00587 return (p-i.p)/STRIDE; 00588 } 00589 00591 T &operator*(){ 00592 return *p; 00593 } 00594 00596 const T &operator*() const{ 00597 return *p; 00598 } 00599 00601 bool operator==(const col_iterator &i) const{ return p == i.p; } 00602 00604 bool operator!=(const col_iterator &i) const{ return p != i.p; } 00605 00607 bool operator<(const col_iterator &i) const{ return p < i.p; } 00608 00610 bool operator<=(const col_iterator &i) const{ return p <= i.p; } 00611 00613 bool operator>=(const col_iterator &i) const{ return p >= i.p; } 00614 00616 bool operator>(const col_iterator &i) const{ return p > i.p; } 00617 00618 }; 00619 00620 // const column operator typedef 00621 typedef const col_iterator const_col_iterator; 00622 00624 iterator begin() { return utils::FixedArray<T,COLS*ROWS>::m_data; } 00625 00627 iterator end() { return utils::FixedArray<T,COLS*ROWS>::m_data+dim(); } 00628 00630 const_iterator begin() const { return utils::FixedArray<T,COLS*ROWS>::m_data; } 00631 00633 const_iterator end() const { return utils::FixedArray<T,COLS*ROWS>::m_data+dim(); } 00634 00636 col_iterator col_begin(unsigned int col) { return col_iterator(begin()+col); } 00637 00639 col_iterator col_end(unsigned int col) { return col_iterator(begin()+col+dim()); } 00640 00642 const_col_iterator col_begin(unsigned int col) const { return col_iterator(const_cast<T*>(begin())+col); } 00643 00645 const_col_iterator col_end(unsigned int col) const { return col_iterator(const_cast<T*>(begin())+col+dim()); } 00646 00648 row_iterator row_begin(unsigned int row) { return begin()+row*cols(); } 00649 00651 row_iterator row_end(unsigned int row) { return begin()+(row+1)*cols(); } 00652 00654 const_row_iterator row_begin(unsigned int row) const { return begin()+row*cols(); } 00655 00657 const_row_iterator row_end(unsigned int row) const { return begin()+(row+1)*cols(); } 00658 00660 00672 template<unsigned int MCOLS> 00673 void mult(const FixedMatrix<T,MCOLS,COLS> &m, FixedMatrix<T,MCOLS,ROWS> &dst) const{ 00674 for(unsigned int c=0;c<MCOLS;++c){ 00675 for(unsigned int r=0;r<ROWS;++r){ 00676 // std::cout << "calling inner_product" << std::endl; 00677 dst(c,r) = std::inner_product(m.col_begin(c),m.col_end(c),row_begin(r),T(0)); 00678 } 00679 } 00680 } 00682 00705 template<unsigned int MCOLS> 00706 FixedMatrix<T,MCOLS,ROWS> operator*(const FixedMatrix<T,MCOLS,COLS> &m) const{ 00707 FixedMatrix<T,MCOLS,ROWS> d; 00708 mult(m,d); 00709 return d; 00710 } 00711 00712 00713 00715 00721 FixedMatrix inv() const throw (InvalidMatrixDimensionException,SingularMatrixException){ 00722 return FixedMatrix(dyn().inv().data()); 00723 } 00724 00726 00729 T det() const throw(InvalidMatrixDimensionException){ 00730 return dyn().det(); 00731 } 00732 00734 FixedMatrix<T,ROWS,COLS> transp() const{ 00735 FixedMatrix<T,ROWS,COLS> d; 00736 for(unsigned int i=0;i<cols();++i){ 00737 FixedMatrixBase::optimized_copy<const_col_iterator, typename FixedMatrix<T,ROWS,COLS>::row_iterator,DIM>(col_begin(i),col_end(i),d.row_begin(i)); 00738 // std::copy(col_begin(i),col_end(i),d.row_begin(i)); 00739 } 00740 return d; 00741 } 00742 00744 00745 template<unsigned int OTHER_COLS> 00746 T element_wise_inner_product(const FixedMatrix<T,OTHER_COLS,DIM/OTHER_COLS> &other) const { 00747 return std::inner_product(begin(),end(),other.begin(),T(0)); 00748 } 00749 00750 00752 00755 template<unsigned int OTHER_COLS> 00756 FixedMatrix<T,OTHER_COLS,COLS> dot(const FixedMatrix<T,OTHER_COLS,ROWS> &M) const{ 00757 return this->transp() * M; 00758 } 00759 00761 double cond(const double p=2) const { 00762 DynMatrix<T> mat = this->dyn(); 00763 return mat.cond(); 00764 } 00765 00767 T trace() const{ 00768 ICLASSERT_RETURN_VAL(COLS==ROWS,0); 00769 double accu = 0; 00770 for(int i=0;i<DIM;i+=COLS+1){ 00771 accu += begin()[i]; 00772 } 00773 return accu; 00774 } 00775 00777 FixedMatrixPart<T,COLS,row_iterator> row(unsigned int idx){ 00778 return FixedMatrixPart<T,COLS,row_iterator>(row_begin(idx),row_end(idx)); 00779 } 00780 00782 FixedMatrixPart<T,COLS,const_row_iterator> row(unsigned int idx) const{ 00783 return FixedMatrixPart<T,COLS,const_row_iterator>(row_begin(idx),row_end(idx)); 00784 } 00785 00787 FixedMatrixPart<T,ROWS,col_iterator> col(unsigned int idx){ 00788 return FixedMatrixPart<T,ROWS,col_iterator>(col_begin(idx),col_end(idx)); 00789 } 00790 00792 FixedMatrixPart<T,ROWS,const_col_iterator> col(unsigned int idx) const{ 00793 return FixedMatrixPart<T,ROWS,const_col_iterator>(col_begin(idx),col_end(idx)); 00794 } 00795 00797 template<unsigned int X,unsigned int Y,unsigned int WIDTH,unsigned int HEIGHT> 00798 FixedMatrixPart<T,WIDTH*HEIGHT,MatrixSubRectIterator<T> > part(){ 00799 return FixedMatrixPart<T,WIDTH*HEIGHT,MatrixSubRectIterator<T> >( 00800 MatrixSubRectIterator<T>(begin(),COLS,X,Y,WIDTH,HEIGHT), 00801 MatrixSubRectIterator<T>::create_end_iterator(begin(),COLS,X,Y,WIDTH,HEIGHT)); 00802 } 00803 00805 template<unsigned int X,unsigned int Y,unsigned int WIDTH,unsigned int HEIGHT> 00806 const FixedMatrixPart<T,WIDTH*HEIGHT,MatrixSubRectIterator<T> > part() const{ 00807 return const_cast<FixedMatrix<T,COLS,ROWS>*>(this)->part<X,Y,WIDTH,HEIGHT>(); 00808 } 00809 00811 00816 template<unsigned int NEW_WIDTH,unsigned int NEW_HEIGHT> 00817 inline FixedMatrix<T,NEW_WIDTH,NEW_HEIGHT> resize(const T &init=T(0)) const { 00818 FixedMatrix<T,NEW_WIDTH,NEW_HEIGHT> M(init); 00819 for(unsigned int x=0;x<COLS && x < NEW_WIDTH; ++x){ 00820 for(unsigned int y=0;y<ROWS && y < NEW_HEIGHT; ++y){ 00821 M(x,y) = (*this)(x,y); 00822 } 00823 } 00824 return M; 00825 } 00826 00828 00832 static FixedMatrix<T,ROWS,COLS> id(){ 00833 FixedMatrix<T,ROWS,COLS> m(T(0)); 00834 for(unsigned int i=0;i<ROWS && i<COLS;++i){ 00835 m(i,i) = 1; 00836 } 00837 return m; 00838 } 00839 00841 inline double length(T norm=2) const{ 00842 double sumSquares = 0; 00843 for(unsigned int i=0;i<DIM;++i){ 00844 sumSquares += ::pow((double)(*this)[i],(double)norm); 00845 } 00846 return ::pow( sumSquares, 1.0/norm); 00847 } 00848 00850 inline void normalize(T norm=2){ 00851 T l = (T)length(norm); 00852 if(l) (*this) /= l; 00853 } 00854 00856 inline FixedMatrix<T,COLS,ROWS> normalized(T norm=2) const{ 00857 T l = (T)length(norm); 00858 return l ? (*this)/l : *this; 00859 } 00860 00862 template<class otherT> 00863 bool operator==(const FixedMatrix<otherT,COLS,ROWS> &m) const{ 00864 for(unsigned int i=0;i<DIM;++i){ 00865 if(begin()[i] != m[i]) return false; 00866 } 00867 return true; 00868 } 00870 template<class otherT> 00871 bool operator!=(const FixedMatrix<otherT,COLS,ROWS> &m) const{ 00872 return !this->operator==(m); 00873 } 00874 00875 00877 FixedMatrix<T,1,ROWS> diag() const throw (InvalidMatrixDimensionException){ 00878 if(ROWS != COLS) throw InvalidMatrixDimensionException("trace is only possible for sqaure matrices"); 00879 FixedMatrix<T,1,ROWS> t; 00880 for(unsigned int i=0;i<ROWS;++i){ 00881 t[i] = (*this)(i,i); 00882 } 00883 return t; 00884 } 00885 00887 00888 void decompose_QR(FixedMatrix<T,COLS,ROWS> &Q, FixedMatrix<T,COLS,COLS> &R) const{ 00889 DynMatrix<T> Qd = Q.dyn(), Rd = R.dyn(); 00890 dyn().decompose_QR(Qd,Rd); 00891 } 00892 00894 00895 void decompose_RQ(FixedMatrix<T,ROWS,ROWS> &R, FixedMatrix<T,ROWS,ROWS> &Q) const{ 00896 DynMatrix<T> Rd = R.dyn(), Qd = Q.dyn(); 00897 dyn().decompose_RQ(Rd,Qd); 00898 } 00899 00901 00902 void svd(FixedMatrix<T,COLS,ROWS> &U, FixedMatrix<T,1,COLS> &s, FixedMatrix<T,COLS,COLS> &V) const{ 00903 DynMatrix<T> Ud = U.dyn(), sd = s.dyn(), Vd = V.dyn(); 00904 return dyn().svd(Ud,sd,Vd); 00905 } 00906 00908 00909 FixedMatrix<T,ROWS,COLS> pinv(bool useSVD=0, float zeroThreshold=0.00000000000000001) const { 00910 return FixedMatrix<T,ROWS,COLS>(dyn().pinv(useSVD,zeroThreshold).begin()); 00911 } 00912 00914 00927 void eigen(FixedMatrix &eigenvectors, FixedMatrix<T,1,COLS> &eigenvalues) const{ 00928 if(ROWS != COLS) throw InvalidMatrixDimensionException("eigenvalue decomposition is only possible for sqaure matrices (use svd instead!)"); 00929 DynMatrix<T> evecs = eigenvectors.dyn(), evals = eigenvalues.dyn(); 00930 return dyn().eigen(evecs,evals); 00931 } 00932 00933 00934 }; 00935 00937 00938 template<class T,unsigned int WIDTH,unsigned int HEIGHT, unsigned int HEIGHT2> 00939 inline FixedMatrix<T, WIDTH, HEIGHT + HEIGHT2> operator%(const FixedMatrix<T, WIDTH, HEIGHT> &a, 00940 const FixedMatrix<T,WIDTH,HEIGHT2> &b){ 00941 FixedMatrix<T,WIDTH,HEIGHT+HEIGHT2> M; 00942 for(unsigned int i=0;i<HEIGHT;++i) M.row(i) = a.row(i); 00943 for(unsigned int i=0;i<HEIGHT2;++i) M.row(i+HEIGHT) = b.row(i); 00944 return M; 00945 } 00946 00948 00949 template<class T,unsigned int WIDTH,unsigned int HEIGHT, unsigned int WIDTH2> 00950 inline FixedMatrix<T, WIDTH + WIDTH2, HEIGHT> operator,(const FixedMatrix<T, WIDTH, HEIGHT> &a, 00951 const FixedMatrix<T,WIDTH2,HEIGHT> &b){ 00952 FixedMatrix<T,WIDTH+WIDTH2,HEIGHT> M; 00953 for(unsigned int i=0;i<WIDTH;++i) M.col(i) = a.col(i); 00954 for(unsigned int i=0;i<WIDTH2;++i) M.col(i+WIDTH) = b.col(i); 00955 return M; 00956 } 00957 00958 00959 00960 00962 00964 template<class T, unsigned int M_ROWS_AND_COLS,unsigned int V_COLS> 00965 inline FixedMatrix<T,V_COLS,M_ROWS_AND_COLS> &operator*=(FixedMatrix<T,V_COLS,M_ROWS_AND_COLS> &v, 00966 const FixedMatrix<T,M_ROWS_AND_COLS,M_ROWS_AND_COLS> &m){ 00967 return v = (v*m); 00968 } 00969 00971 00972 template<class T, unsigned int COLS, unsigned int ROWS> 00973 inline std::ostream &operator<<(std::ostream &s,const FixedMatrix<T,COLS,ROWS> &m){ 00974 return s << m.dyn(); 00975 } 00976 00978 00979 template<class T, unsigned int COLS, unsigned int ROWS> 00980 inline std::istream &operator>>(std::istream &s,FixedMatrix<T,COLS,ROWS> &m){ 00981 DynMatrix<T> dyn = m.dyn(); 00982 return s >> dyn; 00983 } 00984 00985 00987 template<class T> ICLMath_IMP 00988 FixedMatrix<T, 2, 2> create_rot_2D(T angle); 00989 00991 template<class T> ICLMath_IMP 00992 FixedMatrix<T, 3, 3> create_hom_3x3(T angle, T dx = 0, T dy = 0, T v0 = 0, T v1 = 0); 00993 00995 template<class T> 00996 inline FixedMatrix<T, 3, 3> create_hom_3x3_trans(T dx, T dy){ 00997 FixedMatrix<T,3,3> m = FixedMatrix<T,3,3>::id(); 00998 m(2,0)=dx; 00999 m(2,1)=dy; 01000 return m; 01001 } 01002 01003 01005 enum AXES { sxyz, sxyx, sxzy, sxzx, syzx, syzy, 01006 syxz, syxy, szxy, szxz, szyx, szyz, 01007 rzyx, rxyx, ryzx, rxzx, rxzy, ryzy, 01008 rzxy, ryxy, ryxz, rzxz, rxyz, rzyz }; 01009 extern ICLMath_API const AXES AXES_DEFAULT; // rxyz 01010 01012 template<class T> ICLMath_IMP 01013 FixedMatrix<T, 3, 3> create_rot_3D(T axisX, T axisY, T axisZ, T angle); 01014 01016 template<class T> ICLMath_IMP 01017 FixedMatrix<T,3,3> create_rot_3D (T ai, T aj, T ak, AXES axes=AXES_DEFAULT); 01019 template<class T> ICLMath_IMP 01020 FixedMatrix<T,4,4> create_hom_4x4(T rx, T ry, T rz, 01021 T dx=0, T dy=0, T dz=0, 01022 T v0=0, T v1=0, T v2=0, 01023 AXES axes=AXES_DEFAULT); 01024 01026 template<class T> ICLMath_IMP 01027 FixedMatrix<T, 4, 4> create_rot_4x4(T axisX, T axisY, T axisZ, T angle); 01028 01029 01031 template<class T> 01032 inline FixedMatrix<T, 4, 4> create_hom_4x4_trans(T dx, T dy, T dz){ 01033 FixedMatrix<T,4,4> m = FixedMatrix<T,4,4>::id(); 01034 m(3,0)=dx; 01035 m(3,1)=dy; 01036 m(3,2)=dz; 01037 return m; 01038 } 01039 01041 template<class T> ICLMath_IMP 01042 FixedMatrix<T,1,3> extract_euler_angles(const FixedMatrix<T,3,3> &m, 01043 AXES axes=AXES_DEFAULT); 01044 template<class T> ICLMath_IMP 01045 FixedMatrix<T,1,3> extract_euler_angles(const FixedMatrix<T,4,4> &m, 01046 AXES axes=AXES_DEFAULT); 01047 01048 01050 template<class T,unsigned int N, class Iterator> template<unsigned int COLS> 01051 inline FixedMatrixPart<T,N,Iterator>& FixedMatrixPart<T,N,Iterator>::operator=(const FixedMatrix<T,COLS,N/COLS> &m){ 01052 FixedMatrixBase::optimized_copy<const T*,Iterator,N>(m.begin(),m.end(),begin); 01053 //std::copy(m.begin(),m.end(),begin); 01054 return *this; 01055 } 01056 template<class T,unsigned int N, class Iterator> template<class T2, unsigned int COLS> 01057 inline FixedMatrixPart<T,N,Iterator>& FixedMatrixPart<T,N,Iterator>::operator=(const FixedMatrix<T2,COLS,N/COLS> &m){ 01058 std::transform(m.begin(),m.end(),begin,utils::clipped_cast<T2,T>); 01059 return *this; 01060 } 01063 #ifdef ICL_HAVE_IPP 01064 #define OPTIMIZED_MATRIX_MULTIPLICATION(LEFT_COLS,LEFT_ROWS,RIGHT_COLS,TYPE,IPPSUFFIX) \ 01065 template<> template<> \ 01066 inline void \ 01067 FixedMatrix<TYPE,RIGHT_COLS,LEFT_ROWS>::mult \ 01068 ( \ 01069 const FixedMatrix<TYPE,RIGHT_COLS,LEFT_COLS> &m, \ 01070 FixedMatrix<TYPE,RIGHT_COLS,LEFT_ROWS> &dst \ 01071 ) const { \ 01072 static const unsigned int ST=sizeof(TYPE); \ 01073 ippmMul_mm_##IPPSUFFIX(data(),LEFT_COLS*ST,ST,LEFT_COLS,LEFT_ROWS, \ 01074 m.data(),RIGHT_COLS*ST,ST,RIGHT_COLS,LEFT_COLS, \ 01075 dst.data(),RIGHT_COLS*ST,ST); \ 01076 } 01077 01078 OPTIMIZED_MATRIX_MULTIPLICATION(2,2,2,float,32f); 01079 OPTIMIZED_MATRIX_MULTIPLICATION(3,3,3,float,32f); 01080 OPTIMIZED_MATRIX_MULTIPLICATION(4,4,4,float,32f); 01081 01082 OPTIMIZED_MATRIX_MULTIPLICATION(2,2,2,double,64f); 01083 OPTIMIZED_MATRIX_MULTIPLICATION(3,3,3,double,64f); 01084 OPTIMIZED_MATRIX_MULTIPLICATION(4,4,4,double,64f); 01085 #undef OPTIMIZED_MATRIX_MULTIPLICATION 01086 01087 01088 #endif 01089 01090 #define USE_OPTIMIZED_INV_AND_DET_FOR_2X2_3X3_AND_4X4_MATRICES 01091 01092 #ifdef USE_OPTIMIZED_INV_AND_DET_FOR_2X2_3X3_AND_4X4_MATRICES 01093 01095 // this functions are implemented in iclFixedMatrix.cpp. All templates are 01096 // instantiated for float and double 01097 01098 template<class T> ICLMath_IMP 01099 void icl_util_get_fixed_4x4_matrix_inv(const T *src, T*dst); 01100 template<class T> ICLMath_IMP 01101 void icl_util_get_fixed_3x3_matrix_inv(const T *src, T*dst); 01102 template<class T> ICLMath_IMP 01103 void icl_util_get_fixed_2x2_matrix_inv(const T *src, T*dst); 01104 01105 template<class T> ICLMath_IMP 01106 T icl_util_get_fixed_4x4_matrix_det(const T *src); 01107 template<class T> ICLMath_IMP 01108 T icl_util_get_fixed_3x3_matrix_det(const T *src); 01109 template<class T> ICLMath_IMP 01110 T icl_util_get_fixed_2x2_matrix_det(const T *src); 01111 01112 #define SPECIALISED_MATRIX_INV_AND_DET(D,T) \ 01113 template<> \ 01114 inline FixedMatrix<T,D,D> FixedMatrix<T,D,D>::inv() const \ 01115 throw (InvalidMatrixDimensionException,SingularMatrixException){ \ 01116 FixedMatrix<T,D,D> r; \ 01117 icl_util_get_fixed_##D##x##D##_matrix_inv<T>(begin(),r.begin()); \ 01118 return r; \ 01119 } \ 01120 template<> \ 01121 inline T FixedMatrix<T,D,D>::det() const \ 01122 throw(InvalidMatrixDimensionException){ \ 01123 return icl_util_get_fixed_##D##x##D##_matrix_det<T>(begin()); \ 01124 } 01125 01126 01127 SPECIALISED_MATRIX_INV_AND_DET(2,float); 01128 SPECIALISED_MATRIX_INV_AND_DET(3,float); 01129 SPECIALISED_MATRIX_INV_AND_DET(4,float); 01130 SPECIALISED_MATRIX_INV_AND_DET(2,double); 01131 SPECIALISED_MATRIX_INV_AND_DET(3,double); 01132 SPECIALISED_MATRIX_INV_AND_DET(4,double); 01133 01134 01135 #undef SPECIALISED_MATRIX_INV_AND_DET 01136 01138 #endif 01139 01140 01141 #ifdef WIN32 01142 // this is temporary fix! 01143 // because Homography2D is exported and therefore the base class is exported too 01144 // we need to import this in executables/libraries 01145 template class ICLMath_API FixedMatrix<float, 3, 3>; 01146 template class ICLMath_API FixedMatrix<double, 3, 3>; 01147 #endif 01148 01149 } // namespace math 01150 } 01151