Index: /branches/eam_branches/ipp-20230313/psLib/src/math/psMinimizePolyFit.c =================================================================== --- /branches/eam_branches/ipp-20230313/psLib/src/math/psMinimizePolyFit.c (revision 42491) +++ /branches/eam_branches/ipp-20230313/psLib/src/math/psMinimizePolyFit.c (revision 42492) @@ -48,5 +48,5 @@ /* DEFINE STATEMENTS */ /*****************************************************************************/ -#define CZW 0 +#define CZW 1 # define USE_GAUSS_JORDAN 1 @@ -1030,4 +1030,51 @@ } +// 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,6 +1083,8 @@ // This function accepts F32 and F64 input vectors. + // bool psVectorIRLSFitPolynomial1D( psPolynomial1D *poly, + psStats *stats, const psVector *mask, psVectorMaskType maskValue, @@ -1078,123 +1127,51 @@ } - // 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__); Index: /branches/eam_branches/ipp-20230313/psLib/src/math/psMinimizePolyFit.h =================================================================== --- /branches/eam_branches/ipp-20230313/psLib/src/math/psMinimizePolyFit.h (revision 42491) +++ /branches/eam_branches/ipp-20230313/psLib/src/math/psMinimizePolyFit.h (revision 42492) @@ -134,4 +134,14 @@ ); +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 Index: /branches/eam_branches/ipp-20230313/psLib/src/math/psPolynomial.c =================================================================== --- /branches/eam_branches/ipp-20230313/psLib/src/math/psPolynomial.c (revision 42491) +++ /branches/eam_branches/ipp-20230313/psLib/src/math/psPolynomial.c (revision 42492) @@ -806,71 +806,4 @@ } -// 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,4 +935,117 @@ } +// 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, Index: /branches/eam_branches/ipp-20230313/psLib/src/math/psPolynomial.h =================================================================== --- /branches/eam_branches/ipp-20230313/psLib/src/math/psPolynomial.h (revision 42491) +++ /branches/eam_branches/ipp-20230313/psLib/src/math/psPolynomial.h (revision 42492) @@ -161,8 +161,15 @@ ) 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,