Index: trunk/psLib/src/math/psMatrix.c
===================================================================
--- trunk/psLib/src/math/psMatrix.c	(revision 26001)
+++ trunk/psLib/src/math/psMatrix.c	(revision 26892)
@@ -1033,5 +1033,5 @@
 }
 
-psVector *psMatrixSolveSVD(psVector *out, const psImage *matrix, const psVector *vector)
+psVector *psMatrixSolveSVD(psVector *out, const psImage *matrix, const psVector *vector, float thresh)
 {
     #define psMatrixSolveSVD_EXIT {psFree(out); return NULL; }
@@ -1063,4 +1063,29 @@
     gsl_vector_free(work);
 
+    if (isfinite(thresh) && thresh > 0.0) {
+        // Trim the singular values
+        double total = 0.0;             // Total of singular values
+        for (int i = 0; i < numCols; i++) {
+            total += gsl_vector_get(S, i);
+        }
+        thresh *= total;
+        for (int i = 0; i < numCols; i++) {
+            double value = gsl_vector_get(S, i); // Singular value
+            if (value < thresh) {
+                psTrace("psLib.math", 5, "Trimming singular value %d: %lg", i, value);
+                gsl_vector_set(S, i, 0.0);
+#if 0
+                for (int j = 0; j < numCols; j++) {
+                    // Being thorough; probably unnecessary
+                    gsl_matrix_set(V, j, i, 0.0);
+                    gsl_matrix_set(A, j, i, 0.0);
+                }
+#endif
+            } else {
+                psTrace("psLib.math", 5, "Singular value %d: %lg", i, value);
+            }
+        }
+    }
+
     // Solve system (or minimise least-squares if overconstrained): Ax = b
     gsl_vector *b = gsl_vector_alloc(numCols); // Vector b
@@ -1093,8 +1118,6 @@
 }
 
-
-
 // This code supplied by Andy Becker (becker@astro.washington.edu)
-psImage *psMatrixSVD(psImage* evec, psVector* eval, const psImage* in)
+psImage *psMatrixSVD_old(psImage* evec, psVector* eval, const psImage* in)
 {
     #define psMatrixSVD_EXIT {psFree(evec); psFree(eval); return NULL;}
@@ -1145,2 +1168,56 @@
     return evec;
 }
+
+// this is basically a wrapper for the gsl function: gsl_linalg_SV_decomp() SVD decomposes
+// matrix A based on the following equation: A = U w V^T .  This function (as usual for SVD
+// implementations) returns V not V^T.  U and V are returned to images; w is returned to a
+// vector representing the diagonal of w.  The input image A is not modified.  U, V, and w may
+// be supplied as NULL or may be allocated; their lengths are set here to match the
+// dimensionality of A.  XXX there is no error handling for the gsl functions (anywhere in
+// psMatrix.c)
+bool psMatrixSVD(psImage **U, psVector **w, psImage **V, const psImage *A)
+{
+    // Error checks  Missing one for eval
+    PS_ASSERT_PTR_NON_NULL(U, false);
+    PS_ASSERT_PTR_NON_NULL(w, false);
+    PS_ASSERT_PTR_NON_NULL(V, false);
+    PS_ASSERT_PTR_NON_NULL(A, false);
+
+    // A is provided with size Nx,Ny = numCols,numRows
+    // U has size Nx,Ny
+    // V has size Nx,Nx
+    // w has size Nx
+
+    // Initialize data
+    int numRows = A->numRows;
+    int numCols = A->numCols;
+
+    *U = psImageRecycle(*U,  numCols, numRows, A->type.type);
+    *V = psImageRecycle(*V,  numCols, numCols, A->type.type);
+    *w = psVectorRecycle(*w, numCols, A->type.type);
+
+    gsl_matrix *Agsl = gsl_matrix_alloc(numRows, numCols);
+    gsl_matrix *Vgsl = gsl_matrix_alloc(numCols, numCols);
+    gsl_vector *Sgsl = gsl_vector_alloc(numCols);
+    gsl_vector *work = gsl_vector_alloc(numCols);
+
+    // Copy psImage data into GSL matrix data
+    matrixPStoGSL(Agsl, A);
+
+    // Calculate SVD decomposition
+    gsl_linalg_SV_decomp(Agsl, Vgsl, Sgsl, work);
+
+    // Copy GSL matrix data to psImage data
+    matrixGSLtoPS(*V, Vgsl);
+    matrixGSLtoPS(*U, Agsl);  // gsl_linalg_SV_decomp replaces A with U
+    vectorGSLtoPS(*w, Sgsl);
+
+    // Free GSL data
+    gsl_matrix_free(Agsl);
+    gsl_matrix_free(Vgsl);
+    gsl_vector_free(Sgsl);
+    gsl_vector_free(work);
+
+    return true;
+}
+