qr.c

Go to the documentation of this file.

 
/*****************************************************************************/
#include "libtpcmodel.h"
/*****************************************************************************/
 
/*****************************************************************************/
/* Local functions */
int qr_get_next_col(
  double **mat, const unsigned int rows, const unsigned int cols,
  unsigned int row_pos, unsigned int *p, unsigned int *max_loc
);
void qr_swap_cols(
  unsigned int *p, const unsigned int i, const unsigned int j
);
void qr_householder(
  double **mat, const unsigned int rows, const unsigned int cols, 
  const unsigned int row_pos, const unsigned int col_pos, double *result
);
void qr_apply_householder(
  double **mat, double *rhs, const unsigned int rows, const unsigned int cols, 
  double *house, const unsigned int row_pos, unsigned int *p
);
void qr_back_solve(
  double **mat, double *rhs, const unsigned int rows, const unsigned int cols, 
  double *sol, unsigned int *p
);
/*****************************************************************************/
 
/*****************************************************************************/
int qrLSQ(
  double **mat, 
  double *rhs, 
  double *sol, 
  const unsigned int rows, 
  const unsigned int cols,
  double *r2
) {
  if(rows<1 || cols<1) return(1);
  if(mat==NULL || rhs==NULL || sol==NULL) return(2);
 
  unsigned int i, j, max_loc;
  unsigned int *p=NULL;
  double *buf=NULL, *ptr, *v=NULL, *orig_b=NULL, **A=NULL;
 
  /* Allocate memory for index vector and Householder transform vector */
  /* and original data matrix and vector. */
  p=malloc(sizeof(unsigned int)*cols);
  A=malloc(sizeof(double*)*rows);
  buf=malloc(sizeof(double)*(rows*cols+rows+rows));
  if(p==NULL || A==NULL || buf==NULL) {
    free(p); free(A); free(buf); 
    return(3);
  }
  ptr=buf; 
  for(i=0; i<rows; i++) {A[i]=ptr; ptr+=cols;}
  v=ptr; ptr+=rows; orig_b=ptr;
  /* copy the original data */
  for(i=0; i<rows; i++) {
    orig_b[i]=rhs[i];
    for(j=0; j<cols; j++) A[i][j]=mat[i][j];
  }
 
  /* Initial permutation vector. */
  for(i=0; i<cols; i++) p[i]=i;
  
  /* Apply rotators to make R and Q'*b */
  for(i=0; i<cols; i++) {
    if(qr_get_next_col(A, rows, cols, i, p, &max_loc)==0)
      qr_swap_cols(p, i, max_loc);
    qr_householder(A, rows, cols, i, p[i], v);
    qr_apply_householder(A, rhs, rows, cols, v, i, p);
  }
 
  /* Back solve Rx = Q'*b */
  qr_back_solve(A, rhs, rows, cols, sol, p);
 
  /* Compute fitted b[] (matrix-vector product A*x) using non-modified A */
  for(i=0; i<rows; i++) {
    rhs[i]=0.0;
    for(j=0; j<cols; j++) rhs[i]+=mat[i][j]*sol[j];
  }
 
  /* Compute R^2, if requested */
  if(r2!=NULL) {
    double ss=0, d;
    for(i=0; i<rows; i++) {
      d=orig_b[i]-rhs[i];
      ss+=d*d;
    }
    *r2=ss;
  }
 
  /* Collect garbage. */
  free(p); free(A); free(buf);
  return(0);
}
 
 
int qr_get_next_col(
  double **mat, 
  const unsigned int rows, 
  const unsigned int cols,
  const unsigned int row_pos, 
  unsigned int *p,
  unsigned int *max_loc
) {
  if(mat==NULL || rows<1 || cols<1 || p==NULL) return(-1);
 
  unsigned int i, j, maxloc;
  double col_norm, max_norm;
 
  // Compute the norms of the sub columns and find the maximum
  max_norm=DBL_MIN; maxloc=0;
  for(j=0; j<cols; j++) {
    col_norm=0;
    for(i=row_pos; i<rows; i++) col_norm+=mat[i][p[j]]*mat[i][p[j]];
    if(col_norm>max_norm) {max_norm=col_norm; maxloc=j;}
  }
  if(max_norm!=DBL_MIN) {
    if(max_loc!=NULL) *max_loc=maxloc;  
    return(0); 
  }
  return(1);
}
 
