Changeset 25027 for branches/pap/psLib/src/math/psMatrix.c

Timestamp:

Aug 7, 2009, 4:08:25 PM (17 years ago)

Author:

Paul Price

Message:

Merging trunk (r25026) to get up-to-date on old branch.

Location:

branches/pap

Files:

• . (modified) (1 prop)
• psLib/src/math/psMatrix.c (modified) (9 diffs)

branches/pap/psLib/src/math/psMatrix.c

@@ -199 +199 @@
 /*****************************************************************************/
 
-psImage* psMatrixLUD(psImage* out,
+psImage* psMatrixLUDecomposition(psImage* out,
                      psVector** perm,
                      const psImage* in)
@@ -210 +210 @@
 
 
-    #define psMatrixLUD_EXIT {psFree(out); return NULL;}
+    #define psMatrixLUDecomposition_EXIT {psFree(out); return NULL;}
 
     // Error checks
-    PS_ASSERT_GENERAL_IMAGE_NON_NULL(in, psMatrixLUD_EXIT);
-    PS_CHECK_POINTERS(in, out, psMatrixLUD_EXIT);
-    PS_CHECK_DIMEN_AND_TYPE(in, PS_DIMEN_IMAGE, psMatrixLUD_EXIT);
-    PS_ASSERT_GENERAL_PTR_NON_NULL(perm, psMatrixLUD_EXIT);
+    PS_ASSERT_GENERAL_IMAGE_NON_NULL(in, psMatrixLUDecomposition_EXIT);
+    PS_CHECK_POINTERS(in, out, psMatrixLUDecomposition_EXIT);
+    PS_CHECK_DIMEN_AND_TYPE(in, PS_DIMEN_IMAGE, psMatrixLUDecomposition_EXIT);
+    PS_ASSERT_GENERAL_PTR_NON_NULL(perm, psMatrixLUDecomposition_EXIT);
 
     out = psImageRecycle(out, in->numCols, in->numRows, in->type.type);
 
-    PS_CHECK_SQUARE(in, psMatrixLUD_EXIT); // gsl_linalg_LU_decomp would fail on non-square input.
-    PS_CHECK_SQUARE(out, psMatrixLUD_EXIT);
+    PS_CHECK_SQUARE(in, psMatrixLUDecomposition_EXIT); // gsl_linalg_LU_decomp would fail on non-square input.
+    PS_CHECK_SQUARE(out, psMatrixLUDecomposition_EXIT);
 
     // Initialize data
@@ -237 +237 @@
                 "Failed to allocate the permutation vector; "
                 "could not determine the cooresponding data type.");
-        psMatrixLUD_EXIT;
+        psMatrixLUDecomposition_EXIT;
     }
 
@@ -259 +259 @@
 }
 
