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

Timestamp:

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

Author:

eugene

Message:

adding psPolynomial1DCopy, psPolynomial1DRecycle; working on the polynomial IRLS fitting

File:

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

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__);