Index: /branches/rel-1_0/psLib/src/math/psMatrix.c =================================================================== --- /branches/rel-1_0/psLib/src/math/psMatrix.c (revision 12336) +++ /branches/rel-1_0/psLib/src/math/psMatrix.c (revision 12337) @@ -22,6 +22,6 @@ * @author Andy Becker, University of Washington (SVD). * - * @version $Revision: 1.47.2.1 $ $Name: not supported by cvs2svn $ - * @date $Date: 2007-03-08 13:15:49 $ + * @version $Revision: 1.47.2.2 $ $Name: not supported by cvs2svn $ + * @date $Date: 2007-03-08 23:21:39 $ * * Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii @@ -41,4 +41,5 @@ #include #include +#include #include "psMemory.h" @@ -322,4 +323,76 @@ PS_ASSERT_VECTOR_TYPE(b, PS_TYPE_F64, false); PS_ASSERT_INT_EQUAL(a->numCols, a->numRows, false); +#if 1 // Use a more robust LU decomposition from GSL + // Initialize GSL data from a and b + gsl_matrix *a_gsl = gsl_matrix_alloc(a->numRows, a->numCols); + psImageToGslMatrix(a_gsl, a); + gsl_vector *b_gsl = gsl_vector_alloc(b->n); + psVectorToGslVector(b_gsl, b); + + gsl_permutation *perm_gsl = gsl_permutation_alloc(b->n); + + // Solve for {x} in equation: {b} = [A]{x} + int signum = 0; + if (gsl_linalg_LU_decomp(a_gsl, perm_gsl, &signum) != 0) { + psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Singular Matrix.\n"); + psTrace("psLib.math", 4, "LU decomposition failed.\n"); + + gsl_matrix_free(a_gsl); + gsl_vector_free(b_gsl); + gsl_permutation_free(perm_gsl); + + return false; + } + + gsl_vector *x_gsl = gsl_vector_alloc(b->n); + int solver_failed = gsl_linalg_LU_solve(a_gsl, perm_gsl, b_gsl, x_gsl); + + for (int i = 0; i < b->n && !solver_failed; i++) { // gsl_linalg_LU_solve doesn't always catch errors + if (!gsl_finite(b_gsl->data[i])) { + solver_failed = 1; + } + } + + if (solver_failed != 0) { + psError(PS_ERR_BAD_PARAMETER_VALUE, true, "LU solver failed.\n"); + psTrace("psLib.math", 4, "LU solver failed.\n"); + + gsl_matrix_free(a_gsl); + gsl_vector_free(x_gsl); + gsl_vector_free(b_gsl); + gsl_permutation_free (perm_gsl); + + return false; + } + + gslVectorToPsVector(b, x_gsl); // copy answer back to b + gsl_vector_free(b_gsl); + gsl_vector_free(x_gsl); + + // Invert A as we may want to use it for a covariance + gsl_matrix *ia_gsl = gsl_matrix_alloc(a->numRows, a->numCols); // inverse matrix if it exists + if (a->numRows == a->numCols) { + if (gsl_linalg_LU_invert(a_gsl, perm_gsl, ia_gsl) != 0) { + psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Singular Matrix.\n"); + psTrace("psLib.math", 4, "Singular Matrix (1).\n"); + + gsl_matrix_free(a_gsl); + gsl_matrix_free(ia_gsl); + gsl_permutation_free (perm_gsl); + + return false; + } + + // Copy a^{-1} back to a + gslMatrixToPsImage(a, ia_gsl); + } + + // Free GSL data + gsl_matrix_free(a_gsl); + gsl_matrix_free(ia_gsl); + gsl_permutation_free (perm_gsl); + + return true; +#else int Nx = a->numCols; PS_ASSERT_VECTOR_SIZE(b, (long int)Nx, false); @@ -327,4 +400,109 @@ psF64 *vector = b->data.F64; psF64 **matrix = a->data.F64; + + int *indxc = psAlloc(Nx*sizeof(int)); + int *indxr = psAlloc(Nx*sizeof(int)); + int *ipiv = psAlloc(Nx*sizeof(int)); + memset(ipiv, 0, Nx*sizeof(int)); + + int irow = 0; + int icol = 0; + + for (int i = 0; i < Nx; i++) { + double big = 0.0; + for (int j = 0; j < Nx; j++) { + if (!isfinite(matrix[i][j])) { + psError(PS_ERR_BAD_PARAMETER_VALUE, 3, + "Input matrix contains non-finite elements: matrix[%d][%d] is %.2f\n", + i, j, matrix[i][j]); + goto fescape; + } + if (ipiv[j] != 1) { + for (int k = 0; k < Nx; k++) { + if (ipiv[k] == 0) { + if (fabs(matrix[j][k]) >= big) { + big = fabs(matrix[j][k]); + irow = j; + icol = k; + } + } else { + if (ipiv[k] > 1) { + psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Singular Matrix (1).\n"); + psTrace("psLib.math", 4, "Singular Matrix (1).\n"); + goto fescape; + } + } + } + } + } + ipiv[icol]++; + if (irow != icol) { + for (int l = 0; l < Nx; l++) { + PS_SWAP(matrix[irow][l], matrix[icol][l]); + } + PS_SWAP(vector[irow], vector[icol]); + } + indxr[i] = irow; + indxc[i] = icol; + if (matrix[icol][icol] > -FLT_EPSILON && matrix[icol][icol] < FLT_EPSILON) { + psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Singular Matrix (2).\n"); + psTrace (__func__, 4, "Singular Matrix (2).\n"); + goto fescape; + } + double pivinv = 1.0 / matrix[icol][icol]; + matrix[icol][icol] = 1.0; + for (int l = 0; l < Nx; l++) { + matrix[icol][l] *= pivinv; + } + vector[icol] *= pivinv; + + for (int ll = 0; ll < Nx; ll++) { + if (ll != icol) { + double dum = matrix[ll][icol]; + matrix[ll][icol] = 0.0; + for (int l = 0; l < Nx; l++) { + matrix[ll][l] -= matrix[icol][l]*dum; + } + vector[ll] -= vector[icol]*dum; + } + } + } + + for (int l = Nx - 1; l >= 0; l--) { + if (indxr[l] != indxc[l]) { + for (int k = 0; k < Nx; k++) { + PS_SWAP(matrix[k][indxr[l]], matrix[k][indxc[l]]); + } + } + } + psFree(ipiv); + psFree(indxr); + psFree(indxc); + return true; + +fescape: + psFree(ipiv); + psFree(indxr); + psFree(indxc); + return false; +#endif +} + +// This is a temporary gauss-jordan solver based on gene's +// version based on the Numerical Recipes version +bool psMatrixGJSolveF32(psImage *a, + psVector *b + ) +{ + PS_ASSERT_IMAGE_NON_NULL(a, false); + PS_ASSERT_VECTOR_NON_NULL(b, false); + PS_ASSERT_IMAGE_TYPE(a, PS_TYPE_F32, false); + PS_ASSERT_VECTOR_TYPE(b, PS_TYPE_F32, false); + PS_ASSERT_INT_EQUAL(a->numCols, a->numRows, false); + int Nx = a->numCols; + PS_ASSERT_VECTOR_SIZE(b, (long int)Nx, false); + + psF32 *vector = b->data.F32; + psF32 **matrix = a->data.F32; int *indxc = psAlloc(Nx*sizeof(int)); int *indxr = psAlloc(Nx*sizeof(int)); @@ -414,107 +592,4 @@ } -// This is a temporary gauss-jordan solver based on gene's -// version based on the Numerical Recipes version -bool psMatrixGJSolveF32(psImage *a, - psVector *b - ) -{ - PS_ASSERT_IMAGE_NON_NULL(a, false); - PS_ASSERT_VECTOR_NON_NULL(b, false); - PS_ASSERT_IMAGE_TYPE(a, PS_TYPE_F32, false); - PS_ASSERT_VECTOR_TYPE(b, PS_TYPE_F32, false); - PS_ASSERT_INT_EQUAL(a->numCols, a->numRows, false); - int Nx = a->numCols; - PS_ASSERT_VECTOR_SIZE(b, (long int)Nx, false); - - psF32 *vector = b->data.F32; - psF32 **matrix = a->data.F32; - int *indxc = psAlloc(Nx*sizeof(int)); - int *indxr = psAlloc(Nx*sizeof(int)); - int *ipiv = psAlloc(Nx*sizeof(int)); - memset(ipiv, 0, Nx*sizeof(int)); - - int irow = 0; - int icol = 0; - - for (int i = 0; i < Nx; i++) { - double big = 0.0; - for (int j = 0; j < Nx; j++) { - if (!isfinite(matrix[i][j])) { - psError(PS_ERR_BAD_PARAMETER_VALUE, 3, - "Input matrix contains non-finite elements: matrix[%d][%d] is %.2f\n", - i, j, matrix[i][j]); - goto fescape; - } - if (ipiv[j] != 1) { - for (int k = 0; k < Nx; k++) { - if (ipiv[k] == 0) { - if (fabs(matrix[j][k]) >= big) { - big = fabs(matrix[j][k]); - irow = j; - icol = k; - } - } else { - if (ipiv[k] > 1) { - // psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Singular Matrix (1).\n"); - psTrace("psLib.math", 4, "Singular Matrix (1).\n"); - goto fescape; - } - } - } - } - } - ipiv[icol]++; - if (irow != icol) { - for (int l = 0; l < Nx; l++) { - PS_SWAP(matrix[irow][l], matrix[icol][l]); - } - PS_SWAP(vector[irow], vector[icol]); - } - indxr[i] = irow; - indxc[i] = icol; - if (matrix[icol][icol] == 0.0) { - // psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Singular Matrix (2).\n"); - psTrace (__func__, 4, "Singular Matrix (2).\n"); - goto fescape; - } - double pivinv = 1.0 / matrix[icol][icol]; - matrix[icol][icol] = 1.0; - for (int l = 0; l < Nx; l++) { - matrix[icol][l] *= pivinv; - } - vector[icol] *= pivinv; - - for (int ll = 0; ll < Nx; ll++) { - if (ll != icol) { - double dum = matrix[ll][icol]; - matrix[ll][icol] = 0.0; - for (int l = 0; l < Nx; l++) { - matrix[ll][l] -= matrix[icol][l]*dum; - } - vector[ll] -= vector[icol]*dum; - } - } - } - - for (int l = Nx - 1; l >= 0; l--) { - if (indxr[l] != indxc[l]) { - for (int k = 0; k < Nx; k++) { - PS_SWAP(matrix[k][indxr[l]], matrix[k][indxc[l]]); - } - } - } - psFree(ipiv); - psFree(indxr); - psFree(indxc); - return true; - -fescape: - psFree(ipiv); - psFree(indxr); - psFree(indxc); - return false; -} - psImage* psMatrixInvert(psImage* out, const psImage* in,