Changeset 23983

Timestamp:

Apr 28, 2009, 10:59:22 AM (17 years ago)

Author:

eugene

Message:

replace LU-decomp solver with real Gauss-Jordan elimination (which actually inverts A) -- based on Kahan description : see Ohana/src/libohana/doc/kahan-gji.pdf

File:

• trunk/psLib/src/math/psMatrix.c (modified) (3 diffs)

trunk/psLib/src/math/psMatrix.c

@@ -318 +318 @@
 // 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.
+# if (0) 
 bool psMatrixGJSolve(psImage *a,
                      psVector *b
@@ -328 +329 @@
     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
@@ -410 +390 @@
 }
 
+# else 
+
+// Gauss-Jordan elimination using full pivots based on 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 their implementation.  XXX this really needs to check on round-off errors (see version by
+// William Kahan
+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;
+
+        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 (!finite(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;
+            // XXX Kahan replaces the 0.0 pivot with epsilon*(largest element in column) + underFlow
+
+            /* 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;
+            // XXX measure the pivot growth and trigger on over/under flow
+            // growth *= tmpval;
+            // fprintf (stderr, "column: %d, growth: %e, epsilon: %e\n", maxcol, growth, epsilon);
+
+            /* 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;
+
+        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 (!finite(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;
+            // XXX Kahan replaces the 0.0 pivot with epsilon*(largest element in column) + underFlow
+
+            /* 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;
+            // XXX measure the pivot growth and trigger on over/under flow
+            // growth *= tmpval;
+            // fprintf (stderr, "column: %d, growth: %e, epsilon: %e\n", maxcol, growth, epsilon);
+
+            /* 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;
+}
+
+# endif
 
 psImage* psMatrixInvert(psImage* out,