Index: trunk/psLib/src/math/psMinimize.c =================================================================== --- trunk/psLib/src/math/psMinimize.c (revision 5655) +++ trunk/psLib/src/math/psMinimize.c (revision 5818) @@ -10,6 +10,6 @@ * @author EAM, IfA * - * @version $Revision: 1.147 $ $Name: not supported by cvs2svn $ - * @date $Date: 2005-12-01 23:36:23 $ + * @version $Revision: 1.148 $ $Name: not supported by cvs2svn $ + * @date $Date: 2005-12-20 22:41:06 $ * * Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii @@ -113,4 +113,5 @@ } + // XXX: Check error codes! psGaussJordan(Alpha, Beta); psFree(A); @@ -1441,21 +1442,69 @@ } -// here is the definition for BuildSums4D. ifdef'ed away until it is used -// by psPolynomial4DFit.. -# if (0) - /****************************************************************************** +/****************************************************************************** +BuildSums3D(sums, x, y, z, nXterm, nYterm, nZterm): this routine calculates the powers of +input parameter "x", "y", and "z" between 0 and input parameter nXterms*2, +nYterm*2, and nZterm*2. The result is returned as a psImage sums. + *****************************************************************************/ +static psF64 ***BuildSums3D( + psF64 ***sums, + psF64 x, + psF64 y, + psF64 z, + psS32 nXterm, + psS32 nYterm, + psS32 nZterm) +{ + psS32 nXsum = 0; + psS32 nYsum = 0; + psS32 nZsum = 0; + psF64 xSum = 1.0; + psF64 ySum = 1.0; + psF64 zSum = 1.0; + + nXsum = 2*nXterm; + nYsum = 2*nYterm; + nZsum = 2*nZterm; + if (sums == NULL) { + sums = (psF64 ***) psAlloc (nXsum*sizeof(psF64)); + for (int i = 0; i < nXsum; i++) { + sums[i] = (psF64 **) psAlloc (nYsum*sizeof(psF64)); + for (int j = 0; j < nYsum; j++) { + sums[i][j] = (psF64 *) psAlloc (nZsum*sizeof(psF64)); + } + } + } + // careful with this function: there is no size checking and realloc for reuse + + zSum = 1.0; + for (int k = 0; k < nZsum; k++) { + ySum = zSum; + for (int j = 0; j < nYsum; j++) { + xSum = ySum; + for (int i = 0; i < nXsum; i++) { + sums[i][j][k] = xSum; + xSum *= x; + } + ySum *= y; + } + zSum *= z; + } + return (sums); +} + +/****************************************************************************** BuildSums4D(sums, x, y, z, t, nXterm, nYterm, nZterm, nTterm). equiv to BuildSums2D(). The result is returned as a double **** - *****************************************************************************/ - static double ****BuildSums4D( - psF64 ****sums, - psF64 x, - psF64 y, - psF64 z, - psF64 t, - psS32 nXterm, - psS32 nYterm, - psS32 nZterm, - psS32 nTterm) +*****************************************************************************/ +static psF64 ****BuildSums4D( + psF64 ****sums, + psF64 x, + psF64 y, + psF64 z, + psF64 t, + psS32 nXterm, + psS32 nYterm, + psS32 nZterm, + psS32 nTterm) { psS32 nXsum = 0; @@ -1505,5 +1554,4 @@ return (sums); } -# endif /* BuildSums4D */ /****************************************************************************** @@ -1556,7 +1604,15 @@ /****************************************************************************** -vectorFitPolynomial1DCheb(): This routine will fit a Chebyshev polynomial of -degree myPoly to the data points (x, y) and return the coefficients of that +vectorFitPolynomial1DChebSlow(): This routine will fit a Chebyshev polynomial +of degree myPoly to the data points (x, y) and return the coefficients of that polynomial. + + NOTE: We currently have implemented two algorithms. This one is + non-standard. It ignores the orthogonal properties of the Chebyshev + polys, and their known zero values. Instead, we do build a system of + linear equations based on minimizing the chi-squared for all data points + and we then solve those equations. This method is significantly slower + than the other algorithm. It was explicitly requested that we implement + this algorithm. XXX: mask, maskValue, yErr are currently ignored. @@ -1564,5 +1620,5 @@ XXX: Change arg order to that of the psLib function. *****************************************************************************/ -static psPolynomial1D *vectorFitPolynomial1DCheby( +static psPolynomial1D *vectorFitPolynomial1DChebySlow( psPolynomial1D* myPoly, const psVector *mask, @@ -1572,5 +1628,122 @@ const psVector* x) { - // XXX: these ASSERTS are redundant. + PS_ASSERT_POLY_NON_NULL(myPoly, NULL); + PS_ASSERT_INT_LARGER_THAN_OR_EQUAL(myPoly->nX, 0, NULL); + PS_ASSERT_VECTOR_NON_NULL(y, NULL); + PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL); + if (yErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(y, yErr, NULL); + PS_ASSERT_VECTOR_TYPE(yErr, PS_TYPE_F64, NULL); + } + if (x != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(y, x, NULL); + PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL); + } + psS32 NUM_POLY = myPoly->nX+1; + psS32 NUM_DATA = x->n; + psPolynomial1D **chebPolys = createChebyshevPolys(NUM_POLY); + if (0) { + for (psS32 j = 0; j < NUM_POLY; j++) { + PS_POLY_PRINT_1D(chebPolys[j]); + } + } + + // Pre-compute the various Chebyshev polys T_i(x[j]) for all x[] + psImage *tMatrix = psImageAlloc(NUM_DATA, NUM_POLY, PS_TYPE_F64); + for (psS32 p = 0 ; p < NUM_POLY ; p++) { + for (psS32 d = 0 ; d < NUM_DATA ; d++) { + tMatrix->data.F64[p][d] = psPolynomial1DEval(chebPolys[p], x->data.F64[d]); + } + } + + // Compute the A matrix + psImage *A = psImageAlloc(NUM_POLY, NUM_POLY, PS_TYPE_F64); + for (psS32 i = 0 ; i < NUM_POLY ; i++) { + for (psS32 j = 0 ; j < NUM_POLY ; j++) { + A->data.F64[i][j] = 0.0; + for (psS32 d = 0 ; d < NUM_DATA ; d++) { + A->data.F64[i][j]+= (tMatrix->data.F64[j][d] * tMatrix->data.F64[i][d]); + } + } + // This is because of the last term in: f(x) = SUM[c_i * T_i(x)] - c_0/2 + for (psS32 d = 0 ; d < NUM_DATA ; d++) { + A->data.F64[i][0] -= (tMatrix->data.F64[i][d]/2.0); + } + } + + // Compute the B vector + psVector *B = psVectorAlloc(NUM_POLY, PS_TYPE_F64); + for (psS32 i = 0 ; i < NUM_POLY ; i++) { + B->data.F64[i] = 0.0; + for (psS32 d = 0 ; d < NUM_DATA ; d++) { + B->data.F64[i] += (y->data.F64[d] * tMatrix->data.F64[i][d]); + + } + } + + // GaussJordan version + if (0) { + // does the solution in place + // XXX: Check error codes! + psGaussJordan (A, B); + + // the first nTerm entries in B correspond directly to the desired + // polynomial coefficients. this is only true for the 1D case + for (psS32 k = 0; k < NUM_POLY; k++) { + myPoly->coeff[k] = B->data.F64[k]; + } + } else { + // LUD version of the fit + psImage *ALUD = NULL; + psVector* outPerm = NULL; + psVector* coeffs = NULL; + + ALUD = psImageAlloc(NUM_POLY, NUM_POLY, PS_TYPE_F64); + ALUD = psMatrixLUD(ALUD, &outPerm, A); + coeffs = psMatrixLUSolve(coeffs, ALUD, B, outPerm); + for (psS32 k = 0; k < NUM_POLY; k++) { + myPoly->coeff[k] = coeffs->data.F64[k]; + } + + psFree(ALUD); + psFree(coeffs); + psFree(outPerm); + } + + psFree(A); + psFree(B); + psFree(tMatrix); + for (psS32 i=0;inX, NULL); @@ -1614,4 +1787,5 @@ for (psS32 i=0;idata.F64[i] = fScalar->data.F64; - psFree(fScalar); + // XXX: This is somewhat of a hack to handle cases where the x vector does + // not fully span the [-1.0:1.0] interval. We set the values of f[] to the + // values of y[] at those endpoints. + // XXX: This only works if x[] is increasing. + + if (X <= x->data.F64[0]) { + f->data.F64[i] = y->data.F64[0]; + } else if (X >= x->data.F64[x->n-1]) { + f->data.F64[i] = y->data.F64[x->n-1]; + } else { + fScalar = p_psVectorInterpolate((psVector *) x, (psVector *) y, + 3, &tmpScalar); + f->data.F64[i] = fScalar->data.F64; + psFree(fScalar); + } psTrace(".psLib.dataManip.vectorFitPolynomial1DCheby", 6, @@ -1650,4 +1832,6 @@ return(myPoly); } + + /****************************************************************************** @@ -1759,4 +1943,5 @@ if (0) { // does the solution in place + // XXX: Check error codes! psGaussJordan (A, B); @@ -1859,11 +2044,17 @@ 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 vector with Chebyshev polynomials.\n"); + } if (x == NULL) { // If x==NULL, create an x64 vector with x values set to (-1:1). PS_VECTOR_GEN_CHEBY_INDEX(x64, f64->n); } - // XXX: Change arg order. - // XXX: Must modify this routine so that x64 or fErr64 can be NULL. - poly = vectorFitPolynomial1DCheby(poly, NULL, 0, f64, fErr64, x64); + + if (1) { + poly = vectorFitPolynomial1DChebySlow(poly, NULL, 0, f64, fErr64, x64); + } else { + poly = vectorFitPolynomial1DChebyFast(poly, NULL, 0, f64, fErr64, x64); + } if (x == NULL) { psFree(x64); @@ -2102,4 +2293,5 @@ // does the solution in place + // XXX: Check error codes! psGaussJordan (A, B); @@ -2475,7 +2667,8 @@ } + /****************************************************************************** ****************************************************************************** - 3-D Vector Fitting Code. + 3-D Vector Fit Code. ****************************************************************************** *****************************************************************************/ @@ -2526,6 +2719,136 @@ } - psError(PS_ERR_UNKNOWN, true, "3-D Polynomial Fitting is not Implemented.\n"); - return (NULL); + psImage *A = NULL; + psVector *B = NULL; + psF64 ***Sums = NULL; + psF64 wt; + psS32 nTerm; + + // XXX:Watch for changes to the psPolys: nTerm != nOrder. + psS32 nXterm = 1 + myPoly->nX; + psS32 nYterm = 1 + myPoly->nY; + psS32 nZterm = 1 + myPoly->nZ; + nTerm = nXterm * nYterm * nZterm; + + A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); + B = psVectorAlloc(nTerm, PS_TYPE_F64); + + // Initialize data structures. + psVectorInit (B, 0.0); + psImageInit (A, 0.0); + + // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)*y, ... + + // Build the B and A data structs. + for (int k = 0; k < x->n; k++) { + if ((mask != NULL) && (mask->data.U8[k] & maskValue)) { + continue; + } + + Sums = BuildSums3D(Sums, x->data.F64[k], y->data.F64[k], z->data.F64[k], nXterm, nYterm, nZterm); + + 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(fErr->data.F64[k]); + } + + // we could skip half of the array and assign at the end + // we must handle masked orders + for (int ix = 0; ix < nXterm; ix++) { + for (int iy = 0; iy < nYterm; iy++) { + for (int iz = 0; iz < nZterm; iz++) { + if (myPoly->mask[ix][iy][iz]) + continue; + int nx = ix+iy*nXterm+iz*nXterm*nYterm; + B->data.F64[nx] += f->data.F64[k] * Sums[ix][iy][iz] * wt; + } + } + } + + for (int ix = 0; ix < nXterm; ix++) { + for (int iy = 0; iy < nYterm; iy++) { + for (int iz = 0; iz < nZterm; iz++) { + if (myPoly->mask[ix][iy][iz]) + continue; + int nx = ix+iy*nXterm+iz*nXterm*nYterm; + for (int jx = 0; jx < nXterm; jx++) { + for (int jy = 0; jy < nYterm; jy++) { + for (int jz = 0; jz < nZterm; jz++) { + if (myPoly->mask[jx][jy][jz]) + continue; + int ny = jx+jy*nXterm+jz*nXterm*nYterm; + A->data.F64[nx][ny] += Sums[ix+jx][iy+jy][iz+jz] * wt; + } + } + } + } + } + } + } + + for (int ix = 0; ix < nXterm; ix++) { + for (int iy = 0; iy < nYterm; iy++) { + for (int iz = 0; iz < nZterm; iz++) { + if (!myPoly->mask[ix][iy][iz]) + continue; + int nx = ix+iy*nXterm+iz*nXterm*nYterm; + B->data.F64[nx] = 0; + for (int jx = 0; jx < nXterm; jx++) { + for (int jy = 0; jy < nYterm; jy++) { + for (int jz = 0; jz < nZterm; jz++) { + int ny = jx+jy*nXterm+jz*nXterm*nYterm; + A->data.F64[nx][ny] = (nx == ny) ? 1 : 0; + } + } + } + } + } + } + + // PS_IMAGE_PRINT_F64(A); + // PS_VECTOR_PRINT_F64(B); + // does the solution in place + if (false == psGaussJordan (A, B)) { + psFree(A); + psFree(B); + + for (int ix = 0; ix < 2*nXterm; ix++) { + for (int iy = 0; iy < 2*nYterm; iy++) { + psFree(Sums[ix][iy]); + } + psFree(Sums[ix]); + } + psFree(Sums); + psError(PS_ERR_UNKNOWN, false, "Failed to perform GaussJordan elimination.\n"); + return(NULL); + } + + // select the appropriate solution entries + for (int ix = 0; ix < nXterm; ix++) { + for (int iy = 0; iy < nYterm; iy++) { + for (int iz = 0; iz < nZterm; iz++) { + int nx = ix+iy*nXterm+iz*nXterm*nYterm; + myPoly->coeff[ix][iy][iz] = B->data.F64[nx]; + myPoly->coeffErr[ix][iy][iz] = sqrt(A->data.F64[nx][nx]); + } + } + } + + psFree(A); + psFree(B); + + for (int ix = 0; ix < 2*nXterm; ix++) { + for (int iy = 0; iy < 2*nYterm; iy++) { + psFree(Sums[ix][iy]); + } + psFree(Sums[ix]); + } + psFree(Sums); + + psTrace(".psLib.dataManip.VectorFitPolynomial3DOrd", 4, + "---- VectorFitPolynomial3DOrd() begin ----\n"); + return (myPoly); } @@ -2599,5 +2922,5 @@ PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, NULL); if (z->type.type != PS_TYPE_F64) { - PS_VECTOR_GEN_F64_FROM_F32(y64, z); + PS_VECTOR_GEN_F64_FROM_F32(z64, z); } else { z64 = (psVector *) z; @@ -2714,5 +3037,5 @@ PS_ASSERT_PTR_NON_NULL(stats, NULL); PS_ASSERT_VECTOR_NON_NULL(f, NULL); - PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F32, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL); if (mask != NULL) { PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL); @@ -2721,20 +3044,87 @@ PS_ASSERT_VECTOR_NON_NULL(x, NULL); PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL); - PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F32, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL); PS_ASSERT_VECTOR_NON_NULL(y, NULL); PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL); - PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F32, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL); PS_ASSERT_VECTOR_NON_NULL(f, NULL); - PS_ASSERT_VECTORS_SIZE_EQUAL(f, f, NULL); - PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F32, NULL); + // PS_ASSERT_VECTORS_SIZE_EQUAL(f, f, NULL); + // PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL); if (fErr != NULL) { PS_ASSERT_VECTORS_SIZE_EQUAL(fErr, mask, NULL); - PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F32, NULL); - } - - psLogMsg(__func__, PS_LOG_WARN, "WARNING: This function has not been implemented. Returning NULL.\n"); - return(NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, NULL); + } + + // clipping range defined by min and max and/or clipSigma + float minClipSigma; + float 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 *fit = NULL; + psVector *resid = psVectorAlloc (x->n, PS_TYPE_F64); + + // eventual expansion: user supplies one of various stats option pairs, + // eg (SAMPLE_MEAN | SAMPLE_STDEV) and the correct pair is used to + // evaluate the clipping sigma + // for now, for the SAMPLE_MEDIAN and SAMPLE_STDEV to be used + stats->options |= (PS_STAT_SAMPLE_MEDIAN | PS_STAT_SAMPLE_STDEV); + + for (int N = 0; N < stats->clipIter; N++) { + int Nkeep = 0; + + poly = psVectorFitPolynomial3D (poly, mask, maskValue, f, fErr, x, y, z); + fit = psPolynomial3DEvalVector (poly, x, y, z); + resid = (psVector *) psBinaryOp (resid, (void *) f, "-", (void *) fit); + + stats = psVectorStats (stats, resid, NULL, mask, maskValue); + float minClipValue = -minClipSigma*stats->sampleStdev; + float maxClipValue = +maxClipSigma*stats->sampleStdev; + psTrace (".psphot.VectorClipFit", 5, "resid stats: %f +/- %f\n", stats->sampleMedian, stats->sampleStdev); + psTrace (".psphot.VectorClipFit", 5, "min clip: %f, max clip: %f\n", minClipValue, maxClipValue); + + // 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 (int i = 0; i < resid->n; i++) { + if ((mask != NULL) && (mask->data.U8[i] & maskValue)) { + continue; + } + if (resid->data.F64[i] - stats->sampleMedian > maxClipValue) { + if (mask != NULL) { + mask->data.U8[i] |= 0x01; + } + continue; + } + if (resid->data.F64[i] - stats->sampleMedian < minClipValue) { + if (mask != NULL) { + mask->data.U8[i] |= 0x01; + } + continue; + } + Nkeep ++; + } + + psTrace (".psphot.VectorClipFit", 4, "keeping %d of %d pts for fit\n", + Nkeep, x->n); + + psFree (fit); + } + // Free local temporary variables + psFree (resid); + + if (poly == NULL) { + psError(PS_ERR_UNKNOWN, true, "Could not fit a polynomial to the data. Returning NULL.\n"); + return(NULL); + } + return(poly); } - /****************************************************************************** @@ -2792,6 +3182,154 @@ } - psError(PS_ERR_UNKNOWN, true, "4-D Polynomial Fitting is not Implemented.\n"); - return (NULL); + // I think this is 1 dimension down + psImage *A = NULL; + psVector *B = NULL; + psF64 ****Sums = NULL; + psF64 wt; + psS32 nTerm; + + // XXX:Watch for changes to the psPolys: nTerm != nOrder. + psS32 nXterm = 1 + myPoly->nX; + psS32 nYterm = 1 + myPoly->nY; + psS32 nZterm = 1 + myPoly->nZ; + psS32 nTterm = 1 + myPoly->nZ; + nTerm = nXterm * nYterm * nZterm * nTterm; + + A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); + B = psVectorAlloc(nTerm, PS_TYPE_F64); + + // Initialize data structures. + psVectorInit (B, 0.0); + psImageInit (A, 0.0); + + // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)*y, ... + + // Build the B and A data structs. + for (int k = 0; k < x->n; k++) { + if ((mask != NULL) && (mask->data.U8[k] & maskValue)) { + continue; + } + + Sums = BuildSums4D(Sums, x->data.F64[k], y->data.F64[k], z->data.F64[k], t->data.F64[k], nXterm, nYterm, nZterm, nTterm); + + 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(fErr->data.F64[k]); + } + + // we could skip half of the array and assign at the end + // we must handle masked orders + for (int ix = 0; ix < nXterm; ix++) { + for (int iy = 0; iy < nYterm; iy++) { + for (int iz = 0; iz < nZterm; iz++) { + for (int it = 0; it < nTterm; it++) { + if (myPoly->mask[ix][iy][iz][it]) + continue; + int nx = ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm; + B->data.F64[nx] += f->data.F64[k] * Sums[ix][iy][iz][it] * wt; + } + } + } + } + + for (int ix = 0; ix < nXterm; ix++) { + for (int iy = 0; iy < nYterm; iy++) { + for (int iz = 0; iz < nZterm; iz++) { + for (int it = 0; it < nTterm; it++) { + if (myPoly->mask[ix][iy][iz][it]) + continue; + int nx = ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm; + for (int jx = 0; jx < nXterm; jx++) { + for (int jy = 0; jy < nYterm; jy++) { + for (int jz = 0; jz < nZterm; jz++) { + for (int jt = 0; jt < nTterm; jt++) { + if (myPoly->mask[jx][jy][jz][jt]) + continue; + int ny = jx+jy*nXterm+jz*nXterm*nYterm+jt*nXterm*nYterm*nZterm; + A->data.F64[nx][ny]+= Sums[ix+jx][iy+jy][iz+jz][it+jt] * wt; + } + } + } + } + } + } + } + } + } + + for (int ix = 0; ix < nXterm; ix++) { + for (int iy = 0; iy < nYterm; iy++) { + for (int iz = 0; iz < nZterm; iz++) { + for (int it = 0; it < nTterm; it++) { + if (!myPoly->mask[ix][iy][iz][it]) + continue; + int nx = ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm; + B->data.F64[nx] = 0; + for (int jx = 0; jx < nXterm; jx++) { + for (int jy = 0; jy < nYterm; jy++) { + for (int jz = 0; jz < nZterm; jz++) { + for (int jt = 0; jt < nTterm; jt++) { + int ny = jx+jy*nXterm+jz*nXterm*nYterm+jt*nXterm*nYterm*nZterm; + A->data.F64[nx][ny] = (nx == ny) ? 1 : 0; + } + } + } + } + } + } + } + } + + // does the solution in place + + if (false == psGaussJordan(A, B)) { + psFree(A); + psFree(B); + 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); + psError(PS_ERR_UNKNOWN, false, "Failed to perform GaussJordan elimination.\n"); + return(NULL); + } + + // select the appropriate solution entries + for (int ix = 0; ix < nXterm; ix++) { + for (int iy = 0; iy < nYterm; iy++) { + for (int iz = 0; iz < nZterm; iz++) { + for (int it = 0; it < nTterm; it++) { + int nx = ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm; + myPoly->coeff[ix][iy][iz][it] = B->data.F64[nx]; + myPoly->coeffErr[ix][iy][iz][it] = sqrt(A->data.F64[nx][nx]); + } + } + } + } + + psFree(A); + psFree(B); + + 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); + + psTrace(".psLib.dataManip.VectorFitPolynomial3DOrd", 4, + "---- VectorFitPolynomial3DOrd() begin ----\n"); + return (myPoly); } @@ -3029,8 +3567,73 @@ } - psLogMsg(__func__, PS_LOG_WARN, "WARNING: This function has not been implemented. Returning NULL.\n"); - - return(NULL); + // clipping range defined by min and max and/or clipSigma + float minClipSigma; + float 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 *fit = NULL; + psVector *resid = psVectorAlloc (x->n, PS_TYPE_F64); + + // eventual expansion: user supplies one of various stats option pairs, + // eg (SAMPLE_MEAN | SAMPLE_STDEV) and the correct pair is used to + // evaluate the clipping sigma + // for now, for the SAMPLE_MEDIAN and SAMPLE_STDEV to be used + stats->options |= (PS_STAT_SAMPLE_MEDIAN | PS_STAT_SAMPLE_STDEV); + + for (int N = 0; N < stats->clipIter; N++) { + int Nkeep = 0; + + poly = psVectorFitPolynomial4D (poly, mask, maskValue, f, fErr, x, y, z, t); + fit = psPolynomial4DEvalVector (poly, x, y, z, t); + resid = (psVector *) psBinaryOp (resid, (void *) f, "-", (void *) fit); + + stats = psVectorStats (stats, resid, NULL, mask, maskValue); + float minClipValue = -minClipSigma*stats->sampleStdev; + float maxClipValue = +maxClipSigma*stats->sampleStdev; + psTrace (".psphot.VectorClipFit", 5, "resid stats: %f +/- %f\n", stats->sampleMedian, stats->sampleStdev); + psTrace (".psphot.VectorClipFit", 5, "min clip: %f, max clip: %f\n", minClipValue, maxClipValue); + + // 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 (int i = 0; i < resid->n; i++) { + if ((mask != NULL) && (mask->data.U8[i] & maskValue)) { + continue; + } + if (resid->data.F64[i] - stats->sampleMedian > maxClipValue) { + if (mask != NULL) { + mask->data.U8[i] |= 0x01; + } + continue; + } + if (resid->data.F64[i] - stats->sampleMedian < minClipValue) { + if (mask != NULL) { + mask->data.U8[i] |= 0x01; + } + continue; + } + Nkeep ++; + } + + psTrace (".psphot.VectorClipFit", 4, "keeping %d of %d pts for fit\n", + Nkeep, x->n); + + psFree (fit); + } + // Free local temporary variables + psFree (resid); + + if (poly == NULL) { + psError(PS_ERR_UNKNOWN, true, "Could not fit a polynomial to the data. Returning NULL.\n"); + return(NULL); + } + return(poly); } - -