Index: /trunk/psLib/src/math/psMinimize.c
===================================================================
--- /trunk/psLib/src/math/psMinimize.c	(revision 4857)
+++ /trunk/psLib/src/math/psMinimize.c	(revision 4858)
@@ -9,6 +9,6 @@
  *  @author GLG, MHPCC
  *
- *  @version $Revision: 1.130 $ $Name: not supported by cvs2svn $
- *  @date $Date: 2005-08-11 23:04:32 $
+ *  @version $Revision: 1.131 $ $Name: not supported by cvs2svn $
+ *  @date $Date: 2005-08-24 01:04:37 $
  *
  *  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
@@ -28,4 +28,5 @@
 #include "psImage.h"
 #include "psImageStructManip.h"
+#include "psBinaryOp.h"
 /*****************************************************************************/
 /* DEFINE STATEMENTS                                                         */
@@ -2131,2 +2132,391 @@
 
 
+/******************************************************************************
+EAM Code:
+ *****************************************************************************/
+
+// XXX EAM : my alternate EAMBuildSums1D
+static psVector *EAMBuildSums1D(
+    psVector* sums,
+    psF64 x,
+    psS32 nTerm)
+{
+    psS32 nSum = 0;
+    psF64 xSum = 0.0;
+
+    nSum = 2*nTerm;
+    if (sums == NULL) {
+        sums = psVectorAlloc(nSum, PS_TYPE_F64);
+    }
+    if (nSum > sums->n) {
+        sums = psVectorRealloc(sums, nSum);
+    }
+
+    xSum = 1.0;
+    for (int i = 0; i < nSum; i++) {
+        sums->data.F64[i] = xSum;
+        xSum *= x;
+    }
+    return (sums);
+}
+
+// XXX EAM : test version of 1d fitting
+psPolynomial1D* VectorFitPolynomial1DOrd_EAM(
+    psPolynomial1D* myPoly,
+    psVector *mask,
+    const psVector *x,
+    const psVector *y,
+    const psVector *yErr
+)
+{
+    // I think this is 1 dimension down
+    psImage*     A = NULL;
+    psVector*    B = NULL;
+    psVector* xSums = NULL;
+    psS32 nTerm;
+    psS32 nOrder;
+    psF64 wt;
+
+    psTrace(".psLib.dataManip.VectorFitPolynomial1DOrd", 4,
+            "---- VectorFitPolynomial1DOrd() begin ----\n");
+
+    if (psTraceGetLevel (".psLib.dataManip.VectorFitPolynomial1DOrd") >= 5) {
+        FILE *f = psTraceGetDestination ();
+        fprintf (f, "VectorFitPolynomial1D()\n");
+        for (int i = 0; i < x->n; i++) {
+            fprintf (f, "(x, y, yErr) is (%f, %f, %f)\n", x->data.F64[i], y->data.F64[i], yErr->data.F64[i]);
+        }
+    }
+
+    nTerm = myPoly->n;
+    nOrder = nTerm - 1;
+
+    A     = psImageAlloc(nTerm, nTerm, PS_TYPE_F64);
+    B     = psVectorAlloc(nTerm, PS_TYPE_F64);
+
+    //
+    // Initialize data structures.
+    // XXX: Use psLib function.
+    //
+    PS_VECTOR_SET_F64(B, 0.0);
+    PS_IMAGE_SET_F64(A, 0.0);
+
+    // xSums look like: 1, x, x^2, ... x^(2n+1)
+    // Build the B and A data structs.
+    for (int k = 0; k < x->n; k++) {
+        if ((mask != NULL) && mask->data.U8[k])
+            continue;
+        xSums = EAMBuildSums1D(xSums, x->data.F64[k], nTerm);
+
+        if (yErr == NULL) {
+            wt = 1.0;
+        } else {
+            // this should filter yErr == 0 values
+            wt = 1.0 / PS_SQR(yErr->data.F64[k]);
+        }
+        for (int i = 0; i < nTerm; i++) {
+            B->data.F64[i] += y->data.F64[k] * xSums->data.F64[i] * wt;
+        }
+
+        // we could skip half of the array and assign at the end
+        // we must handle masked orders
+        for (int i = 0; i < nTerm; i++) {
+            for (int j = 0; j < nTerm; j++) {
+                A->data.F64[i][j] += xSums->data.F64[i + j] * wt;
+            }
+        }
+    }
+
+    // GaussJordan version
+    if (0) {
+        // does the solution in place
+        psGaussJordan (A, B);
+
+        // the first nTerm entries in B correspond directly to the desired
+        // polynomial coefficients.  this is only true for the 1D case
+        for (int k = 0; k < nTerm; k++) {
+            myPoly->coeff[k] = B->data.F64[k];
+        }
+    } else
+        // LUD version of the fit
+    {
+        psImage *ALUD = NULL;
+        psVector* outPerm = NULL;
+        psVector* coeffs = NULL;
+
+        ALUD = psImageAlloc(nTerm, nTerm, PS_TYPE_F64);
+        ALUD = psMatrixLUD(ALUD, &outPerm, A);
+        coeffs = psMatrixLUSolve(coeffs, ALUD, B, outPerm);
+        for (int k = 0; k < nTerm; k++) {
+            myPoly->coeff[k] = coeffs->data.F64[k];
+        }
+    }
+
+    psFree(A);
+    psFree(B);
+    psFree(xSums);
+
+    psTrace(".psLib.dataManip.VectorFitPolynomial1DOrd", 4,
+            "---- VectorFitPolynomial1DOrd() begin ----\n");
+    return (myPoly);
+}
+
+// XXX EAM : this version uses the F64 vectors
+psVector *Polynomial2DEvalVectorD(
+    const psPolynomial2D *myPoly,
+    const psVector *x,
+    const psVector *y
+)
+{
+    PS_ASSERT_POLY_NON_NULL(myPoly, NULL);
+    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
+    PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL);
+    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
+    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL);
+
+    psVector *tmp;
+    psS32 vecLen=x->n;
+
+    // Determine the length of the output vector to by the minimum of the x,y vectors
+    if (y->n < vecLen) {
+        vecLen = y->n;
+    }
+
+    // Create output vector to return
+    tmp = psVectorAlloc(vecLen, PS_TYPE_F64);
+
+    // Evaluate the polynomial at the specified points
+    for (psS32 i=0; i<vecLen; i++) {
+        tmp->data.F64[i] = psPolynomial2DEval(myPoly, x->data.F64[i], y->data.F64[i]);
+    }
+
+    // Return output vector
+    return(tmp);
+}
+
+// XXX EAM : EAMBuildSums2D in analogy with EAMBuildSums1D
+static psImage *EAMBuildSums2D(
+    psImage *sums,
+    psF64 x,
+    psF64 y,
+    psS32 nXterm,
+    psS32 nYterm
+)
+{
+    psS32 nXsum = 0;
+    psS32 nYsum = 0;
+    psF64 xSum = 1.0;
+    psF64 ySum = 1.0;
+
+    nXsum = 2*nXterm;
+    nYsum = 2*nYterm;
+    if (sums == NULL) {
+        sums = psImageAlloc(nXsum, nYsum, PS_TYPE_F64);
+    }
+    if ((nXsum != sums->numCols) || (nYsum != sums->numRows)) {
+        psFree (sums);
+        sums = psImageAlloc(nXsum, nYsum, PS_TYPE_F64);
+    }
+
+    ySum = 1.0;
+    for (int j = 0; j < nYsum; j++) {
+        xSum = ySum;
+        for (int i = 0; i < nXsum; i++) {
+            sums->data.F64[i][j] = xSum;
+            xSum *= x;
+        }
+        ySum *= y;
+    }
+    return (sums);
+}
+
+// XXX EAM : test version of 2d fitting
+psPolynomial2D* VectorFitPolynomial2DOrd_EAM(
+    psPolynomial2D* myPoly,
+    psVector* mask,
+    const psVector* x,
+    const psVector* y,
+    const psVector* z,
+    const psVector* zErr
+)
+{
+    // I think this is 1 dimension down
+    psImage*     A = NULL;
+    psVector*    B = NULL;
+    psImage*   Sums = NULL;
+    psF64 wt;
+    psS32 nTerm;
+
+    // XXX:Watch for changes to the psPolys: nTerm != nOrder.
+    psS32 nXterm = myPoly->nX;
+    psS32 nYterm = myPoly->nY;
+    nTerm = nXterm * nYterm;
+
+    A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64);
+    B = psVectorAlloc(nTerm, PS_TYPE_F64);
+
+    //
+    // Initialize data structures.
+    // XXX: Use psLib function.
+    //
+    PS_VECTOR_SET_F64(B, 0.0);
+    PS_IMAGE_SET_F64(A, 0.0);
+
+    // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)
+
+    // Build the B and A data structs.
+    for (int k  = 0; k < x->n; k++) {
+        if ((mask != NULL) && mask->data.U8[k])
+            continue;
+        Sums = EAMBuildSums2D(Sums, x->data.F64[k], y->data.F64[k], nXterm, nYterm);
+
+        if (zErr == NULL) {
+            wt = 1.0;
+        } else {
+            // XXX: this should probably by zErr^2 !!
+            // this should filter zErr == 0 values
+            // XXX: Why isn't this zErr^2?
+            wt = 1.0 / zErr->data.F64[k];
+        }
+
+        // we could skip half of the array and assign at the end
+        // we must handle masked orders
+        for (int n = 0; n < nXterm; n++) {
+            for (int m = 0; m < nYterm; m++) {
+                B->data.F64[n+m*nXterm] += z->data.F64[k] * Sums->data.F64[n][m] * wt;
+            }
+        }
+
+        for (int i = 0; i < nXterm; i++) {
+            for (int j = 0; j < nYterm; j++) {
+                for (int n = 0; n < nXterm; n++) {
+                    for (int m = 0; m < nYterm; m++) {
+                        A->data.F64[i+j*nXterm][n+m*nXterm] += Sums->data.F64[i+n][j+m] * wt;
+                    }
+                }
+            }
+        }
+    }
+
+    // does the solution in place
+    // XXX: Check return codes!
+    psGaussJordan (A, B);
+
+    // XXX: Check return codes!
+    // ALUD = psMatrixLUD(ALUD, &outPerm, A);
+    // coeffs = psMatrixLUSolve(coeffs, ALUD, B, outPerm);
+
+    for (int n = 0; n < nXterm; n++) {
+        for (int m = 0; m < nYterm; m++) {
+            myPoly->coeff[n][m] = B->data.F64[n+m*nXterm];
+        }
+    }
+
+    psFree(A);
+    psFree(B);
+    psFree(Sums);
+
+    psTrace(".psLib.dataManip.VectorFitPolynomial2DOrd", 4,
+            "---- VectorFitPolynomial2DOrd() begin ----\n");
+    return (myPoly);
+}
+
+
+psPolynomial2D* RobustFit2D_nomask(
+    psPolynomial2D* poly,
+    const psVector* x,
+    const psVector* y,
+    const psVector* z,
+    const psVector* dz
+)
+{
+    psVector *X;
+    psVector *Y;
+    psVector *Z;
+    psVector *dZ;
+
+    psVector *zFit   = NULL;
+    psVector *zResid = NULL;
+    psStats  *stats  = NULL;
+
+    X  = psVectorCopy (NULL, x, PS_TYPE_F64);
+    Y  = psVectorCopy (NULL, y, PS_TYPE_F64);
+    Z  = psVectorCopy (NULL, z, PS_TYPE_F64);
+    dZ = psVectorCopy (NULL, dz, PS_TYPE_F64);
+
+    for (int N = 0; N < 3; N++) {
+        // XXX EAM : this would be better defined with an element mask
+        poly   = VectorFitPolynomial2DOrd_EAM(poly, NULL, X, Y, Z, dZ);
+        zFit   = Polynomial2DEvalVectorD(poly, x, y);
+        zResid = (psVector *) psBinaryOp(NULL, (void *) z, "-", (void *) zFit);
+
+        stats = psStatsAlloc (PS_STAT_CLIPPED_MEAN | PS_STAT_CLIPPED_STDEV);
+        stats  = psVectorStats (stats, zResid, NULL, NULL, 0);
+        psTrace (".psphot.RobustFit", 4, "residual stats for robust fit:  %g +/- %g (%d pts)\n", stats->clippedMean, stats->clippedStdev, stats->clippedNvalues);
+
+        // re-create X, Y, Z, dZ if pts are valid
+        int n = 0;
+        for (int i = 0; i < zResid->n; i++) {
+            if (fabs(zResid->data.F64[i] - stats->clippedMean) > 3*stats->clippedStdev) {
+                continue;
+            }
+            X->data.F64[n]  =  x->data.F64[i];
+            Y->data.F64[n]  =  y->data.F64[i];
+            Z->data.F64[n]  =  z->data.F64[i];
+            dZ->data.F64[n] = dz->data.F64[i];
+            n++;
+        }
+        X->n = n;
+        Y->n = n;
+        Z->n = n;
+        dZ->n = n;
+    }
+    return (poly);
+}
+
+// XXX EAM : be careful here with F32 vs F64 vectors
+psPolynomial2D* RobustFit2D(psPolynomial2D* poly,
+                            psVector* mask,
+                            const psVector* x,
+                            const psVector* y,
+                            const psVector* z,
+                            const psVector* dz)
+{
+    PS_ASSERT_VECTOR_NON_NULL(mask, NULL);
+    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
+    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
+    PS_ASSERT_VECTOR_NON_NULL(z, NULL);
+    PS_ASSERT_VECTOR_NON_NULL(dz, NULL);
+
+    psVector *zFit   = NULL;
+    psVector *zResid = psVectorAlloc (x->n, PS_TYPE_F64);
+    psStats  *stats  = psStatsAlloc (PS_STAT_SAMPLE_MEAN | PS_STAT_SAMPLE_STDEV);
+
+    for (int N = 0; N < 3; N++) {
+        poly   = VectorFitPolynomial2DOrd_EAM (poly, mask, x, y, z, dz);
+        zFit   = Polynomial2DEvalVectorD (poly, x, y);
+        zResid = (psVector *) psBinaryOp(zResid, (void *) z, "-", (void *) zFit);
+
+        stats  = psVectorStats (stats, zResid, NULL, mask, 1);
+        psTrace (".psphot.RobustFit", 4, "residual stats for robust fit:  %g +/- %g\n",
+                 stats->sampleMean, stats->sampleStdev);
+
+        // set mask if pts are not valid
+        // we are masking out any point which is out of range
+        // recovery is not allowed with this scheme
+        for (int i = 0; i < zResid->n; i++) {
+            if (mask->data.U8[i])
+                continue;
+            if (fabs(zResid->data.F64[i] - stats->sampleMean) > 3*stats->sampleStdev) {
+                mask->data.U8[i] = 1;
+                continue;
+            }
+        }
+        psFree (zFit);
+    }
+    psFree (zResid);
+    psFree (stats);
+    return (poly);
+}
+
+