Index: /branches/eam_branches/ipp-20230313/psLib/src/math/psMinimizePolyFit.c =================================================================== --- /branches/eam_branches/ipp-20230313/psLib/src/math/psMinimizePolyFit.c (revision 42505) +++ /branches/eam_branches/ipp-20230313/psLib/src/math/psMinimizePolyFit.c (revision 42506) @@ -69,4 +69,11 @@ if ((ORIG != NULL) && (ORIG->type.type != PS_TYPE_F64)) { psFree(TEMP); } +psVector *psVector_GetModifiedErrors_Caucy +( const psVector *f, + const psVector *fEval, + const psVector *fErr, + const psVector *mask, + psVectorMaskType maskValue); + /*****************************************************************************/ /* TYPE DEFINITIONS */ @@ -90,8 +97,8 @@ returned as a psVector sums. *****************************************************************************/ -static psVector *BuildSums1D( - psVector* sums, - psF64 x, - psS32 nTerm) +static psVector *BuildSums1D +( psVector* sums, + psF64 x, + psS32 nTerm) { psS32 nSum = 0; @@ -1031,4 +1038,1845 @@ } +// 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, + psStats *stats, + 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->type == PS_POLYNOMIAL_ORD, false); // XXX for now, only allow ORD + PS_ASSERT_POLY_NON_NULL(poly, false); + PS_ASSERT_VECTOR_NON_NULL(f, false); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false); + if (mask != NULL) { + 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); + } + if (xIn != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(xIn, f, false); + PS_ASSERT_VECTOR_TYPE(xIn, f->type.type, false); + } + + // Internal pointers for possibly NULL vectors. + psVector *x = (xIn != NULL) ? psMemIncrRefCounter((psVector *) xIn) : psVectorCreate(NULL, 0, f->n, 1, f->type.type); + + // 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"); + return false; + } + + // 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); + + // 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"); + psFree(x); + psFree(modErr); + return false; + } + + // 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; + } + +# if (0) + // XXX test: + FILE *ftest = fopen ("irls.wt.dat", "w"); + for (int i = 0; i < modErr->n; i++) { + if (modErr->type.type == PS_TYPE_F64) { + fprintf (ftest, "%d %f\n", i, modErr->data.F64[i]); + } else { + fprintf (ftest, "%d %f\n", i, modErr->data.F32[i]); + } + } + fclose (ftest); +# endif + psFree (modErr); + } + + // Free local temporary variables + psFree(x); + psFree(polyOld); + + psTrace("psLib.math", 3, "---- %s() end ----\n", __func__); + return true; +} + +/****************************************************************************** + ****************************************************************************** + 2-D Vector Code. + ****************************************************************************** + *****************************************************************************/ + +/****************************************************************************** +VectorFitPolynomial2DOrd(myPoly, *mask, maskValue, *f, *fErr, *x, *y): This is +a private routine which will fit a 2-D polynomial to a set of (x, y)-(f) +pairs. All non-NULL vectors must be of type PS_TYPE_F64. + + *****************************************************************************/ +static bool VectorFitPolynomial2DOrd( + psPolynomial2D* myPoly, + const psVector* mask, + psVectorMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y) +{ + psTrace("psLib.math", 4, "---- %s() begin ----\n", __func__); + PS_ASSERT_POLY_NON_NULL(myPoly, false); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, false); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nY, false); + PS_ASSERT_VECTOR_NON_NULL(f, false); + PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, false); + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(y, fErr, false); + PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F64, false); + } + PS_ASSERT_VECTOR_NON_NULL(x, false); + PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); + PS_ASSERT_VECTOR_NON_NULL(y, false); + PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); + if (mask != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(y, mask, false); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); + } + + // Number of polynomial terms + int nXterm = 1 + myPoly->nX; // Number of terms in x + int nYterm = 1 + myPoly->nY; // Number of terms in y + int nTerm = nXterm * nYterm; // Total number of terms + + psImage *A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); // Least-squares matrix + psVector *B = psVectorAlloc(nTerm, PS_TYPE_F64); // Least-squares vector + + // Initialize data structures. + if (!psImageInit(A, 0.0) || !psVectorInit(B, 0.0)) { + psError(PS_ERR_UNKNOWN, false, "Could initialize data structures A, B. Returning NULL.\n"); + psFree(A); + psFree(B); + psTrace("psLib.math", 6, "---- %s() End ----\n", __func__); + return false; + } + + // Dereference stuff, to make the loop go faster + psF64 **matrix = A->data.F64; // Dereference the least-squares matrix + psF64 *vector = B->data.F64; // Dereference the least-squares vector + psMaskType **coeffMask = myPoly->coeffMask; // Dereference mask for polynomial terms + psVectorMaskType *dataMask = NULL; // Dereference mask for data + if (mask) { + dataMask = mask->data.PS_TYPE_VECTOR_MASK_DATA; + } + psF64 *xData = x->data.F64; // Dereference x + psF64 *yData = y->data.F64; // Dereference y + psF64 *fData = f->data.F64; // Dereference f + psF64 *fErrData = NULL; // Dereference fErr + if (fErr) { + fErrData = fErr->data.F64; + } + + // Build the least-squares matrix and vector + psImage *xySums = NULL; // The sums: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1) + for (int k = 0; k < x->n; k++) { + if (dataMask && dataMask[k] & maskValue) { + continue; + } + xySums = BuildSums2D(xySums, xData[k], yData[k], nXterm, nYterm); + psF64 **sums = xySums->data.F64;// Dereference sums + + double wt; // Weight + if (!fErrData) { + wt = 1.0; + } else { + // this filters fErr == 0 values + wt = (fErrData[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErrData[k]); + } + + // Iterating over the matrix + for (int i = 0; i < nTerm; i++) { + int l = i / nYterm; // x index + int m = i % nYterm; // y index + if (coeffMask[l][m] & PS_POLY_MASK_SET) { + matrix[i][i] = 1.0; + continue; + } + vector[i] += fData[k] * sums[l][m] * wt; + matrix[i][i] += sums[2*l][2*m] * wt; // The diagonal entry + for (int j = i + 1; j < nTerm; j++) { // Doing the upper diagonal only: we will use symmetry + int p = j / nYterm; // x index + int q = j % nYterm; // y index + if (coeffMask[p][q] & PS_POLY_MASK_SET) { + continue; + } + double value = sums[l+p][m+q] * wt; // Value to add in + matrix[i][j] += value; + matrix[j][i] += value; // Taking advantage of the symmetry + } + } + } + psFree(xySums); + + // elements which are masked for fitting need to be subtracted from the vector + for (int i = 0; i < nTerm; i++) { + int ix = i / nYterm; // x index + int iy = i % nYterm; // y index + if (coeffMask[ix][iy] & PS_POLY_MASK_BOTH) { + continue; + } + for (int j = 0; j < nTerm; j++) { // The upper diagonal only: we will use symmetry + int jx = j / nYterm; // x index + int jy = j % nYterm; // y index + if (coeffMask[jx][jy] & PS_POLY_MASK_SET) { + continue; + } + if (!(coeffMask[jx][jy] & PS_POLY_MASK_FIT)) { + continue; + } + vector[i] -= matrix[i][j]*myPoly->coeff[jx][jy]; + } + } + + // set the un-fitted and un-set elements to 0 or 1 for pivots + for (int i = 0; i < nTerm; i++) { + int ix = i / nYterm; // x index + int iy = i % nYterm; // y index + if (coeffMask[ix][iy] & PS_POLY_MASK_BOTH) { + for (int j = 0; j < nTerm; j++) { // The upper diagonal only: we will use symmetry + matrix[i][j] = 0.0; + matrix[j][i] = 0.0; + } + matrix[i][i] = 1.0; + continue; + } + } + + if (psTraceGetLevel("psLib.math") >= 4) { + printf("Least-squares vector:\n"); + for (int i = 0; i < nTerm; i++) { + printf("%f ", B->data.F64[i]); + } + printf("\n"); + printf("Least-squares matrix:\n"); + for (int i = 0; i < nTerm; i++) { + for (int j = 0; j < nTerm; j++) { + printf("%f ", A->data.F64[i][j]); + } + printf("\n"); + } + } + + bool status = false; + if (USE_GAUSS_JORDAN) { + status = psMatrixGJSolve(A, B); + } else { + status = psMatrixLUSolve(A, B); + } + if (!status) { + psError(PS_ERR_UNKNOWN, false, "Could not solve linear equations.\n"); + goto escape; + } + + // select the appropriate solution entries (retain the incoming values if masked on the fit) + for (int i = 0; i < nTerm; i++) { + int ix = i / nYterm; // x index + int iy = i % nYterm; // y index + if (coeffMask[ix][iy] & PS_POLY_MASK_FIT) continue; + myPoly->coeff[ix][iy] = B->data.F64[i]; + myPoly->coeffErr[ix][iy] = sqrt(A->data.F64[i][i]); + } + psFree(A); + psFree(B); + return true; + +escape: + psFree (A); + psFree (B); + return false; +} + +/****************************************************************************** +VectorFitPolynomial2DCheb(myPoly, *mask, maskValue, *f, *fErr, *x, *y): This is +a private routine which will fit a 2-D polynomial to a set of (x, y)-(f) +pairs. All non-NULL vectors must be of type PS_TYPE_F64. + + *****************************************************************************/ +static bool VectorFitPolynomial2DCheb( + psPolynomial2D* myPoly, + const psVector *f, + const psVector *x, + const psVector *y) +{ + psTrace("psLib.math", 4, "---- %s() begin ----\n", __func__); + PS_ASSERT_POLY_NON_NULL(myPoly, false); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, false); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nY, false); + PS_ASSERT_VECTOR_NON_NULL(f, false); + PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, false); + PS_ASSERT_VECTOR_NON_NULL(x, false); + PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); + PS_ASSERT_VECTOR_NON_NULL(y, false); + PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); + + // Number of polynomial terms + int nXterm = 1 + myPoly->nX; // Number of terms in x + int nYterm = 1 + myPoly->nY; // Number of terms in y + int nTerm = nXterm * nYterm; // Total number of terms + if (nXterm > 9) { + psError(PS_ERR_UNKNOWN, false, "failed 2D chebyshev fit: orders higher than 9 are not yet coded\n"); + return false; + } + if (nYterm > 9) { + psError(PS_ERR_UNKNOWN, false, "failed 2D chebyshev fit: orders higher than 9 are not yet coded\n"); + return false; + } + + // determine scale factors + if (!psChebyshevSetScale (myPoly, x, 0)) { psError(PS_ERR_UNKNOWN, false, "failed 2D chebyshev fit.\n"); return false; } + if (!psChebyshevSetScale (myPoly, y, 1)) { psError(PS_ERR_UNKNOWN, false, "failed 2D chebyshev fit.\n"); return false; } + + // generate normalized vectors + psVector *xNorm = psChebyshevNormVector (myPoly, x, 0); + psVector *yNorm = psChebyshevNormVector (myPoly, y, 1); + + // generate the N cheb polynomials based on xNorm, yNorm + psArray *xPolySet = psArrayAlloc (nXterm); + for (int i = 0; i < nXterm; i++) { + xPolySet->data[i] = psChebyshevPolyVector (xNorm, i); + } + psArray *yPolySet = psArrayAlloc (nYterm); + for (int i = 0; i < nYterm; i++) { + yPolySet->data[i] = psChebyshevPolyVector (yNorm, i); + } + + psF64 *fData = f->data.F64; // Dereference f + + psImage *A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); // Least-squares matrix + psVector *B = psVectorAlloc(nTerm, PS_TYPE_F64); // Least-squares vector + + // Initialize data structures (should not be able to fail) + psAssert (psImageInit(A, 0.0), "Could initialize data structures A"); + psAssert (psVectorInit(B, 0.0), "Could initialize data structures B"); + + // Dereference stuff, to make the loop go faster + psF64 **matrix = A->data.F64; // Dereference the least-squares matrix + psF64 *vector = B->data.F64; // Dereference the least-squares vector + + // loop over all elements of the data vector + for (int k = 0; k < x->n; k++) { + + if (!finite(fData[k])) continue; + + // XXX can we only calculate the upper diagonal? + int nelem = 0; + for (int jx = 0; jx < nXterm; jx++) { + psVector *jxCheb = xPolySet->data[jx]; + for (int jy = 0; jy < nYterm; jy++) { + psVector *jyCheb = yPolySet->data[jy]; + psF64 chebValue = jxCheb->data.F64[k] * jyCheb->data.F64[k]; + + vector[nelem] += fData[k] * chebValue; + + int melem = 0; + for (int kx = 0; kx < nXterm; kx++) { + psVector *kxCheb = xPolySet->data[kx]; + for (int ky = 0; ky < nYterm; ky++) { + psVector *kyCheb = yPolySet->data[ky]; + matrix[nelem][melem] += chebValue * kxCheb->data.F64[k]*kyCheb->data.F64[k]; + melem++; + } + } + nelem++; + } + } + } + + if (psTraceGetLevel("psLib.math") >= 4) { + printf("Least-squares vector:\n"); + for (int i = 0; i < nTerm; i++) { + printf("%f ", B->data.F64[i]); + } + printf("\n"); + printf("Least-squares matrix:\n"); + for (int i = 0; i < nTerm; i++) { + for (int j = 0; j < nTerm; j++) { + printf("%f ", A->data.F64[i][j]); + } + printf("\n"); + } + } + + bool status = false; + if (USE_GAUSS_JORDAN) { + status = psMatrixGJSolve(A, B); + } else { + status = psMatrixLUSolve(A, B); + } + if (!status) { + psError(PS_ERR_UNKNOWN, false, "Could not solve linear equations.\n"); + goto escape; + } + + // unroll the result: + int nelem = 0; + for (int jx = 0; jx < nXterm; jx++) { + for (int jy = 0; jy < nYterm; jy++) { + myPoly->coeff[jx][jy] = B->data.F64[nelem]; + myPoly->coeffErr[jx][jy] = sqrt(A->data.F64[nelem][nelem]); + nelem ++; + } + } + psFree(A); + psFree(B); + + psFree (xNorm); + psFree (yNorm); + psFree (xPolySet); + psFree (yPolySet); + + return true; + +escape: + psFree (A); + psFree (B); + return false; +} + +/****************************************************************************** +psVectorFitPolynomial2D(): This routine fits a 2D polynomial of arbitrary +degree (specified in poly) to the data points (x, y)-(f) and returns that +polynomial. Types F32 and F64 are supported, however, type F32 is done via +vector conversion only. + *****************************************************************************/ +bool psVectorFitPolynomial2D( + psPolynomial2D *poly, + const psVector *mask, + psVectorMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y) +{ + PS_ASSERT_POLY_NON_NULL(poly, false); + // PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, false); + + PS_ASSERT_VECTOR_NON_NULL(f, false); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false); + PS_ASSERT_VECTOR_NON_NULL(x, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); + PS_ASSERT_VECTOR_NON_NULL(y, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); + if (mask != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, false); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); + } + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, false); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, false); + } + + // Convert input vectors to F64 if necessary. + psVector *f64 = (f->type.type == PS_TYPE_F64) ? (psVector *) f : psVectorCopy(NULL, f, PS_TYPE_F64); + psVector *x64 = (x->type.type == PS_TYPE_F64) ? (psVector *) x : psVectorCopy(NULL, x, PS_TYPE_F64); + psVector *y64 = (y->type.type == PS_TYPE_F64) ? (psVector *) y : psVectorCopy(NULL, y, PS_TYPE_F64); + + psVector *fErr64 = NULL; + if (fErr != NULL) { + fErr64 = (fErr->type.type == PS_TYPE_F64) ? (psVector *) fErr : psVectorCopy(NULL, fErr, PS_TYPE_F64); + } + + bool result = true; + + switch (poly->type) { + case PS_POLYNOMIAL_ORD: + result = VectorFitPolynomial2DOrd(poly, mask, maskValue, f64, fErr64, x64, y64); + if (!result) { + psError(PS_ERR_UNKNOWN, true, "Could not fit polynomial. Returning NULL.\n"); + } + break; + case PS_POLYNOMIAL_CHEB: + if (mask != NULL) { + psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring mask and maskValue with Chebyshev polynomials.\n"); + } + if (fErr != NULL) { + psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring error values for Chebyshev polynomials.\n"); + } + result = VectorFitPolynomial2DCheb(poly, f64, x64, y64); + if (!result) { + psError(PS_ERR_UNKNOWN, true, "Could not fit polynomial. Returning NULL.\n"); + } + break; + default: + psError(PS_ERR_UNKNOWN, true, "Incorrect polynomial type. Returning NULL.\n"); + result = false; + break; + } + + // Free psVectors that were created for NULL arguments. + PS_FREE_TEMP_F64_VECTOR (f, f64); + PS_FREE_TEMP_F64_VECTOR (x, x64); + PS_FREE_TEMP_F64_VECTOR (y, y64); + PS_FREE_TEMP_F64_VECTOR (fErr, fErr64); + + return result; +} + +bool psVectorClipFitPolynomial2D( + psPolynomial2D *poly, + psStats *stats, + const psVector *mask, + psVectorMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y) +{ + psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__); + PS_ASSERT_POLY_NON_NULL(poly, false); + PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, false); + PS_ASSERT_PTR_NON_NULL(stats, false); + PS_ASSERT_VECTOR_NON_NULL(mask, false); + PS_ASSERT_VECTOR_NON_NULL(f, false); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false); + + PS_ASSERT_VECTOR_NON_NULL(x, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); + PS_ASSERT_VECTOR_TYPE(x, f->type.type, false); + + PS_ASSERT_VECTOR_NON_NULL(y, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); + PS_ASSERT_VECTOR_TYPE(y, f->type.type, false); + + PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, false); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); + + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, false); + PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, false); + } + + // the user supplies one of various stats option pairs, + // determine the desired mean and stdev STATS options: + // XXX enforce consistency? + // XXX psStatsGetValue() probably has inverted precedence + psStatsOptions meanOption = stats->options & (PS_STAT_SAMPLE_MEAN | PS_STAT_SAMPLE_MEDIAN | PS_STAT_ROBUST_MEDIAN | PS_STAT_CLIPPED_MEAN | PS_STAT_FITTED_MEAN | PS_STAT_FITTED_MEAN); + psStatsOptions stdevOption = stats->options & (PS_STAT_SAMPLE_STDEV | PS_STAT_ROBUST_STDEV | PS_STAT_CLIPPED_STDEV | PS_STAT_FITTED_STDEV | PS_STAT_FITTED_STDEV); + if (!meanOption) { + psError(PS_ERR_UNKNOWN, true, "no valid mean stats option selected"); + return false; + } + if (!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 *resid = psVectorAlloc(f->n, PS_TYPE_F64); + + psTrace("psLib.math", 4, "stats->clipIter is %d\n", stats->clipIter); + psTrace("psLib.math", 4, "(minClipSigma, maxClipSigma) is (%.2f, %.2f)\n", minClipSigma, maxClipSigma); + + 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") >= 7) { + if (mask != NULL) { + for (psS32 i = 0 ; i < mask->n ; i++) { + psTrace("psLib.math", 7, "mask[%d] is %d\n", i, mask->data.PS_TYPE_VECTOR_MASK_DATA[i]); + } + } + } + + if (!psVectorFitPolynomial2D(poly, mask, maskValue, f, fErr, x, y)) { + psError(PS_ERR_UNKNOWN, false, "Could not fit a polynomial to the data. Returning false.\n"); + psFree(resid); + return false; + } + + psVector *fit = psPolynomial2DEvalVector(poly, x, y); + if (fit == NULL) { + psError(PS_ERR_UNKNOWN, false, "Could not call psPolynomial3DEvalVector(). Returning NULL.\n"); + 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") >= 7) { + 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", 7, "point %d at %f %f : value, fit : %f %f resid: %f\n", + i, x->data.F32[i], y->data.F32[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 NULL.\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 (fit->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++; + } + psTrace("psLib.math", 4, "keeping %d of %ld pts for fit\n", Nkeep, x->n); + stats->clippedNvalues = Nkeep; + psFree(fit); + } + // Free local temporary variables + psFree(resid); + + psTrace("psLib.math", 3, "---- %s() end ----\n", __func__); + return true; +} + +// This function accepts F32 and F64 input vectors. +bool psVectorIRLSFitPolynomial2D( + psPolynomial2D *poly, + psStats *stats, + const psVector *mask, + psVectorMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *xIn, + const psVector *yIn) +{ + psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__); + + PS_ASSERT (poly->type == PS_POLYNOMIAL_ORD, false); // XXX for now, only allow ORD + PS_ASSERT_POLY_NON_NULL(poly, false); + PS_ASSERT_VECTOR_NON_NULL(f, false); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false); + if (mask != NULL) { + 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); + } + if (xIn != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(xIn, f, false); + PS_ASSERT_VECTOR_TYPE(xIn, f->type.type, false); + } + if (yIn != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(yIn, f, false); + PS_ASSERT_VECTOR_TYPE(yIn, f->type.type, false); + } + + // Internal pointers for possibly NULL vectors. + psVector *x = (xIn != NULL) ? psMemIncrRefCounter((psVector *) xIn) : psVectorCreate(NULL, 0, f->n, 1, f->type.type); + psVector *y = (yIn != NULL) ? psMemIncrRefCounter((psVector *) yIn) : psVectorCreate(NULL, 0, f->n, 1, f->type.type); + + // initial fit with nominal errors + if (!psVectorFitPolynomial2D(poly, mask, maskValue, f, fErr, x, y)) { + psError(PS_ERR_UNKNOWN, false, "Could not fit polynomial. Returning false.\n"); + psFree(x); + psFree(y); + return false; + } + + // use polyOld to save the last fit + psPolynomial2D *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 psVectorFitPolynomial2D()\n", N); + + // evaluate the fit at the input positions + psVector *fEval = psPolynomial2DEvalVector (poly, x, y); + + // 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 = psPolynomial2DCopy (polyOld, poly); + + // calculate a new fit with modified errors: + if (!psVectorFitPolynomial2D(poly, mask, maskValue, f, modErr, x, y)) { + psError(PS_ERR_UNKNOWN, false, "Could not fit polynomial. Returning false.\n"); + psFree(x); + psFree(y); + psFree(modErr); + return false; + } + + // has the solution converged? + converged = true; + for (int ix = 0; ix <= poly->nX; ix++) { + for (int iy = 0; iy <= poly->nY; iy++) { + if ((fabs(poly->coeff[ix][iy] - polyOld->coeff[ix][iy]) > FIT_TOLERANCE * fabs(poly->coeff[ix][iy])) && + (fabs(poly->coeff[ix][iy] - polyOld->coeff[ix][iy]) > FLT_TOLERANCE)) + converged = false; + } + } + psFree (modErr); + } + + // Free local temporary variables + psFree(x); + psFree(y); + psFree(polyOld); + + psTrace("psLib.math", 3, "---- %s() end ----\n", __func__); + return true; +} + +/****************************************************************************** + ****************************************************************************** + 3-D Vector Code. + ****************************************************************************** + *****************************************************************************/ + +/****************************************************************************** +VectorFitPolynomial3DOrd(myPoly, *mask, maskValue, *f, *fErr, *x, *y, *z): +This is a private routine which will fit a 3-D polynomial to a set of (x, +y, z)-(f) pairs. All non-NULL vectors must be of type PS_TYPE_F64. + + *****************************************************************************/ +static bool VectorFitPolynomial3DOrd( + psPolynomial3D* myPoly, + const psVector* mask, + psVectorMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z) +{ + psTrace("psLib.math", 4, "---- %s() begin ----\n", __func__); + PS_ASSERT_POLY_NON_NULL(myPoly, false); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, false); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nY, false); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nZ, false); + + PS_ASSERT_VECTOR_NON_NULL(f, false); + PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, false); + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(y, fErr, false); + PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F64, false); + } + PS_ASSERT_VECTOR_NON_NULL(x, false); + PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); + PS_ASSERT_VECTOR_NON_NULL(y, false); + PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); + PS_ASSERT_VECTOR_NON_NULL(z, false); + PS_ASSERT_VECTOR_TYPE(z, PS_TYPE_F64, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, false); + if (mask != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, false); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); + } + + int nXterm = 1 + myPoly->nX; // Number of x terms + int nYterm = 1 + myPoly->nY; // Number of y terms + int nZterm = 1 + myPoly->nZ; // Number of z terms + int nTerm = nXterm * nYterm * nZterm; // Total number of terms + int nData = x->n; // Number of data points + psImage *A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); // Least-squares matrix + psVector *B = psVectorAlloc(nTerm, PS_TYPE_F64); // Least-squares vector + + // Initialize data structures. + if (!psImageInit(A, 0.0) || !psVectorInit(B, 0.0)) { + psError(PS_ERR_UNKNOWN, false, "Could initialize data structures A, B. Returning NULL.\n"); + psFree(A); + psFree(B); + psTrace("psLib.math", 4, "---- %s() End ----\n", __func__); + return false; + } + + // Dereference points for speed in the loop + psF64 **matrix = A->data.F64; // Least-squares matrix + psF64 *vector = B->data.F64; // Least-squares vector + psF64 *xData = x->data.F64; // x + psF64 *yData = y->data.F64; // y + psF64 *zData = z->data.F64; // z + psF64 *fData = f->data.F64; // f + psF64 *fErrData = NULL; // Error in f + if (fErr) { + fErrData = fErr->data.F64; + } + psVectorMaskType *dataMask = NULL; // Mask for data + if (mask) { + dataMask = mask->data.PS_TYPE_VECTOR_MASK_DATA; + } + psMaskType ***coeffMask = myPoly->coeffMask; // Mask for polynomial terms + int nYZterm = nYterm * nZterm; // Multiplication of the numbers, to calculate the index + + // Build the B and A data structs. + psF64 ***Sums = NULL; // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)*y, ... + for (int k = 0; k < nData; k++) { + if (dataMask && dataMask[k] & maskValue) { + continue; + } + + Sums = BuildSums3D(Sums, xData[k], yData[k], zData[k], nXterm, nYterm, nZterm); + + double wt; + if (fErr == NULL) { + wt = 1.0; + } else { + // this filters fErr == 0 values + wt = (fErr->data.F64[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErrData[k]); + } + + for (int i = 0; i < nTerm; i++) { + int ix = i / nYZterm; // x index + int iy = (i % nYZterm) / nZterm; // y index + int iz = (i % nYZterm) % nZterm; // z index + if (coeffMask[ix][iy][iz] & PS_POLY_MASK_BOTH) { + matrix[i][i] = 1.0; + continue; + } + + vector[i] += fData[k] * Sums[ix][iy][iz] * wt; + matrix[i][i] += Sums[2*ix][2*iy][2*iz] * wt; + for (int j = i + 1; j < nTerm; j++) { + int jx = j / (nYZterm); // x index + int jy = (j % nYZterm) / nZterm; // y index + int jz = (j % nYZterm) % nZterm; // z index + if (coeffMask[jx][jy][jz] & PS_POLY_MASK_BOTH) { + continue; + } + double value = Sums[ix+jx][iy+jy][iz+jz] * wt; + matrix[i][j] += value; + matrix[j][i] += value; + } + } + } + + // Free the sums + for (psS32 ix = 0; ix < 2*nXterm; ix++) { + for (psS32 iy = 0; iy < 2*nYterm; iy++) { + psFree(Sums[ix][iy]); + } + psFree(Sums[ix]); + } + psFree(Sums); + + + bool status = false; + if (USE_GAUSS_JORDAN) { + status = psMatrixGJSolve(A, B); + } else { + status = psMatrixLUSolve(A, B); + } + if (!status) { + psError(PS_ERR_UNKNOWN, false, "Could not solve linear equations.\n"); + goto escape; + } + + // select the appropriate solution entries + for (int i = 0; i < nTerm; i++) { + int ix = i / nYZterm; // x index + int iy = (i % nYZterm) / nZterm; // y index + int iz = (i % nYZterm) % nZterm; // z index + if (coeffMask[ix][iy][iz] & PS_POLY_MASK_FIT) continue; + myPoly->coeff[ix][iy][iz] = B->data.F64[i]; + myPoly->coeffErr[ix][iy][iz] = sqrt(A->data.F64[i][i]); + } + psFree(A); + psFree(B); + return true; + +escape: + psFree(A); + psFree(B); + return false; +} + +/****************************************************************************** +psVectorFitPolynomial3D(): This routine fits a 3D polynomial of arbitrary +degree (specified in poly) to the data points (x, y, z)-(f) and returns that +polynomial. Types F32 and F64 are supported, however, type F32 is done via +vector conversion only. + *****************************************************************************/ +bool psVectorFitPolynomial3D( + psPolynomial3D *poly, + const psVector *mask, + psVectorMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z) +{ + PS_ASSERT_POLY_NON_NULL(poly, false); + PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, false); + + PS_ASSERT_VECTOR_NON_NULL(f, false); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false); + PS_ASSERT_VECTOR_NON_NULL(x, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); + PS_ASSERT_VECTOR_NON_NULL(y, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); + PS_ASSERT_VECTOR_NON_NULL(z, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, false); + if (mask != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, false); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); + } + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, false); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, false); + } + + // Convert input vectors to F64 if necessary. + psVector *f64 = (f->type.type == PS_TYPE_F64) ? (psVector *) f : psVectorCopy(NULL, f, PS_TYPE_F64); + psVector *x64 = (x->type.type == PS_TYPE_F64) ? (psVector *) x : psVectorCopy(NULL, x, PS_TYPE_F64); + psVector *y64 = (y->type.type == PS_TYPE_F64) ? (psVector *) y : psVectorCopy(NULL, y, PS_TYPE_F64); + psVector *z64 = (z->type.type == PS_TYPE_F64) ? (psVector *) z : psVectorCopy(NULL, z, PS_TYPE_F64); + + psVector *fErr64 = NULL; + if (fErr != NULL) { + fErr64 = (fErr->type.type == PS_TYPE_F64) ? (psVector *) fErr : psVectorCopy(NULL, fErr, PS_TYPE_F64); + } + + bool result = true; + + switch (poly->type) { + case PS_POLYNOMIAL_ORD: + result = VectorFitPolynomial3DOrd(poly, mask, maskValue, f64, fErr64, x64, y64, z64); + if (!result) { + psError(PS_ERR_UNKNOWN, true, "Could not fit polynomial. Returning NULL.\n"); + } + break; + case PS_POLYNOMIAL_CHEB: + if (mask != NULL) { + psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring mask and maskValue with Chebyshev polynomials.\n"); + } + psError(PS_ERR_UNKNOWN, true, "3-D Chebyshev polynomial vector fitting has not been implemented. Returning NULL.\n"); + result = false; + break; + default: + psError(PS_ERR_UNKNOWN, true, "Incorrect polynomial type. Returning NULL.\n"); + result = false; + break; + } + + // Free psVectors that were created for NULL arguments. + PS_FREE_TEMP_F64_VECTOR (f, f64); + PS_FREE_TEMP_F64_VECTOR (x, x64); + PS_FREE_TEMP_F64_VECTOR (y, y64); + PS_FREE_TEMP_F64_VECTOR (z, z64); + PS_FREE_TEMP_F64_VECTOR (fErr, fErr64); + + return result; +} + +bool psVectorClipFitPolynomial3D( + psPolynomial3D *poly, + psStats *stats, + const psVector *mask, + psVectorMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z) +{ + psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__); + PS_ASSERT_POLY_NON_NULL(poly, false); + PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, false); + PS_ASSERT_PTR_NON_NULL(stats, false); + PS_ASSERT_VECTOR_NON_NULL(mask, false); + PS_ASSERT_VECTOR_NON_NULL(f, false); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false); + + PS_ASSERT_VECTOR_NON_NULL(x, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); + PS_ASSERT_VECTOR_TYPE(x, f->type.type, false); + + PS_ASSERT_VECTOR_NON_NULL(y, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); + PS_ASSERT_VECTOR_TYPE(y, f->type.type, false); + + PS_ASSERT_VECTOR_NON_NULL(z, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, false); + PS_ASSERT_VECTOR_TYPE(z, f->type.type, false); + + PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, false); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); + + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, false); + PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, false); + } + + // the user supplies one of various stats option pairs, + // determine the desired mean and stdev STATS options: + // XXX enforce consistency? + // XXX psStatsGetValue() probably has inverted precedence + psStatsOptions meanOption = stats->options & (PS_STAT_SAMPLE_MEAN | PS_STAT_SAMPLE_MEDIAN | PS_STAT_ROBUST_MEDIAN | PS_STAT_CLIPPED_MEAN | PS_STAT_FITTED_MEAN | PS_STAT_FITTED_MEAN); + psStatsOptions stdevOption = stats->options & (PS_STAT_SAMPLE_STDEV | PS_STAT_ROBUST_STDEV | PS_STAT_CLIPPED_STDEV | PS_STAT_FITTED_STDEV | PS_STAT_FITTED_STDEV); + if (!meanOption) { + psError(PS_ERR_UNKNOWN, true, "no valid mean stats option selected"); + return false; + } + if (!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 *resid = psVectorAlloc(f->n, PS_TYPE_F64); + + psTrace("psLib.math", 4, "stats->clipIter is %d\n", stats->clipIter); + psTrace("psLib.math", 4, "(minClipSigma, maxClipSigma) is (%.2f, %.2f)\n", minClipSigma, maxClipSigma); + + 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 (!psVectorFitPolynomial3D(poly, mask, maskValue, f, fErr, x, y, z)) { + psError(PS_ERR_UNKNOWN, false, "Could not fit a polynomial to the data. Returning NULL.\n"); + psFree(resid); + return false; + } + psVector *fit = psPolynomial3DEvalVector(poly, x, y, z); + if (fit == NULL) { + psError(PS_ERR_UNKNOWN, false, "Could not call psPolynomial3DEvalVector(). Returning NULL.\n"); + 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.F64[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 NULL.\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++; + } + psTrace("psLib.math", 6, "keeping %d of %ld pts for fit\n", Nkeep, x->n); + stats->clippedNvalues = Nkeep; + psFree(fit); + } + // Free local temporary variables + psFree(resid); + + psTrace("psLib.math", 3, "---- %s() end ----\n", __func__); + return true; +} + +// This function accepts F32 and F64 input vectors. +bool psVectorIRLSFitPolynomial3D( + psPolynomial3D *poly, + psStats *stats, + const psVector *mask, + psVectorMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *xIn, + const psVector *yIn, + const psVector *zIn) +{ + psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__); + + PS_ASSERT (poly->type == PS_POLYNOMIAL_ORD, false); // XXX for now, only allow ORD + PS_ASSERT_POLY_NON_NULL(poly, false); + PS_ASSERT_VECTOR_NON_NULL(f, false); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false); + if (mask != NULL) { + 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); + } + if (xIn != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(xIn, f, false); + PS_ASSERT_VECTOR_TYPE(xIn, f->type.type, false); + } + if (yIn != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(yIn, f, false); + PS_ASSERT_VECTOR_TYPE(yIn, f->type.type, false); + } + if (zIn != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(zIn, f, false); + PS_ASSERT_VECTOR_TYPE(zIn, f->type.type, false); + } + + // Internal pointers for possibly NULL vectors. + psVector *x = (xIn != NULL) ? psMemIncrRefCounter((psVector *) xIn) : psVectorCreate(NULL, 0, f->n, 1, f->type.type); + psVector *y = (yIn != NULL) ? psMemIncrRefCounter((psVector *) yIn) : psVectorCreate(NULL, 0, f->n, 1, f->type.type); + psVector *z = (zIn != NULL) ? psMemIncrRefCounter((psVector *) zIn) : psVectorCreate(NULL, 0, f->n, 1, f->type.type); + + // initial fit with nominal errors + if (!psVectorFitPolynomial3D(poly, mask, maskValue, f, fErr, x, y, z)) { + psError(PS_ERR_UNKNOWN, false, "Could not fit polynomial. Returning false.\n"); + psFree(x); + psFree(y); + psFree(z); + return false; + } + + // use polyOld to save the last fit + psPolynomial3D *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 psVectorFitPolynomial3D()\n", N); + + // evaluate the fit at the input positions + psVector *fEval = psPolynomial3DEvalVector (poly, x, y, z); + + // 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 = psPolynomial3DCopy (polyOld, poly); + + // calculate a new fit with modified errors: + if (!psVectorFitPolynomial3D(poly, mask, maskValue, f, modErr, x, y, z)) { + psError(PS_ERR_UNKNOWN, false, "Could not fit polynomial. Returning false.\n"); + psFree(x); + psFree(y); + psFree(z); + psFree(modErr); + return false; + } + + // has the solution converged? + converged = true; + for (int ix = 0; ix <= poly->nX; ix++) { + for (int iy = 0; iy <= poly->nY; iy++) { + for (int iz = 0; iz <= poly->nZ; iz++) { + if ((fabs(poly->coeff[ix][iy][iz] - polyOld->coeff[ix][iy][iz]) > FIT_TOLERANCE * fabs(poly->coeff[ix][iy][iz])) && + (fabs(poly->coeff[ix][iy][iz] - polyOld->coeff[ix][iy][iz]) > FLT_TOLERANCE)) + converged = false; + } + } + } + psFree (modErr); + } + + // Free local temporary variables + psFree(x); + psFree(y); + psFree(z); + psFree(polyOld); + + psTrace("psLib.math", 3, "---- %s() end ----\n", __func__); + return true; +} + +/****************************************************************************** + ****************************************************************************** + 4-D Vector Code. + ****************************************************************************** + *****************************************************************************/ +/****************************************************************************** +VectorFitPolynomial4DOrd(myPoly, *mask, maskValue, *f, *fErr, *x, *y, *z, *t): +This is a private routine which will fit a 4-D polynomial to a set of (x, +y, z, t)-(f) pairs. All non-NULL vectors must be of type PS_TYPE_F64. + + *****************************************************************************/ +static bool VectorFitPolynomial4DOrd( + psPolynomial4D* myPoly, + const psVector* mask, + psVectorMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z, + const psVector *t) +{ + psTrace("psLib.math", 4, "---- %s() begin ----\n", __func__); + PS_ASSERT_POLY_NON_NULL(myPoly, false); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, false); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nY, false); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nZ, false); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nT, false); + PS_ASSERT_VECTOR_NON_NULL(f, false); + PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, false); + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(y, fErr, false); + PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F64, false); + } + PS_ASSERT_VECTOR_NON_NULL(x, false); + PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); + PS_ASSERT_VECTOR_NON_NULL(y, false); + PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); + PS_ASSERT_VECTOR_NON_NULL(z, false); + PS_ASSERT_VECTOR_TYPE(z, PS_TYPE_F64, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, false); + PS_ASSERT_VECTOR_NON_NULL(t, false); + PS_ASSERT_VECTOR_TYPE(t, PS_TYPE_F64, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, t, false); + if (mask) { + PS_ASSERT_VECTORS_SIZE_EQUAL(y, mask, false); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); + } + + + int nXterm = 1 + myPoly->nX; // Number of x terms + int nYterm = 1 + myPoly->nY; // Number of y terms + int nZterm = 1 + myPoly->nZ; // Number of z terms + int nTterm = 1 + myPoly->nT; // Number of t terms + int nTerm = nXterm * nYterm * nZterm * nTterm; // Total number of terms + int nData = x->n; // Number of data points + psImage *A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); // Least-squares matrix + psVector *B = psVectorAlloc(nTerm, PS_TYPE_F64); // Least-squares vector + + // Initialize data structures. + if (!psImageInit(A, 0.0) || !psVectorInit(B, 0.0)) { + psError(PS_ERR_UNKNOWN, false, "Could initialize data structures A, B. Returning NULL.\n"); + psFree(A); + psFree(B); + psTrace("psLib.math", 4, "---- %s() End ----\n", __func__); + return false; + } + + // Dereference points for speed in the loop + psF64 **matrix = A->data.F64; // Least-squares matrix + psF64 *vector = B->data.F64; // Least-squares vector + psF64 *xData = x->data.F64; // x + psF64 *yData = y->data.F64; // y + psF64 *zData = z->data.F64; // z + psF64 *tData = t->data.F64; // t + psF64 *fData = f->data.F64; // f + psF64 *fErrData = NULL; // Error in f + if (fErr) { + fErrData = fErr->data.F64; + } + psVectorMaskType *dataMask = NULL; // Mask for data + if (mask) { + dataMask = mask->data.PS_TYPE_VECTOR_MASK_DATA; + } + psMaskType ****coeffMask = myPoly->coeffMask; // Mask for polynomial terms + int nYZTterm = nYterm * nZterm * nTterm; // Multiplication of the numbers, for calculating the index + int nZTterm = nZterm * nTterm; // Multiplication of the numbers, for calculating the index + + // Build the B and A data structs. + psF64 ****Sums = NULL; // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)*y, ... + for (int k = 0; k < nData; k++) { + if (dataMask && dataMask[k] & maskValue) { + continue; + } + + Sums = BuildSums4D(Sums, xData[k], yData[k], zData[k], tData[k], nXterm, nYterm, nZterm, nTterm); + + double wt; + if (fErr == NULL) { + wt = 1.0; + } else { + // this filters fErr == 0 values + wt = (fErr->data.F64[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErrData[k]); + } + + for (int i = 0; i < nTerm; i++) { + int ix = i / (nYZTterm); // x index + int iy = (i % (nYZTterm)) / (nZTterm); // y index + int iz = ((i % (nYZTterm)) % (nZTterm)) / nTterm; // z index + int it = ((i % (nYZTterm)) % (nZTterm)) % nTterm; // t index + if (coeffMask[ix][iy][iz][it] & PS_POLY_MASK_BOTH) { + matrix[i][i] = 1.0; + continue; + } + + vector[i] += fData[k] * Sums[ix][iy][iz][it] * wt; + matrix[i][i] += Sums[2*ix][2*iy][2*iz][2*it] * wt; + for (int j = i + 1; j < nTerm; j++) { + int jx = j / nYZTterm; // x index + int jy = (j % nYZTterm) / nZTterm; // y index + int jz = ((j % nYZTterm) % nZTterm) / nTterm; // z index + int jt = ((j % nYZTterm) % nZTterm) % nTterm; // t index + if (coeffMask[jx][jy][jz][jt] & PS_POLY_MASK_BOTH) { + continue; + } + double value = Sums[ix+jx][iy+jy][iz+jz][it+jt] * wt; + matrix[i][j] += value; + matrix[j][i] += value; + } + } + } + + // Free the sums + if (Sums == NULL) { + assert (nData == 0); + } else { + for (int ix = 0; ix < 2*nXterm; ix++) { + for (int iy = 0; iy < 2*nYterm; iy++) { + for (int iz = 0; iz < 2*nZterm; iz++) { + psFree(Sums[ix][iy][iz]); + } + psFree(Sums[ix][iy]); + } + psFree(Sums[ix]); + } + psFree(Sums); + } + + bool status = false; + if (USE_GAUSS_JORDAN) { + status = psMatrixGJSolve(A, B); + } else { + status = psMatrixLUSolve(A, B); + } + if (!status) { + psError(PS_ERR_UNKNOWN, false, "Could not solve linear equations.\n"); + goto escape; + } + + // select the appropriate solution entries + for (int i = 0; i < nTerm; i++) { + int ix = i / nYZTterm; // x index + int iy = (i % nYZTterm) / nZTterm; // y index + int iz = ((i % nYZTterm) % nZTterm) / nTterm; // z index + int it = ((i % nYZTterm) % nZTterm) % nTterm; // t index + if (coeffMask[ix][iy][iz][it] & PS_POLY_MASK_FIT) continue; + myPoly->coeff[ix][iy][iz][it] = B->data.F64[i]; + myPoly->coeffErr[ix][iy][iz][it] = sqrt(A->data.F64[i][i]); + } + psFree(A); + psFree(B); + return true; + +escape: + psFree(A); + psFree(B); + return false; +} + +/****************************************************************************** +psVectorFitPolynomial4D(): This routine fits a 4D polynomial of arbitrary +degree (specified in poly) to the data points (x, y, z, t)-(f) and returns +that polynomial. Types F32 and F64 are supported, however, type F32 is done +via vector conversion only. + *****************************************************************************/ +bool psVectorFitPolynomial4D( + psPolynomial4D *poly, + const psVector *mask, + psVectorMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z, + const psVector *t) +{ + PS_ASSERT_POLY_NON_NULL(poly, false); + PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, false); + + PS_ASSERT_VECTOR_NON_NULL(f, false); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false); + PS_ASSERT_VECTOR_NON_NULL(x, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); + PS_ASSERT_VECTOR_NON_NULL(y, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); + PS_ASSERT_VECTOR_NON_NULL(z, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, false); + PS_ASSERT_VECTOR_NON_NULL(t, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, t, false); + if (mask) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, false); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); + } + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, false); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, false); + } + + // Convert input vectors to F64 if necessary. + psVector *f64 = (f->type.type == PS_TYPE_F64) ? (psVector *) f : psVectorCopy(NULL, f, PS_TYPE_F64); + psVector *x64 = (x->type.type == PS_TYPE_F64) ? (psVector *) x : psVectorCopy(NULL, x, PS_TYPE_F64); + psVector *y64 = (y->type.type == PS_TYPE_F64) ? (psVector *) y : psVectorCopy(NULL, y, PS_TYPE_F64); + psVector *z64 = (z->type.type == PS_TYPE_F64) ? (psVector *) z : psVectorCopy(NULL, z, PS_TYPE_F64); + psVector *t64 = (t->type.type == PS_TYPE_F64) ? (psVector *) t : psVectorCopy(NULL, t, PS_TYPE_F64); + + psVector *fErr64 = NULL; + if (fErr != NULL) { + fErr64 = (fErr->type.type == PS_TYPE_F64) ? (psVector *) fErr : psVectorCopy(NULL, fErr, PS_TYPE_F64); + } + + bool result = true; + + switch (poly->type) { + case PS_POLYNOMIAL_ORD: + result = VectorFitPolynomial4DOrd(poly, mask, maskValue, f64, fErr64, x64, y64, z64, t64); + if (!result) { + psError(PS_ERR_UNKNOWN, true, "Could not fit polynomial. Returning NULL.\n"); + } + break; + case PS_POLYNOMIAL_CHEB: + if (mask != NULL) { + psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring mask and maskValue with Chebyshev polynomials.\n"); + } + psError(PS_ERR_UNKNOWN, true, "4-D Chebyshev polynomial vector fitting has not been implemented. Returning NULL.\n"); + result = false; + break; + default: + psError(PS_ERR_UNKNOWN, true, "Incorrect polynomial type. Returning NULL.\n"); + result = false; + break; + } + + // Free psVectors that were created for NULL arguments. + PS_FREE_TEMP_F64_VECTOR (f, f64); + PS_FREE_TEMP_F64_VECTOR (x, x64); + PS_FREE_TEMP_F64_VECTOR (y, y64); + PS_FREE_TEMP_F64_VECTOR (z, z64); + PS_FREE_TEMP_F64_VECTOR (t, t64); + PS_FREE_TEMP_F64_VECTOR (fErr, fErr64); + + return result; +} + + +bool psVectorClipFitPolynomial4D( + psPolynomial4D *poly, + psStats *stats, + const psVector *mask, + psVectorMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z, + const psVector *t) +{ + psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__); + PS_ASSERT_POLY_NON_NULL(poly, false); + PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, false); + PS_ASSERT_PTR_NON_NULL(stats, false); + PS_ASSERT_VECTOR_NON_NULL(mask, false); + PS_ASSERT_VECTOR_NON_NULL(f, false); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false); + + PS_ASSERT_VECTOR_NON_NULL(x, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); + PS_ASSERT_VECTOR_TYPE(x, f->type.type, false); + + PS_ASSERT_VECTOR_NON_NULL(y, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); + PS_ASSERT_VECTOR_TYPE(y, f->type.type, false); + + PS_ASSERT_VECTOR_NON_NULL(z, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, false); + PS_ASSERT_VECTOR_TYPE(z, f->type.type, false); + + PS_ASSERT_VECTOR_NON_NULL(t, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, t, false); + PS_ASSERT_VECTOR_TYPE(t, f->type.type, false); + + PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, false); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); + + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, false); + PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, false); + } + + // the user supplies one of various stats option pairs, + // determine the desired mean and stdev STATS options: + // XXX enforce consistency? + // XXX psStatsGetValue() probably has inverted precedence + psStatsOptions meanOption = stats->options & (PS_STAT_SAMPLE_MEAN | PS_STAT_SAMPLE_MEDIAN | PS_STAT_ROBUST_MEDIAN | PS_STAT_CLIPPED_MEAN | PS_STAT_FITTED_MEAN | PS_STAT_FITTED_MEAN); + psStatsOptions stdevOption = stats->options & (PS_STAT_SAMPLE_STDEV | PS_STAT_ROBUST_STDEV | PS_STAT_CLIPPED_STDEV | PS_STAT_FITTED_STDEV | PS_STAT_FITTED_STDEV); + if (!meanOption) { + psError(PS_ERR_UNKNOWN, true, "no valid mean stats option selected"); + return false; + } + if (!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 *resid = psVectorAlloc(f->n, PS_TYPE_F64); + + psTrace("psLib.math", 4, "stats->clipIter is %d\n", stats->clipIter); + psTrace("psLib.math", 4, "(minClipSigma, maxClipSigma) is (%.2f, %.2f)\n", minClipSigma, maxClipSigma); + + for (psS32 N = 0; N < stats->clipIter; N++) { + psTrace("psLib.math", 6, "Loop iteration %d. Calling psVectorFitPolynomial4D()\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 (!psVectorFitPolynomial4D (poly, mask, maskValue, f, fErr, x, y, z, t)) { + psError(PS_ERR_UNKNOWN, false, "Could not fit a polynomial to the data. Returning NULL.\n"); + psFree(resid); + return false; + } + + psVector *fit = psPolynomial4DEvalVector (poly, x, y, z, t); + if (fit == NULL) { + psError(PS_ERR_UNKNOWN, false, "Could not call psPolynomial4DEvalVector(). Returning NULL.\n"); + 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.F64[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 NULL.\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++; + } + psTrace("psLib.math", 6, "keeping %d of %ld pts for fit\n", Nkeep, x->n); + stats->clippedNvalues = Nkeep; + psFree (fit); + } + // Free local temporary variables + psFree (resid); + + psTrace("psLib.math", 3, "---- %s() end ----\n", __func__); + return true; +} + +// This function accepts F32 and F64 input vectors. +bool psVectorIRLSFitPolynomial4D( + psPolynomial4D *poly, + psStats *stats, + const psVector *mask, + psVectorMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *xIn, + const psVector *yIn, + const psVector *zIn, + const psVector *tIn) +{ + psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__); + + PS_ASSERT (poly->type == PS_POLYNOMIAL_ORD, false); // XXX for now, only allow ORD + PS_ASSERT_POLY_NON_NULL(poly, false); + PS_ASSERT_VECTOR_NON_NULL(f, false); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false); + if (mask != NULL) { + 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); + } + if (xIn != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(xIn, f, false); + PS_ASSERT_VECTOR_TYPE(xIn, f->type.type, false); + } + if (yIn != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(yIn, f, false); + PS_ASSERT_VECTOR_TYPE(yIn, f->type.type, false); + } + if (zIn != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(zIn, f, false); + PS_ASSERT_VECTOR_TYPE(zIn, f->type.type, false); + } + if (tIn != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(tIn, f, false); + PS_ASSERT_VECTOR_TYPE(tIn, f->type.type, false); + } + + // Internal pointers for possibly NULL vectors. + psVector *x = (xIn != NULL) ? psMemIncrRefCounter((psVector *) xIn) : psVectorCreate(NULL, 0, f->n, 1, f->type.type); + psVector *y = (yIn != NULL) ? psMemIncrRefCounter((psVector *) yIn) : psVectorCreate(NULL, 0, f->n, 1, f->type.type); + psVector *z = (zIn != NULL) ? psMemIncrRefCounter((psVector *) zIn) : psVectorCreate(NULL, 0, f->n, 1, f->type.type); + psVector *t = (tIn != NULL) ? psMemIncrRefCounter((psVector *) tIn) : psVectorCreate(NULL, 0, f->n, 1, f->type.type); + + // initial fit with nominal errors + if (!psVectorFitPolynomial4D(poly, mask, maskValue, f, fErr, x, y, z, t)) { + psError(PS_ERR_UNKNOWN, false, "Could not fit polynomial. Returning false.\n"); + psFree(x); + psFree(y); + psFree(z); + psFree(t); + return false; + } + + // use polyOld to save the last fit + psPolynomial4D *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 psVectorFitPolynomial4D()\n", N); + + // evaluate the fit at the input positions + psVector *fEval = psPolynomial4DEvalVector (poly, x, y, z, t); + + // 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 = psPolynomial4DCopy (polyOld, poly); + + // calculate a new fit with modified errors: + if (!psVectorFitPolynomial4D(poly, mask, maskValue, f, modErr, x, y, z, t)) { + psError(PS_ERR_UNKNOWN, false, "Could not fit polynomial. Returning false.\n"); + psFree(x); + psFree(y); + psFree(z); + psFree(t); + psFree(modErr); + return false; + } + + // has the solution converged? + converged = true; + for (int ix = 0; ix <= poly->nX; ix++) { + for (int iy = 0; iy <= poly->nY; iy++) { + for (int iz = 0; iz <= poly->nZ; iz++) { + for (int it = 0; it <= poly->nT; it++) { + if ((fabs(poly->coeff[ix][iy][iz][it] - polyOld->coeff[ix][iy][iz][it]) > FIT_TOLERANCE * fabs(poly->coeff[ix][iy][iz][it])) && + (fabs(poly->coeff[ix][iy][iz][it] - polyOld->coeff[ix][iy][iz][it]) > FLT_TOLERANCE)) + converged = false; + } + } + } + } + psFree (modErr); + } + + // Free local temporary variables + psFree(x); + psFree(y); + psFree(z); + psFree(t); + psFree(polyOld); + + psTrace("psLib.math", 3, "---- %s() end ----\n", __func__); + return true; +} + +// ######################## utilities ################### + +// Used by IRLS fitting // This function assumes the input vectors (f, fEval, fErr) all have the same type // This requirement is already enforced in the calling function (psVectorIRLSFitPolynomial1D) @@ -1078,1541 +2926,2 @@ } -// 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, - psStats *stats, - 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); - if (mask != NULL) { - 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; - } - } - - // 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); - return false; - } - - // 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); - - // 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(modErr); - return false; - } - - // 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; - } - -# if (0) - // XXX test: - FILE *ftest = fopen ("irls.wt.dat", "w"); - for (int i = 0; i < modErr->n; i++) { - if (modErr->type.type == PS_TYPE_F64) { - fprintf (ftest, "%d %f\n", i, modErr->data.F64[i]); - } else { - fprintf (ftest, "%d %f\n", i, modErr->data.F32[i]); - } - } - fclose (ftest); -# endif - psFree (modErr); - } - - // Free local temporary variables - if (xIn == NULL) psFree(x); - psFree(polyOld); - - psTrace("psLib.math", 3, "---- %s() end ----\n", __func__); - return true; -} - -/****************************************************************************** - ****************************************************************************** - 2-D Vector Code. - ****************************************************************************** - *****************************************************************************/ - -/****************************************************************************** -VectorFitPolynomial2DOrd(myPoly, *mask, maskValue, *f, *fErr, *x, *y): This is -a private routine which will fit a 2-D polynomial to a set of (x, y)-(f) -pairs. All non-NULL vectors must be of type PS_TYPE_F64. - - *****************************************************************************/ -static bool VectorFitPolynomial2DOrd( - psPolynomial2D* myPoly, - const psVector* mask, - psVectorMaskType maskValue, - const psVector *f, - const psVector *fErr, - const psVector *x, - const psVector *y) -{ - psTrace("psLib.math", 4, "---- %s() begin ----\n", __func__); - PS_ASSERT_POLY_NON_NULL(myPoly, false); - PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, false); - PS_ASSERT_INT_NONNEGATIVE(myPoly->nY, false); - PS_ASSERT_VECTOR_NON_NULL(f, false); - PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, false); - if (fErr != NULL) { - PS_ASSERT_VECTORS_SIZE_EQUAL(y, fErr, false); - PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F64, false); - } - PS_ASSERT_VECTOR_NON_NULL(x, false); - PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); - PS_ASSERT_VECTOR_NON_NULL(y, false); - PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); - if (mask != NULL) { - PS_ASSERT_VECTORS_SIZE_EQUAL(y, mask, false); - PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); - } - - // Number of polynomial terms - int nXterm = 1 + myPoly->nX; // Number of terms in x - int nYterm = 1 + myPoly->nY; // Number of terms in y - int nTerm = nXterm * nYterm; // Total number of terms - - psImage *A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); // Least-squares matrix - psVector *B = psVectorAlloc(nTerm, PS_TYPE_F64); // Least-squares vector - - // Initialize data structures. - if (!psImageInit(A, 0.0) || !psVectorInit(B, 0.0)) { - psError(PS_ERR_UNKNOWN, false, "Could initialize data structures A, B. Returning NULL.\n"); - psFree(A); - psFree(B); - psTrace("psLib.math", 6, "---- %s() End ----\n", __func__); - return false; - } - - // Dereference stuff, to make the loop go faster - psF64 **matrix = A->data.F64; // Dereference the least-squares matrix - psF64 *vector = B->data.F64; // Dereference the least-squares vector - psMaskType **coeffMask = myPoly->coeffMask; // Dereference mask for polynomial terms - psVectorMaskType *dataMask = NULL; // Dereference mask for data - if (mask) { - dataMask = mask->data.PS_TYPE_VECTOR_MASK_DATA; - } - psF64 *xData = x->data.F64; // Dereference x - psF64 *yData = y->data.F64; // Dereference y - psF64 *fData = f->data.F64; // Dereference f - psF64 *fErrData = NULL; // Dereference fErr - if (fErr) { - fErrData = fErr->data.F64; - } - - // Build the least-squares matrix and vector - psImage *xySums = NULL; // The sums: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1) - for (int k = 0; k < x->n; k++) { - if (dataMask && dataMask[k] & maskValue) { - continue; - } - xySums = BuildSums2D(xySums, xData[k], yData[k], nXterm, nYterm); - psF64 **sums = xySums->data.F64;// Dereference sums - - double wt; // Weight - if (!fErrData) { - wt = 1.0; - } else { - // this filters fErr == 0 values - wt = (fErrData[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErrData[k]); - } - - // Iterating over the matrix - for (int i = 0; i < nTerm; i++) { - int l = i / nYterm; // x index - int m = i % nYterm; // y index - if (coeffMask[l][m] & PS_POLY_MASK_SET) { - matrix[i][i] = 1.0; - continue; - } - vector[i] += fData[k] * sums[l][m] * wt; - matrix[i][i] += sums[2*l][2*m] * wt; // The diagonal entry - for (int j = i + 1; j < nTerm; j++) { // Doing the upper diagonal only: we will use symmetry - int p = j / nYterm; // x index - int q = j % nYterm; // y index - if (coeffMask[p][q] & PS_POLY_MASK_SET) { - continue; - } - double value = sums[l+p][m+q] * wt; // Value to add in - matrix[i][j] += value; - matrix[j][i] += value; // Taking advantage of the symmetry - } - } - } - psFree(xySums); - - // elements which are masked for fitting need to be subtracted from the vector - for (int i = 0; i < nTerm; i++) { - int ix = i / nYterm; // x index - int iy = i % nYterm; // y index - if (coeffMask[ix][iy] & PS_POLY_MASK_BOTH) { - continue; - } - for (int j = 0; j < nTerm; j++) { // The upper diagonal only: we will use symmetry - int jx = j / nYterm; // x index - int jy = j % nYterm; // y index - if (coeffMask[jx][jy] & PS_POLY_MASK_SET) { - continue; - } - if (!(coeffMask[jx][jy] & PS_POLY_MASK_FIT)) { - continue; - } - vector[i] -= matrix[i][j]*myPoly->coeff[jx][jy]; - } - } - - // set the un-fitted and un-set elements to 0 or 1 for pivots - for (int i = 0; i < nTerm; i++) { - int ix = i / nYterm; // x index - int iy = i % nYterm; // y index - if (coeffMask[ix][iy] & PS_POLY_MASK_BOTH) { - for (int j = 0; j < nTerm; j++) { // The upper diagonal only: we will use symmetry - matrix[i][j] = 0.0; - matrix[j][i] = 0.0; - } - matrix[i][i] = 1.0; - continue; - } - } - - if (psTraceGetLevel("psLib.math") >= 4) { - printf("Least-squares vector:\n"); - for (int i = 0; i < nTerm; i++) { - printf("%f ", B->data.F64[i]); - } - printf("\n"); - printf("Least-squares matrix:\n"); - for (int i = 0; i < nTerm; i++) { - for (int j = 0; j < nTerm; j++) { - printf("%f ", A->data.F64[i][j]); - } - printf("\n"); - } - } - - bool status = false; - if (USE_GAUSS_JORDAN) { - status = psMatrixGJSolve(A, B); - } else { - status = psMatrixLUSolve(A, B); - } - if (!status) { - psError(PS_ERR_UNKNOWN, false, "Could not solve linear equations.\n"); - goto escape; - } - - // select the appropriate solution entries (retain the incoming values if masked on the fit) - for (int i = 0; i < nTerm; i++) { - int ix = i / nYterm; // x index - int iy = i % nYterm; // y index - if (coeffMask[ix][iy] & PS_POLY_MASK_FIT) continue; - myPoly->coeff[ix][iy] = B->data.F64[i]; - myPoly->coeffErr[ix][iy] = sqrt(A->data.F64[i][i]); - } - psFree(A); - psFree(B); - return true; - -escape: - psFree (A); - psFree (B); - return false; -} - -/****************************************************************************** -VectorFitPolynomial2DCheb(myPoly, *mask, maskValue, *f, *fErr, *x, *y): This is -a private routine which will fit a 2-D polynomial to a set of (x, y)-(f) -pairs. All non-NULL vectors must be of type PS_TYPE_F64. - - *****************************************************************************/ -static bool VectorFitPolynomial2DCheb( - psPolynomial2D* myPoly, - const psVector *f, - const psVector *x, - const psVector *y) -{ - psTrace("psLib.math", 4, "---- %s() begin ----\n", __func__); - PS_ASSERT_POLY_NON_NULL(myPoly, false); - PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, false); - PS_ASSERT_INT_NONNEGATIVE(myPoly->nY, false); - PS_ASSERT_VECTOR_NON_NULL(f, false); - PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, false); - PS_ASSERT_VECTOR_NON_NULL(x, false); - PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); - PS_ASSERT_VECTOR_NON_NULL(y, false); - PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); - - // Number of polynomial terms - int nXterm = 1 + myPoly->nX; // Number of terms in x - int nYterm = 1 + myPoly->nY; // Number of terms in y - int nTerm = nXterm * nYterm; // Total number of terms - if (nXterm > 9) { - psError(PS_ERR_UNKNOWN, false, "failed 2D chebyshev fit: orders higher than 9 are not yet coded\n"); - return false; - } - if (nYterm > 9) { - psError(PS_ERR_UNKNOWN, false, "failed 2D chebyshev fit: orders higher than 9 are not yet coded\n"); - return false; - } - - // determine scale factors - if (!psChebyshevSetScale (myPoly, x, 0)) { psError(PS_ERR_UNKNOWN, false, "failed 2D chebyshev fit.\n"); return false; } - if (!psChebyshevSetScale (myPoly, y, 1)) { psError(PS_ERR_UNKNOWN, false, "failed 2D chebyshev fit.\n"); return false; } - - // generate normalized vectors - psVector *xNorm = psChebyshevNormVector (myPoly, x, 0); - psVector *yNorm = psChebyshevNormVector (myPoly, y, 1); - - // generate the N cheb polynomials based on xNorm, yNorm - psArray *xPolySet = psArrayAlloc (nXterm); - for (int i = 0; i < nXterm; i++) { - xPolySet->data[i] = psChebyshevPolyVector (xNorm, i); - } - psArray *yPolySet = psArrayAlloc (nYterm); - for (int i = 0; i < nYterm; i++) { - yPolySet->data[i] = psChebyshevPolyVector (yNorm, i); - } - - psF64 *fData = f->data.F64; // Dereference f - - psImage *A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); // Least-squares matrix - psVector *B = psVectorAlloc(nTerm, PS_TYPE_F64); // Least-squares vector - - // Initialize data structures (should not be able to fail) - psAssert (psImageInit(A, 0.0), "Could initialize data structures A"); - psAssert (psVectorInit(B, 0.0), "Could initialize data structures B"); - - // Dereference stuff, to make the loop go faster - psF64 **matrix = A->data.F64; // Dereference the least-squares matrix - psF64 *vector = B->data.F64; // Dereference the least-squares vector - - // loop over all elements of the data vector - for (int k = 0; k < x->n; k++) { - - if (!finite(fData[k])) continue; - - // XXX can we only calculate the upper diagonal? - int nelem = 0; - for (int jx = 0; jx < nXterm; jx++) { - psVector *jxCheb = xPolySet->data[jx]; - for (int jy = 0; jy < nYterm; jy++) { - psVector *jyCheb = yPolySet->data[jy]; - psF64 chebValue = jxCheb->data.F64[k] * jyCheb->data.F64[k]; - - vector[nelem] += fData[k] * chebValue; - - int melem = 0; - for (int kx = 0; kx < nXterm; kx++) { - psVector *kxCheb = xPolySet->data[kx]; - for (int ky = 0; ky < nYterm; ky++) { - psVector *kyCheb = yPolySet->data[ky]; - matrix[nelem][melem] += chebValue * kxCheb->data.F64[k]*kyCheb->data.F64[k]; - melem++; - } - } - nelem++; - } - } - } - - if (psTraceGetLevel("psLib.math") >= 4) { - printf("Least-squares vector:\n"); - for (int i = 0; i < nTerm; i++) { - printf("%f ", B->data.F64[i]); - } - printf("\n"); - printf("Least-squares matrix:\n"); - for (int i = 0; i < nTerm; i++) { - for (int j = 0; j < nTerm; j++) { - printf("%f ", A->data.F64[i][j]); - } - printf("\n"); - } - } - - bool status = false; - if (USE_GAUSS_JORDAN) { - status = psMatrixGJSolve(A, B); - } else { - status = psMatrixLUSolve(A, B); - } - if (!status) { - psError(PS_ERR_UNKNOWN, false, "Could not solve linear equations.\n"); - goto escape; - } - - // unroll the result: - int nelem = 0; - for (int jx = 0; jx < nXterm; jx++) { - for (int jy = 0; jy < nYterm; jy++) { - myPoly->coeff[jx][jy] = B->data.F64[nelem]; - myPoly->coeffErr[jx][jy] = sqrt(A->data.F64[nelem][nelem]); - nelem ++; - } - } - psFree(A); - psFree(B); - - psFree (xNorm); - psFree (yNorm); - psFree (xPolySet); - psFree (yPolySet); - - return true; - -escape: - psFree (A); - psFree (B); - return false; -} - -/****************************************************************************** -psVectorFitPolynomial2D(): This routine fits a 2D polynomial of arbitrary -degree (specified in poly) to the data points (x, y)-(f) and returns that -polynomial. Types F32 and F64 are supported, however, type F32 is done via -vector conversion only. - *****************************************************************************/ -bool psVectorFitPolynomial2D( - psPolynomial2D *poly, - const psVector *mask, - psVectorMaskType maskValue, - const psVector *f, - const psVector *fErr, - const psVector *x, - const psVector *y) -{ - PS_ASSERT_POLY_NON_NULL(poly, false); - // PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, false); - - PS_ASSERT_VECTOR_NON_NULL(f, false); - PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false); - PS_ASSERT_VECTOR_NON_NULL(x, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); - PS_ASSERT_VECTOR_NON_NULL(y, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); - if (mask != NULL) { - PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, false); - PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); - } - if (fErr != NULL) { - PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, false); - PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, false); - } - - // Convert input vectors to F64 if necessary. - psVector *f64 = (f->type.type == PS_TYPE_F64) ? (psVector *) f : psVectorCopy(NULL, f, PS_TYPE_F64); - psVector *x64 = (x->type.type == PS_TYPE_F64) ? (psVector *) x : psVectorCopy(NULL, x, PS_TYPE_F64); - psVector *y64 = (y->type.type == PS_TYPE_F64) ? (psVector *) y : psVectorCopy(NULL, y, PS_TYPE_F64); - - psVector *fErr64 = NULL; - if (fErr != NULL) { - fErr64 = (fErr->type.type == PS_TYPE_F64) ? (psVector *) fErr : psVectorCopy(NULL, fErr, PS_TYPE_F64); - } - - bool result = true; - - switch (poly->type) { - case PS_POLYNOMIAL_ORD: - result = VectorFitPolynomial2DOrd(poly, mask, maskValue, f64, fErr64, x64, y64); - if (!result) { - psError(PS_ERR_UNKNOWN, true, "Could not fit polynomial. Returning NULL.\n"); - } - break; - case PS_POLYNOMIAL_CHEB: - if (mask != NULL) { - psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring mask and maskValue with Chebyshev polynomials.\n"); - } - if (fErr != NULL) { - psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring error values for Chebyshev polynomials.\n"); - } - result = VectorFitPolynomial2DCheb(poly, f64, x64, y64); - if (!result) { - psError(PS_ERR_UNKNOWN, true, "Could not fit polynomial. Returning NULL.\n"); - } - break; - default: - psError(PS_ERR_UNKNOWN, true, "Incorrect polynomial type. Returning NULL.\n"); - result = false; - break; - } - - // Free psVectors that were created for NULL arguments. - PS_FREE_TEMP_F64_VECTOR (f, f64); - PS_FREE_TEMP_F64_VECTOR (x, x64); - PS_FREE_TEMP_F64_VECTOR (y, y64); - PS_FREE_TEMP_F64_VECTOR (fErr, fErr64); - - return result; -} - -bool psVectorClipFitPolynomial2D( - psPolynomial2D *poly, - psStats *stats, - const psVector *mask, - psVectorMaskType maskValue, - const psVector *f, - const psVector *fErr, - const psVector *x, - const psVector *y) -{ - psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__); - PS_ASSERT_POLY_NON_NULL(poly, false); - PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, false); - PS_ASSERT_PTR_NON_NULL(stats, false); - PS_ASSERT_VECTOR_NON_NULL(mask, false); - PS_ASSERT_VECTOR_NON_NULL(f, false); - PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false); - - PS_ASSERT_VECTOR_NON_NULL(x, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); - PS_ASSERT_VECTOR_TYPE(x, f->type.type, false); - - PS_ASSERT_VECTOR_NON_NULL(y, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); - PS_ASSERT_VECTOR_TYPE(y, f->type.type, false); - - PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, false); - PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); - - if (fErr != NULL) { - PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, false); - PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, false); - } - - // the user supplies one of various stats option pairs, - // determine the desired mean and stdev STATS options: - // XXX enforce consistency? - // XXX psStatsGetValue() probably has inverted precedence - psStatsOptions meanOption = stats->options & (PS_STAT_SAMPLE_MEAN | PS_STAT_SAMPLE_MEDIAN | PS_STAT_ROBUST_MEDIAN | PS_STAT_CLIPPED_MEAN | PS_STAT_FITTED_MEAN | PS_STAT_FITTED_MEAN); - psStatsOptions stdevOption = stats->options & (PS_STAT_SAMPLE_STDEV | PS_STAT_ROBUST_STDEV | PS_STAT_CLIPPED_STDEV | PS_STAT_FITTED_STDEV | PS_STAT_FITTED_STDEV); - if (!meanOption) { - psError(PS_ERR_UNKNOWN, true, "no valid mean stats option selected"); - return false; - } - if (!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 *resid = psVectorAlloc(f->n, PS_TYPE_F64); - - psTrace("psLib.math", 4, "stats->clipIter is %d\n", stats->clipIter); - psTrace("psLib.math", 4, "(minClipSigma, maxClipSigma) is (%.2f, %.2f)\n", minClipSigma, maxClipSigma); - - 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") >= 7) { - if (mask != NULL) { - for (psS32 i = 0 ; i < mask->n ; i++) { - psTrace("psLib.math", 7, "mask[%d] is %d\n", i, mask->data.PS_TYPE_VECTOR_MASK_DATA[i]); - } - } - } - - if (!psVectorFitPolynomial2D(poly, mask, maskValue, f, fErr, x, y)) { - psError(PS_ERR_UNKNOWN, false, "Could not fit a polynomial to the data. Returning false.\n"); - psFree(resid); - return false; - } - - psVector *fit = psPolynomial2DEvalVector(poly, x, y); - if (fit == NULL) { - psError(PS_ERR_UNKNOWN, false, "Could not call psPolynomial3DEvalVector(). Returning NULL.\n"); - 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") >= 7) { - 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", 7, "point %d at %f %f : value, fit : %f %f resid: %f\n", - i, x->data.F32[i], y->data.F32[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 NULL.\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 (fit->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++; - } - psTrace("psLib.math", 4, "keeping %d of %ld pts for fit\n", Nkeep, x->n); - stats->clippedNvalues = Nkeep; - psFree(fit); - } - // Free local temporary variables - psFree(resid); - - psTrace("psLib.math", 3, "---- %s() end ----\n", __func__); - return true; -} - - -/****************************************************************************** - ****************************************************************************** - 3-D Vector Code. - ****************************************************************************** - *****************************************************************************/ - -/****************************************************************************** -VectorFitPolynomial3DOrd(myPoly, *mask, maskValue, *f, *fErr, *x, *y, *z): -This is a private routine which will fit a 3-D polynomial to a set of (x, -y, z)-(f) pairs. All non-NULL vectors must be of type PS_TYPE_F64. - - *****************************************************************************/ -static bool VectorFitPolynomial3DOrd( - psPolynomial3D* myPoly, - const psVector* mask, - psVectorMaskType maskValue, - const psVector *f, - const psVector *fErr, - const psVector *x, - const psVector *y, - const psVector *z) -{ - psTrace("psLib.math", 4, "---- %s() begin ----\n", __func__); - PS_ASSERT_POLY_NON_NULL(myPoly, false); - PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, false); - PS_ASSERT_INT_NONNEGATIVE(myPoly->nY, false); - PS_ASSERT_INT_NONNEGATIVE(myPoly->nZ, false); - - PS_ASSERT_VECTOR_NON_NULL(f, false); - PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, false); - if (fErr != NULL) { - PS_ASSERT_VECTORS_SIZE_EQUAL(y, fErr, false); - PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F64, false); - } - PS_ASSERT_VECTOR_NON_NULL(x, false); - PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); - PS_ASSERT_VECTOR_NON_NULL(y, false); - PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); - PS_ASSERT_VECTOR_NON_NULL(z, false); - PS_ASSERT_VECTOR_TYPE(z, PS_TYPE_F64, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, false); - if (mask != NULL) { - PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, false); - PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); - } - - int nXterm = 1 + myPoly->nX; // Number of x terms - int nYterm = 1 + myPoly->nY; // Number of y terms - int nZterm = 1 + myPoly->nZ; // Number of z terms - int nTerm = nXterm * nYterm * nZterm; // Total number of terms - int nData = x->n; // Number of data points - psImage *A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); // Least-squares matrix - psVector *B = psVectorAlloc(nTerm, PS_TYPE_F64); // Least-squares vector - - // Initialize data structures. - if (!psImageInit(A, 0.0) || !psVectorInit(B, 0.0)) { - psError(PS_ERR_UNKNOWN, false, "Could initialize data structures A, B. Returning NULL.\n"); - psFree(A); - psFree(B); - psTrace("psLib.math", 4, "---- %s() End ----\n", __func__); - return false; - } - - // Dereference points for speed in the loop - psF64 **matrix = A->data.F64; // Least-squares matrix - psF64 *vector = B->data.F64; // Least-squares vector - psF64 *xData = x->data.F64; // x - psF64 *yData = y->data.F64; // y - psF64 *zData = z->data.F64; // z - psF64 *fData = f->data.F64; // f - psF64 *fErrData = NULL; // Error in f - if (fErr) { - fErrData = fErr->data.F64; - } - psVectorMaskType *dataMask = NULL; // Mask for data - if (mask) { - dataMask = mask->data.PS_TYPE_VECTOR_MASK_DATA; - } - psMaskType ***coeffMask = myPoly->coeffMask; // Mask for polynomial terms - int nYZterm = nYterm * nZterm; // Multiplication of the numbers, to calculate the index - - // Build the B and A data structs. - psF64 ***Sums = NULL; // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)*y, ... - for (int k = 0; k < nData; k++) { - if (dataMask && dataMask[k] & maskValue) { - continue; - } - - Sums = BuildSums3D(Sums, xData[k], yData[k], zData[k], nXterm, nYterm, nZterm); - - double wt; - if (fErr == NULL) { - wt = 1.0; - } else { - // this filters fErr == 0 values - wt = (fErr->data.F64[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErrData[k]); - } - - for (int i = 0; i < nTerm; i++) { - int ix = i / nYZterm; // x index - int iy = (i % nYZterm) / nZterm; // y index - int iz = (i % nYZterm) % nZterm; // z index - if (coeffMask[ix][iy][iz] & PS_POLY_MASK_BOTH) { - matrix[i][i] = 1.0; - continue; - } - - vector[i] += fData[k] * Sums[ix][iy][iz] * wt; - matrix[i][i] += Sums[2*ix][2*iy][2*iz] * wt; - for (int j = i + 1; j < nTerm; j++) { - int jx = j / (nYZterm); // x index - int jy = (j % nYZterm) / nZterm; // y index - int jz = (j % nYZterm) % nZterm; // z index - if (coeffMask[jx][jy][jz] & PS_POLY_MASK_BOTH) { - continue; - } - double value = Sums[ix+jx][iy+jy][iz+jz] * wt; - matrix[i][j] += value; - matrix[j][i] += value; - } - } - } - - // Free the sums - for (psS32 ix = 0; ix < 2*nXterm; ix++) { - for (psS32 iy = 0; iy < 2*nYterm; iy++) { - psFree(Sums[ix][iy]); - } - psFree(Sums[ix]); - } - psFree(Sums); - - - bool status = false; - if (USE_GAUSS_JORDAN) { - status = psMatrixGJSolve(A, B); - } else { - status = psMatrixLUSolve(A, B); - } - if (!status) { - psError(PS_ERR_UNKNOWN, false, "Could not solve linear equations.\n"); - goto escape; - } - - // select the appropriate solution entries - for (int i = 0; i < nTerm; i++) { - int ix = i / nYZterm; // x index - int iy = (i % nYZterm) / nZterm; // y index - int iz = (i % nYZterm) % nZterm; // z index - if (coeffMask[ix][iy][iz] & PS_POLY_MASK_FIT) continue; - myPoly->coeff[ix][iy][iz] = B->data.F64[i]; - myPoly->coeffErr[ix][iy][iz] = sqrt(A->data.F64[i][i]); - } - psFree(A); - psFree(B); - return true; - -escape: - psFree(A); - psFree(B); - return false; -} - -/****************************************************************************** -psVectorFitPolynomial3D(): This routine fits a 3D polynomial of arbitrary -degree (specified in poly) to the data points (x, y, z)-(f) and returns that -polynomial. Types F32 and F64 are supported, however, type F32 is done via -vector conversion only. - *****************************************************************************/ -bool psVectorFitPolynomial3D( - psPolynomial3D *poly, - const psVector *mask, - psVectorMaskType maskValue, - const psVector *f, - const psVector *fErr, - const psVector *x, - const psVector *y, - const psVector *z) -{ - PS_ASSERT_POLY_NON_NULL(poly, false); - PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, false); - - PS_ASSERT_VECTOR_NON_NULL(f, false); - PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false); - PS_ASSERT_VECTOR_NON_NULL(x, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); - PS_ASSERT_VECTOR_NON_NULL(y, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); - PS_ASSERT_VECTOR_NON_NULL(z, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, false); - if (mask != NULL) { - PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, false); - PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); - } - if (fErr != NULL) { - PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, false); - PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, false); - } - - // Convert input vectors to F64 if necessary. - psVector *f64 = (f->type.type == PS_TYPE_F64) ? (psVector *) f : psVectorCopy(NULL, f, PS_TYPE_F64); - psVector *x64 = (x->type.type == PS_TYPE_F64) ? (psVector *) x : psVectorCopy(NULL, x, PS_TYPE_F64); - psVector *y64 = (y->type.type == PS_TYPE_F64) ? (psVector *) y : psVectorCopy(NULL, y, PS_TYPE_F64); - psVector *z64 = (z->type.type == PS_TYPE_F64) ? (psVector *) z : psVectorCopy(NULL, z, PS_TYPE_F64); - - psVector *fErr64 = NULL; - if (fErr != NULL) { - fErr64 = (fErr->type.type == PS_TYPE_F64) ? (psVector *) fErr : psVectorCopy(NULL, fErr, PS_TYPE_F64); - } - - bool result = true; - - switch (poly->type) { - case PS_POLYNOMIAL_ORD: - result = VectorFitPolynomial3DOrd(poly, mask, maskValue, f64, fErr64, x64, y64, z64); - if (!result) { - psError(PS_ERR_UNKNOWN, true, "Could not fit polynomial. Returning NULL.\n"); - } - break; - case PS_POLYNOMIAL_CHEB: - if (mask != NULL) { - psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring mask and maskValue with Chebyshev polynomials.\n"); - } - psError(PS_ERR_UNKNOWN, true, "3-D Chebyshev polynomial vector fitting has not been implemented. Returning NULL.\n"); - result = false; - break; - default: - psError(PS_ERR_UNKNOWN, true, "Incorrect polynomial type. Returning NULL.\n"); - result = false; - break; - } - - // Free psVectors that were created for NULL arguments. - PS_FREE_TEMP_F64_VECTOR (f, f64); - PS_FREE_TEMP_F64_VECTOR (x, x64); - PS_FREE_TEMP_F64_VECTOR (y, y64); - PS_FREE_TEMP_F64_VECTOR (z, z64); - PS_FREE_TEMP_F64_VECTOR (fErr, fErr64); - - return result; -} - -bool psVectorClipFitPolynomial3D( - psPolynomial3D *poly, - psStats *stats, - const psVector *mask, - psVectorMaskType maskValue, - const psVector *f, - const psVector *fErr, - const psVector *x, - const psVector *y, - const psVector *z) -{ - psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__); - PS_ASSERT_POLY_NON_NULL(poly, false); - PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, false); - PS_ASSERT_PTR_NON_NULL(stats, false); - PS_ASSERT_VECTOR_NON_NULL(mask, false); - PS_ASSERT_VECTOR_NON_NULL(f, false); - PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false); - - PS_ASSERT_VECTOR_NON_NULL(x, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); - PS_ASSERT_VECTOR_TYPE(x, f->type.type, false); - - PS_ASSERT_VECTOR_NON_NULL(y, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); - PS_ASSERT_VECTOR_TYPE(y, f->type.type, false); - - PS_ASSERT_VECTOR_NON_NULL(z, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, false); - PS_ASSERT_VECTOR_TYPE(z, f->type.type, false); - - PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, false); - PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); - - if (fErr != NULL) { - PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, false); - PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, false); - } - - // the user supplies one of various stats option pairs, - // determine the desired mean and stdev STATS options: - // XXX enforce consistency? - // XXX psStatsGetValue() probably has inverted precedence - psStatsOptions meanOption = stats->options & (PS_STAT_SAMPLE_MEAN | PS_STAT_SAMPLE_MEDIAN | PS_STAT_ROBUST_MEDIAN | PS_STAT_CLIPPED_MEAN | PS_STAT_FITTED_MEAN | PS_STAT_FITTED_MEAN); - psStatsOptions stdevOption = stats->options & (PS_STAT_SAMPLE_STDEV | PS_STAT_ROBUST_STDEV | PS_STAT_CLIPPED_STDEV | PS_STAT_FITTED_STDEV | PS_STAT_FITTED_STDEV); - if (!meanOption) { - psError(PS_ERR_UNKNOWN, true, "no valid mean stats option selected"); - return false; - } - if (!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 *resid = psVectorAlloc(f->n, PS_TYPE_F64); - - psTrace("psLib.math", 4, "stats->clipIter is %d\n", stats->clipIter); - psTrace("psLib.math", 4, "(minClipSigma, maxClipSigma) is (%.2f, %.2f)\n", minClipSigma, maxClipSigma); - - 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 (!psVectorFitPolynomial3D(poly, mask, maskValue, f, fErr, x, y, z)) { - psError(PS_ERR_UNKNOWN, false, "Could not fit a polynomial to the data. Returning NULL.\n"); - psFree(resid); - return false; - } - psVector *fit = psPolynomial3DEvalVector(poly, x, y, z); - if (fit == NULL) { - psError(PS_ERR_UNKNOWN, false, "Could not call psPolynomial3DEvalVector(). Returning NULL.\n"); - 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.F64[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 NULL.\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++; - } - psTrace("psLib.math", 6, "keeping %d of %ld pts for fit\n", Nkeep, x->n); - stats->clippedNvalues = Nkeep; - psFree(fit); - } - // Free local temporary variables - psFree(resid); - - psTrace("psLib.math", 3, "---- %s() end ----\n", __func__); - return true; -} - -/****************************************************************************** - ****************************************************************************** - 4-D Vector Code. - ****************************************************************************** - *****************************************************************************/ -/****************************************************************************** -VectorFitPolynomial4DOrd(myPoly, *mask, maskValue, *f, *fErr, *x, *y, *z, *t): -This is a private routine which will fit a 4-D polynomial to a set of (x, -y, z, t)-(f) pairs. All non-NULL vectors must be of type PS_TYPE_F64. - - *****************************************************************************/ -static bool VectorFitPolynomial4DOrd( - psPolynomial4D* myPoly, - const psVector* mask, - psVectorMaskType maskValue, - const psVector *f, - const psVector *fErr, - const psVector *x, - const psVector *y, - const psVector *z, - const psVector *t) -{ - psTrace("psLib.math", 4, "---- %s() begin ----\n", __func__); - PS_ASSERT_POLY_NON_NULL(myPoly, false); - PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, false); - PS_ASSERT_INT_NONNEGATIVE(myPoly->nY, false); - PS_ASSERT_INT_NONNEGATIVE(myPoly->nZ, false); - PS_ASSERT_INT_NONNEGATIVE(myPoly->nT, false); - PS_ASSERT_VECTOR_NON_NULL(f, false); - PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, false); - if (fErr != NULL) { - PS_ASSERT_VECTORS_SIZE_EQUAL(y, fErr, false); - PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F64, false); - } - PS_ASSERT_VECTOR_NON_NULL(x, false); - PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); - PS_ASSERT_VECTOR_NON_NULL(y, false); - PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); - PS_ASSERT_VECTOR_NON_NULL(z, false); - PS_ASSERT_VECTOR_TYPE(z, PS_TYPE_F64, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, false); - PS_ASSERT_VECTOR_NON_NULL(t, false); - PS_ASSERT_VECTOR_TYPE(t, PS_TYPE_F64, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, t, false); - if (mask) { - PS_ASSERT_VECTORS_SIZE_EQUAL(y, mask, false); - PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); - } - - - int nXterm = 1 + myPoly->nX; // Number of x terms - int nYterm = 1 + myPoly->nY; // Number of y terms - int nZterm = 1 + myPoly->nZ; // Number of z terms - int nTterm = 1 + myPoly->nT; // Number of t terms - int nTerm = nXterm * nYterm * nZterm * nTterm; // Total number of terms - int nData = x->n; // Number of data points - psImage *A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); // Least-squares matrix - psVector *B = psVectorAlloc(nTerm, PS_TYPE_F64); // Least-squares vector - - // Initialize data structures. - if (!psImageInit(A, 0.0) || !psVectorInit(B, 0.0)) { - psError(PS_ERR_UNKNOWN, false, "Could initialize data structures A, B. Returning NULL.\n"); - psFree(A); - psFree(B); - psTrace("psLib.math", 4, "---- %s() End ----\n", __func__); - return false; - } - - // Dereference points for speed in the loop - psF64 **matrix = A->data.F64; // Least-squares matrix - psF64 *vector = B->data.F64; // Least-squares vector - psF64 *xData = x->data.F64; // x - psF64 *yData = y->data.F64; // y - psF64 *zData = z->data.F64; // z - psF64 *tData = t->data.F64; // t - psF64 *fData = f->data.F64; // f - psF64 *fErrData = NULL; // Error in f - if (fErr) { - fErrData = fErr->data.F64; - } - psVectorMaskType *dataMask = NULL; // Mask for data - if (mask) { - dataMask = mask->data.PS_TYPE_VECTOR_MASK_DATA; - } - psMaskType ****coeffMask = myPoly->coeffMask; // Mask for polynomial terms - int nYZTterm = nYterm * nZterm * nTterm; // Multiplication of the numbers, for calculating the index - int nZTterm = nZterm * nTterm; // Multiplication of the numbers, for calculating the index - - // Build the B and A data structs. - psF64 ****Sums = NULL; // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)*y, ... - for (int k = 0; k < nData; k++) { - if (dataMask && dataMask[k] & maskValue) { - continue; - } - - Sums = BuildSums4D(Sums, xData[k], yData[k], zData[k], tData[k], nXterm, nYterm, nZterm, nTterm); - - double wt; - if (fErr == NULL) { - wt = 1.0; - } else { - // this filters fErr == 0 values - wt = (fErr->data.F64[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErrData[k]); - } - - for (int i = 0; i < nTerm; i++) { - int ix = i / (nYZTterm); // x index - int iy = (i % (nYZTterm)) / (nZTterm); // y index - int iz = ((i % (nYZTterm)) % (nZTterm)) / nTterm; // z index - int it = ((i % (nYZTterm)) % (nZTterm)) % nTterm; // t index - if (coeffMask[ix][iy][iz][it] & PS_POLY_MASK_BOTH) { - matrix[i][i] = 1.0; - continue; - } - - vector[i] += fData[k] * Sums[ix][iy][iz][it] * wt; - matrix[i][i] += Sums[2*ix][2*iy][2*iz][2*it] * wt; - for (int j = i + 1; j < nTerm; j++) { - int jx = j / nYZTterm; // x index - int jy = (j % nYZTterm) / nZTterm; // y index - int jz = ((j % nYZTterm) % nZTterm) / nTterm; // z index - int jt = ((j % nYZTterm) % nZTterm) % nTterm; // t index - if (coeffMask[jx][jy][jz][jt] & PS_POLY_MASK_BOTH) { - continue; - } - double value = Sums[ix+jx][iy+jy][iz+jz][it+jt] * wt; - matrix[i][j] += value; - matrix[j][i] += value; - } - } - } - - // Free the sums - if (Sums == NULL) { - assert (nData == 0); - } else { - for (int ix = 0; ix < 2*nXterm; ix++) { - for (int iy = 0; iy < 2*nYterm; iy++) { - for (int iz = 0; iz < 2*nZterm; iz++) { - psFree(Sums[ix][iy][iz]); - } - psFree(Sums[ix][iy]); - } - psFree(Sums[ix]); - } - psFree(Sums); - } - - bool status = false; - if (USE_GAUSS_JORDAN) { - status = psMatrixGJSolve(A, B); - } else { - status = psMatrixLUSolve(A, B); - } - if (!status) { - psError(PS_ERR_UNKNOWN, false, "Could not solve linear equations.\n"); - goto escape; - } - - // select the appropriate solution entries - for (int i = 0; i < nTerm; i++) { - int ix = i / nYZTterm; // x index - int iy = (i % nYZTterm) / nZTterm; // y index - int iz = ((i % nYZTterm) % nZTterm) / nTterm; // z index - int it = ((i % nYZTterm) % nZTterm) % nTterm; // t index - if (coeffMask[ix][iy][iz][it] & PS_POLY_MASK_FIT) continue; - myPoly->coeff[ix][iy][iz][it] = B->data.F64[i]; - myPoly->coeffErr[ix][iy][iz][it] = sqrt(A->data.F64[i][i]); - } - psFree(A); - psFree(B); - return true; - -escape: - psFree(A); - psFree(B); - return false; -} - -/****************************************************************************** -psVectorFitPolynomial4D(): This routine fits a 4D polynomial of arbitrary -degree (specified in poly) to the data points (x, y, z, t)-(f) and returns -that polynomial. Types F32 and F64 are supported, however, type F32 is done -via vector conversion only. - *****************************************************************************/ -bool psVectorFitPolynomial4D( - psPolynomial4D *poly, - const psVector *mask, - psVectorMaskType maskValue, - const psVector *f, - const psVector *fErr, - const psVector *x, - const psVector *y, - const psVector *z, - const psVector *t) -{ - PS_ASSERT_POLY_NON_NULL(poly, false); - PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, false); - - PS_ASSERT_VECTOR_NON_NULL(f, false); - PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false); - PS_ASSERT_VECTOR_NON_NULL(x, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); - PS_ASSERT_VECTOR_NON_NULL(y, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); - PS_ASSERT_VECTOR_NON_NULL(z, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, false); - PS_ASSERT_VECTOR_NON_NULL(t, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, t, false); - if (mask) { - PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, false); - PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); - } - if (fErr != NULL) { - PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, false); - PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, false); - } - - // Convert input vectors to F64 if necessary. - psVector *f64 = (f->type.type == PS_TYPE_F64) ? (psVector *) f : psVectorCopy(NULL, f, PS_TYPE_F64); - psVector *x64 = (x->type.type == PS_TYPE_F64) ? (psVector *) x : psVectorCopy(NULL, x, PS_TYPE_F64); - psVector *y64 = (y->type.type == PS_TYPE_F64) ? (psVector *) y : psVectorCopy(NULL, y, PS_TYPE_F64); - psVector *z64 = (z->type.type == PS_TYPE_F64) ? (psVector *) z : psVectorCopy(NULL, z, PS_TYPE_F64); - psVector *t64 = (t->type.type == PS_TYPE_F64) ? (psVector *) t : psVectorCopy(NULL, t, PS_TYPE_F64); - - psVector *fErr64 = NULL; - if (fErr != NULL) { - fErr64 = (fErr->type.type == PS_TYPE_F64) ? (psVector *) fErr : psVectorCopy(NULL, fErr, PS_TYPE_F64); - } - - bool result = true; - - switch (poly->type) { - case PS_POLYNOMIAL_ORD: - result = VectorFitPolynomial4DOrd(poly, mask, maskValue, f64, fErr64, x64, y64, z64, t64); - if (!result) { - psError(PS_ERR_UNKNOWN, true, "Could not fit polynomial. Returning NULL.\n"); - } - break; - case PS_POLYNOMIAL_CHEB: - if (mask != NULL) { - psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring mask and maskValue with Chebyshev polynomials.\n"); - } - psError(PS_ERR_UNKNOWN, true, "4-D Chebyshev polynomial vector fitting has not been implemented. Returning NULL.\n"); - result = false; - break; - default: - psError(PS_ERR_UNKNOWN, true, "Incorrect polynomial type. Returning NULL.\n"); - result = false; - break; - } - - // Free psVectors that were created for NULL arguments. - PS_FREE_TEMP_F64_VECTOR (f, f64); - PS_FREE_TEMP_F64_VECTOR (x, x64); - PS_FREE_TEMP_F64_VECTOR (y, y64); - PS_FREE_TEMP_F64_VECTOR (z, z64); - PS_FREE_TEMP_F64_VECTOR (t, t64); - PS_FREE_TEMP_F64_VECTOR (fErr, fErr64); - - return result; -} - - -bool psVectorClipFitPolynomial4D( - psPolynomial4D *poly, - psStats *stats, - const psVector *mask, - psVectorMaskType maskValue, - const psVector *f, - const psVector *fErr, - const psVector *x, - const psVector *y, - const psVector *z, - const psVector *t) -{ - psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__); - PS_ASSERT_POLY_NON_NULL(poly, false); - PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, false); - PS_ASSERT_PTR_NON_NULL(stats, false); - PS_ASSERT_VECTOR_NON_NULL(mask, false); - PS_ASSERT_VECTOR_NON_NULL(f, false); - PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, false); - - PS_ASSERT_VECTOR_NON_NULL(x, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, false); - PS_ASSERT_VECTOR_TYPE(x, f->type.type, false); - - PS_ASSERT_VECTOR_NON_NULL(y, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, false); - PS_ASSERT_VECTOR_TYPE(y, f->type.type, false); - - PS_ASSERT_VECTOR_NON_NULL(z, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, false); - PS_ASSERT_VECTOR_TYPE(z, f->type.type, false); - - PS_ASSERT_VECTOR_NON_NULL(t, false); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, t, false); - PS_ASSERT_VECTOR_TYPE(t, f->type.type, false); - - PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, false); - PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_VECTOR_MASK, false); - - if (fErr != NULL) { - PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, false); - PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, false); - } - - // the user supplies one of various stats option pairs, - // determine the desired mean and stdev STATS options: - // XXX enforce consistency? - // XXX psStatsGetValue() probably has inverted precedence - psStatsOptions meanOption = stats->options & (PS_STAT_SAMPLE_MEAN | PS_STAT_SAMPLE_MEDIAN | PS_STAT_ROBUST_MEDIAN | PS_STAT_CLIPPED_MEAN | PS_STAT_FITTED_MEAN | PS_STAT_FITTED_MEAN); - psStatsOptions stdevOption = stats->options & (PS_STAT_SAMPLE_STDEV | PS_STAT_ROBUST_STDEV | PS_STAT_CLIPPED_STDEV | PS_STAT_FITTED_STDEV | PS_STAT_FITTED_STDEV); - if (!meanOption) { - psError(PS_ERR_UNKNOWN, true, "no valid mean stats option selected"); - return false; - } - if (!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 *resid = psVectorAlloc(f->n, PS_TYPE_F64); - - psTrace("psLib.math", 4, "stats->clipIter is %d\n", stats->clipIter); - psTrace("psLib.math", 4, "(minClipSigma, maxClipSigma) is (%.2f, %.2f)\n", minClipSigma, maxClipSigma); - - for (psS32 N = 0; N < stats->clipIter; N++) { - psTrace("psLib.math", 6, "Loop iteration %d. Calling psVectorFitPolynomial4D()\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 (!psVectorFitPolynomial4D (poly, mask, maskValue, f, fErr, x, y, z, t)) { - psError(PS_ERR_UNKNOWN, false, "Could not fit a polynomial to the data. Returning NULL.\n"); - psFree(resid); - return false; - } - - psVector *fit = psPolynomial4DEvalVector (poly, x, y, z, t); - if (fit == NULL) { - psError(PS_ERR_UNKNOWN, false, "Could not call psPolynomial4DEvalVector(). Returning NULL.\n"); - 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.F64[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 NULL.\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++; - } - psTrace("psLib.math", 6, "keeping %d of %ld pts for fit\n", Nkeep, x->n); - stats->clippedNvalues = Nkeep; - psFree (fit); - } - // Free local temporary variables - psFree (resid); - - psTrace("psLib.math", 3, "---- %s() end ----\n", __func__); - return true; -} Index: /branches/eam_branches/ipp-20230313/psLib/src/math/psMinimizePolyFit.h =================================================================== --- /branches/eam_branches/ipp-20230313/psLib/src/math/psMinimizePolyFit.h (revision 42505) +++ /branches/eam_branches/ipp-20230313/psLib/src/math/psMinimizePolyFit.h (revision 42506) @@ -144,4 +144,40 @@ ); +bool psVectorIRLSFitPolynomial2D( + psPolynomial2D *poly, + psStats *stats, + const psVector *mask, + psVectorMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y +); + +bool psVectorIRLSFitPolynomial3D( + psPolynomial3D *poly, + psStats *stats, + const psVector *mask, + psVectorMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z +); + +bool psVectorIRLSFitPolynomial4D( + psPolynomial4D *poly, + psStats *stats, + const psVector *mask, + psVectorMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z, + const psVector *t +); + /// @} #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 42505) +++ /branches/eam_branches/ipp-20230313/psLib/src/math/psPolynomial.c (revision 42506) @@ -944,13 +944,11 @@ bool match = true; match &= (poly->type == type); - match &= (poly->nX == type); + match &= (poly->nX == nX); if (!match) { - psFree (poly->coeff); - psFree (poly->coeffErr); - psFree (poly->coeffMask); + polynomial1DFree (poly); // frees the coeffs poly->type = type; - poly->nX = nX; + poly->nX = nX; poly->coeff = psAlloc((1 + nX) * sizeof(psF64)); @@ -976,20 +974,13 @@ bool match = true; match &= (poly->type == type); - match &= (poly->nX == type); - match &= (poly->nY == type); + match &= (poly->nX == nX); + match &= (poly->nY == nY); 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); + polynomial2DFree (poly); // frees the coeffs poly->type = type; - poly->nX = nX; - poly->nY = nY; + poly->nX = nX; + poly->nY = nY; poly->coeff = psAlloc((1 + nX) * sizeof(psF64 *)); @@ -1012,12 +1003,123 @@ } -// XXX 3D, 4D versions +bool psPolynomial3DRecycle(psPolynomial3D *poly, + psPolynomialType type, + unsigned int nX, + unsigned int nY, + unsigned int nZ) +{ + PS_ASSERT_INT_NONNEGATIVE(nX, NULL); + PS_ASSERT_INT_NONNEGATIVE(nY, NULL); + PS_ASSERT_INT_NONNEGATIVE(nZ, NULL); + + bool match = true; + match &= (poly->type == type); + match &= (poly->nX == nX); + match &= (poly->nY == nY); + match &= (poly->nZ == nZ); + + if (!match) { + polynomial3DFree (poly); // frees the coeffs + + poly->type = type; + poly->nX = nX; + poly->nY = nY; + poly->nZ = nZ; + + poly->coeff = psAlloc((nX + 1) * sizeof(psF64 **)); + poly->coeffErr = psAlloc((nX + 1) * sizeof(psF64 **)); + poly->coeffMask = (psMaskType ***)psAlloc((nX + 1) * sizeof(psMaskType **)); + for (int ix = 0; ix < (1 + nX); ix++) { + poly->coeff[ix] = psAlloc((nY + 1) * sizeof(psF64 *)); + poly->coeffErr[ix] = psAlloc((nY + 1) * sizeof(psF64 *)); + poly->coeffMask[ix] = (psMaskType **)psAlloc((nY + 1) * sizeof(psMaskType *)); + for (int iy = 0; iy < (nY + 1); iy++) { + poly->coeff[ix][iy] = psAlloc((nZ + 1) * sizeof(psF64)); + poly->coeffErr[ix][iy] = psAlloc((nZ + 1) * sizeof(psF64)); + poly->coeffMask[ix][iy] = (psMaskType *)psAlloc((nZ + 1) * sizeof(psMaskType)); + } + } + } + for (int ix = 0; ix < (1 + nX); ix++) { + for (int iy = 0; iy < (1 + nY); iy++) { + for (int iz = 0; iz < (1 + nZ); iz++) { + poly->coeff[ix][iy][iz] = 0.0; + poly->coeffErr[ix][iy][iz] = 0.0; + poly->coeffMask[ix][iy][iz] = PS_POLY_MASK_NONE; + } + } + } + return(true); +} + +bool psPolynomial4DRecycle(psPolynomial4D *poly, + psPolynomialType type, + unsigned int nX, + unsigned int nY, + unsigned int nZ, + unsigned int nT) +{ + PS_ASSERT_INT_NONNEGATIVE(nX, NULL); + PS_ASSERT_INT_NONNEGATIVE(nY, NULL); + PS_ASSERT_INT_NONNEGATIVE(nZ, NULL); + PS_ASSERT_INT_NONNEGATIVE(nT, NULL); + + bool match = true; + match &= (poly->type == type); + match &= (poly->nX == nX); + match &= (poly->nY == nY); + match &= (poly->nZ == nZ); + match &= (poly->nT == nT); + + if (!match) { + polynomial4DFree (poly); // frees the coeffs + + poly->type = type; + poly->nX = nX; + poly->nY = nY; + poly->nZ = nZ; + poly->nT = nT; + + poly->coeff = psAlloc((nX + 1) * sizeof(psF64 ***)); + poly->coeffErr = psAlloc((nX + 1) * sizeof(psF64 ***)); + poly->coeffMask = (psMaskType ****)psAlloc((nX + 1) * sizeof(psMaskType ***)); + for (int ix = 0; ix < (nX + 1); ix++) { + poly->coeff[ix] = psAlloc((nY + 1) * sizeof(psF64 **)); + poly->coeffErr[ix] = psAlloc((nY + 1) * sizeof(psF64 **)); + poly->coeffMask[ix] = (psMaskType ***)psAlloc((nY + 1) * sizeof(psMaskType **)); + for (int iy = 0; iy < (nY + 1); iy++) { + poly->coeff[ix][iy] = psAlloc((nZ + 1) * sizeof(psF64 *)); + poly->coeffErr[ix][iy] = psAlloc((nZ + 1) * sizeof(psF64 *)); + poly->coeffMask[ix][iy] = (psMaskType **)psAlloc((nZ + 1) * sizeof(psMaskType *)); + for (int iz = 0; iz < (nZ + 1); iz++) { + poly->coeff[ix][iy][iz] = psAlloc((nT + 1) * sizeof(psF64)); + poly->coeffErr[ix][iy][iz] = psAlloc((nT + 1) * sizeof(psF64)); + poly->coeffMask[ix][iy][iz] = (psMaskType *)psAlloc((nT + 1) * sizeof(psMaskType)); + } + } + } + } + for (int ix = 0; ix < (1 + nX); ix++) { + for (int iy = 0; iy < (1 + nY); iy++) { + for (int iz = 0; iz < (1 + nZ); iz++) { + for (int it = 0; it < (1 + nT); it++) { + poly->coeff[ix][iy][iz][it] = 0.0; + poly->coeffErr[ix][iy][iz][it] = 0.0; + poly->coeffMask[ix][iy][iz][it] = PS_POLY_MASK_NONE; + } + } + } + } + return(true); +} + +// ######## Copy polynomials ######## psPolynomial1D *psPolynomial1DCopy(psPolynomial1D *out, - psPolynomial1D *poly) + psPolynomial1D *poly) { if (out == NULL) { - out = psPolynomial1DAlloc (poly->type, poly->nX); + out = psPolynomial1DAlloc (poly->type, poly->nX); } else { - psPolynomial1DRecycle (out, poly->type, poly->nX); + psPolynomial1DRecycle (out, poly->type, poly->nX); } @@ -1030,18 +1132,60 @@ } psPolynomial2D *psPolynomial2DCopy(psPolynomial2D *out, - psPolynomial2D *poly) + psPolynomial2D *poly) { if (out == NULL) { - out = psPolynomial2DAlloc (poly->type, poly->nX, poly->nY); + out = psPolynomial2DAlloc (poly->type, poly->nX, poly->nY); } else { - psPolynomial2DRecycle (out, poly->type, poly->nX, poly->nY); + 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]; - } + 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 *psPolynomial3DCopy(psPolynomial3D *out, + psPolynomial3D *poly) +{ + if (out == NULL) { + out = psPolynomial3DAlloc (poly->type, poly->nX, poly->nY, poly->nZ); + } else { + psPolynomial3DRecycle (out, poly->type, poly->nX, poly->nY, poly->nZ); + } + + for (int ix = 0; ix < (1 + poly->nX); ix++) { + for (int iy = 0; iy < (1 + poly->nY); iy++) { + for (int iz = 0; iz < (1 + poly->nZ); iz++) { + out->coeff[ix][iy][iz] = poly->coeff[ix][iy][iz]; + out->coeffErr[ix][iy][iz] = poly->coeffErr[ix][iy][iz]; + out->coeffMask[ix][iy][iz] = poly->coeffMask[ix][iy][iz]; + } + } + } + return(out); +} +psPolynomial4D *psPolynomial4DCopy(psPolynomial4D *out, + psPolynomial4D *poly) +{ + if (out == NULL) { + out = psPolynomial4DAlloc (poly->type, poly->nX, poly->nY, poly->nZ, poly->nT); + } else { + psPolynomial4DRecycle (out, poly->type, poly->nX, poly->nY, poly->nZ, poly->nT); + } + + for (int ix = 0; ix < (1 + poly->nX); ix++) { + for (int iy = 0; iy < (1 + poly->nY); iy++) { + for (int iz = 0; iz < (1 + poly->nZ); iz++) { + for (int it = 0; it < (1 + poly->nT); it++) { + out->coeff[ix][iy][iz][it] = poly->coeff[ix][iy][iz][it]; + out->coeffErr[ix][iy][iz][it] = poly->coeffErr[ix][iy][iz][it]; + out->coeffMask[ix][iy][iz][it] = poly->coeffMask[ix][iy][iz][it]; + } + } + } } return(out); @@ -1080,5 +1224,5 @@ switch (x->type.type) { - case PS_TYPE_F64: + case PS_TYPE_F64: tmp = psVectorAlloc(x->n, PS_TYPE_F64); for (unsigned int i=0;in;i++) { @@ -1086,5 +1230,5 @@ } break; - case PS_TYPE_F32: + case PS_TYPE_F32: tmp = psVectorAlloc(x->n, PS_TYPE_F32); for (unsigned int i=0;in;i++) { @@ -1092,5 +1236,5 @@ } break; - default: + default: psError(PS_ERR_UNKNOWN, false, "invalid input data type.\n"); return (NULL); Index: /branches/eam_branches/ipp-20230313/psLib/src/math/psPolynomial.h =================================================================== --- /branches/eam_branches/ipp-20230313/psLib/src/math/psPolynomial.h (revision 42505) +++ /branches/eam_branches/ipp-20230313/psLib/src/math/psPolynomial.h (revision 42506) @@ -164,15 +164,28 @@ psPolynomialType type, unsigned int nX); - bool psPolynomial2DRecycle(psPolynomial2D *poly, psPolynomialType type, unsigned int nX, unsigned int nY); +bool psPolynomial3DRecycle(psPolynomial3D *poly, + psPolynomialType type, + unsigned int nX, + unsigned int nY, + unsigned int nZ); +bool psPolynomial4DRecycle(psPolynomial4D *poly, + psPolynomialType type, + unsigned int nX, + unsigned int nY, + unsigned int nZ, + unsigned int nT); psPolynomial1D *psPolynomial1DCopy(psPolynomial1D *out, psPolynomial1D *poly); - psPolynomial2D *psPolynomial2DCopy(psPolynomial2D *out, psPolynomial2D *poly); +psPolynomial3D *psPolynomial3DCopy(psPolynomial3D *out, + psPolynomial3D *poly); +psPolynomial4D *psPolynomial4DCopy(psPolynomial4D *out, + psPolynomial4D *poly); /** Evaluates a 1-D polynomial at specific coordinates.