sba_lapack.c

sparse bundle ajustment的源码

    }

  /* Cholesky decomposition of A */
  //F77_FUNC(dpotf2)("U", (int *)&m, a, (int *)&m, (int *)&info);
  F77_FUNC(dpotrf)("U", (int *)&m, a, (int *)&m, (int *)&info);
  /* error treatment */
  if(info!=0){
    if(info<0){
      fprintf(stderr, "LAPACK error: illegal value for argument %d of dpotf2/dpotrf in sba_Axb_Chol()\n", -info);
      exit(1);
    }
    else{
      fprintf(stderr, "LAPACK error: the leading minor of order %d is not positive definite,\nthe factorization could not be completed for dpotf2/dpotrf in sba_Axb_Chol()\n", info);
      return 0;
    }
  }

  /* below are two alternative ways for solving the linear system: */
#if 1
  /* use the computed cholesky in one lapack call */
  F77_FUNC(dpotrs)("U", (int *)&m, (int *)&nrhs, a, (int *)&m, b, (int *)&m, &info);
  if(info<0){
    fprintf(stderr, "LAPACK error: illegal value for argument %d of dpotrs in sba_Axb_Chol()\n", -info);
    exit(1);
  }
#else
  /* solve the linear systems U^T y = b, U x = y */
  F77_FUNC(dtrtrs)("U", "T", "N", (int *)&m, (int *)&nrhs, a, (int *)&m, b, (int *)&m, &info);
  /* error treatment */
  if(info!=0){
    if(info<0){
      fprintf(stderr, "LAPACK error: illegal value for argument %d of dtrtrs in sba_Axb_Chol()\n", -info);
      exit(1);
    }
    else{
      fprintf(stderr, "LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in sba_Axb_Chol()\n", info);
      return 0;
    }
  }

  /* solve U x = y */
  F77_FUNC(dtrtrs)("U", "N", "N", (int *)&m, (int *)&nrhs, a, (int *)&m, b, (int *)&m, &info);
  /* error treatment */
  if(info!=0){
    if(info<0){
      fprintf(stderr, "LAPACK error: illegal value for argument %d of dtrtrs in sba_Axb_Chol()\n", -info);
      exit(1);
    }
    else{
      fprintf(stderr, "LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in sba_Axb_Chol()\n", info);
      return 0;
    }
  }
#endif /* 1 */

	/* copy the result in x */
	for(i=0; i<m; ++i)
    x[i]=b[i];

	return 1;
}

/*
 * This function returns the solution of Ax = b
 *
 * The function employs LU decomposition:
 * If A=L U with L lower and U upper triangular, then the original system
 * amounts to solving
 * L y = b, U x = y
 *
 * A is mxm, b is mx1. Argument iscolmaj specifies whether A is
 * stored in column or row major order. Note that if iscolmaj==1
 * this function modifies A and B!
 *
 * The function returns 0 in case of error,
 * 1 if successfull
 *
 * This function is often called repetitively to solve problems of identical
 * dimensions. To avoid repetitive malloc's and free's, allocated memory is
 * retained between calls and free'd-malloc'ed when not of the appropriate size.
 * A call with NULL as the first argument forces this memory to be released.
 */
int sba_Axb_LU(double *A, double *B, double *x, int m, int iscolmaj)
{
static double *buf=NULL;
static int buf_sz=0;

int a_sz, ipiv_sz, b_sz, tot_sz;
register int i, j;
int info, *ipiv, nrhs=1;
double *a, *b;
   
    if(A==NULL){
      if(buf) free(buf);
      buf=NULL;
      buf_sz=0;

      return 1;
    }

    /* calculate required memory size */
    ipiv_sz=m;
    a_sz=(iscolmaj)? 0 : m*m;
    b_sz=(iscolmaj)? 0 : m;
    tot_sz=ipiv_sz*sizeof(int) + (a_sz + b_sz)*sizeof(double);

    if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
      if(buf) free(buf); /* free previously allocated memory */

      buf_sz=tot_sz;
      buf=(double *)malloc(buf_sz);
      if(!buf){
        fprintf(stderr, "memory allocation in sba_Axb_LU() failed!\n");
        exit(1);
      }
    }

    ipiv=(int *)buf;
    if(!iscolmaj){
    	a=(double *)(ipiv + ipiv_sz);
    	b=a+a_sz;

    /* store A (column major!) into a and B into b */
	  for(i=0; i<m; ++i){
		  for(j=0; j<m; ++j)
        	a[i+j*m]=A[i*m+j];

      	b[i]=B[i];
    	}
    }
    else{ /* no copying is necessary */
      a=A;
      b=B;
    }

  /* LU decomposition for A */
	F77_FUNC(dgetrf)((int *)&m, (int *)&m, a, (int *)&m, ipiv, (int *)&info);  
	if(info!=0){
		if(info<0){
			fprintf(stderr, "argument %d of dgetrf illegal in sba_Axb_LU()\n", -info);
			exit(1);
		}
		else{
			fprintf(stderr, "singular matrix A for dgetrf in sba_Axb_LU()\n");
			return 0;
		}
	}

  /* solve the system with the computed LU */
  F77_FUNC(dgetrs)("N", (int *)&m, (int *)&nrhs, a, (int *)&m, ipiv, b, (int *)&m, (int *)&info);
	if(info!=0){
		if(info<0){
			fprintf(stderr, "argument %d of dgetrs illegal in sba_Axb_LU()\n", -info);
			exit(1);
		}
		else{
			fprintf(stderr, "unknown error for dgetrs in sba_Axb_LU()\n");
			return 0;
		}
	}

	/* copy the result in x */
	for(i=0; i<m; ++i){
		x[i]=b[i];
	}

	return 1;
}

