Changeset 37827 for branches/eam_branches/ipp-pv3-20140717-merge/psLib/src/imageops/psImageMapFit.c

Timestamp:

Jan 12, 2015, 12:35:53 PM (12 years ago)

Author:

eugene

Message:

merge changes from trunk

File:

• branches/eam_branches/ipp-pv3-20140717-merge/psLib/src/imageops/psImageMapFit.c (modified) (9 diffs)

branches/eam_branches/ipp-pv3-20140717-merge/psLib/src/imageops/psImageMapFit.c

@@ -18 +18 @@
 
 #include <stdio.h>
+#include <stdlib.h>
 #include "psError.h"
 #include "psAbort.h"
@@ -35 +36 @@
 #include "psImageStructManip.h"
 #include "psImageMap.h"
+#include "psSparse.h"
 // #include "psImagePixelInterpolate.h"
 // #include "psImageUnbin.h"
@@ -118 +120 @@
     psImageInit (A, 0.0);
     psVectorInit (B, 0.0);
-
+    
     // we are looping over the Nx,Ny image map elements;
     // the matrix equation contains Nx*Ny rows and columns
@@ -262 +264 @@
             int I = n + Nx * m;
             B->data.F32[I] = fi_rx_ry + fi_rx_py + fi_px_ry + fi_px_py;
-
+            
             // insert these values into their corresponding locations in A, B
             // float Sum = 0.0;
@@ -273 +275 @@
                     int J = (n + jn) + Nx * (m + jm);
                     A->data.F32[J][I] = sA[jn][jm];
+                    
                     // fprintf (stderr, "A %d %d (%d %d : %d %d): %f\n", I, J, n, m, n + jn, m + jm, sA[jn][jm]);
                     // Sum += sA[jn][jm];
@@ -282 +285 @@
     }
     // fprintf (stderr, "Total: %f\n", Total);
+
+    double MaxPivot = 0.0;
+    for (int i = 0; i < Nx*Ny; i++) {
+      MaxPivot = PS_MAX(MaxPivot, fabs(A->data.F32[i][i]));
+      // fprintf (stderr, "piv, max: %f : %f\n", A->data.F32[i][i], MaxPivot);
+    }
 
     // test for empty diagonal elements (unconstained cells), mark, and set pivots to 1.0
     psVector *Empty = psVectorAlloc (Nx*Ny, PS_TYPE_S8);
     psVectorInit (Empty, 0);
