Changeset 12337

Timestamp:

Mar 8, 2007, 1:21:39 PM (19 years ago)

Author:

rhl

Message:

Use GSL LU decomposition for psMatrixGJSolve

File:

• branches/rel-1_0/psLib/src/math/psMatrix.c (modified) (5 diffs)

branches/rel-1_0/psLib/src/math/psMatrix.c

@@ -22 +22 @@
  *  @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 +41 @@
 #include <gsl/gsl_permutation.h>
 #include <gsl/gsl_eigen.h>
+#include <gsl/gsl_math.h>
 
 #include "psMemory.h"
@@ -322 +323 @@
     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 +400 @@
     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 +592 @@
 }
 
-// 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,