-psVector* psMatrixLUSolve(psVector* out,
+psVector* psMatrixLUSolution(psVector* out,
                           const psImage* LU,
                           const psVector* RHS,
@@ -316 +316 @@
 }
 
-// This used to be "a temporary gauss-jordan solver based on gene's version based on the Numerical Recipes
-// version".  However, it's been removed due to copyright, and replaced with LU Decomposition solving.
-bool psMatrixGJSolve(psImage *a,
+psImage *psMatrixLUInvert(psImage *out,
+                          const psImage* LU,
+                          const psVector* perm)
+{
+    psS32 numRows = 0;
+    psS32 numCols = 0;
+    gsl_matrix *lu, *inverse;
+    gsl_permutation permGSL;
+
+    #define LUSOLVE_CLEANUP {psFree(out); return NULL;}
+
+    // Error checks
+    PS_ASSERT_GENERAL_IMAGE_NON_NULL(LU, LUSOLVE_CLEANUP);
+    PS_CHECK_DIMEN_AND_TYPE(LU, PS_DIMEN_IMAGE, LUSOLVE_CLEANUP);
+    PS_ASSERT_GENERAL_IMAGE_NON_EMPTY(LU, LUSOLVE_CLEANUP);
+    PS_ASSERT_GENERAL_VECTOR_NON_NULL(perm, LUSOLVE_CLEANUP);
+
+    out = psImageRecycle(out, LU->numCols, LU->numRows, LU->type.type);
+
+    // Initialize data
+    numRows = LU->numRows;
+    numCols = LU->numCols;
+
+    // Initialize GSL data
+    lu = gsl_matrix_alloc(numRows, numCols);
+    psImageToGslMatrix(lu, LU);
+
+    permGSL.size = perm->n;
+    permGSL.data = (psPtr)(perm->data.U8);
+
+    inverse = gsl_matrix_alloc(numRows, numCols);
+
+    // Solve for {x} in equation: {b} = [A]{x}
+    gsl_linalg_LU_invert(lu, &permGSL, inverse);
+
+    // Copy GSL vector data to psVector data
+    gslMatrixToPsImage(out, inverse);
+
+    // Free GSL data
+    gsl_matrix_free(lu);
+    gsl_matrix_free(inverse);
+
+    return out;
+}
+
+// This is the LU Decomposition version of the matrix equation solver.  It solves the equation
+// Ax = B, where A is a square matrix (NxN) and B is a vector of length N.  This solver only
+// yields the solution for x, which is returned to the vector B.  This now DOES calculate the
+// inverse of A.  A and B may be F32 or F64  XXX can they differ?
+bool psMatrixLUSolve(psImage *a,
                      psVector *b
                     )
@@ -328 +375 @@
     PS_ASSERT_INT_EQUAL(a->numCols, a->numRows, false);
     PS_ASSERT_VECTOR_SIZE(b, (long int)a->numCols, false);
-
-# if (0)
-    // XXX expand this out explicitly below
-    // Check for non-finite entries
-#define MATRIX_CHECK_NONFINITE_CASE(TYPE,MATRIX) \
-  case PS_TYPE_##TYPE: { \
-      ps##TYPE **values = MATRIX->data.TYPE; /* Dereference */ \
-      int numCols = (MATRIX)->numCols, numRows = (MATRIX)->numRows; /* Size of matrix */ \
-      for (int i = 0; i < numRows; i++) { \
-          for (int j = 0; j < numCols; j++) { \
-              if (!isfinite(values[i][j])) { \
-                  // psError(PS_ERR_BAD_PARAMETER_VALUE, 3,             
-                  // "Input matrix contains non-finite elements: matrix[%d][%d] is %.2f\n", 
-                  // i, j, values[i][j]);                               
-                  return false; \
-              } \
-          } \
-      } \
-      break; \
-  }
-# endif
 
     // Check for non-finite entries in matrix
@@ -390 +416 @@
     // Decompose the matrix and solve
     psVector *perm = NULL;              // Permutation vector
-    psImage *lu = psMatrixLUD(NULL, &perm, a); // LU decomposed matrix
+    psImage *lu = psMatrixLUDecomposition(NULL, &perm, a); // LU decomposed matrix
     if (!lu) {
         psError(PS_ERR_UNKNOWN, false, "Unable to generate LU decomposed matrix");
@@ -396 +422 @@
         return false;
     }
-    psVector *ans = psMatrixLUSolve(NULL, lu, b, perm); // Answer
+    psVector *ans = psMatrixLUSolution(NULL, lu, b, perm); // Answer
+
+    // invert the matrix : check here for an ill-conditioned matrix?
+    psMatrixLUInvert (a, lu, perm);
+
     psFree(lu);
     psFree(perm);
@@ -410 +440 @@
 }
 