/*
 * This function returns the solution of Ax = b
 *
 * The function is based on SVD decomposition:
 * If A=U D V^T with U, V orthogonal and D diagonal, the linear system becomes
 * (U D V^T) x = b or x=V D^{-1} U^T b
 * Note that V D^{-1} U^T is the pseudoinverse A^+
 *
 * A is mxm, b is mx1. Argument iscolmaj specifies whether A is
 * stored in column or row major order. Note that if iscolmaj==1
 * this function modifies A!
 *
 * The function returns 0 in case of error, 1 if successfull
 *
 * This function is often called repetitively to solve problems of identical
 * dimensions. To avoid repetitive malloc's and free's, allocated memory is
 * retained between calls and free'd-malloc'ed when not of the appropriate size.
 * A call with NULL as the first argument forces this memory to be released.
 */
int sba_Axb_SVD(double *A, double *B, double *x, int m, int iscolmaj)
{
static double *buf=NULL;
static int buf_sz=0;
static double eps=-1.0;

register int i, j;
double *a, *u, *s, *vt, *work;
int a_sz, u_sz, s_sz, vt_sz, tot_sz;
double thresh, one_over_denom;
register double sum;
int info, rank, worksz, *iwork, iworksz;
   
  if(A==NULL){
    if(buf) free(buf);
    buf=NULL;
    buf_sz=0;

    return 1;
  }

  /* calculate required memory size */
#ifndef SBA_LS_SCARCE_MEMORY
  worksz=-1; // workspace query. Keep in mind that dgesdd requires more memory than dgesvd
  /* note that optimal work size is returned in thresh */
  F77_FUNC(dgesdd)("A", (int *)&m, (int *)&m, NULL, (int *)&m, NULL, NULL, (int *)&m, NULL, (int *)&m,
          (double *)&thresh, (int *)&worksz, NULL, &info);
  /* F77_FUNC(dgesvd)("A", "A", (int *)&m, (int *)&m, NULL, (int *)&m, NULL, NULL, (int *)&m, NULL, (int *)&m,
          (double *)&thresh, (int *)&worksz, &info); */
  worksz=(int)thresh;
#else
  worksz=m*(7*m+4); // min worksize for dgesdd
  //worksz=5*m; // min worksize for dgesvd
#endif /* SBA_LS_SCARCE_MEMORY */
  iworksz=8*m;
  a_sz=(!iscolmaj)? m*m : 0;
  u_sz=m*m; s_sz=m; vt_sz=m*m;

  tot_sz=iworksz*sizeof(int) + (a_sz + u_sz + s_sz + vt_sz + worksz)*sizeof(double);

  if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
    if(buf) free(buf); /* free previously allocated memory */

    buf_sz=tot_sz;
    buf=(double *)malloc(buf_sz);
    if(!buf){
      fprintf(stderr, "memory allocation in sba_Axb_SVD() failed!\n");
      exit(1);
    }
  }

  iwork=(int *)buf;
  if(!iscolmaj){
    a=(double *)(iwork+iworksz);
    /* store A (column major!) into a */
    for(i=0; i<m; ++i)
      for(j=0; j<m; ++j)
        a[i+j*m]=A[i*m+j];
  }
  else{
    a=A; /* no copying required */
  }

  u=((double *)(iwork+iworksz)) + a_sz;
  s=u+u_sz;
  vt=s+s_sz;
  work=vt+vt_sz;

  /* SVD decomposition of A */
  F77_FUNC(dgesdd)("A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, iwork, &info);
  //F77_FUNC(dgesvd)("A", "A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, &info);

  /* error treatment */
  if(info!=0){
    if(info<0){
      fprintf(stderr, "LAPACK error: illegal value for argument %d of dgesdd/dgesvd in sba_Axb_SVD()\n", -info);
      exit(1);
    }
    else{
      fprintf(stderr, "LAPACK error: dgesdd (dbdsdc)/dgesvd (dbdsqr) failed to converge in sba_Axb_SVD() [info=%d]\n", info);

      return 0;
    }
  }

  if(eps<0.0){
    double aux;

    /* compute machine epsilon. DBL_EPSILON should do also */
    for(eps=1.0; aux=eps+1.0, aux-1.0>0.0; eps*=0.5)
                              ;
    eps*=2.0;
  }

  /* compute the pseudoinverse in a */
  memset(a, 0, m*m*sizeof(double)); /* initialize to zero */
  for(rank=0, thresh=eps*s[0]; rank<m && s[rank]>thresh; ++rank){
    one_over_denom=1.0/s[rank];

    for(j=0; j<m; ++j)
      for(i=0; i<m; ++i)
        a[i*m+j]+=vt[rank+i*m]*u[j+rank*m]*one_over_denom;
  }

	/* compute A^+ b in x */
	for(i=0; i<m; ++i){
	  for(j=0, sum=0.0; j<m; ++j)
      sum+=a[i*m+j]*B[j];
    x[i]=sum;
  }

	return 1;
}

