Changeset 42491

Timestamp:

Aug 13, 2023, 2:05:42 PM (3 years ago)

Author:

eugene

Message:

adding IRLS fitting to 1D vector fits

File:

• branches/eam_branches/ipp-20230313/psLib/src/math/psMinimizePolyFit.c (modified) (1 diff)

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

@@ -1030 +1030 @@
 }
 
+// These should probably be tunable:
+# define FIT_TOLERANCE 1e-4
+# define FLT_TOLERANCE 1e-6
+# define WEIGHT_THRESHOLD 0.3
+
+// This function accepts F32 and F64 input vectors.
+bool psVectorIRLSFitPolynomial1D(
+    psPolynomial1D *poly,
+    const psVector *mask,
+    psVectorMaskType maskValue,
+    const psVector *f,
+    const psVector *fErr,
+    const psVector *xIn)
+{
+    psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__);
+    PS_ASSERT_POLY_NON_NULL(poly, false);
+    PS_ASSERT_VECTOR_NON_NULL(f, false);
+    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false);
+    PS_ASSERT_VECTOR_NON_NULL(mask, false);
+    PS_ASSERT_VECTORS_SIZE_EQUAL(mask, f, false);
+    PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false);
+
+    if (fErr != NULL) {
+        PS_ASSERT_VECTORS_SIZE_EQUAL(fErr, f, false);
+        PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, false);
+    }
+
+    // Internal pointers for possibly NULL vectors.
+    psVector *x = NULL;
+    if (xIn != NULL) {
+        PS_ASSERT_VECTORS_SIZE_EQUAL(xIn, f, false);
+        PS_ASSERT_VECTOR_TYPE(xIn, f->type.type, false);
+        x = (psVector *) xIn;
+    } else {
+        if (poly->type == PS_POLYNOMIAL_ORD) {
+            x = psVectorCreate(NULL, 0, f->n, 1, f->type.type);
+        } else if (poly->type == PS_POLYNOMIAL_CHEB) {
+            if (f->type.type == PS_TYPE_F32) {
+                PS_VECTOR_GEN_CHEBY_INDEX(x, f->n, PS_TYPE_F32);
+            } else if (f->type.type == PS_TYPE_F64) {
+                PS_VECTOR_GEN_CHEBY_INDEX(x, f->n, PS_TYPE_F64);
+            }
+        } else {
+            psError(PS_ERR_UNKNOWN, true, "Error, bad poly type.\n");
+            return false;
+        }
+    }
+
+    // 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
+    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++) {
+        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)) {
+            psError(PS_ERR_UNKNOWN, false, "Could not fit polynomial.  Returning false.\n");
+            if (xIn == NULL) {
+                psFree(x);
+            }
+            psFree(resid);
+            
+            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);
+
+    psTrace("psLib.math", 3, "---- %s() end ----\n", __func__);
+    return true;
+}
 
 /******************************************************************************