void qr_swap_cols(unsigned int *p, const unsigned int i, const unsigned int j)
{
  unsigned int temp;
  temp=p[i]; p[i]=p[j]; p[j]=temp;
}
 
void qr_householder(
  double **mat, 
  const unsigned int rows, 
  const unsigned int cols,
  const unsigned int row_pos,
  const unsigned int col_pos, 
  double *result
) {
  if(rows<1 || cols<1 || mat==NULL || result==NULL) return;
 
  unsigned int i;
  double norm=0.0;
 
  for(i=row_pos; i<rows; i++) norm+=mat[i][col_pos]*mat[i][col_pos];
  if(norm==0.0) return;
 
  norm=sqrt(norm);
  result[0]=(mat[row_pos][col_pos]-norm);
  for(i=1; i<(rows-row_pos); i++) result[i]=mat[i+row_pos][col_pos];
 
  norm=0.0;
  for(i=0; i<(rows-row_pos); i++) norm+=result[i]*result[i];
  if(norm==0) return;
 
  norm=sqrt(norm);
  for(i=0; i<(rows-row_pos); i++) result[i]*=(1.0/norm);
}
 
void qr_apply_householder(
  double **mat,
  double *rhs,
  const unsigned int rows,
  const unsigned int cols, 
  double *house,
  const unsigned int row_pos,
  unsigned int *p
) {
  if(mat==NULL || rhs==NULL || rows<1 || cols<1 || house==NULL || p==NULL) return;
 
  unsigned int i, j, k, n;
  double sum;
  double **hhmat;
  double **mat_cpy;
  double *rhs_cpy;
 
  // Get the dimensions for the Q matrix.
  n=rows-row_pos;
 
  // Allocate memory.
  hhmat=malloc(sizeof(double*)*n);
  for(i=0; i<n; i++) hhmat[i]=malloc(sizeof(double)*n);
  mat_cpy=malloc(sizeof(double*)*rows);
  for(i=0; i<rows; i++) mat_cpy[i]=malloc(sizeof(double)*cols);
  rhs_cpy=malloc(sizeof(double)*rows);
  
  // Copy the matrix.
  for(i=0; i<rows; i++)
    for(j=0; j<cols; j++)
      mat_cpy[i][j]=mat[i][j];
 
  // Copy the right hand side.
  for(i=0; i<rows; i++) rhs_cpy[i]=rhs[i];
 
  // Build the Q matrix from the Householder transform.
  for(j=0; j<n; j++)
    for(i=0; i<n; i++)
      if(i!=j) hhmat[i][j]=-2.0*house[j]*house[i];
      else  hhmat[i][j]=1.0-2.0*house[j]*house[i];
 
  // Multiply by the Q matrix.
  for(k=0; k<cols; k++)
    for(j=0; j<n; j++) {
      sum=0.0;
      for(i=0; i<n; i++) sum+=hhmat[j][i]*mat_cpy[i+row_pos][p[k]];
      mat[j+row_pos][p[k]]=sum;
    }
 
  // Multiply the rhs by the Q matrix.
  for(j=0; j<n; j++) {
    sum=0.0;
    for(i=0; i<n; i++) sum+=hhmat[i][j]*rhs_cpy[i+row_pos];
    rhs[j+row_pos]=sum;
  }
 
  // Collect garbage.
  for(i=0; i<(rows-row_pos); i++) free(hhmat[i]);
  for(i=0; i<rows; i++) free(mat_cpy[i]);
  free(hhmat);
  free(mat_cpy);
  free(rhs_cpy);
}
 
