/* compute the Cholesky decompostion of C in W, s.t. C=W^t W and W is upper triangular */
int LEVMAR_CHOLESKY(LM_REAL *C, LM_REAL *W, int m)
{
register int i, j;
int info;
/* copy weights array C to W (in column-major order!) so that LAPACK won't destroy it */
for(i=0; i<m; i++)
for(j=0; j<m; j++)
W[i+j*m]=C[i*m+j];
/* cholesky decomposition */
POTF2("U", (int *)&m, W, (int *)&m, (int *)&info);
/* error treatment */
if(info!=0){
if(info<0){
fprintf(stderr, "LAPACK error: illegal value for argument %d of dpotf2 in %s\n", -info, LCAT(LEVMAR_DER, "()"));
exit(1);
}
else{
fprintf(stderr, "LAPACK error: the leading minor of order %d is not positive definite,\n%s()\n", info,
RCAT("and the cholesky factorization could not be completed in ", LEVMAR_CHOLESKY));
return LM_ERROR;
}
}
/* the decomposition is in the upper part of W (in column-major order!).
* copying it to the lower part and zeroing the upper transposes
* W in row-major order
*/
for(i=0; i<m; i++)
for(j=0; j<i; j++){
W[i+j*m]=W[j+i*m];
W[j+i*m]=0.0;
}
return 0;
}
/*
* This function returns the solution of Ax=b
*
* The function assumes that A is symmetric & postive definite and employs
* the Cholesky decomposition:
* If A=U^T U with U upper triangular, the system to be solved becomes
* (U^T U) x = b
* This amount to solving U^T y = b for y and then U x = y for x
*
* A is mxm, b is mx1
*
* 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 AX_EQ_B_CHOL(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
{
LM_REAL stackBuf[16384];
const int stackBuf_sz = 16384;
__STATIC__ LM_REAL *buf=NULL;
__STATIC__ int buf_sz=0;
LM_REAL *a, *b;
int a_sz, b_sz, tot_sz;
register int i, j;
int info, nrhs=1;
if(!A)
#ifdef LINSOLVERS_RETAIN_MEMORY
{
if(buf) free(buf);
buf=NULL;
buf_sz=0;
return 1;
}
#else
return 1; /* NOP */
#endif /* LINSOLVERS_RETAIN_MEMORY */
/* calculate required memory size */
a_sz=m*m;
b_sz=m;
tot_sz=a_sz + b_sz;
if(tot_sz <= stackBuf_sz)
{
a=stackBuf;
}
else
{
#ifdef LINSOLVERS_RETAIN_MEMORY
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=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_CHOL) "() failed!\n");
exit(1);
}
}
#else
buf_sz=tot_sz;
buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
if(!buf){
fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_CHOL) "() failed!\n");
exit(1);
}
#endif /* LINSOLVERS_RETAIN_MEMORY */
a=buf;
}
b=a+a_sz;
/* store A (column major!) into a anb 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];
}
/* Cholesky decomposition of A */
POTF2("U", (int *)&m, a, (int *)&m, (int *)&info);
/* error treatment */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", POTF2) " in ", AX_EQ_B_CHOL) "()\n", -info);
exit(1);
}
else{
//fprintf(stderr, RCAT(RCAT("LAPACK error: the leading minor of order %d is not positive definite,\nthe factorization could not be completed for ", POTF2) " in ", AX_EQ_B_CHOL) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
if(buf) free(buf); /* free previously allocated memory */
#endif
return 0;
}
}
/* solve the linear system U^T y = b */
TRTRS("U", "T", "N", (int *)&m, (int *)&nrhs, a, (int *)&m, b, (int *)&m, &info);
/* error treatment */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_CHOL) "()\n", -info);
exit(1);
}
else{
fprintf(stderr, RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_CHOL) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
if(buf) free(buf); /* free previously allocated memory */
#endif
return 0;
}
}
/* solve the linear system U x = y */
TRTRS("U", "N", "N", (int *)&m, (int *)&nrhs, a, (int *)&m, b, (int *)&m, &info);
/* error treatment */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) "in ", AX_EQ_B_CHOL) "()\n", -info);
exit(1);
}
else{
//fprintf(stderr, RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_CHOL) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
if(buf) free(buf); /* free previously allocated memory */
#endif
return 0;
}
}
/* copy the result in x */
for(i=0; i<m; i++)
x[i]=b[i];
#ifndef LINSOLVERS_RETAIN_MEMORY
if(buf) free(buf); /* free previously allocated memory */
#endif
return 1;
}