+    double MinPivot = 0.025*MaxPivot;
     for (int i = 0; i < Nx*Ny; i++) {
-        if (A->data.F32[i][i] == 0.0) {
+      if (fabs(A->data.F32[i][i]) < MinPivot) {
             Empty->data.S8[i] = 1;
             for (int j = 0; j < Nx*Ny; j++) {
@@ -320 +330 @@
         return true;
     }
-
+    
     // set bad values to NaN
     for (int i = 0; i < Nx*Ny; i++) {
@@ -341 +351 @@
     psFree (B);
     psFree (Empty);
-
     *pGoodFit = true;
     return true;
@@ -402 +411 @@
         psS32 Nkeep = 0;
         if (!psImageMapFit(pGoodFit, map, mask, maskValue, x, y, f, df)) {
+            psError(PS_ERR_UNKNOWN, false, "Could not fit image map.\n");
+            psFree(resid);
+            if (!inMask) psFree (mask);
+            return false;
+        }
+        if (!*pGoodFit) {
+            psWarning ("bad fit to image map, try something else");
+            psFree(resid);
+            if (!inMask) psFree (mask);
+            return true;
+        }
+
+        psVector *fit = psImageMapEvalVector(map, mask, maskValue, x, y);
+        if (fit == NULL) {
+            psError(PS_ERR_UNKNOWN, false, "Failure in psImageMapEvalVector().\n");
+            psFree(resid);
+            if (!inMask) psFree (mask);
+            return false;
+        }
+        for (int i = 0 ; i < f->n ; i++) {
+            resid->data.F32[i] = (f->data.F32[i] - fit->data.F32[i]);
+        }
+
+        if (!psVectorStats(stats, resid, NULL, mask, maskValue)) {
+            psError(PS_ERR_UNKNOWN, false, "Failure to compute statistics on the resid vector.\n");
+            psFree(resid);
+            psFree(fit);
+            if (!inMask) psFree (mask);
+            return false;
+        }
+
+        double meanValue = psStatsGetValue (stats, meanOption);
+        double stdevValue = psStatsGetValue (stats, stdevOption);
+
+        psTrace("psLib.imageops", 5, "Mean is %f\n", meanValue);
+        psTrace("psLib.imageops", 5, "Stdev is %f\n", stdevValue);
+        psF32 minClipValue = -minClipSigma*stdevValue;
+        psF32 maxClipValue = +maxClipSigma*stdevValue;
+
+        // 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 (psS32 i = 0; i < resid->n; i++) {
+            // XXX this prevents recovery of previously masked values
+            if (mask->data.PS_TYPE_VECTOR_MASK_DATA[i] & maskValue) {
+                continue;
+            }
+
+            if ((resid->data.F32[i] - meanValue > maxClipValue) || (resid->data.F32[i] - meanValue < minClipValue)) {
+                psTrace("psLib.imageops", 6, "Masking element %d  : %f vs %f : resid is %f\n", i, f->data.F32[i], fit->data.F32[i], resid->data.F32[i]);
+                mask->data.PS_TYPE_VECTOR_MASK_DATA[i] |= 0x01;
+                continue;
+            }
+            Nkeep++;
+        }
+
+        // We should probably exit this loop if no new elements were masked since the fit won't
+        // change.
+        psTrace("psLib.imageops", 6, "keeping %d of %ld pts for fit\n", Nkeep, x->n);
+        stats->clippedNvalues = Nkeep;
+        psFree(fit);
+    }
+
+    // Free local temporary variables
+    psFree(resid);
+    if (!inMask) psFree (mask);
+    *pGoodFit = true; // XXX probably don't need to set this (set by psImageMapFit)
+    return true;
+}
+
+// CZW: 2014-10-09
+// Sparse versions of MapFit and MapFitClip that assume the matrices are not filled.
+bool psImageMapFitSparse(bool *pGoodFit, psImageMap *map, const psVector *mask, psVectorMaskType maskValue,
+                   const psVector *x, const psVector *y, const psVector *f, const psVector *df)
+{
+    // XXX Add Asserts
+
+    *pGoodFit = false;
+
+    // dimensions of the output map image
+    int Nx = map->binning->nXruff;
+    int Ny = map->binning->nYruff;
+
+    
+    // no spatial information, just calculate mean & stdev
+    if ((Nx == 1) && (Ny == 1)) {
+        psStatsInit(map->stats);
+
+        // the user has supplied one of various stats option pairs,
+        psStatsOptions mean = psStatsMeanOption(map->stats->options);
+        psStatsOptions stdev = psStatsStdevOption(map->stats->options);
+        if (!psStatsSingleOption(mean)) {
+            psError(PS_ERR_UNKNOWN, true, "no valid mean stats option selected");
+            return false;
+        }
+        if (!psStatsSingleOption(stdev)) {
+            psError(PS_ERR_UNKNOWN, true, "no valid stdev stats option selected");
+            return false;
+        }
+
+        // XXX does ROBUST_MEDIAN work with weight?
+        if (!psVectorStats(map->stats, f, NULL, mask, maskValue)) {
+            psError(PS_ERR_UNKNOWN, false, "failure to measure stats");
+            return false;
+        }
+
+        map->map->data.F32[0][0]   = psStatsGetValue(map->stats, mean);
+        map->error->data.F32[0][0] = psStatsGetValue(map->stats, stdev);
+        if (isfinite(map->map->data.F32[0][0]) && isfinite( map->error->data.F32[0][0])) {
+            *pGoodFit = true;
+        }
+        return true;
+    }
+
+    if (Nx == 1) {
+        bool status;
+        status = psImageMapFit1DinY (pGoodFit, map, mask, maskValue, x, y, f, df);
+        return status;
+    }
+    if (Ny == 1) {
+        bool status;
+        status = psImageMapFit1DinX (pGoodFit, map, mask, maskValue, x, y, f, df);
+        return status;
+    }
+
+    // set up the redirection table so we can use sA[-1][-1], etc
+    // XXX psKernel does this for you --- PAP.
+    float SAm[3][3], *SAv[3], **sA;
+
+    for (int i = 0; i < 3; i++) {
+        SAv[i] = SAm[i] + 1;
+    }
+    sA = SAv + 1;
+
+    // elements of the matrix equation Ax = B; we are solving for the vector x
+    // psImage *A = psImageAlloc (Nx*Ny, Nx*Ny, PS_TYPE_F32);
+    // psVector *B = psVectorAlloc (Nx*Ny, PS_TYPE_F32);
+
+    // psImageInit (A, 0.0);
+    // psVectorInit (B, 0.0);
+
+    // CZW: call to psSparseAlloc
+    // It should match old A, and each element of that should only touch four others.
+    psSparse *Asparse = psSparseAlloc(Nx * Ny, 4 * Nx * Ny); 
+    
+    // we are looping over the Nx,Ny image map elements;
+    // the matrix equation contains Nx*Ny rows and columns
+
+    for (int n = 0; n < Nx; n++) {
+        for (int m = 0; m < Ny; m++) {
+            // define & init summing variables
+            float rx_rx_ry_ry = 0;
+            float rx_rx_dy_ry = 0;
+            float dx_rx_ry_ry = 0;
+            float dx_rx_dy_ry = 0;
+            float fi_rx_ry    = 0;
+            float rx_rx_py_py = 0;
+            float rx_rx_qy_py = 0;
+            float dx_rx_py_py = 0;
+            float dx_rx_qy_py = 0;
+            float fi_rx_py    = 0;
+            float px_px_ry_ry = 0;
+            float px_px_dy_ry = 0;
+            float qx_px_ry_ry = 0;
+            float qx_px_dy_ry = 0;
+            float fi_px_ry    = 0;
+            float px_px_py_py = 0;
+            float px_px_qy_py = 0;
+            float qx_px_py_py = 0;
+            float qx_px_qy_py = 0;
+            float fi_px_py    = 0;
+
+            // generate the sums for the fitting matrix element I,J
+            // I = n + nX*m
+            // J = (n + jn) + nX*(m + jm)
+            for (int i = 0; i < x->n; i++) {
+
+                if (mask && (mask->data.PS_TYPE_VECTOR_MASK_DATA[i] & maskValue)) continue;
+
+                // base coordinate offset for this point (x,y) relative to this map element (n,m)
+                float dx = psImageBinningGetRuffX (map->binning, x->data.F32[i]) - (n + 0.5);
+                float dy = psImageBinningGetRuffY (map->binning, y->data.F32[i]) - (m + 0.5);
+
+                // edge cases to include:
+                bool edgeX = false;
+                edgeX |= ((n == 1) && (dx < -1.0));
+                edgeX |= ((n == Nx - 2) && (dx > +1.0));
+
+                bool edgeY = false;
+                edgeY |= ((m == 1) && (dy < -1.0));
+                edgeY |= ((m == Ny - 2) && (dy > +1.0));
+
+                // skip points outside of 2x2 grid centered on n,m:
+                if (!edgeX && (fabs(dx) > 1.0)) continue;
+                if (!edgeY && (fabs(dy) > 1.0)) continue;
+
+                // related offset values
+                float rx = 1.0 - dx;
+                float ry = 1.0 - dy;
+                float px = 1.0 + dx;
+                float py = 1.0 + dy;
+                float qx = -dx;
+                float qy = -dy;
+
+                // data value & weight for this point
+                float fi = f->data.F32[i];
+                if (!isfinite(fi)) continue;
+
+                float wt = 1.0;
+                if (df != NULL) {
+                    if (df->data.F32[i] == 0.0) {
+                        wt = 0.0;
+                    } else {
+                        if (!isfinite(df->data.F32[i])) continue;
+                        wt = 1.0 / PS_SQR(df->data.F32[i]); // XXX test for dz == NULL or dz_i = 0
+                    }
+                }
+
+                // sum the appropriate elements for the different quadrants
+
+                int Qx = (dx >= 0) ? 1 : 0;
+                if (n ==      0) Qx = 1;
+                if (n == Nx - 1) Qx = 0;
+
+                int Qy = (dy >= 0) ? 1 : 0;
+                if (m ==      0) Qy = 1;
+                if (m == Ny - 1) Qy = 0;
+
+                assert (isfinite(fi));
+                assert (isfinite(wt));
+                assert (isfinite(rx));
+                assert (isfinite(ry));
+
+                // points at offset 1,1
+                if ((Qx == 1) && (Qy == 1)) {
+                    rx_rx_ry_ry += rx*rx*ry*ry*wt;
+                    rx_rx_dy_ry += rx*rx*dy*ry*wt;
+                    dx_rx_ry_ry += dx*rx*ry*ry*wt;
+                    dx_rx_dy_ry += dx*rx*dy*ry*wt;
+                    fi_rx_ry    += fi*rx*ry*wt;
+                }
+                // points at offset 1,0
+                if ((Qx == 1) && (Qy == 0)) {
+                    rx_rx_py_py += rx*rx*py*py*wt;
+                    rx_rx_qy_py += rx*rx*qy*py*wt;
+                    dx_rx_py_py += dx*rx*py*py*wt;
+                    dx_rx_qy_py += dx*rx*qy*py*wt;
+                    fi_rx_py    += fi*rx*py*wt;
+                }
+                // points at offset 0,1
+                if ((Qx == 0) && (Qy == 1)) {
+                    px_px_ry_ry += px*px*ry*ry*wt;
+                    px_px_dy_ry += px*px*dy*ry*wt;
+                    qx_px_ry_ry += qx*px*ry*ry*wt;
+                    qx_px_dy_ry += qx*px*dy*ry*wt;
+                    fi_px_ry    += fi*px*ry*wt;
+                }
+                // points at offset 0,0
+                if ((Qx == 0) && (Qy == 0)) {
+                    px_px_py_py += px*px*py*py*wt;
+                    px_px_qy_py += px*px*qy*py*wt;
+                    qx_px_py_py += qx*px*py*py*wt;
+                    qx_px_qy_py += qx*px*qy*py*wt;
+                    fi_px_py    += fi*px*py*wt;
+                }
+            }
+
+            // the chi-square derivatives have elements of the form g(n+jn,m+jm)*A(jn,jm),
+            // jn,jm = -1 to +1. Convert the sums above into the correct coefficients
+            sA[-1][-1] = qx_px_qy_py;
+            sA[-1][ 0] = qx_px_ry_ry + qx_px_py_py;
+            sA[-1][+1] = qx_px_dy_ry;
+            sA[ 0][-1] = rx_rx_qy_py + px_px_qy_py;
+            sA[ 0][ 0] = rx_rx_ry_ry + px_px_ry_ry + rx_rx_py_py + px_px_py_py;
+            sA[ 0][+1] = rx_rx_dy_ry + px_px_dy_ry;
+            sA[+1][-1] = dx_rx_qy_py;
+            sA[+1][ 0] = dx_rx_ry_ry + dx_rx_py_py;
+            sA[+1][+1] = dx_rx_dy_ry;
+
+            // I[ 0][ 0] = index for this n,m element:
+            int I = n + Nx * m;
+            //            B->data.F32[I] = fi_rx_ry + fi_rx_py + fi_px_ry + fi_px_py;
+            // CZW: call to psSparseVector Element
+            if (fi_rx_ry + fi_rx_py + fi_px_ry + fi_px_py == 0.0) {
+              psSparseVectorElement(Asparse, I, 1.0);
+            }
+            else {
+              psSparseVectorElement(Asparse, I, fi_rx_ry + fi_rx_py + fi_px_ry + fi_px_py);
+            }
+            
+            //      printf("ADDING: %d %g \n",I, fi_rx_ry + fi_rx_py + fi_px_ry + fi_px_py);
+            // insert these values into their corresponding locations in A, B
+            for (int jn = -1; jn <= +1; jn++) {
+                if (n + jn <   0) continue;
+                if (n + jn >= Nx) continue;
+                for (int jm = -1; jm <= +1; jm++) {
+                    if (m + jm <   0) continue;
+                    if (m + jm >= Ny) continue;
+                    int J = (n + jn) + Nx * (m + jm);
+                    //              printf("A: %d %d %g\n",J,I,sA[jn][jm]);
+                    // A->data.F32[J][I] = sA[jn][jm];
+                    // CZW: call to psSparseMatrixElement
+                    if (J < I) { continue; }
+                    psSparseMatrixElement(Asparse,J,I,sA[jn][jm]); // Ensure J < I?
+                    
+                }
+            }
+        }
+    }
+
+    // test for empty diagonal elements (unconstained cells), mark, and set pivots to 1.0
+    // CZW: I'm not totally sure how to check these in the sparse context.
+    // Iterate over all ii pairs, and manually check the structure?
+#if (0)
+    psVector *Empty = psVectorAlloc (Nx*Ny, PS_TYPE_S8);
+    psVectorInit (Empty, 0);
+    for (int i = 0; i < Nx*Ny; i++) {
+        if (A->data.F32[i][i] == 0.0) {
+            Empty->data.S8[i] = 1;
+            for (int j = 0; j < Nx*Ny; j++) {
+                A->data.F32[i][j] = 0.0;
+                A->data.F32[j][i] = 0.0;
+            }
+            A->data.F32[i][i] = 1.0;
+            B->data.F32[i] = 0.0;
+        }
+    }
+#endif 
+
+    // CZW: call to psSparseSolve
+    psVector *solution = psVectorAlloc(Nx*Ny, PS_TYPE_F32);
+    psSparseConstraint Constraint;
+    Constraint.paramDelta = 1e-3;
+    Constraint.paramMin   = -1e5;
+    Constraint.paramMax   = 1e5;
+    solution = psSparseSolve(solution, Constraint, Asparse, 1000);
+    if (!solution) {
+      psFree(solution);
+      psFree(Asparse);
+      return(false);
+    }
+
+#if (0)
+    // CZW: This is a continuation of the above information
+    // set bad values to NaN
+    for (int i = 0; i < Nx*Ny; i++) {
+        if (Empty->data.S8[i]) {
+            B->data.F32[i] = NAN;
+            A->data.F32[i][i] = 0;
+        }
+    }
+#endif
+
+    for (int n = 0; n < Nx; n++) {
+        for (int m = 0; m < Ny; m++) {
+            int I = n + Nx * m;
+            map->map->data.F32[m][n] = solution->data.F32[I];
+            map->error->data.F32[m][n] = NAN; // sqrt(A->data.F32[I][I]); // CZW: fix this to be a real error.
+        }
+    }
+
+    //    psFree (A);
+    //    psFree (B);
+    //    psFree (Empty);
+    // CZW: free things
+    psFree(solution);
+    psFree(Asparse);
+    
+    *pGoodFit = true;
+    return true;
+}
+
+// measure residuals on each pass and clip outliers based on stats
+bool psImageMapClipFitSparse(bool *pGoodFit, psImageMap *map, psStats *stats, psVector *inMask, psVectorMaskType maskValue,
+                             const psVector *x, const psVector *y, const psVector *f, const psVector *df)
+{
+    // XXX add in full PS_ASSERTS
+    psAssert(map, "impossible");
+    psAssert(stats, "impossible");
+    psAssert(x, "impossible");
+    psAssert(y, "impossible");
+    psAssert(f, "impossible");
+
+    *pGoodFit = false;
+
+    // the user supplies one of various stats option pairs,
+    // determine the desired mean and stdev STATS options:
+    psStatsOptions meanOption = psStatsMeanOption(stats->options);
+    psStatsOptions stdevOption = psStatsStdevOption(stats->options);
+    if (!psStatsSingleOption(meanOption)) {
+        psError(PS_ERR_UNKNOWN, true, "no valid mean stats option selected");
+        return false;
+    }
+    if (!psStatsSingleOption(stdevOption)) {
+        psError(PS_ERR_UNKNOWN, true, "no valid stdev stats option selected");
+        return false;
+    }
+
+    // clipping range defined by min and max and/or clipSigma
+    psF32 minClipSigma;
+    psF32 maxClipSigma;
+    if (isfinite(stats->max)) {
+        maxClipSigma = fabs(stats->max);
+    } else {
+        maxClipSigma = fabs(stats->clipSigma);
+    }
+    if (isfinite(stats->min)) {
+        minClipSigma = fabs(stats->min);
+    } else {
+        minClipSigma = fabs(stats->clipSigma);
+    }
+
+    psVector *mask = inMask;
+    if (!inMask) {
+        mask = psVectorAlloc (x->n, PS_TYPE_VECTOR_MASK);
+        psVectorInit (mask, 0);
+    }
+
+    // vector to store residuals
+    psVector *resid = psVectorAlloc(f->n, PS_TYPE_F32);
+
+    psTrace("psLib.imageops", 4, "stats->clipIter is %d\n", stats->clipIter);
+    psTrace("psLib.imageops", 4, "(minClipSigma, maxClipSigma) is (%.2f, %.2f)\n", minClipSigma, maxClipSigma);
+
+    for (psS32 N = 0; N < stats->clipIter; N++) {
+        psTrace("psLib.imageops", 6, "Loop iteration %d.  Calling psImageMapFit()\n", N);
+        psS32 Nkeep = 0;
+        if (!psImageMapFitSparse(pGoodFit, map, mask, maskValue, x, y, f, df)) {
             psError(PS_ERR_UNKNOWN, false, "Could not fit image map.\n");
             psFree(resid);