Image Component Library (ICL)
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines
FixedMatrix.h
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines