Changeset 42492

Timestamp:

Aug 15, 2023, 4:56:54 AM (3 years ago)

Author:

eugene

Message:

adding psPolynomial1DCopy, psPolynomial1DRecycle; working on the polynomial IRLS fitting

Location:

branches/eam_branches/ipp-20230313/psLib/src/math

Files:

• psMinimizePolyFit.c (modified) (4 diffs)
• psMinimizePolyFit.h (modified) (1 diff)
• psPolynomial.c (modified) (2 diffs)
• psPolynomial.h (modified) (1 diff)

branches/eam_branches/ipp-20230313/psLib/src/math/psMinimizePolyFit.c

@@ -48 +48 @@
 /* DEFINE STATEMENTS                                                         */
 /*****************************************************************************/
-#define CZW 0
+#define CZW 1
 
 # define USE_GAUSS_JORDAN 1
@@ -1030 +1030 @@
 }
 
+// This function assumes the input vectors (f, fEval, fErr) all have the same type
+// This requirement is already enforced in the calling function (psVectorIRLSFitPolynomial1D)
+psVector *psVector_GetModifiedErrors_Caucy (
+    const psVector *f,
+    const psVector *fEval,
+    const psVector *fErr,
+    const psVector *mask,
+    psVectorMaskType maskValue) {
+
+  psVector *mErr = psVectorAlloc (f->n, f->type.type);
+
+  switch (f->type.type) {
+    case PS_TYPE_F64:
+      {
+          psF64 *fPtr     = f->data.F64;
+          psF64 *fErrPtr  = fErr->data.F64;
+          psF64 *mErrPtr  = mErr->data.F64;
+          psF64 *fEvalPtr = fEval->data.F64;
+          for (int i = 0; i < f->n; i++, fPtr++, fEvalPtr++, fErrPtr++, mErrPtr++) {
+              if (mask && (mask->data.PS_TYPE_VECTOR_MASK_DATA[i] & maskValue)) continue;
+              double dF = (*fPtr - *fEvalPtr) / 2.385;
+              double dV = PS_SQR(*fErrPtr) + PS_SQR(dF);
+              *mErrPtr = sqrt(dV);
+          }
+      }
+      break;
+    case PS_TYPE_F32:
+      {
+          psF32 *fPtr     = f->data.F32;
+          psF32 *fErrPtr  = fErr->data.F32;
+          psF32 *mErrPtr  = mErr->data.F32;
+          psF32 *fEvalPtr = fEval->data.F32;
+          for (int i = 0; i < f->n; i++, fPtr++, fEvalPtr++, fErrPtr++, mErrPtr++) {
+              if (mask && (mask->data.PS_TYPE_VECTOR_MASK_DATA[i] & maskValue)) continue;
+              double dF = (*fPtr - *fEvalPtr) / 2.385;
+              double dV = PS_SQR(*fErrPtr) + PS_SQR(dF);
+              *mErrPtr = sqrt(dV);
+          }
+      }
+      break;
+    default:
+        psError(PS_ERR_UNKNOWN, false, "invalid input data type.\n");
+        return NULL;
+  }
+  return mErr;
+}
+
 // These should probably be tunable:
 # define FIT_TOLERANCE 1e-4
@@ -1036 +1083 @@
 
 // This function accepts F32 and F64 input vectors.
+  //
 bool psVectorIRLSFitPolynomial1D(
     psPolynomial1D *poly,
+    psStats *stats,
     const psVector *mask,
     psVectorMaskType maskValue,
@@ -1078 +1127 @@
     }
 
-    // IRLS fitting is performed by adjusting the total weights for each point based on
-    // the distance of the value from the last fit.  start with weights set to 1.0 (the
-    // max) and adjust as needed.
-
-    psVector *weights = psVectorAlloc(f->n, PS_TYPE_F64);
-    psVectorInit (weights, 1.0);
-
-    // initial fit with weights set to 1.0
+    // initial fit with nominal errors
     if (!psVectorFitPolynomial1D(poly, mask, maskValue, f, fErr, x)) {
         psError(PS_ERR_UNKNOWN, false, "Could not fit polynomial.  Returning false.\n");
         if (xIn == NULL) psFree(x);
-        psFree (weights);
         return false;
     }
 
-    psVector *fEval = psPolynomial1DEvalVector (poly, x);
-
-    for (psS32 N = 0; N < stats->clipIter; N++) {
+    // use polyOld to save the last fit
+    psPolynomial1D *polyOld = NULL;
+
+    // use clipIter as max number of iterations
+    bool converged = false;
+    for (psS32 N = 0; !converged && (N < stats->clipIter); N++) {
         psTrace("psLib.math", 6, "Loop iteration %d.  Calling psVectorFitPolynomial1D()\n", N);
-        psS32 Nkeep = 0;
-        if (psTraceGetLevel("psLib.math") >= 6) {
-            if (mask != NULL) {
-                for (psS32 i = 0 ; i < mask->n ; i++) {
-                    psTrace("psLib.math", 6,  "mask[%d] is %d\n", i, mask->data.PS_TYPE_VECTOR_MASK_DATA[i]);
-                }
-            }
-        }
-        if (!psVectorFitPolynomial1D(poly, mask, maskValue, f, fErr, x)) {
+
+        // evaluate the fit at the input positions
+        psVector *fEval = psPolynomial1DEvalVector (poly, x);
+
+        // calculate modified errors based on the deviation from the fit
+        psVector *modErr = psVector_GetModifiedErrors_Caucy (f, fEval, fErr, mask, maskValue);
+        psFree (fEval);
+
+        // save the last fit (recycle the structure once allocated)
+        polyOld = psPolynomial1DCopy (polyOld, poly);
+
+        // calculate a new fit with modified errors:
+        if (!psVectorFitPolynomial1D(poly, mask, maskValue, f, modErr, x)) {
             psError(PS_ERR_UNKNOWN, false, "Could not fit polynomial.  Returning false.\n");
-            if (xIn == NULL) {
-                psFree(x);
-            }
-            psFree(resid);
-            
+            if (xIn == NULL) psFree(x);
+            psFree(modErr);
             return false;
         }
 
-        psVector *fit = psPolynomial1DEvalVector(poly, x);
-        if (fit == NULL) {
-            psError(PS_ERR_UNKNOWN, false, "Could not call psPolynomial3DEvalVector().  Returning false.\n");
-            if (xIn == NULL) {
-                psFree(x);
-            }
-            psFree(resid);
-            return false;
-        }
-        for (psS32 i = 0 ; i < f->n ; i++) {
-            if (f->type.type == PS_TYPE_F64) {
-                resid->data.F64[i] = f->data.F64[i] - fit->data.F64[i];
-            } else {
-                resid->data.F64[i] = (psF64) (f->data.F32[i] - fit->data.F32[i]);
-            }
-        }
-        if (psTraceGetLevel("psLib.math") >= 6) {
-            if (mask != NULL) {
-                for (psS32 i = 0 ; i < mask->n ; i++) {
-                    if (!((mask != NULL) && (mask->data.PS_TYPE_VECTOR_MASK_DATA[i] & maskValue))) {
-                        psTrace("psLib.math", 6,  "(f, fit)[%d] is (%f, %f).  resid is (%f)\n",
-                                i, f->data.F32[i], fit->data.F32[i], resid->data.F64[i]);
-                    }
-                }
-            }
-        }
-
-        if (!psVectorStats(stats, resid, NULL, mask, maskValue)) {
-            psError(PS_ERR_UNKNOWN, false, "Could not compute statistics on the resid vector.  Returning false.\n");
-            psFree(resid);
-            psFree(fit);
-            return false;
-        }
-
-        double meanValue = psStatsGetValue (stats, meanOption);
-        double stdevValue = psStatsGetValue (stats, stdevOption);
-
-        psTrace("psLib.math", 5, "Mean is %f\n", meanValue);
-        psTrace("psLib.math", 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++) {
-            if ((mask != NULL) && (mask->data.PS_TYPE_VECTOR_MASK_DATA[i] & maskValue)) {
-                continue;
-            }
-
-            if ((resid->data.F64[i] - meanValue > maxClipValue) || (resid->data.F64[i] - meanValue < minClipValue)) {
-                if (f->type.type == PS_TYPE_F64) {
-                    psTrace("psLib.math", 6, "Masking element %d (%f).  resid->data.F64[%d] is %f\n",
-                            i, fit->data.F64[i], i, resid->data.F64[i]);
-                } else {
-                    psTrace("psLib.math", 6, "Masking element %d (%f).  resid->data.F64[%d] is %f\n",
-                            i, fit->data.F32[i], i, resid->data.F64[i]);
-                }
-
-                if (mask != NULL) {
-                    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 polynomial fit won't change.
-        //
-        psTrace("psLib.math", 6, "keeping %d of %ld pts for fit\n", Nkeep, x->n);
-        stats->clippedNvalues = Nkeep;
-        psFree(fit);
-    }
-
-    // Free psVectors that were created for NULL arguments.
-    if (xIn == NULL) {
-        psFree(x);
-    }
-    // Free other local temporary variables
-    psFree(resid);
+        // has the solution converged?
+        converged = true;
+        for (int ix = 0; ix <= poly->nX; ix++) {
+          if ((fabs(poly->coeff[ix] - polyOld->coeff[ix]) > FIT_TOLERANCE * fabs(poly->coeff[ix])) && 
+              (fabs(poly->coeff[ix] - polyOld->coeff[ix]) > FLT_TOLERANCE))
+            converged = false;
+        }
+
+        psFree (modErr);
+    }
+
+    // Free local temporary variables
+    if (xIn == NULL) psFree(x);
+    psFree(polyOld);
 
     psTrace("psLib.math", 3, "---- %s() end ----\n", __func__);

branches/eam_branches/ipp-20230313/psLib/src/math/psMinimizePolyFit.h

@@ -134 +134 @@
 );
 
+bool psVectorIRLSFitPolynomial1D(
+    psPolynomial1D *poly,
+    psStats *stats,
+    const psVector *mask,
+    psVectorMaskType maskValue,
+    const psVector *f,
+    const psVector *fErr,
+    const psVector *x
+);
+
 /// @}
 #endif // #ifndef PS_MINIMIZE_POLYFIT_H

branches/eam_branches/ipp-20230313/psLib/src/math/psPolynomial.c

@@ -806 +806 @@
 }
 
-// XXX add 1D, 3D, 4D versions
-bool psPolynomial2DRecycle(psPolynomial2D *poly,
-                           psPolynomialType type,
-                           unsigned int nX,
-                           unsigned int nY)
-{
-    PS_ASSERT_INT_NONNEGATIVE(nX, NULL);
-    PS_ASSERT_INT_NONNEGATIVE(nY, NULL);
-
-    bool match = true;
-    match &= (poly->type == type);
-    match &= (poly->nX == type);
-    match &= (poly->nY == type);
-
-    if (!match) {
-        for (int i = 0; i < poly->nX + 1; i++) {
-            psFree (poly->coeff[i]);
-            psFree (poly->coeffErr[i]);
-            psFree (poly->coeffMask[i]);
-        }
-        psFree (poly->coeff);
-        psFree (poly->coeffErr);
-        psFree (poly->coeffMask);
-
-        poly->type = type;
-        poly->nX = nX;
-        poly->nY = nY;
-
-        poly->coeff = psAlloc((1 + nX) * sizeof(psF64 *));
-        poly->coeffErr = psAlloc((1 + nX) * sizeof(psF64 *));
-        poly->coeffMask = (psMaskType **)psAlloc((1 + nX) * sizeof(psMaskType *));
-        for (int i = 0; i < (1 + nX); i++) {
-            poly->coeff[i] = psAlloc((1 + nY) * sizeof(psF64));
-            poly->coeffErr[i] = psAlloc((1 + nY) * sizeof(psF64));
-            poly->coeffMask[i] = (psMaskType *)psAlloc((1 + nY) * sizeof(psMaskType));
-        }
-    }
-    for (int i = 0; i < (1 + nX); i++) {
-        for (int j = 0; j < (1 + nY); j++) {
-            poly->coeff[i][j] = 0.0;
-            poly->coeffErr[i][j] = 0.0;
-            poly->coeffMask[i][j] = PS_POLY_MASK_NONE;
-        }
-    }
-    return(true);
-}
-
-// XXX add 1D, 3D, 4D versions
-psPolynomial2D *psPolynomial2DCopy(psPolynomial2D *out,
-                                   psPolynomial2D *poly)
-{
-    if (out == NULL) {
-        out = psPolynomial2DAlloc (poly->type, poly->nX, poly->nY);
-    } else {
-        psPolynomial2DRecycle (out, poly->type, poly->nX, poly->nY);
-    }
-
-    for (int i = 0; i < (1 + poly->nX); i++) {
-        for (int j = 0; j < (1 + poly->nY); j++) {
-            out->coeff[i][j] = poly->coeff[i][j];
-            out->coeffErr[i][j] = poly->coeffErr[i][j];
-            out->coeffMask[i][j] = poly->coeffMask[i][j];
-        }
-    }
-    return(out);
-}
-
 psPolynomial3D* psPolynomial3DAlloc(
     psPolynomialType type,
@@ -1002 +935 @@
 }
 
+// XXX add 3D, 4D versions
+bool psPolynomial1DRecycle(psPolynomial1D *poly,
+                           psPolynomialType type,
+                           unsigned int nX)
+{
+    PS_ASSERT_INT_NONNEGATIVE(nX, NULL);
+
+    bool match = true;
+    match &= (poly->type == type);
+    match &= (poly->nX == type);
+
+    if (!match) {
+        psFree (poly->coeff);
+        psFree (poly->coeffErr);
+        psFree (poly->coeffMask);
+
+        poly->type = type;
+        poly->nX = nX;
+
+        poly->coeff = psAlloc((1 + nX) * sizeof(psF64));
+        poly->coeffErr = psAlloc((1 + nX) * sizeof(psF64));
+        poly->coeffMask = (psMaskType *)psAlloc((1 + nX) * sizeof(psMaskType));
+    }
+    for (int i = 0; i < (1 + nX); i++) {
+        poly->coeff[i] = 0.0;
+        poly->coeffErr[i] = 0.0;
+        poly->coeffMask[i] = PS_POLY_MASK_NONE;
+    }
+    return(true);
+}
+
+bool psPolynomial2DRecycle(psPolynomial2D *poly,
+                           psPolynomialType type,
+                           unsigned int nX,
+                           unsigned int nY)
+{
+    PS_ASSERT_INT_NONNEGATIVE(nX, NULL);
+    PS_ASSERT_INT_NONNEGATIVE(nY, NULL);
+
+    bool match = true;
+    match &= (poly->type == type);
+    match &= (poly->nX == type);
+    match &= (poly->nY == type);
+
+    if (!match) {
+        for (int i = 0; i < poly->nX + 1; i++) {
+            psFree (poly->coeff[i]);
+            psFree (poly->coeffErr[i]);
+            psFree (poly->coeffMask[i]);
+        }
+        psFree (poly->coeff);
+        psFree (poly->coeffErr);
+        psFree (poly->coeffMask);
+
+        poly->type = type;
+        poly->nX = nX;
+        poly->nY = nY;
+
+        poly->coeff = psAlloc((1 + nX) * sizeof(psF64 *));
+        poly->coeffErr = psAlloc((1 + nX) * sizeof(psF64 *));
+        poly->coeffMask = (psMaskType **)psAlloc((1 + nX) * sizeof(psMaskType *));
+        for (int i = 0; i < (1 + nX); i++) {
+            poly->coeff[i] = psAlloc((1 + nY) * sizeof(psF64));
+            poly->coeffErr[i] = psAlloc((1 + nY) * sizeof(psF64));
+            poly->coeffMask[i] = (psMaskType *)psAlloc((1 + nY) * sizeof(psMaskType));
+        }
+    }
+    for (int i = 0; i < (1 + nX); i++) {
+        for (int j = 0; j < (1 + nY); j++) {
+            poly->coeff[i][j] = 0.0;
+            poly->coeffErr[i][j] = 0.0;
+            poly->coeffMask[i][j] = PS_POLY_MASK_NONE;
+        }
+    }
+    return(true);
+}
+
+// XXX 3D, 4D versions
+psPolynomial1D *psPolynomial1DCopy(psPolynomial1D *out,
+                                   psPolynomial1D *poly)
+{
+    if (out == NULL) {
+        out = psPolynomial1DAlloc (poly->type, poly->nX);
+    } else {
+        psPolynomial1DRecycle (out, poly->type, poly->nX);
+    }
+
+    for (int i = 0; i < (1 + poly->nX); i++) {
+        out->coeff[i] = poly->coeff[i];
+        out->coeffErr[i] = poly->coeffErr[i];
+        out->coeffMask[i] = poly->coeffMask[i];
+    }
+    return(out);
+}
+psPolynomial2D *psPolynomial2DCopy(psPolynomial2D *out,
+                                   psPolynomial2D *poly)
+{
+    if (out == NULL) {
+        out = psPolynomial2DAlloc (poly->type, poly->nX, poly->nY);
+    } else {
+        psPolynomial2DRecycle (out, poly->type, poly->nX, poly->nY);
+    }
+
+    for (int i = 0; i < (1 + poly->nX); i++) {
+        for (int j = 0; j < (1 + poly->nY); j++) {
+            out->coeff[i][j] = poly->coeff[i][j];
+            out->coeffErr[i][j] = poly->coeffErr[i][j];
+            out->coeffMask[i][j] = poly->coeffMask[i][j];
+        }
+    }
+    return(out);
+}
+
 /* note these functions accept unscaled values and apply the scaling saved on poly */
 psF64 psPolynomial1DEval(const psPolynomial1D* poly,

branches/eam_branches/ipp-20230313/psLib/src/math/psPolynomial.h

@@ -161 +161 @@
 ) PS_ATTR_MALLOC;
 
+bool psPolynomial1DRecycle(psPolynomial1D *poly,
+                           psPolynomialType type,
+                           unsigned int nX);
+
 bool psPolynomial2DRecycle(psPolynomial2D *poly,
                            psPolynomialType type,
                            unsigned int nX,
                            unsigned int nY);
+
+psPolynomial1D *psPolynomial1DCopy(psPolynomial1D *out,
+                                   psPolynomial1D *poly);
 
 psPolynomial2D *psPolynomial2DCopy(psPolynomial2D *out,