Index: /branches/pap/psLib/src/math/psMatrix.c =================================================================== --- /branches/pap/psLib/src/math/psMatrix.c (revision 25861) +++ /branches/pap/psLib/src/math/psMatrix.c (revision 25862) @@ -94,102 +94,101 @@ LHS_NAME.data = RHS_NAME; - -/*****************************************************************************/ -/* FILE STATIC FUNCTIONS */ -/*****************************************************************************/ - -static void psVectorToGslVector(gsl_vector *outGslVector, const psVector *inVector); -static void gslVectorToPsVector(psVector *outVector, gsl_vector *inGslVector); -static void psImageToGslMatrix(gsl_matrix *outGslMatrix, const psImage *inImage); -static void gslMatrixToPsImage(psImage *outImage, gsl_matrix *inGslMatrix); +//////////////////////////////////////////////////////////////////////////////// +// Conversion functions +//////////////////////////////////////////////////////////////////////////////// + +// gsl_vector holds *doubles*, so we can directly copy F64, but need to convert F32 /** Static function to copy psF32 or psF64 vector data to a GSL vector */ -static void psVectorToGslVector(gsl_vector *outGslVector, - const psVector *inVector) -{ - psU32 i = 0; - psU32 n = 0; - - - n = inVector->n; - for(i=0; itype.type == PS_TYPE_F32) { - outGslVector->data[i] = (psF64)inVector->data.F32[i]; +static void vectorPStoGSL(gsl_vector *out, const psVector *in) +{ + long n = in->n; // Size of input + switch (in->type.type) { + case PS_TYPE_F32: + for (long i = 0; i < n; i++) { + out->data[i] = in->data.F32[i]; + } + break; + case PS_TYPE_F64: + memcpy(out->data, in->data.F64, n * PSELEMTYPE_SIZEOF(PS_TYPE_F64)); + break; + default: + psAbort("Unsupported vector type: %x\n", in->type.type); + } + return; +} + +/** Static function to copy GSL vector data to a psF32 or psF64 vector */ +static void vectorGSLtoPS(psVector *out, const gsl_vector *in) +{ + long n = out->n; // Size of output + switch (out->type.type) { + case PS_TYPE_F32: + for (long i = 0; i < n; i++) { + out->data.F32[i] = in->data[i]; + } + break; + case PS_TYPE_F64: + memcpy(out->data.F64, in->data, n * PSELEMTYPE_SIZEOF(PS_TYPE_F64)); + break; + default: + psAbort("Unsupported vector type: %x\n", out->type.type); + } + return; +} + + +/** Static function to copy psF32 or psF64 image data to a GSL matrix */ +static void matrixPStoGSL(gsl_matrix *out, const psImage *in) +{ + int numCols = in->numCols, numRows = in->numRows; // Size of matrix + switch (in->type.type) { + case PS_TYPE_F32: + for (int y = 0, i = 0; y < numRows; y++) { + for (int x = 0; x < numCols; x++, i++) { + out->data[i] = in->data.F32[y][x]; + } + } + break; + case PS_TYPE_F64: + if (in->parent) { + for (int y = 0, i = 0; y < numRows; y++, i += numCols) { + memcpy(&out->data[i], in->data.F64[y], numCols * PSELEMTYPE_SIZEOF(PS_TYPE_F64)); + } } else { - outGslVector->data[i] = inVector->data.F64[i]; - } - } -} - -/** Static function to copy GSL vector data to a psF32 or psF64 vector */ -static void gslVectorToPsVector(psVector *outVector, - gsl_vector *inGslVector) -{ - psU32 i = 0; - psU32 n = 0; - - - n = outVector->n; - for(i=0; itype.type == PS_TYPE_F32) { - outVector->data.F32[i] = (psF32)inGslVector->data[i]; + memcpy(out->data, in->data.F64, numCols * numRows * PSELEMTYPE_SIZEOF(PS_TYPE_F64)); + } + break; + default: + psAbort("Unsupported vector type: %x\n", in->type.type); + } + return; +} + +/** Static function to copy GSL matrix data to a psF32 or psF64 image */ +static void matrixGSLtoPS(psImage *out, gsl_matrix *in) +{ + int numCols = out->numCols, numRows = out->numRows; // Size of matrix + switch (out->type.type) { + case PS_TYPE_F32: + for (int y = 0, i = 0; y < numRows; y++) { + for (int x = 0; x < numCols; x++, i++) { + out->data.F32[y][x] = in->data[i]; + } + } + break; + case PS_TYPE_F64: + if (out->parent) { + for (int y = 0, i = 0; y < numRows; y++, i += numCols) { + memcpy(out->data.F64[y], &in->data[i], numCols * PSELEMTYPE_SIZEOF(PS_TYPE_F64)); + } } else { - outVector->data.F64[i] = inGslVector->data[i]; - } - } -} - -/** Static function to copy psF32 or psF64 image data to a GSL matrix */ -static void psImageToGslMatrix(gsl_matrix *outGslMatrix, - const psImage *inImage) -{ - psU32 i = 0; - psU32 j = 0; - psU32 numRows = 0; - psU32 numCols = 0; - - - numRows = inImage->numRows; - numCols = inImage->numCols; - if(inImage->type.type == PS_TYPE_F32) { - for(i=0; idata[i*numCols+j] = inImage->data.F32[i][j]; - } - } - } else { - for(i=0; idata[i*numCols+j] = inImage->data.F64[i][j]; - } - } - } -} - -/** Static function to copy GSL matrix data to a psF32 or psF64 image */ -static void gslMatrixToPsImage(psImage *outImage, - gsl_matrix *inGslMatrix) -{ - psU32 i = 0; - psU32 j = 0; - psU32 numRows = 0; - psU32 numCols = 0; - - - numRows = outImage->numRows; - numCols = outImage->numCols; - if(outImage->type.type == PS_TYPE_F32) { - for(i=0; idata.F32[i][j] = inGslMatrix->data[i*numCols+j]; - } - } - } else { - for(i=0; idata.F64[i][j] = inGslMatrix->data[i*numCols+j]; - } - } - } + memcpy(out->data.F64, in->data, numCols * numRows * PSELEMTYPE_SIZEOF(PS_TYPE_F64)); + } + break; + default: + psAbort("Unsupported vector type: %x\n", out->type.type); + } + return; } @@ -245,5 +244,5 @@ // Copy psImage data into GSL matrix data - psImageToGslMatrix(lu, in); + matrixPStoGSL(lu, in); // Calculate LU decomposition @@ -251,5 +250,5 @@ // Copy GSL matrix data to psImage data - gslMatrixToPsImage(out, lu); + matrixGSLtoPS(out, lu); // Free GSL data @@ -293,7 +292,7 @@ // Initialize GSL data lu = gsl_matrix_alloc(numRows, numCols); - psImageToGslMatrix(lu, LU); + matrixPStoGSL(lu, LU); b = gsl_vector_alloc(RHS->n); - psVectorToGslVector(b, RHS); + vectorPStoGSL(b, RHS); x = gsl_vector_alloc(RHS->n); @@ -306,5 +305,5 @@ // Copy GSL vector data to psVector data - gslVectorToPsVector(out, x); + vectorGSLtoPS(out, x); // Free GSL data @@ -341,5 +340,5 @@ // Initialize GSL data lu = gsl_matrix_alloc(numRows, numCols); - psImageToGslMatrix(lu, LU); + matrixPStoGSL(lu, LU); permGSL.size = perm->n; @@ -352,5 +351,5 @@ // Copy GSL vector data to psVector data - gslMatrixToPsImage(out, inverse); + matrixGSLtoPS(out, inverse); // Free GSL data @@ -701,5 +700,5 @@ lu = gsl_matrix_alloc(numRows, numCols); inv = gsl_matrix_alloc(numRows, numCols); - psImageToGslMatrix(lu, in); + matrixPStoGSL(lu, in); // Invert data and calculate determinant @@ -716,5 +715,5 @@ // Copy GSL matrix data to psImage data - gslMatrixToPsImage(out, inv); + matrixGSLtoPS(out, inv); // Free GSL structs @@ -749,5 +748,5 @@ perm = gsl_permutation_alloc(numRows); lu = gsl_matrix_alloc(numRows, numCols); - psImageToGslMatrix(lu, in); + matrixPStoGSL(lu, in); // Calculate determinant @@ -877,5 +876,5 @@ inGSL = gsl_matrix_alloc(numRows, numCols); - psImageToGslMatrix(inGSL, in); + matrixPStoGSL(inGSL, in); outGSL = gsl_matrix_alloc(numRows, numCols); @@ -892,5 +891,5 @@ // Copy GSL matrix data to psImage data - gslMatrixToPsImage(out, outGSL); + matrixGSLtoPS(out, outGSL); // Free GSL structs @@ -1026,4 +1025,65 @@ } +psVector *psMatrixSolveSVD(const psImage *matrix, const psVector *vector) +{ + PS_ASSERT_IMAGE_NON_NULL(matrix, NULL); + PS_CHECK_DIMEN_AND_TYPE(matrix, PS_DIMEN_IMAGE, {return NULL;}); + PS_ASSERT_VECTOR_NON_NULL(vector, NULL); + PS_CHECK_DIMEN_AND_TYPE(vector, PS_DIMEN_VECTOR, {return NULL;}); + + int numCols = matrix->numCols, numRows = matrix->numRows; // Size of matrix + + // Decompose matrix: A = U S V^T + gsl_matrix *A = gsl_matrix_alloc(numRows, numCols); // Input matrix in GSL-speak; becomes matrix U + gsl_matrix *V = gsl_matrix_alloc(numCols, numCols); // Untransposed matrix V + gsl_vector *S = gsl_vector_alloc(numCols); // Singular values + gsl_vector *work = gsl_vector_alloc(numCols); // Work space for GSL + + matrixPStoGSL(A, matrix); + + int gslStatus = 0; // Status of GSL + if ((gslStatus = gsl_linalg_SV_decomp(A, V, S, work))) { + const char *err = gsl_strerror(gslStatus); + psError(PS_ERR_UNKNOWN, true, "Unable to decompose matrix: %s", err); + gsl_matrix_free(A); + gsl_matrix_free(V); + gsl_vector_free(S); + gsl_vector_free(work); + return NULL; + } + gsl_vector_free(work); + + // Solve system (or minimise least-squares if overconstrained): Ax = b + gsl_vector *b = gsl_vector_alloc(numCols); // Vector b + gsl_vector *x = gsl_vector_alloc(numCols); // Solution + + vectorPStoGSL(b, vector); + + if ((gslStatus = gsl_linalg_SV_solve(A, V, S, b, x))) { + const char *err = gsl_strerror(gslStatus); + psError(PS_ERR_UNKNOWN, true, "Unable to solve matrix equation: %s", err); + gsl_matrix_free(A); + gsl_matrix_free(V); + gsl_vector_free(S); + gsl_vector_free(b); + gsl_vector_free(x); + return NULL; + } + + gsl_matrix_free(A); + gsl_matrix_free(V); + gsl_vector_free(S); + gsl_vector_free(b); + + psVector *solution = psVectorAlloc(numCols, PS_TYPE_F64); + + vectorGSLtoPS(solution, x); + gsl_vector_free(x); + + return solution; +} + + + // This code supplied by Andy Becker (becker@astro.washington.edu) psImage *psMatrixSVD(psImage* evec, psVector* eval, const psImage* in) @@ -1050,5 +1110,5 @@ // Copy psImage data into GSL matrix data - psImageToGslMatrix(A, in); + matrixPStoGSL(A, in); // Calculate SVD decomposition @@ -1056,6 +1116,6 @@ // Copy GSL matrix data to psImage data - gslMatrixToPsImage(evec, V); - gslVectorToPsVector(eval, S); + matrixGSLtoPS(evec, V); + vectorGSLtoPS(eval, S); // Take the square root of eval Index: /branches/pap/psLib/src/math/psMatrix.h =================================================================== --- /branches/pap/psLib/src/math/psMatrix.h (revision 25861) +++ /branches/pap/psLib/src/math/psMatrix.h (revision 25862) @@ -66,7 +66,7 @@ */ psImage *psMatrixLUInvert( - psImage *out, ///< place result here if not NULL - const psImage* LU, ///< LU-decomposed matrix. - const psVector* perm ///< Permutation vector resulting from psMatrixLUD function. + psImage *out, ///< place result here if not NULL + const psImage* LU, ///< LU-decomposed matrix. + const psVector* perm ///< Permutation vector resulting from psMatrixLUD function. ); @@ -186,4 +186,13 @@ ); +/// Solve a matrix equation using Singular Value Decomposition +/// +/// Solves Ax = b for x +psVector *psMatrixSolveSVD( + const psImage *matrix, ///< Matrix to be solved + const psVector *vector ///< Vector of values + ); + + /// Single value decomposition, provided by Andy Becker psImage *psMatrixSVD(psImage* evec, psVector* eval, const psImage* in);