/*
 * This function returns the solution of Ax = b for a real symmetric matrix A
 *
 * The function is based on UDUT factorization with the pivoting
 * strategy of Bunch and Kaufman:
 * A is factored as U*D*U^T where U is upper triangular and
 * D symmetric and block diagonal (aka spectral decomposition,
 * Banachiewicz factorization, modified Cholesky factorization)
 *
 * A is mxm, b is mx1. Argument iscolmaj specifies whether A is
 * stored in column or row major order. Note that if iscolmaj==1
 * this function modifies A and B!
 *
 * The function returns 0 in case of error,
 * 1 if successfull
 *
 * This function is often called repetitively to solve problems of identical
 * dimensions. To avoid repetitive malloc's and free's, allocated memory is
 * retained between calls and free'd-malloc'ed when not of the appropriate size.
 * A call with NULL as the first argument forces this memory to be released.
 */
int sba_Axb_BK(double *A, double *B, double *x, int m, int iscolmaj)
{
static double *buf=NULL;
static int buf_sz=0, nb=0;

int a_sz, ipiv_sz, b_sz, work_sz, tot_sz;
register int i, j;
int info, *ipiv, nrhs=1;
double *a, *b, *work;
   
    if(A==NULL){
      if(buf) free(buf);
      buf=NULL;
      buf_sz=0;

      return 1;
    }

    /* calculate required memory size */
    ipiv_sz=m;
    a_sz=(iscolmaj)? 0 : m*m;
    b_sz=(iscolmaj)? 0 : m;
    if(!nb){
#ifndef SBA_LS_SCARCE_MEMORY
      double tmp;

      work_sz=-1; // workspace query; optimal size is returned in tmp
      F77_FUNC(dsytrf)("U", (int *)&m, NULL, (int *)&m, NULL, (double *)&tmp, (int *)&work_sz, (int *)&info);
      nb=((int)tmp)/m; // optimal worksize is m*nb
#else
      nb=-1; // min worksize is 1
#endif /* SBA_LS_SCARCE_MEMORY */
    }
    work_sz=(nb!=-1)? nb*m : 1;
    tot_sz=ipiv_sz*sizeof(int) + (a_sz + b_sz + work_sz)*sizeof(double);

    if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
      if(buf) free(buf); /* free previously allocated memory */

      buf_sz=tot_sz;
      buf=(double *)malloc(buf_sz);
      if(!buf){
        fprintf(stderr, "memory allocation in sba_Axb_BK() failed!\n");
        exit(1);
      }
    }

    ipiv=(int *)buf;
    if(!iscolmaj){
    	a=(double *)(ipiv + ipiv_sz);
    	b=a+a_sz;
    	work=b+b_sz;

      /* store A into a and B into b; A is assumed to be symmetric, hence
       * the column and row major order representations are the same
       */
      for(i=0; i<m; ++i){
        a[i]=A[i];
        b[i]=B[i];
      }
      for(j=m*m; i<j; ++i) // copy remaining rows; note that i is not re-initialized
        a[i]=A[i];
    }
    else{ /* no copying is necessary */
      a=A;
      b=B;
    	work=(double *)(ipiv + ipiv_sz);
    }

  /* factorization for A */
	F77_FUNC(dsytrf)("U", (int *)&m, a, (int *)&m, ipiv, work, (int *)&work_sz, (int *)&info);
	if(info!=0){
		if(info<0){
			fprintf(stderr, "argument %d of dsytrf illegal in sba_Axb_BK()\n", -info);
			exit(1);
		}
		else{
			fprintf(stderr, "singular block diagonal matrix D for dsytrf in sba_Axb_BK() [D(%d, %d) is zero]\n", info, info);
			return 0;
		}
	}

  /* solve the system with the computed factorization */
  F77_FUNC(dsytrs)("U", (int *)&m, (int *)&nrhs, a, (int *)&m, ipiv, b, (int *)&m, (int *)&info);
	if(info!=0){
		if(info<0){
			fprintf(stderr, "argument %d of dsytrs illegal in sba_Axb_BK()\n", -info);
			exit(1);
		}
		else{
			fprintf(stderr, "unknown error for dsytrs in sba_Axb_BK()\n");
			return 0;
		}
	}

	/* copy the result in x */
	for(i=0; i<m; ++i){
		x[i]=b[i];
	}

	return 1;