+// This is the Gauss-Jordan elimination version of the matrix equation solver.  It solves the
+// equation Ax = B, where A is a square matrix (NxN) and B is a vector of length N.  This
+// solver calculates both the solution for x, which is returned to the vector B and the inverse
+// of A (returned in A).  A and B may be F32 or F64, but must match.
+
+// Gauss-Jordan elimination using full pivots based on William Kahan's BASIC example and Press
+// et al's description.  Substantially reworked for psLib style: major modifications to conform
+// to C indexing, use a boolean to track the completed pivot rows and catch the singular matrix
+// early on.  Also, much cleaner control loops than the Press implementation.
+
+// (based on version by William Kahan -- see Ohana/src/libohana/doc/kahan-gji.pdf)
+
+# define MAX_RANGE 1.0e7
+// MAX_RANGE is used to test for ill-conditioned input matrices.  For an ill-conditioned
+// matrix, one or more of the pivots trends towards zero, and growth goes to infinity.  Rather
+// than allow this to go to the numerical precision, I am raising an error if |growth| > MAX_RANGE
+bool psMatrixGJSolve(psImage *a,
+                     psVector *b
+    )
+{
+    PS_ASSERT_IMAGE_NON_NULL(a, false);
+    PS_ASSERT_VECTOR_NON_NULL(b, false);
+    PS_ASSERT_IMAGE_TYPE_F32_OR_F64(a, false);
+    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(b, false);
+    psAssert (a->type.type == b->type.type, "types must match for now");
+    PS_ASSERT_INT_EQUAL(a->numCols, a->numRows, false);
+    PS_ASSERT_VECTOR_SIZE(b, (long int)a->numCols, false);
+
+    // Check for non-finite entries in matrix
+    switch (a->type.type) {
+      case PS_TYPE_F32: {
+          psF32 **values = a->data.F32; /* Dereference */
+          int numCols = a->numCols, numRows = a->numRows; /* Size of matrix */
+          for (int i = 0; i < numRows; i++) {
+              for (int j = 0; j < numCols; j++) {
+                  if (!isfinite(values[i][j])) {
+                      return false;
+                  }
+              }
+          }
+          break;
+      }
+      case PS_TYPE_F64: {
+          psF64 **values = a->data.F64; /* Dereference */
+          int numCols = a->numCols, numRows = a->numRows; /* Size of matrix */
+          for (int i = 0; i < numRows; i++) {
+              for (int j = 0; j < numCols; j++) {
+                  if (!isfinite(values[i][j])) {
+                      return false;
+                  }
+              }
+          }
+          break;
+      }
+      default:
+        psAbort("Should never get here.");
+    }
+  
+    // Following the algorithm laid out by Press et al., we loop along the matrix diagonal, but
+    // we do not operate on the diagonal elements in order.  Instead, we are looking for the
+    // current max element and operating on that diagonal element.  This is effectively column
+    // pivoting.  Row pivoting is perfomed explicitly.
+
+    int nSquare = a->numCols;
+
+    psVector *colIndexV = psVectorAlloc(nSquare, PS_TYPE_S32);
+    psVector *rowIndexV = psVectorAlloc(nSquare, PS_TYPE_S32);
+    psVector *pivotV    = psVectorAlloc(nSquare, PS_TYPE_S32);
+    psVectorInit (pivotV, 0.0);
+
+    if (a->type.type == PS_TYPE_F32) {
+        psF32 **A = a->data.F32;
+        psF32  *B = b->data.F32;
+        int *colIndex = colIndexV->data.S32;
+        int *rowIndex = rowIndexV->data.S32;
+        int *pivot    = pivotV->data.S32;
+        psF32 growth = 1.0;
+
+        for (int diag = 0; diag < nSquare; diag++) {
+
+            psF32 maxval = 0.0;
+            int maxrow = 0;
+            int maxcol = 0;
+
+            // search for the next pivot
+            for (int row = 0; row < nSquare; row++) {
+                if (!isfinite(A[row][diag])) goto escape;
+
+                // if we have already operated on this row (pivot[row] is true), skip it
+                if (pivot[row]) continue;
+
+                // if we have not yet operated on this row (pivot[row] is false), look for pivot for this row
+                for (int col = 0; col < nSquare; col++) {
+                    if (pivot[col]) continue;
+                    if (fabs (A[row][col]) < maxval) continue;
+                    maxval = fabs (A[row][col]);
+                    maxrow = row;
+                    maxcol = col;
+                }
+            }
+
+            // if pivot[maxcol] is set, we have already done this row: this implies a singular matrix
+            if (pivot[maxcol]) goto escape;
+            pivot[maxcol] = 1;
+
+            // if the selected pivot is off the diagonal, do a row swap
+            if (maxrow != maxcol) {
+                for (int col = 0; col < nSquare; col++) PS_SWAP (A[maxrow][col], A[maxcol][col]);
+                PS_SWAP (B[maxrow], B[maxcol]);
+            }
+            rowIndex[diag] = maxrow;
+            colIndex[diag] = maxcol;
+            if (A[maxcol][maxcol] == 0.0) goto escape;
+            // Kahan replaces the 0.0 pivot with epsilon*(largest element in column) + underFlow.
+            // Here we are going to raise an error if the dynamic range is too large.
+
+            /* rescale by pivot reciprocal */
+            psF32 tmpval = 1.0 / A[maxcol][maxcol];
+            A[maxcol][maxcol] = 1.0;
+            for (int col = 0; col < nSquare; col++) A[maxcol][col] *= tmpval;
+            B[maxcol] *= tmpval;
+
+            // check for ill-conditioned matrix: measure the pivot growth and trigger on over/under flow
+            growth *= tmpval;
+            psTrace ("psLib.math", 4, "growth : %e\n", growth);
+            if (fabs(growth) > MAX_RANGE) goto escape;
+
+            /* adjust the elements above the pivot */
+            for (int row = 0; row < nSquare; row++) {
+                if (row == maxcol) continue;
+                tmpval = A[row][maxcol];
+                A[row][maxcol] = 0.0;
+                for (int col = 0; col < nSquare; col++) A[row][col] -= A[maxcol][col]*tmpval;
+                B[row] -= B[maxcol]*tmpval;
+            }
+        }
+
+        // swap back the inverse matrix based on the row swaps above
+        for (int col = nSquare - 1; col >= 0; col--) {
+            if (rowIndex[col] != colIndex[col]) {
+                for (int row = 0; row < nSquare; row++) PS_SWAP (A[row][rowIndex[col]], A[row][colIndex[col]]);
+            }
+        }
+    } else {
+        psF64 **A = a->data.F64;
+        psF64  *B = b->data.F64;
+        int *colIndex = colIndexV->data.S32;
+        int *rowIndex = rowIndexV->data.S32;
+        int *pivot    = pivotV->data.S32;
+        psF64 growth = 1.0;
+
+        for (int diag = 0; diag < nSquare; diag++) {
+
+            psF64 maxval = 0.0;
+            int maxrow = 0;
+            int maxcol = 0;
+
+            // search for the next pivot
+            for (int row = 0; row < nSquare; row++) {
+                if (!isfinite(A[row][diag])) goto escape;
+
+                // if we have already operated on this row (pivot[row] is true), skip it
+                if (pivot[row]) continue;
+
+                // if we have not yet operated on this row (pivot[row] is false), look for pivot for this row
+                for (int col = 0; col < nSquare; col++) {
+                    if (pivot[col]) continue;
+                    if (fabs (A[row][col]) < maxval) continue;
+                    maxval = fabs (A[row][col]);
+                    maxrow = row;
+                    maxcol = col;
+                }
+            }
+
+            // if pivot[maxcol] is set, we have already done this row: this implies a singular matrix
+            if (pivot[maxcol]) goto escape;
+            pivot[maxcol] = 1;
+
+            // if the selected pivot is off the diagonal, do a row swap
+            if (maxrow != maxcol) {
+                for (int col = 0; col < nSquare; col++) PS_SWAP (A[maxrow][col], A[maxcol][col]);
+                PS_SWAP (B[maxrow], B[maxcol]);
+            }
+            rowIndex[diag] = maxrow;
+            colIndex[diag] = maxcol;
+            if (A[maxcol][maxcol] == 0.0) goto escape;
+            // Kahan replaces the 0.0 pivot with epsilon*(largest element in column) + underFlow.
+            // Here we are going to raise an error if the dynamic range is too large.
+
+            /* rescale by pivot reciprocal */
+            psF64 tmpval = 1.0 / A[maxcol][maxcol];
+            A[maxcol][maxcol] = 1.0;
+            for (int col = 0; col < nSquare; col++) A[maxcol][col] *= tmpval;
+            B[maxcol] *= tmpval;
+
+            // check for ill-conditioned matrix: measure the pivot growth and trigger on over/under flow
+            growth *= tmpval;
+            psTrace ("psLib.math", 4, "growth : %e\n", growth);
+            if (fabs(growth) > MAX_RANGE) goto escape;
+
+            /* adjust the elements above the pivot */
+            for (int row = 0; row < nSquare; row++) {
+                if (row == maxcol) continue;
+                tmpval = A[row][maxcol];
+                A[row][maxcol] = 0.0;
+                for (int col = 0; col < nSquare; col++) A[row][col] -= A[maxcol][col]*tmpval;
+                B[row] -= B[maxcol]*tmpval;
+            }
+        }
+
+        // swap back the inverse matrix based on the row swaps above
+        for (int col = nSquare - 1; col >= 0; col--) {
+            if (rowIndex[col] != colIndex[col]) {
+                for (int row = 0; row < nSquare; row++) PS_SWAP (A[row][rowIndex[col]], A[row][colIndex[col]]);
+            }
+        }
+    }
+
+    psFree (pivotV);
+    psFree (rowIndexV);
+    psFree (colIndexV);
+    return true;
+
+escape:
+    psFree (pivotV);
+    psFree (rowIndexV);
+    psFree (colIndexV);
+    return false;
+}
 
 psImage* psMatrixInvert(psImage* out,