void qr_back_solve(
  double **mat, 
  double *rhs, 
  const unsigned int rows,
  const unsigned int cols, 
  double *sol, 
  unsigned int *p
) {
  if(mat==NULL || rhs==NULL || rows<1 || cols<1 || sol==NULL || p==NULL)
    return;
 
  unsigned int i, j, bottom;
  double sum;
 
  /* Fill the solution with zeros initially. */
  for(i=0; i<cols; i++) sol[i]=0.0;
 
  /* Find the first non-zero row from the bottom and start solving from here. */
  i=rows-1; bottom=i;
  while(1) {
    if(fabs(mat[i][p[cols-1]]) > 2.0*DBL_MIN) {bottom=i; break;}
    if(i==0) break; else i--;
  } 
 
  /* Standard back solving routine starting at the first non-zero diagonal. */
  i=bottom;
  while(1) {
    sum=0.0;
    j=cols-1;
    while(1) {
      if(j>i) sum+=sol[p[j]]*mat[i][p[j]];
      if(j==0) break; else j--;
    }
    if(i<cols) { // Added by VO
      if(mat[i][p[i]] > 2.0*DBL_MIN) sol[p[i]]=(rhs[i]-sum)/mat[i][p[i]];
      else sol[p[i]]=0.0;
    }
    if(i==0) break; else i--;
  }
}
/*****************************************************************************/
 
/*****************************************************************************/
int qr(
  double **A,
  int m,
  int n,
  double *B,
  double *X,
  double *rnorm,
  double *tau,
  double *res,
  double **wws,
  double *ws
) {
  int i;
  double *qrRes, *qrTau, **qrWws, *qrWs, *chain;
 
  /* Check the parameters and data */
  if(m<=0 || n<=0 || A==NULL || B==NULL || X==NULL) return(1);
  if(m<n) return(1);
 
  /* Allocate memory for working space, if required */
  if(tau!=NULL) qrTau=tau; else qrTau=(double*)calloc(n, sizeof(double));
  if(res!=NULL) qrRes=res; else qrRes=(double*)calloc(m, sizeof(double));
  if(wws!=NULL) {
    qrWws=wws; chain=(double*)NULL;
  } else {
    qrWws=(double**)malloc(m * sizeof(double*));
    chain=(double*)malloc(m*n * sizeof(double));
    for(i=0; i<m; i++) qrWws[i]=chain + i*n;
  }
  if(ws!=NULL) qrWs=ws; else qrWs=(double*)calloc(2*m, sizeof(double));
  if(qrTau==NULL || qrRes==NULL || qrWws==NULL || qrWs==NULL) return(1);
 
  /* Form the householder decomposition and solve the least square problem */
  if(qr_decomp(A, m, n, qrTau, qrWws, qrWs)) return(2);
  if(qr_solve(A, m, n, qrTau, B, X, qrRes, rnorm, qrWws, qrWs)) return(3);
 
  /* Free working space, if it was allocated here */
  if(tau==NULL) free(qrTau);
  if(res==NULL) free(qrRes);
  if(wws==NULL) {free(qrWws); free(chain);}
  if(ws==NULL) free(qrWs);
  for(i=0; i<n; i++) if(isnan(X[i])) return(4);
  return(0);
} /* qr */
/*****************************************************************************/
 
