matrix.h

这是一款用C++编写的实现gmm算法的程序

 
  inline Matrix& SwapColumn(unsigned int i1, unsigned int i2){
    if((i1<column)&&(i2<column)){
      float tmp;
      for (unsigned int j = 0; j < row; j++){
        tmp            = _[j*column+i1];
        _[j*column+i1] = _[j*column+i2];
        _[j*column+i2] = tmp;        
      }        
    }
    return *this; 
  }
  
  void Print() const
  {
    std::cout << "Matrix " <<row<<"x"<<column<<std::endl;;
    for (unsigned int j = 0; j < row; j++){
      for (unsigned int i = 0; i < column; i++)
        std::cout << _[j*column+i] <<" ";
      std::cout << std::endl;
    }
  }
  
  void QRDecomposition(Matrix & Q, Matrix & R){
    Matrix QR;
    QRDecomposition(Q,R,QR);
  }
  
  void QRDecomposition(Matrix & Q, Matrix & R, Matrix & QR){    
    if(row>=column){
      QR = *this;
    }else{
      Transpose(QR);
    }
    unsigned int m = QR.row;
    unsigned int n = QR.column;
    Vector RDiag(n);
        
    for(unsigned int k=0;k<n;k++){
      float nrm = 0.0f;
      for (unsigned int i = k; i < m; i++) {
        nrm = hypot_s(nrm, QR(i,k));
      }
      if (nrm != 0.0f) {
        if(QR(k,k)<0.0f){
          nrm = -nrm;
        }
        for (unsigned int i = k; i < m; i++) {
            QR(i,k) /= nrm;
        }
        QR(k,k)+=1.0f;

        for (unsigned int j = k + 1; j < n; j++) {
          float s = 0.0f;
          for (unsigned int i = k; i < m; i++) {
              s += QR(i,k) * QR(i,j);
          }
          s = -s / QR(k,k);
          for (unsigned int i = k; i < m; i++) {
              QR(i,j) += s * QR(i,k);
          }
        }
      }
      RDiag(k) = -nrm;
    }
    
    R.Resize(n,n);
    for(unsigned int i = 0; i < n; i++) {
      for(unsigned int j = 0; j < n; j++) {
        if(i<j){
          R(i,j) = QR(i,j); 
        }else if(i==j){
          R(i,j) = RDiag(i);
        }else{
          R(i,j) = 0.0f;
        }
      }
    }

    Q.Resize(m,n);
    for(int k= n-1;k>=0;k--){
      for(unsigned int i = 0; i < m; i++) {
        Q(i,k) = 0.0f;
      }
      Q(k,k)=1.0f;
      for(unsigned int j = k; j < n; j++) {
        if(QR(k,k)!=0.0f){
          float s = 0.0f;
          for(unsigned int i = k; i < m; i++) {
            s += QR(i,k) * Q(i,j);
          }
          s = -s / QR(k,k);
          for(unsigned int i = k; i < m; i++) {
            Q(i,j) = Q(i,j) + s*QR(i,k);
          }
        }
      }       
    }
  }

  
  Matrix& Tridiagonalize(Matrix &result,Matrix &trans){
    result.Resize(2,row);
    Matrix A(*this);
    trans = A;
    if(row==0) return result;
    
    
    int n = row;
    int l,k,j,i;
    float scale,hh,h,g,f;
    for(i=n-1;i>=1;i--){
      l = i-1;
      h = scale = 0.0f;
      if(l>0){
        for(k=0;k<=l;k++)
          scale += fabs(A._[i*column+k]);
        if(scale == 0.0f){
          result._[column+i] = A._[i*column+l];
        }else{
          for(k=0;k<=l;k++){
            A._[i*column+k] /= scale;
            h += A._[i*column+k]*A._[i*column+k];
          }
          f= A._[i*column+l];
          g=(f>=0.0f?-sqrt(h):sqrt(h));
          result._[column+i] = scale*g;
          h-=f*g;
          A._[i*column+l] = f-g;
          f=0.0f;
          for(j=0;j<=l;j++){
            A._[j*column+i] = A._[i*column+j] /h;
            g=0.0f;
            for(k=0;k<=j;k++)
              g+=  A._[j*column+k]*A._[i*column+k];
            for(k=j+1;k<=l;k++)
              g+=  A._[k*column+j]*A._[i*column+k];
            result._[column+j] = g/h;
            f+= result._[column+j]*A._[i*column+j];
          }
          hh = f/(h+h);
          for(j=0;j<=l;j++){
            f = A._[i*column+j];
            result._[column+j]=g=result._[column+j]-hh*f;
            for(k=0;k<=j;k++)
              A._[j*column+k] -= (f*result._[column+k]+g*A._[i*column+k]);            
          }             
        }
      }else{
        result._[column+i] = A._[i*column+l];        
      }
      result._[i]=h;  
    }
    result._[0]=0.0f;  
    result._[column+0]=0.0f;
    for(i=0;i<n;i++){
      l=i-1;
      if(result._[i]){
        for(j=0;j<=l;j++){
          g=0.0f;
          for(k=0;k<=l;k++)
            g+= A._[i*column+k]*A._[k*column+j]; 
          for(k=0;k<=l;k++)
            A._[k*column+j] -= g*A._[k*column+i]; 
        }  
      }
      result._[i] = A._[i*column+i];
      A._[i*column+i] = 1.0f;
      for(j=0;j<=l;j++) A._[j*column+i]=A._[i*column+j]=0.0f;
    }
    trans = A;
    return result;
  }
    
  Matrix& TriDiag(Matrix &tri){
    Resize(tri.ColumnSize(),tri.ColumnSize(),false);
    Zero();
    for(unsigned int i=0;i<column;i++){
      _[i*(column+1)] = tri._[i];
      if(i<column-1)
        _[i*(column+1)+1] = tri._[column+i+1];
      if(i>0)  
        _[i*(column+1)-1] = tri._[column+i];
    }
    return *this;
  }
  
  int TriEigen(Vector &eigenValues, Matrix& eigenVectors,int maxIter = 30){
    if(row!=2) return -1;
    if(column==0) return -1;
    GetRow(0,eigenValues);
    Vector e;
    GetRow(1,e);
    
    const int n = column;
    int m,l,iter,i,k;
    float s,r,p,g,f,dd,c,b;
    
    for(i=1;i<n;i++) e._[i-1] = e._[i];
    e._[n-1] = 0.0f;

    for(l=0;l<n;l++){
      iter=0;
      do{
        for(m=l;m<=n-2;m++){
          dd = fabs(eigenValues._[m])+fabs(eigenValues._[m+1]);
          if((float)(fabs(e._[m])+dd) == dd) break;  
        }
        if(m!=l){
          if(iter++==maxIter) {
            //printf("Error: too many ierations...\n");
            break;
          }
          g=(eigenValues._[l+1]-eigenValues._[l])/(2.0f*e[l]);
          r=hypot_s(g,1.0f);
          g=eigenValues._[m]-eigenValues._[l]+e._[l]/(g+SIGN2(r,g));
          s=c=1.0f;
          p=0.0f;
          for(i=m-1;i>=l;i--){
            f=s*e._[i];
            b=c*e._[i];
            e._[i+1] =(r=hypot_s(f,g));
            if(r==0.0f){
              eigenValues._[i+1]-=p;
              e._[m] = 0.0f;
              break;  
            }
            s=f/r;
            c=g/r;
            g=eigenValues._[i+1]-p;
            r=(eigenValues._[i]-g)*s+2.0f*c*b;
            eigenValues._[i+1]=g+(p=s*r);
            g=c*r-b;
            for(k=0;k<n;k++){
              f=eigenVectors._[k*n+i+1];
              eigenVectors._[k*n+i+1]=s*eigenVectors._[k*n+i]+c*f;
              eigenVectors._[k*n+i]=c*eigenVectors._[k*n+i]-s*f;                
            }            
          }
          if((r==0.0f)&&(i>=0)) continue;
          eigenValues._[l]-=p;
          e._[l] = g;
          e._[m] = 0.0f;
        }        
      }while(m!=l); 
    } 
       
    return iter;
  }

  Matrix& SortColumnAbs(Vector & values){
    const int k = (values.Size()<column?values.Size():column);
    float cmax;
    int maxId;
    for(int i=0;i<k-1;i++){
      cmax  = fabs(values._[i]);
      maxId = i;
      for(int j=i+1;j<k;j++){
        if(cmax<fabs(values._[j])){
          cmax = fabs(values._[j]);
          maxId = j;  
        }            
      }
      if(maxId!=i){
        float tmp       = values._[i];
        values._[i]     = values._[maxId];
        values._[maxId] = tmp;
        SwapColumn(i,maxId);
      }     
    }  
    return *this;
  }

  Matrix& GramSchmidt(Vector &base){
    Matrix unit(row,1);
    unit.SetColumn(base,0);
    Matrix ext;
    unit.HCat(*this,ext);
    ext.GramSchmidt();
    (*this) = ext;
    return *this;  
  }

  
  Matrix& GramSchmidt(){
    Vector res(row),tmp(row),tmp2(row),tmp3(row);
    for(unsigned int i=0;i<column;i++){
      GetColumn(i,tmp);
      res = tmp;        
      for(unsigned int j=0;j<i;j++){
        GetColumn(j,tmp2);
        res-=tmp2.Mult((tmp2.Dot(tmp)),tmp3);          
      }
      float norm = res.Norm();
      if(norm>EPSILON){
        res /= norm;
      }else{
        res.Zero();
      }
      SetColumn(res,i);  
    }  
    return *this;    
  }

  Matrix& RemoveZeroColumns(){
    int zeroCnt = 0;
    int colCnt  = 0;
    while(colCnt < int(column)-zeroCnt){

      bool bIsZero = true;
      for(unsigned int j=0;j<row;j++){
        if(fabs(_[j*column+colCnt])>EPSILON){
          bIsZero = false;
          break;
        }
      }
      if(bIsZero){
        if(colCnt<int(column)-1-zeroCnt){
          SwapColumn(colCnt,int(column)-1-zeroCnt);
        }
        zeroCnt++;
      }else{
        colCnt++;
      }              
    }
    Resize(row,column-zeroCnt,true);
    return *this;       
  }

  Matrix& ClearColumn(unsigned int col){
    if(col<column){
      for(unsigned int i=0;i<row;i++){
        _[i*column+col] = 0.0f;
      }      
    }  
    return *this;
  }

  Vector SumRow(){
    Vector result(column);
    return SumRow(result);
  }

  Vector SumColumn(){
    Vector result(row);
    return SumColumn(result);
  }
  
  Vector & SumRow(Vector & result){
    result.Resize(column,false);
    result.Zero();
    for(unsigned int i=0;i<column;i++){
      for(unsigned int j=0;j<row;j++){
        result._[i] += _[j*column+i];
      }      
    }
    return result;  
  }

  Vector & SumColumn(Vector & result){
    result.Resize(row,false);
    result.Zero();
    for(unsigned int j=0;j<row;j++){
      for(unsigned int i=0;i<column;i++){
        result._[j] += _[j*column+i];
      }      
    }
    return result;  
  }
  
protected:

  inline void Release(){
    if(_!=NULL) delete [] _; 
    row    = 0;
    column = 0;
    _      = NULL;
  }  
public:  
  inline virtual void Resize(unsigned int rowSize, unsigned int colSize, bool copy = true){
    if((row!=rowSize)||(column!=colSize)){
      if((rowSize)&&(colSize)){
        float *arr = new float[rowSize*colSize];
        if(copy){
          unsigned int mj = (row<rowSize?row:rowSize);
          unsigned int mi = (column<colSize?column:colSize);
          
          for (unsigned int j = 0; j < mj; j++){
            for (unsigned int i = 0; i < mi; i++)
              arr[j*colSize+i] = _[j*column+i];
            for (unsigned int i = mi; i < colSize; i++)
              arr[j*colSize+i] = 0.0f;
          }
          for (unsigned int j = mj; j < rowSize; j++){
            for (unsigned int i = 0; i < colSize; i++)
              arr[j*colSize+i] = 0.0f;            
          }
        }
        if(_!=NULL) delete [] _; 
        _      = arr;
        row    = rowSize;
        column = colSize;        
      }else{
        Release();
      }
    }
  }
};


#endif