/*****************************************************************************/
int qr_decomp(
  double **a,
  int M,
  int N,
  double *tau,
  double **cchain,
  double *chain
) {
  //printf("qr_decomp()\n");
  int i, m, n, MNmin;
  double *subvector, **submatrix;
 
  /* Local variables */
  if(M<N) MNmin=M; else MNmin=N;
  if(MNmin<1 || a==NULL || tau==NULL || cchain==NULL || chain==NULL) return(1);
 
  subvector=chain;
  submatrix=cchain;
 
  for(i=0; i<MNmin; i++) {
    //printf("i=%d (MNmin=%d)\n", i, MNmin);
    /* Compute the Householder transformation to reduce the j-th column of the matrix A to 
       a multiple of the j-th unit vector. Householder vector h_i is saved in the lower triangular 
       part of the column and Householder coefficient tau_i in the vector tau. */
    for(m=i; m<M; m++) subvector[m-i]=a[m][i];
    tau[i] = householder_transform(subvector, M-i);
    for(m=i; m<M; m++) a[m][i]=subvector[m-i];
 
    /* Apply the transformation to the remaining columns to get upper triangular part of matrix R  */
    if(i+1 < N) {
      for(m=i; m<M; m++)
        for(n=i+1; n<N; n++)
          submatrix[m-i][n-i-1]=a[m][n];
      if(householder_hm(tau[i], subvector, submatrix, M-i, N-i)) return(2);
      for(m=i; m<M; m++)
        for(n=i+1; n<N; n++)
          a[m][n]=submatrix[m-i][n-i-1];
    }
  }
 
  return(0);
}
/*****************************************************************************/
 
/*****************************************************************************/
int qr_solve(
  double **QR,
  int M,
  int N,
  double *tau,
  double *b,
  double *x,
  double *residual,
  double *resNorm,
  double **cchain,
  double *chain
) {
  //printf("qr_solve()\n");
  if(QR==NULL || tau==NULL || b==NULL || x==NULL || residual==NULL) return(1);
  if(cchain==NULL || chain==NULL) return(1);
  if(M<1 || N<1) return(2);
 
  int MNmin; if(M<N) MNmin=M; else MNmin=N;
 
  int m, n;
  double **Rmatrix=cchain;
  double *h=chain;
  double *w=chain+M;
 
  /* Get matrix R from the upper triangular part of QR
     First the rows N - M-1 are eliminated from R matrix*/
  for(m=0; m<N; m++) for(n=0; n<N; n++) Rmatrix[m][n]=QR[m][n];
  for(m=0; m<M; m++) residual[m]=b[m];
 
  /* Compute b = Q^T b */
  /* Form the product Q^T residual from householder vectors saved in the lower triangle of 
     QR matrix and householder coefficients saved in vector tau. */
  for(int i=0; i<MNmin; i++) {
    for(m=i; m<M; m++) h[m-i]=QR[m][i]; 
    for(m=i; m<M; m++) w[m-i]=residual[m];
    if(householder_hv(tau[i], M-i, h, w)) return(2);
    for(m=i; m<M; m++) residual[m]=w[m-i];
  }
 
  /* Solve R x = b by computing x = R^-1 b */
  for(n=0; n<N; n++) x[n]=residual[n];
  /* back-substitution */
  x[N-1]=x[N-1]/Rmatrix[N-1][N-1];
  for(int i=N-2; i>=0; i--) {
    for(int j=i+1; j<N; j++) x[i]-=Rmatrix[i][j]*x[j];
    x[i]/=Rmatrix[i][i];
  }
 
  /* Compute residual = b - A x = Q (Q^T b - R x) */
  for(n=0; n<N; n++) residual[n]=0.0;
  /* Compute residual= Q*residual */
  /* Form the product Q*residual from householder vectors saved in the lower triangle
     of QR matrix and householder coefficients saved in vector tau. */
  for(int i=MNmin-1; i>=0; i--) {
    for(int m=i; m<M; m++) h[m-i]=QR[m][i]; 
    for(int m=i; m<M; m++) w[m-i]=residual[m];
    if(householder_hv(tau[i], M-i, h, w)) return(2);
    for(int m=i; m<M; m++) residual[m]=w[m-i];
  }
 
  /* Compute norm^2 of the residual vector, if needed */
  if(resNorm!=NULL)
    for(m=0, *resNorm=0.0; m<M; m++) *resNorm +=residual[m]*residual[m];
 
  return(0);
}
/*****************************************************************************/
 
/*****************************************************************************/
int qr_weight(
  int N,
  int M,
  double **A,
  double *b,
  double *weight,
  double *ws
) {
  int n, m;
  double *w;
 
  /* Check the arguments */
  if(N<1 || M<1 || A==NULL || b==NULL || weight==NULL) return(1);
 
  /* Allocate memory, if necessary */
  if(ws==NULL) {
    w=(double*)malloc(M*sizeof(double)); if(w==NULL) return(2);
  } else {
    w=ws;
  }
 
  /* Check that weights are not zero and get the square roots of them to w[]. */
  for(m=0; m<M; m++) {
    if(weight[m]<=1.0e-100) w[m]=1.0e-50;
    else w[m]=sqrt(weight[m]);
  }
 
  /* Multiply rows of matrix A and elements of vector b with weights. */
  for(m=0; m<M; m++) {
    for(n=0; n<N; n++) A[m][n]*=w[m];
    b[m]*=w[m];
  }
 
  if(ws==NULL) free(w);
  return(0);
}
/*****************************************************************************/
 
/*****************************************************************************/
int qrLH(
  const unsigned int m, 
  const unsigned int n,
  double *a,
  double *b,
  double *x,
  double *r2
) {
  /* Check the input */
  if(a==NULL || b==NULL || x==NULL || r2==NULL) return(2);
  if(n<1 || m<n) {*r2=nan(""); return(2);}
 
  /* Initiate output to zeroes, in case of exit because of singularity */
  for(unsigned int ni=0; ni<n; ni++) x[ni]=0.0;
  *r2=0.0;
 
  /* Rotates matrix A into upper triangular form */
  for(unsigned int ni=0; ni<n; ni++) {
    /* Find constants for rotation and diagonal entry */
    double sq=0.0;
    for(unsigned int mi=ni; mi<m; mi++) sq+=a[mi + ni*m]*a[mi + ni*m];
    if(sq==0.0) return(1);
    double qv1=-copysign(sqrt(sq), a[ni + ni*m]);
    double u1=a[ni + ni*m] - qv1;
    a[ni + ni*m]=qv1;
    unsigned int ni1=ni+1;
    /*  Rotate the remaining columns of sub-matrix. */
    for(unsigned int nj=ni1; nj<n; nj++) {
      double dot=u1*a[ni + nj*m];
      for(unsigned int mi=ni1; mi<m; mi++)
        dot+=a[mi + nj*m] * a[mi + ni*m];
      double c=dot/fabs(qv1*u1);
      for(unsigned int mi=ni1; mi<m; mi++)
        a[mi + nj*m]-=c*a[mi + ni*m];
      a[ni + nj*m]-=c*u1;
    }
    /* Rotate vector B */
    double dot=u1*b[ni];
    for(unsigned int mi=ni1; mi<m; mi++)
      dot+=b[mi]*a[mi + ni*m];
    double c=dot/fabs(qv1*u1);
    b[ni]-=c*u1;
    for(unsigned int mi=ni1; mi<m; mi++)
      b[mi]-=c*a[mi + ni*m];
  } // end of rotation loop
 
  /* Solve triangular system by back-substitution. */
  for(unsigned int ni=0; ni<n; ni++) {
    int k=n-ni-1;
    double s=b[k];
    for(unsigned int nj=k+1; nj<n; nj++) 
      s-=a[k + nj*m] * x[nj];
    if(a[k + k*m]==0.0) return(1);
    x[k]=s/a[k + k*m];
  }
 
  /* Calculate the sum of squared residuals. */
  *r2=0.0;
  for(unsigned int mi=n; mi<m; mi++)
    *r2 += b[mi]*b[mi];
 
  return(0);
}
/*****************************************************************************/
 
/*****************************************************************************/