Index: /trunk/psLib/src/math/Makefile.am =================================================================== --- /trunk/psLib/src/math/Makefile.am (revision 6100) +++ /trunk/psLib/src/math/Makefile.am (revision 6101) @@ -9,5 +9,7 @@ psCompare.c \ psMatrix.c \ - psMinimize.c \ + psMinimizeLMM.c \ + psMinimizePowell.c \ + psMinimizePolyFit.c \ psRandom.c \ psPolynomial.c \ @@ -24,5 +26,7 @@ psConstants.h \ psMatrix.h \ - psMinimize.h \ + psMinimizeLMM.h \ + psMinimizePowell.h \ + psMinimizePolyFit.h \ psRandom.h \ psPolynomial.h \ Index: /trunk/psLib/src/math/psMinimizeLMM.c =================================================================== --- /trunk/psLib/src/math/psMinimizeLMM.c (revision 6101) +++ /trunk/psLib/src/math/psMinimizeLMM.c (revision 6101) @@ -0,0 +1,592 @@ +/** @file psMinimize.c + * \brief basic minimization functions + * @ingroup Math + * + * This file will contain functions to minimize an arbitrary function at + * a data point, fit an arbitrary function to a set of data points, and + * fit a 1-D polynomial to a set of data points. + * + * @author GLG, MHPCC + * @author EAM, IfA + * + * @version $Revision: 1.1 $ $Name: not supported by cvs2svn $ + * @date $Date: 2006-01-21 02:43:31 $ + * + * Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii + * + */ +/*****************************************************************************/ +/* INCLUDE FILES */ +/*****************************************************************************/ +#include +#include +#include + +#include "psMinimizeLMM.h" +#include "psImage.h" +#include "psImageStructManip.h" +#include "psLogMsg.h" +/*****************************************************************************/ +/* DEFINE STATEMENTS */ +/*****************************************************************************/ + +/*****************************************************************************/ +/* TYPE DEFINITIONS */ +/*****************************************************************************/ + +/*****************************************************************************/ +/* GLOBAL VARIABLES */ +/*****************************************************************************/ + +/*****************************************************************************/ +/* FILE STATIC VARIABLES */ +/*****************************************************************************/ + +/*****************************************************************************/ +/* FUNCTION IMPLEMENTATION - LOCAL */ +/*****************************************************************************/ + +// XXX EAM : can we use static copies of LUv, LUm, A? +// XXX: Add trace messages, check return codes. +psBool p_psMinLM_GuessABP( + psImage *Alpha, + psVector *Beta, + psVector *Params, + const psImage *alpha, + const psVector *beta, + const psVector *params, + const psVector *paramMask, + const psVector *beta_lim, + const psVector *params_min, + const psVector *params_max, + psF64 lambda) +{ + # define USE_LU_DECOMP 1 + # if (USE_LU_DECOMP) + psVector *LUv = NULL; + psImage *LUm = NULL; + psImage *A = NULL; + psF32 det; + + // LU decomposition version + psTrace(__func__, 5, "using LUD version\n"); + + // set new guess values (creates matrix A) + A = psImageCopy(NULL, alpha, PS_TYPE_F64); + for (int j = 0; j < params->n; j++) { + if ((paramMask != NULL) && (paramMask->data.U8[j])) + continue; + A->data.F64[j][j] = alpha->data.F64[j][j] * (1.0 + lambda); + } + + // solve A*beta = Beta (Alpha = 1/A) + // these operations do not modify the input values (creates LUm, LUv) + LUm = psMatrixLUD(NULL, &LUv, A); + Beta = psMatrixLUSolve(Beta, LUm, beta, LUv); + Alpha = psMatrixInvert(Alpha, A, &det); + + # else + // gauss-jordan version + psTrace(__func__, 5, "using Gauss-J version"); + + // set new guess values (creates matrix A) + Beta = psVectorCopy(Beta, beta, PS_TYPE_F64); + Alpha = psImageCopy(Alpha, alpha, PS_TYPE_F64); + for (int j = 0; j < params->n; j++) { + if ((paramMask != NULL) && (paramMask->data.U8[j])) + continue; + Alpha->data.F64[j][j] = alpha->data.F64[j][j] * (1.0 + lambda); + } + + // XXX: Check error codes! + psGaussJordan(Alpha, Beta); + psFree(A); + psFree(LUm); + psFree(LUv); + # endif + + // apply Beta to get new Params values + for (int j = 0; j < params->n; j++) { + if ((paramMask != NULL) && (paramMask->data.U8[j])) + continue; + // Params->data.F32[j] = params->data.F32[j] - Beta->data.F64[j]; + // compare Beta to beta limits + if (beta_lim != NULL) { + if (fabs(Beta->data.F64[j]) > fabs(beta_lim->data.F32[j])) { + Beta->data.F64[j] = (Beta->data.F64[j] > 0) ? fabs(beta_lim->data.F32[j]) : -fabs(beta_lim->data.F32[j]); + } + } + Params->data.F32[j] = params->data.F32[j] - Beta->data.F64[j]; + // compare new params to param limits + if (params_max != NULL) { + Params->data.F32[j] = PS_MIN (Params->data.F32[j], params_max->data.F32[j]); + } + if (params_min != NULL) { + Params->data.F32[j] = PS_MAX (Params->data.F32[j], params_min->data.F32[j]); + } + } + # if (USE_LU_DECOMP) + psFree(A); + psFree(LUm); + psFree(LUv); + # endif + + return(true); +} + + +// XXX: Add trace messages, check return codes. +bool psMinimizeGaussNewtonDelta( + psVector *delta, + const psVector *params, + const psVector *paramMask, + const psArray *x, + const psVector *y, + const psVector *yWt, + psMinimizeLMChi2Func func) +{ + // allocate internal arrays (current vs Guess) + psImage *alpha = psImageAlloc (params->n, params->n, PS_TYPE_F64); + psImage *Alpha = psImageAlloc (params->n, params->n, PS_TYPE_F64); + psVector *beta = psVectorAlloc(params->n, PS_TYPE_F64); + psVector *Params = psVectorAlloc(params->n, PS_TYPE_F64); + psVector *dy = NULL; + + // the user provides the error or NULL. we need to convert + // to appropriate weights + if (yWt != NULL) { + dy = (psVector *) yWt; + } else { + dy = psVectorAlloc(y->n, PS_TYPE_F32); + psVectorInit(dy, 1.0); + } + + p_psMinLM_SetABX(alpha, beta, params, paramMask, x, y, dy, func); + p_psMinLM_GuessABP(Alpha, delta, Params, alpha, beta, params, paramMask, NULL, NULL, NULL, 0.0); + + psFree(alpha); + psFree(Alpha); + psFree(beta); + psFree(Params); + if (yWt == NULL) { + psFree(dy); + } + return (true); +} + +// measure linear model prediction +psF64 p_psMinLM_dLinear( + const psVector *Beta, + const psVector *beta, + psF64 lambda) +{ + + /* get linear model prediction */ + psF64 dLinear = 0; + psF64 *B = Beta->data.F64; + psF64 *b = beta->data.F64; + for (int i = 0; i < beta->n; i++) { + dLinear += lambda*PS_SQR(B[i]) + B[i]*b[i]; + } + return(0.5*dLinear); +} + +// XXX EAM: this needs to respect the mask on params +// alpha, beta, params are already allocated +psF64 p_psMinLM_SetABX( + psImage *alpha, + psVector *beta, + const psVector *params, + const psVector *paramMask, + const psArray *x, + const psVector *y, + const psVector *dy, + psMinimizeLMChi2Func func) +{ + PS_ASSERT_IMAGE_NON_NULL(alpha, NAN); + PS_ASSERT_VECTOR_NON_NULL(beta, NAN); + PS_ASSERT_VECTOR_NON_NULL(params, NAN); + PS_ASSERT_PTR_NON_NULL(x, NAN); + PS_ASSERT_VECTOR_NON_NULL(y, NAN); + PS_ASSERT_VECTOR_NON_NULL(dy, NAN); + + psF64 chisq; + psF64 delta; + psF64 weight; + psF64 ymodel; + psVector *deriv = psVectorAlloc(params->n, PS_TYPE_F32); + + // zero alpha and beta for summing below + for (int j = 0; j < params->n; j++) { + for (int k = 0; k < params->n; k++) { + alpha->data.F64[j][k] = 0; + } + beta->data.F64[j] = 0; + } + chisq = 0.0; + + // calculate chisq, alpha, beta + for (int i = 0; i < y->n; i++) { + ymodel = func(deriv, params, (psVector *) x->data[i]); + + delta = ymodel - y->data.F32[i]; + chisq += PS_SQR(delta) * dy->data.F32[i]; + + for (int j = 0; j < params->n; j++) { + if ((paramMask != NULL) && (paramMask->data.U8[j])) + continue; + weight = deriv->data.F32[j] * dy->data.F32[i]; + for (int k = 0; k <= j; k++) { + if ((paramMask != NULL) && (paramMask->data.U8[k])) + continue; + alpha->data.F64[j][k] += weight * deriv->data.F32[k]; + } + beta->data.F64[j] += weight * delta; + } + } + + // calculate lower-left half of alpha + for (int j = 1; j < params->n; j++) { + for (int k = 0; k < j; k++) { + alpha->data.F64[k][j] = alpha->data.F64[j][k]; + } + } + + // fill in pivots if we apply a mask + if (paramMask != NULL) { + for (int j = 0; j < params->n; j++) { + if (paramMask->data.U8[j]) { + alpha->data.F64[j][j] = 1; + beta->data.F64[j] = 1; + } + } + } + + psFree(deriv); + return(chisq); +} + + +/****************************************************************************** +psMinimizeLMChi2(): This routine will take an procedure which calculates an +arbitrary function and it's derivative and minimize the chi-squared match +between that function at the specified coords and the specified value at those +coords. + +NOTES: EAM: this is my re-implementation of MinLM + +XXX: Put the ASSERTS in. + +XXX: This must work for both F32 and F64. F32 is currently implemented. + Note: since the LUD routines are only implemented in F64, then we + will have to convert all F32 input vectors to F64 regardless. So, + the F64 port might be. + *****************************************************************************/ +psBool psMinimizeLMChi2( + psMinimization *min, + psImage *covar, + psVector *params, + const psVector *paramMask, + const psArray *x, + const psVector *y, + const psVector *yWt, + psMinimizeLMChi2Func func) +{ + psTrace(__func__, 3, "---- %s() begin ----\n", __func__); + PS_ASSERT_PTR_NON_NULL(min, false); + // XXX: If covar not NULL, do asserts... + PS_ASSERT_VECTOR_NON_NULL(params, false); + PS_ASSERT_VECTOR_NON_EMPTY(params, false); + PS_ASSERT_VECTOR_TYPE(params, PS_TYPE_F32, false); + if (paramMask != NULL) { + PS_ASSERT_VECTOR_TYPE(paramMask, PS_TYPE_U8, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(params, paramMask, false); + } + PS_ASSERT_PTR_NON_NULL(x, false); + for (psS32 i = 0 ; i < x->n ; i++) { + psVector *coord = (psVector *) (x->data[i]); + PS_ASSERT_VECTOR_NON_NULL(coord, false); + PS_ASSERT_VECTOR_TYPE(coord, PS_TYPE_F32, false); + } + PS_ASSERT_VECTOR_NON_NULL(y, false); + PS_ASSERT_VECTOR_NON_EMPTY(y, false); + PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F32, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(x, y, false); + if (yWt != NULL) { + PS_ASSERT_VECTOR_TYPE(yWt, PS_TYPE_F32, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(y, yWt, false); + } + PS_ASSERT_PTR_NON_NULL(func, false); + + // this function has test and current values for several things + // the current best value is in lower case + // the next guess value is in upper case + + // allocate internal arrays (current vs Guess) + psImage *alpha = psImageAlloc(params->n, params->n, PS_TYPE_F64); + psImage *Alpha = psImageAlloc(params->n, params->n, PS_TYPE_F64); + psVector *beta = psVectorAlloc(params->n, PS_TYPE_F64); + psVector *Beta = psVectorAlloc(params->n, PS_TYPE_F64); + psVector *Params = psVectorAlloc(params->n, PS_TYPE_F32); + psVector *dy = NULL; + psF64 Chisq = 0.0; + psF64 lambda = 0.001; + // XXX: Code this properly. Don't use mustFree00. + psBool mustFree00 = false; + psVector *beta_lim = NULL; + psVector *param_min = NULL; + psVector *param_max = NULL; + + // if we are provided a covar image, we expect to find these three vectors in first three rows + if (covar != NULL) { + mustFree00 = true; + beta_lim = psVectorAlloc(params->n, PS_TYPE_F32); + param_min = psVectorAlloc(params->n, PS_TYPE_F32); + param_max = psVectorAlloc(params->n, PS_TYPE_F32); + for (int i = 0; i < params->n; i++) { + beta_lim->data.F32[i] = covar->data.F64[0][i]; + param_min->data.F32[i] = covar->data.F64[1][i]; + param_max->data.F32[i] = covar->data.F64[2][i]; + } + psImageRecycle(covar, params->n, params->n, PS_TYPE_F64); + } + + // why is this needed here??? the initial guess on params is provided by the user + Params = psVectorCopy(Params, params, PS_TYPE_F32); + + // the user provides the error or NULL. we need to convert + // to appropriate weights + if (yWt != NULL) { + dy = (psVector *) yWt; + } else { + dy = psVectorAlloc(y->n, PS_TYPE_F32); + psVectorInit(dy, 1.0); + } + + // calculate initial alpha and beta, set chisq (min->value) + min->value = p_psMinLM_SetABX(alpha, beta, params, paramMask, x, y, dy, func); + if (isnan(min->value)) { + min->iter = min->maxIter; + return(false); + } + // dump some useful info if trace is defined + if (psTraceGetLevel(__func__) >= 6) { + p_psImagePrint(psTraceGetDestination(), alpha, "alpha guess (0)"); + p_psVectorPrint(psTraceGetDestination(), beta, "beta guess (0)"); + p_psVectorPrint(psTraceGetDestination(), params, "params guess (0)"); + } + if (psTraceGetLevel (__func__) >= 6) { + //XXX: p_psVectorPrintRow(psTraceGetDestination(), Params, "params guess"); + } + + // iterate until the tolerance is reached, or give up + while ((min->iter < min->maxIter) && ((min->lastDelta > min->tol) || !isfinite(min->lastDelta))) { + psTrace(__func__, 5, "Iteration number %d. (max iterations is %d).\n", min->iter, min->maxIter); + psTrace(__func__, 5, "Last delta is %f. Min->tol is %f.\n", min->lastDelta, min->tol); + + // set a new guess for Alpha, Beta, Params + p_psMinLM_GuessABP(Alpha, Beta, Params, alpha, beta, params, paramMask, + beta_lim, param_min, param_max, lambda); + + // measure linear model prediction + psF64 dLinear = p_psMinLM_dLinear(Beta, beta, lambda); + + // dump some useful info if trace is defined + if (psTraceGetLevel(__func__) >= 6) { + p_psImagePrint(psTraceGetDestination(), Alpha, "alpha guess (1)"); + p_psVectorPrint(psTraceGetDestination(), Beta, "beta guess (1)"); + p_psVectorPrint(psTraceGetDestination(), Params, "params guess (1)"); + } + if (psTraceGetLevel(__func__) >= 6) { + //XXX: p_psVectorPrintRow(psTraceGetDestination(), Params, "params guess"); + } + + // calculate Chisq for new guess, update Alpha & Beta + Chisq = p_psMinLM_SetABX(Alpha, Beta, Params, paramMask, x, y, dy, func); + + // XXX EAM alternate convergence criterion: + // compare the delta (min->value - Chisq) with the + // expected delta from the linear model (dLinear) + // accept new guess (if improvement), or increase lambda + psF64 rho = (min->value - Chisq) / dLinear; + + psTrace(__func__, 5, "last chisq: %f, new chisq %f, delta: %f, rho: %f\n", min->value, + Chisq, min->lastDelta, rho); + + // dump some useful info if trace is defined + if (psTraceGetLevel(__func__) >= 6) { + p_psImagePrint(psTraceGetDestination(), Alpha, "alpha guess (2)"); + p_psVectorPrint(psTraceGetDestination(), Beta, "beta guess (2)"); + p_psVectorPrint(psTraceGetDestination(), Params, "params guess (2)"); + } + + /* if (Chisq < min->value) { */ + if (rho > 0.0) { + min->lastDelta = (min->value - Chisq) / (dy->n - params->n); + min->value = Chisq; + alpha = psImageCopy(alpha, Alpha, PS_TYPE_F64); + beta = psVectorCopy(beta, Beta, PS_TYPE_F64); + params = psVectorCopy(params, Params, PS_TYPE_F32); + lambda *= 0.1; + } else { + lambda *= 10.0; + } + min->iter++; + } + psTrace(__func__, 5, "chisq: %f, last delta: %f, Niter: %d\n", min->value, min->lastDelta, min->iter); + + // construct & return the covariance matrix (if requested) + if (covar != NULL) { + p_psMinLM_GuessABP(covar, Beta, Params, alpha, beta, params, paramMask, + beta_lim, param_min, param_max, 0.0); + } + + // free the internal temporary data + psFree(alpha); + psFree(Alpha); + psFree(beta); + psFree(Beta); + psFree(Params); + if (yWt == NULL) { + psFree(dy); + } + if (mustFree00 == true) { + psFree(beta_lim); + psFree(param_min); + psFree(param_max); + } + if (min->iter == min->maxIter) { + psTrace(__func__, 3, "---- %s(false) end ----\n", __func__); + return(false); + } + psTrace(__func__, 3, "---- %s(true) end ----\n", __func__); + return(true); +} + +// XXX EAM : temporary gauss-jordan solver based on gene's +// version based on the Numerical Recipes version +bool psGaussJordan( + psImage *a, + psVector *b) +{ + int *indxc,*indxr,*ipiv; + int Nx, icol, irow; + int i, j, k, l, ll; + float big, dum, pivinv; + psF64 *vector; + psF64 **matrix; + + Nx = a->numCols; + matrix = a->data.F64; + vector = b->data.F64; + + indxc = psAlloc(Nx*sizeof(int)); + indxr = psAlloc(Nx*sizeof(int)); + ipiv = psAlloc(Nx*sizeof(int)); + for (j = 0; j < Nx; j++) { + ipiv[j] = 0; + } + + irow = icol = 0; + big = fabs(matrix[0][0]); + + for (i = 0; i < Nx; i++) { + big = 0.0; + for (j = 0; j < Nx; j++) { + if (!isfinite(matrix[i][j])) { + psError(PS_ERR_UNKNOWN, false, "Input matrix contains NaNs: matrix[%d][%d] is %.2f\n", i, j, matrix[i][j]); + goto fescape; + } + if (ipiv[j] != 1) { + for (k = 0; k < Nx; k++) { + if (ipiv[k] == 0) { + if (fabs (matrix[j][k]) >= big) { + big = fabs(matrix[j][k]); + irow = j; + icol = k; + } + } else { + if (ipiv[k] > 1) { + psError(PS_ERR_UNKNOWN, false, "Singular Matrix (1).\n"); + goto fescape; + } + } + } + } + } + ipiv[icol]++; + if (irow != icol) { + for (l = 0; l < Nx; l++) { + PS_SWAP(matrix[irow][l], matrix[icol][l]); + } + PS_SWAP(vector[irow], vector[icol]); + } + indxr[i] = irow; + indxc[i] = icol; + if (matrix[icol][icol] == 0.0) { + psError(PS_ERR_UNKNOWN, false, "Singular Matrix (2).\n"); + goto fescape; + } + pivinv = 1.0 / matrix[icol][icol]; + matrix[icol][icol] = 1.0; + for (l = 0; l < Nx; l++) { + matrix[icol][l] *= pivinv; + } + vector[icol] *= pivinv; + + for (ll = 0; ll < Nx; ll++) { + if (ll != icol) { + dum = matrix[ll][icol]; + matrix[ll][icol] = 0.0; + for (l = 0; l < Nx; l++) { + matrix[ll][l] -= matrix[icol][l]*dum; + } + vector[ll] -= vector[icol]*dum; + } + } + } + + for (l = Nx - 1; l >= 0; l--) { + if (indxr[l] != indxc[l]) { + for (k = 0; k < Nx; k++) { + PS_SWAP(matrix[k][indxr[l]], matrix[k][indxc[l]]); + } + } + } + psFree(ipiv); + psFree(indxr); + psFree(indxc); + return(true); + +fescape: + psFree(ipiv); + psFree(indxr); + psFree(indxc); + return(false); +} + +static void minimizationFree(psMinimization *min) +{ + // There are no dynamically allocated items +} + +psMinimization *psMinimizationAlloc(int maxIter, + float tol) +{ + PS_ASSERT_INT_NONNEGATIVE(maxIter, NULL); + + psMinimization *min = psAlloc(sizeof(psMinimization)); + psMemSetDeallocator(min, (psFreeFunc)minimizationFree); + P_PSMINIMIZATION_SET_MAXITER(min,maxIter); + P_PSMINIMIZATION_SET_TOL(min,tol); + min->value = 0.0; + min->iter = 0; + min->lastDelta = NAN; + + return(min); +} + +bool psMemCheckMinimization(psPtr ptr) +{ + return( psMemGetDeallocator(ptr) == (psFreeFunc)minimizationFree ); +} Index: /trunk/psLib/src/math/psMinimizeLMM.h =================================================================== --- /trunk/psLib/src/math/psMinimizeLMM.h (revision 6101) +++ /trunk/psLib/src/math/psMinimizeLMM.h (revision 6101) @@ -0,0 +1,147 @@ +/** @file psMinimizeLMM.c + * \brief basic minimization functions + * @ingroup Math + * + * This file will contain function prototypes for various Levenberg-Marquadt + * minimization routines. + * + * @author GLG, MHPCC + * + * @version $Revision: 1.1 $ $Name: not supported by cvs2svn $ + * @date $Date: 2006-01-21 02:43:31 $ + * + * Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii + * + */ + +#ifndef PS_MINIMIZE_LMM_H +#define PS_MINIMIZE_LMM_H + +/** \file psMinimize.h + * \brief minimization operations + * \ingroup Stats + */ +/** \addtogroup Stats + * \{ + */ + +#include "psVector.h" +#include "psMemory.h" +#include "psArray.h" +#include "psImage.h" +#include "psMatrix.h" +#include "psPolynomial.h" +#include "psSpline.h" +#include "psStats.h" +#include "psTrace.h" +#include "psError.h" +#include "psConstants.h" + +/** A data structure for minimization routines. + * + * Contains numerical analysis parameters/values + */ +typedef struct +{ + const int maxIter; ///< Convergence limit + const float tol; ///< Error Tolerance + float value; ///< Value of function at minimum + int iter; ///< Number of iterations to date + float lastDelta; ///< The last difference for the fit +} +psMinimization; + +#define P_PSMINIMIZATION_SET_MAXITER(m,val) *(int*)&m->maxIter = val + #define P_PSMINIMIZATION_SET_TOL(m,val) *(float*)&m->tol = val + + + /** Allocates a psMinimization structure. + * + * @return psMinimization* : a new psMinimization struct + */ + psMinimization *psMinimizationAlloc( + int maxIter, ///< Number of minimization iterations to perform. + float tol ///< Requested error tolerance + ); + +/* Checks the type of a particular pointer. + * + * Uses the appropriate deallocation function in psMemBlock to check the ptr datatype. + * + * @return bool: True if the pointer matches a psMinimization structure, false otherwise. + */ +bool psMemCheckMinimization( + psPtr ptr ///< the pointer whose type to check +); + + +/** Specifies the format of a user-defined function that the general Levenberg- + * Marquardt minimizer routine will accept. + * + * @return float: the single float value of the function given the parameters, + * positions, and derivatives. + */ +typedef +float (*psMinimizeLMChi2Func)( + psVector *deriv, ///< derivatives of the function + const psVector *params, ///< the parameters used to evaluate the function + const psVector *x ///< positions for evaluation +); + +/** Minimizes a specified function based on the Levenberg-Marquardt method. + * + * @return bool: True if successful. + */ +bool psMinimizeLMChi2( + psMinimization *min, ///< Minimization specification + psImage *covar, ///< Covariance matrix + psVector *params, ///< "Best Guess" for the parameters that minimize func + const psVector *paramMask, ///< Parameters to be held fixed by the minimizer + const psArray *x, ///< Measurement ordinates of multiple vectors + const psVector *y, ///< Measurement coordinates + const psVector *yErr, ///< Errors in the measurement coordinates + psMinimizeLMChi2Func func ///< Specified function +); + +bool psMinimizeGaussNewtonDelta ( + psVector *delta, + const psVector *params, + const psVector *paramMask, + const psArray *x, + const psVector *y, + const psVector *yErr, + psMinimizeLMChi2Func func +); + +/** Function used to set parameters for generating "best guess" in minimizing Chi-Squared value. + * + * @return psF64: Chi-squared value for new guess + */ +psF64 p_psMinLM_SetABX ( + psImage *alpha, ///< alpha guess + psVector *beta, ///< beta guess + const psVector *params, ///< params guess + const psVector *paramMask, ///< param mask + const psArray *x, ///< Measurement ordinates + const psVector *y, ///< Measurement coordinates + const psVector *dy, ///< Weights calculated from y-errors + psMinimizeLMChi2Func func ///< Specified function +); + + +/** Gauss-Jordan numerical solver. + * + * @return bool: True if successful. + */ +bool psGaussJordan( + psImage *a, ///< Matrix to be solved + psVector *b ///< Vector of values +); + +/* \} */// End of MathGroup Functions + + + + +#endif // #ifndef PS_MINIMIZE_LMM_H + Index: /trunk/psLib/src/math/psMinimizePolyFit.c =================================================================== --- /trunk/psLib/src/math/psMinimizePolyFit.c (revision 6101) +++ /trunk/psLib/src/math/psMinimizePolyFit.c (revision 6101) @@ -0,0 +1,2463 @@ +/** @file psMinimize.c + * \brief basic minimization functions + * @ingroup Math + * + * This file will contain functions to minimize an arbitrary function at + * a data point, fit an arbitrary function to a set of data points, and + * fit a 1-D polynomial to a set of data points. + * + * @author GLG, MHPCC + * @author EAM, IfA + * + * @version $Revision: 1.1 $ $Name: not supported by cvs2svn $ + * @date $Date: 2006-01-21 02:43:31 $ + * + * Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii + * + * XXX: must follow coding name standards on local functions. + * XXX: put local functions in front. + * XXX: For clip-fit functions, what should we do if the mask is NULL? + * XXX: Generate macros for + * PS_ASSERT_VECTOR_TYPES_MATCH() + * PS_ASSERT_VECTOR_SIZES_MATCH() + * They should have better error messages. + * + */ +/*****************************************************************************/ +/* INCLUDE FILES */ +/*****************************************************************************/ +#include +#include +#include + +#include "psMinimizePolyFit.h" +#include "psMinimizeLMM.h" // For Gauss-Jordan routines +#include "psStats.h" +#include "psImage.h" +#include "psImageStructManip.h" +#include "psBinaryOp.h" +#include "psLogMsg.h" +/*****************************************************************************/ +/* DEFINE STATEMENTS */ +/*****************************************************************************/ + +/*****************************************************************************/ +/* TYPE DEFINITIONS */ +/*****************************************************************************/ + +/*****************************************************************************/ +/* GLOBAL VARIABLES */ +/*****************************************************************************/ + +/*****************************************************************************/ +/* FILE STATIC VARIABLES */ +/*****************************************************************************/ + +/*****************************************************************************/ +/* FUNCTION IMPLEMENTATION - LOCAL */ +/*****************************************************************************/ + +/****************************************************************************** + ****************************************************************************** + Analytical 1-D fitting routines. + ****************************************************************************** + *****************************************************************************/ +// XXX: Make this a general type conversion macro, or function +#define PS_VECTOR_GEN_F64_FROM_F32(VECF64, VECF32) \ +VECF64 = psVectorAlloc(VECF32->n, PS_TYPE_F64); \ +for (psS32 i = 0 ; i < VECF32->n ; i++) { \ + VECF64->data.F64[i] = (psF64) VECF32->data.F32[i]; \ +} \ + +// XXX: Consolidate these +#define PS_VECTOR_GEN_CHEBY_INDEX(VECF64, SIZE) \ +VECF64 = psVectorAlloc(SIZE, PS_TYPE_F64); \ +for (psS32 i = 0 ; i < SIZE ; i++) { \ + VECF64->data.F64[i] = ((2.0 / ((psF64) (SIZE - 1))) * ((psF64) i)) - 1.0; \ +}\ + +#define PS_VECTOR_GEN_CHEBY_INDEX_F64(VECF64, SIZE) \ +VECF64 = psVectorAlloc(SIZE, PS_TYPE_F64); \ +for (psS32 i = 0 ; i < SIZE ; i++) { \ + VECF64->data.F64[i] = ((2.0 / ((psF64) (SIZE - 1))) * ((psF64) i)) - 1.0; \ +}\ + +#define PS_VECTOR_GEN_CHEBY_INDEX_F32(VECF32, SIZE) \ +VECF32 = psVectorAlloc(SIZE, PS_TYPE_F32); \ +for (psS32 i = 0 ; i < SIZE ; i++) { \ + VECF32->data.F32[i] = ((2.0 / ((psF32) (SIZE - 1))) * ((psF32) i)) - 1.0; \ +}\ +/****************************************************************************** +BuildSums1D(sums, x, polyOrder, sums): this routine calculates the powers of +input parameter "x" between 0 and input parameter nTerms*2. The result is +returned as a psVector sums. +*****************************************************************************/ +static psVector *BuildSums1D( + psVector* sums, + psF64 x, + psS32 nTerm) +{ + psS32 nSum = 0; + psF64 xSum = 0.0; + + // + // XXX: Why do we multiply by 2 here? It's better to do it outside and + // have the definition of this function remain sensible. + // + nSum = 2*nTerm; + if (sums == NULL) { + sums = psVectorAlloc(nSum, PS_TYPE_F64); + } else if (nSum > sums->n) { + sums = psVectorRealloc(sums, nSum); + } + + xSum = 1.0; + for (int i = 0; i < nSum; i++) { + sums->data.F64[i] = xSum; + xSum *= x; + } + return (sums); +} + +/****************************************************************************** +BuildSums2D(sums, x, y, nXterm, nYterm): this routine calculates the powers of +input parameter "x" and "y" between 0 and input parameter nXterms*2 and +nYterm*2. The result is returned as a psImage sums. + *****************************************************************************/ +static psImage *BuildSums2D( + psImage *sums, + psF64 x, + psF64 y, + psS32 nXterm, + psS32 nYterm) +{ + psS32 nXsum = 0; + psS32 nYsum = 0; + psF64 xSum = 1.0; + psF64 ySum = 1.0; + + nXsum = 2*nXterm; + nYsum = 2*nYterm; + if (sums == NULL) { + sums = psImageAlloc(nXsum, nYsum, PS_TYPE_F64); + } + if ((nXsum != sums->numCols) || (nYsum != sums->numRows)) { + psFree (sums); + sums = psImageAlloc(nXsum, nYsum, PS_TYPE_F64); + } + + xSum = 1.0; + for (int i = 0; i < nXsum; i++) { + ySum = xSum; + for (int j = 0; j < nYsum; j++) { + sums->data.F64[i][j] = ySum; + ySum *= y; + } + xSum *= x; + } + + if (0) { + printf("--------------------- BuildSums2D(%.2f %.2f) ---------------------\n", x, y); + for (int i = 0; i < nXsum; i++) { + for (int j = 0; j < nYsum; j++) { + printf("(%.2f) ", sums->data.F64[i][j]); + } + printf("\n"); + } + } + + return (sums); +} + +/****************************************************************************** +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 3-D array 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 + + if (1) { + 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; + } + } else { + xSum = 1.0; + for (int i = 0; i < nXsum; i++) { + ySum = xSum; + for (int j = 0; j < nYsum; j++) { + zSum = ySum; + for (int k = 0; k < nZsum; k++) { + sums[i][j][k] = zSum; + zSum *= z; + } + ySum *= y; + } + xSum *= x; + } + } + + return (sums); +} + +/****************************************************************************** + BuildSums4D(sums, x, y, z, t, nXterm, nYterm, nZterm, nTterm). equiv to + BuildSums2D(). The result is returned as a double **** +*****************************************************************************/ +static psF64 ****BuildSums4D( + psF64 ****sums, + psF64 x, + psF64 y, + psF64 z, + psF64 t, + psS32 nXterm, + psS32 nYterm, + psS32 nZterm, + psS32 nTterm) +{ + psS32 nXsum = 0; + psS32 nYsum = 0; + psS32 nZsum = 0; + psS32 nTsum = 0; + psF64 xSum = 1.0; + psF64 ySum = 1.0; + psF64 zSum = 1.0; + psF64 tSum = 1.0; + + nXsum = 2*nXterm; + nYsum = 2*nYterm; + nZsum = 2*nZterm; + nTsum = 2*nTterm; + 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)); + for (int k = 0; k < nZsum; k++) { + sums[i][j][k] = (psF64 *) psAlloc (nTsum*sizeof(psF64)); + } + } + } + } + // careful with this function: there is no size checking and realloc for reuse + + tSum = 1.0; + for (int m = 0; m < nTsum; m++) { + zSum = tSum; + 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][m] = xSum; + xSum *= x; + } + ySum *= y; + } + zSum *= z; + } + tSum *= t; + } + return (sums); +} + +/****************************************************************************** + ****************************************************************************** + 1-D Vector Poly Fitting Code. + ****************************************************************************** + *****************************************************************************/ + +/****************************************************************************** +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. + +*****************************************************************************/ +static psPolynomial1D *vectorFitPolynomial1DChebySlow( + psPolynomial1D* myPoly, + const psVector *mask, + psMaskType maskValue, + const psVector* y, + const psVector* yErr, + const psVector* x) +{ + 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); + } + if (mask != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(y, mask, NULL); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL); + } + + psS32 NUM_POLY = myPoly->nX+1; + psS32 NUM_DATA = 0; + if (mask != NULL) { + for (psS32 d = 0 ; d < mask->n ; d++) { + if (!(maskValue & mask->data.U8[d])) { + NUM_DATA++; + } + } + } else { + NUM_DATA = x->n; + } + // XX: psTrace printf("vectorFitPolynomial1DChebySlow(): NUM_DATA is %d\n", NUM_DATA); + + 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++) { + if (mask == NULL) { + for (psS32 d = 0 ; d < NUM_DATA ; d++) { + tMatrix->data.F64[p][d] = psPolynomial1DEval(chebPolys[p], x->data.F64[d]); + } + } else { + psS32 dPtr = 0; + for (psS32 d = 0 ; d < mask->n ; d++) { + if (!(maskValue & mask->data.U8[d])) { + tMatrix->data.F64[p][dPtr] = psPolynomial1DEval(chebPolys[p], x->data.F64[d]); + dPtr++; + } + } + } + } + + // 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; + if (mask == NULL) { + for (psS32 d = 0 ; d < NUM_DATA ; d++) { + B->data.F64[i] += (y->data.F64[d] * tMatrix->data.F64[i][d]); + } + } else { + psS32 dPtr = 0; + for (psS32 d = 0 ; d < mask->n ; d++) { + if (!(maskValue & mask->data.U8[d])) { + B->data.F64[i] += (y->data.F64[d] * tMatrix->data.F64[i][dPtr++]); + } + } + } + } + + // 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); + 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 j; + psS32 k; + psS32 n = y->n; + psF64 fac; + psF64 sum; + PS_VECTOR_GEN_STATIC_RECYCLED(f, n, PS_TYPE_F64); + psScalar *fScalar; + psScalar tmpScalar; + tmpScalar.type.type = PS_TYPE_F64; + + // XXX: These assignments appear too simple to warrant code and + // variable declarations. I retain them here to maintain coherence + // with the NR code. + psF64 min = -1.0; + psF64 max = 1.0; + psF64 bma = 0.5 * (max-min); // 1 + psF64 bpa = 0.5 * (max+min); // 0 + + // In this loop, we first calculate the values of X for which the + // Chebyshev polynomials are zero (see NR, section 5.4). Then we + // calculate the value of the function we are fitting the Chebyshev + // polynomials to at those values of X. This is a bit tricky since + // we don't know that function. So, we instead do 3-order LaGrange + // interpolation at the point X for the psVectors x,y for which we + // are fitting this ChebyShev polynomial to. + + for (psS32 i=0;idata.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, + "(x, X, y, f(X)) is (%f, %f, %f, %f)\n", + x->data.F64[i], X, y->data.F64[i], f->data.F64[i]); + } + + // We have the values for f() at the zero points, we now calculate the + // coefficients of the Chebyshev polynomial: NR 5.8.7. + + fac = 2.0/((psF32) n); + // XXX: is this loop bound correct? + for (j=0;jnX+1;j++) { + sum = 0.0; + for (k=0;kdata.F64[k] * + cos(M_PI * ((psF32) j) * (0.5 + ((psF32) k)) / ((psF32) n)); + } + + myPoly->coeff[j] = fac * sum; + } + + return(myPoly); +} + + + +/****************************************************************************** +VectorFitPolynomial1DOrd(myPoly, *mask, maskValue, *y, *yErr, *x): This is a +private routine which will fit a 1-D polynomial to a set of (x, f) pairs. The +x and fErr vectors may be NULL. All non-NULL vectors must be of type +PS_TYPE_F64. + *****************************************************************************/ +psPolynomial1D* VectorFitPolynomial1DOrd( + psPolynomial1D* myPoly, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x) +{ + psTrace(__func__, 4, "---- VectorFitPolynomial1DOrd() begin ----\n"); + // XXX: these ASSERTS are redundant. + PS_ASSERT_POLY_NON_NULL(myPoly, NULL); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, NULL); + PS_ASSERT_VECTOR_NON_NULL(f, NULL); + PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, NULL); + if (mask != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL); + } + if (x != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL); + PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL); + } + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL); + PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F64, NULL); + } + + psImage* A = NULL; + psVector* B = NULL; + psVector* xSums = NULL; + psS32 nTerm; + psS32 nOrder; + psF64 wt; + + if (psTraceGetLevel (__func__) >= 6) { + psTrace(__func__, 6, "VectorFitPolynomial1D()\n"); + for (psS32 i = 0; i < f->n; i++) { + psTrace(__func__, 6, "(x, f, fErr) is ("); + if (x != NULL) { + psTrace(__func__, 6, "%f, %f, ", x->data.F64[i], f->data.F64[i]); + } else { + psTrace(__func__, 6, "%f, %f, ", (psF64) i, f->data.F64[i]); + } + if (fErr != NULL) { + psTrace(__func__, 6, "%f)\n", fErr->data.F64[i]); + } else { + psTrace(__func__, 6, "NULL)\n"); + } + } + } + + nTerm = 1 + myPoly->nX; + nOrder = nTerm - 1; + + A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); + B = psVectorAlloc(nTerm, PS_TYPE_F64); + + // + // Initialize data structures. + // XXX: Use psLib function. + // + PS_VECTOR_SET_F64(B, 0.0); + PS_IMAGE_SET_F64(A, 0.0); + + // xSums look like: 1, x, x^2, ... x^(2n+1) + // Build the B and A data structs. + // XXX EAM : use temp pointers eg vB = B->data.F64 to save redirects + // XXX EAM : this function is only valid for data vectors of F64 + for (int k = 0; k < f->n; k++) { + if ((mask != NULL) && (mask->data.U8[k] && maskValue)) { + continue; + } + if (x != NULL) { + xSums = BuildSums1D(xSums, x->data.F64[k], nTerm); + } else { + xSums = BuildSums1D(xSums, (psF64) k, nTerm); + } + + if (fErr == NULL) { + wt = 1.0; + } else { + // this filters fErr == 0 values + wt = (fErr->data.F64[k] == 0) ? 0.0 : 1.0 / PS_SQR(fErr->data.F64[k]); + } + for (int i = 0; i < nTerm; i++) { + B->data.F64[i] += f->data.F64[k] * xSums->data.F64[i] * wt; + } + + // we could skip half of the array and assign at the end + // we must handle masked orders + for (int i = 0; i < nTerm; i++) { + for (int j = 0; j < nTerm; j++) { + A->data.F64[i][j] += xSums->data.F64[i + j] * wt; + } + } + } + + // 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 (int k = 0; k < nTerm; 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(nTerm, nTerm, PS_TYPE_F64); + ALUD = psMatrixLUD(ALUD, &outPerm, A); + coeffs = psMatrixLUSolve(coeffs, ALUD, B, outPerm); + for (int k = 0; k < nTerm; k++) { + myPoly->coeff[k] = coeffs->data.F64[k]; + } + psFree(ALUD); + psFree(coeffs); + psFree(outPerm); + } + + + psFree(A); + psFree(B); + psFree(xSums); + + psTrace(__func__, 4, "---- VectorFitPolynomial1DOrd() End ----\n"); + return (myPoly); +} + + +/****************************************************************************** +psVectorFitPolynomial1D(): This routine fits a polynomial of arbitrary degree +(specified in poly) to the data points (x, y) and return that polynomial. +Types F32 and F64 are supported, however, type F32 is done via vector +conversion only. + +XXX: Put the asserts at the beginning. Why is this not failing tests? + *****************************************************************************/ +psPolynomial1D *psVectorFitPolynomial1D( + psPolynomial1D *poly, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x) +{ + // Internal pointers for possibly NULL or mis-typed vectors. + psVector *x64 = NULL; + psVector *f64 = NULL; + psVector *fErr64 = NULL; + + PS_ASSERT_POLY_NON_NULL(poly, NULL); + PS_ASSERT_INT_NONNEGATIVE(poly->nX, NULL); + PS_ASSERT_VECTOR_NON_NULL(f, NULL); + PS_ASSERT_VECTOR_NON_EMPTY(f, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL); + if (mask != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL); + } + if (x != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL); + } + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, NULL); + } + + if (f->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(f64, f); + } else { + f64 = (psVector *) f; + } + + if (x != NULL) { + if (x->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(x64, x); + } else { + x64 = (psVector *) x; + } + } + + if (fErr != NULL) { + if (fErr->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(fErr64, fErr); + } else { + fErr64 = (psVector *) fErr; + } + } + + if (poly->type == PS_POLYNOMIAL_ORD) { + poly = VectorFitPolynomial1DOrd(poly, mask, maskValue, f64, fErr64, x64); + if (poly == NULL) { + psError(PS_ERR_UNKNOWN, false, "Could not fit polynomial. Returning NULL.\n"); + return(NULL); + } + } else if (poly->type == 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 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); + } + + if (1) { + poly = vectorFitPolynomial1DChebySlow(poly, mask, maskValue, f64, fErr64, x64); + } else { + poly = vectorFitPolynomial1DChebyFast(poly, mask, maskValue, f64, fErr64, x64); + } + if (x == NULL) { + psFree(x64); + } + } else { + psError(PS_ERR_UNKNOWN, true, "Incorrect polynomial type. Returning NULL.\n"); + poly = NULL; + } + + // Free psVectors that were created for NULL arguments. + if (f->type.type != PS_TYPE_F64) { + psFree(f64); + } + + if ((x != NULL) && (x->type.type != PS_TYPE_F64)) { + psFree(x64); + } + + if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) { + psFree(fErr64); + } + + return(poly); +} + +// This function accepts F32 and F64 input vectors. +psPolynomial1D *psVectorClipFitPolynomial1D( + psPolynomial1D *poly, + psStats *stats, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *xIn) +{ + psTrace(__func__, 3, "---- %s() begin ----\n", __func__); + PS_ASSERT_POLY_NON_NULL(poly, NULL); + PS_ASSERT_PTR_NON_NULL(stats, NULL); + PS_ASSERT_VECTOR_NON_NULL(f, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL); + PS_ASSERT_VECTOR_NON_NULL(mask, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(mask, f, NULL); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL); + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(fErr, f, NULL); + PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, NULL); + } + + // Internal pointers for possibly NULL vectors. + psVector *x = NULL; + if (xIn != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(xIn, f, NULL); + PS_ASSERT_VECTOR_TYPE(xIn, f->type.type, NULL); + 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_F32(x, f->n); + } else if (f->type.type == PS_TYPE_F64) { + PS_VECTOR_GEN_CHEBY_INDEX_F64(x, f->n); + } + } else { + printf("XXX: error, bad poly type.\n"); + } + } + + // clipping range defined by min and max and/or clipSigma + psF32 minClipSigma; + psF32 maxClipSigma; + + if (isnan(stats->max) || isfinite(stats->max)) { + maxClipSigma = fabs(stats->clipSigma); + } else { + maxClipSigma = fabs(stats->max); + } + if (isnan(stats->min) || isfinite(stats->min)) { + minClipSigma = fabs(stats->clipSigma); + } else { + minClipSigma = fabs(stats->min); + } + psVector *fit = NULL; + psVector *resid = psVectorAlloc(f->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); + + psTrace(__func__, 4, "stats->clipIter is %d\n", stats->clipIter); + psTrace(__func__, 4, "(minClipSigma, maxClipSigma) is (%.2f, %.2f)\n", minClipSigma, maxClipSigma); + // + for (int N = 0; N < stats->clipIter; N++) { + psTrace(__func__, 6, "Loop iteration %d. Calling psVectorFitPolynomial1D()\n"); + int Nkeep = 0; + // XXX: Check error codes + if (psTraceGetLevel(__func__) >= 6) { + if (mask != NULL) { + for (psS32 i = 0 ; i < mask->n ; i++) { + psTrace(__func__, 6, "mask[%d] is %d\n", i, mask->data.U8[i]); + } + } + } + poly = psVectorFitPolynomial1D(poly, mask, maskValue, f, fErr, x); + if (poly == NULL) { + printf("XXX: psVectorFitPolynomial1D() returned NULL: Generate error, free data.\n"); + return(NULL); + } + + fit = psPolynomial1DEvalVector(poly, x); + 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(__func__) >= 6) { + if (mask != NULL) { + for (psS32 i = 0 ; i < mask->n ; i++) { + if (!((mask != NULL) && (mask->data.U8[i] & maskValue))) { + psTrace(__func__, 6, "(f, fit)[%d] is (%f, %f). resid is (%f)\n", + i, f->data.F32[i], fit->data.F32[i], resid->data.F64[i]); + } + } + } + } + + stats = psVectorStats(stats, resid, NULL, mask, maskValue); + psTrace(__func__, 6, "Median is %f\n", stats->sampleMedian); + psTrace(__func__, 6, "Stdev is %f\n", stats->sampleStdev); + psF32 minClipValue = -minClipSigma*stats->sampleStdev; + psF32 maxClipValue = +maxClipSigma*stats->sampleStdev; + + // 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) || + (resid->data.F64[i] - stats->sampleMedian < minClipValue)) { + if (f->type.type == PS_TYPE_F64) { + psTrace(__func__, 6, "Masking element %d (%f). resid->data.F64[%d] is %f\n", + i, fit->data.F64[i], i, resid->data.F64[i]); + } else { + psTrace(__func__, 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.U8[i] |= 0x01; + } + continue; + } + Nkeep++; + } + + // + // XXX: We should probably exit this loop if no new elements were masked + // since the polynomial fit won't change. + // + psTrace(__func__, 6, "keeping %d of %d pts for fit\n", Nkeep, x->n); + psFree(fit); + } + + // Free psVectors that were created for NULL arguments. + if (xIn == NULL) { + psFree(x); + } + // Free other local temporary variables + psFree(resid); + + psTrace(__func__, 3, "---- %s() end ----\n", __func__); + return (poly); +} + + +/****************************************************************************** + ****************************************************************************** + 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. + +// XXX: What about the maskValue? + *****************************************************************************/ +psPolynomial2D* VectorFitPolynomial2DOrd( + psPolynomial2D* myPoly, + const psVector* mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y) +{ + psTrace(__func__, 4, "---- VectorFitPolynomial2DOrd() begin ----\n"); + // These ASSERTS are redundant. + PS_ASSERT_POLY_NON_NULL(myPoly, NULL); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, NULL); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nY, NULL); + + PS_ASSERT_VECTOR_NON_NULL(f, NULL); + PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, NULL); + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(y, fErr, NULL); + PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F64, NULL); + } + PS_ASSERT_VECTOR_NON_NULL(x, NULL); + PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL); + PS_ASSERT_VECTOR_NON_NULL(y, NULL); + PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL); + if (mask != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(y, mask, NULL); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL); + } + + // I think this is 1 dimension down + psImage* A = NULL; + psVector* B = NULL; + psImage* 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; + nTerm = nXterm * nYterm; + + A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); + B = psVectorAlloc(nTerm, PS_TYPE_F64); + + // + // Initialize data structures. + // XXX: Use psLib function. + // + PS_VECTOR_SET_F64(B, 0.0); + PS_IMAGE_SET_F64(A, 0.0); + + // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1) + + // 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 = BuildSums2D(Sums, x->data.F64[k], y->data.F64[k], nXterm, nYterm); + + 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 n = 0; n < nXterm; n++) { + for (int m = 0; m < nYterm; m++) { + B->data.F64[n+m*nXterm] += f->data.F64[k] * Sums->data.F64[n][m] * wt; + } + } + + for (int i = 0; i < nXterm; i++) { + for (int j = 0; j < nYterm; j++) { + for (int n = 0; n < nXterm; n++) { + for (int m = 0; m < nYterm; m++) { + A->data.F64[i+j*nXterm][n+m*nXterm] += Sums->data.F64[i+n][j+m] * wt; + } + } + } + } + } + + // does the solution in place + // XXX: Check error codes! + psGaussJordan (A, B); + + // select the appropriate solution entries + for (int n = 0; n < nXterm; n++) { + for (int m = 0; m < nYterm; m++) { + myPoly->coeff[n][m] = B->data.F64[n+m*nXterm]; + } + } + + psFree(A); + psFree(B); + psFree(Sums); + + psTrace(__func__, 4, "---- VectorFitPolynomial2DOrd() end ----\n"); + return (myPoly); +} + +/****************************************************************************** +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. + *****************************************************************************/ +psPolynomial2D *psVectorFitPolynomial2D( + psPolynomial2D *poly, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y) +{ + // Internal pointers for possibly NULL or mis-typed vectors. + psVector *x64 = NULL; + psVector *y64 = NULL; + psVector *f64 = NULL; + psVector *fErr64 = NULL; + + PS_ASSERT_POLY_NON_NULL(poly, NULL); + PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, NULL); + + PS_ASSERT_VECTOR_NON_NULL(f, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL); + + if (mask != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL); + } + PS_ASSERT_VECTOR_NON_NULL(x, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL); + PS_ASSERT_VECTOR_NON_NULL(y, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL); + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, NULL); + } + + // + // Convert input vectors to F64 if necessary. + // + if (f->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(f64, f); + } else { + f64 = (psVector *) f; + } + + if (x->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(x64, x); + } else { + x64 = (psVector *) x; + } + + if (y->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(y64, y); + } else { + y64 = (psVector *) y; + } + + // + // fErr + // + if (fErr != NULL) { + if (fErr->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(fErr64, fErr); + } else { + fErr64 = (psVector *) fErr; + } + } + + if (poly->type == PS_POLYNOMIAL_ORD) { + poly = VectorFitPolynomial2DOrd(poly, mask, maskValue, f64, fErr64, x64, y64); + if (poly == NULL) { + psError(PS_ERR_UNKNOWN, true, "Could not fit polynomial. Returning NULL.\n"); + // Free psVectors that were created for NULL arguments. + if (f->type.type != PS_TYPE_F64) { + psFree(f64); + } + + if (x->type.type != PS_TYPE_F64) { + psFree(x64); + } + + if (y->type.type != PS_TYPE_F64) { + psFree(y64); + } + + if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) { + psFree(fErr64); + } + return(NULL); + } + } else if (poly->type == PS_POLYNOMIAL_CHEB) { + if (mask != NULL) { + psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring mask and maskValue with Chebyshev polynomials.\n"); + } + psLogMsg(__func__, PS_LOG_WARN, "WARNING: 2-D Chebyshev polynomial vector fitting has not been implemented. Returning NULL.\n"); + psFree(poly); + poly = NULL; + } else { + // Free psVectors that were created for NULL arguments. + if (f->type.type != PS_TYPE_F64) { + psFree(f64); + } + + if (x->type.type != PS_TYPE_F64) { + psFree(x64); + } + + if (y->type.type != PS_TYPE_F64) { + psFree(y64); + } + + if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) { + psFree(fErr64); + } + psError(PS_ERR_UNKNOWN, true, "Incorrect polynomial type. Returning NULL.\n"); + } + + + // Free psVectors that were created for NULL arguments. + if (f->type.type != PS_TYPE_F64) { + psFree(f64); + } + + if (x->type.type != PS_TYPE_F64) { + psFree(x64); + } + + if (y->type.type != PS_TYPE_F64) { + psFree(y64); + } + + if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) { + psFree(fErr64); + } + + return(poly); +} + +psPolynomial2D *psVectorClipFitPolynomial2D( + psPolynomial2D *poly, + psStats *stats, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y) +{ + psTrace(__func__, 3, "---- %s() begin ----\n", __func__); + PS_ASSERT_POLY_NON_NULL(poly, NULL); + PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, NULL); + PS_ASSERT_PTR_NON_NULL(stats, NULL); + PS_ASSERT_VECTOR_NON_NULL(mask, NULL); + PS_ASSERT_VECTOR_NON_NULL(f, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL); + if (mask != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL); + } + PS_ASSERT_VECTOR_NON_NULL(x, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL); + PS_ASSERT_VECTOR_TYPE(x, f->type.type, NULL); + + PS_ASSERT_VECTOR_NON_NULL(y, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL); + PS_ASSERT_VECTOR_TYPE(y, f->type.type, NULL); + + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL); + PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, 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 *resid = psVectorAlloc(f->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); + psTrace(__func__, 4, "stats->clipIter is %d\n", stats->clipIter); + psTrace(__func__, 4, "(minClipSigma, maxClipSigma) is (%.2f, %.2f)\n", minClipSigma, maxClipSigma); + + for (int N = 0; N < stats->clipIter; N++) { + psTrace(__func__, 6, "Loop iteration %d. Calling psVectorFitPolynomial1D()\n"); + int Nkeep = 0; + if (psTraceGetLevel(__func__) >= 6) { + if (mask != NULL) { + for (psS32 i = 0 ; i < mask->n ; i++) { + psTrace(__func__, 6, "mask[%d] is %d\n", i, mask->data.U8[i]); + } + } + } + + poly = psVectorFitPolynomial2D(poly, mask, maskValue, f, fErr, x, y); + // XXX: Check error codes + psVector *fit = psPolynomial2DEvalVector(poly, x, y); + + 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(__func__) >= 6) { + if (mask != NULL) { + for (psS32 i = 0 ; i < mask->n ; i++) { + if (!((mask != NULL) && (mask->data.U8[i] & maskValue))) { + psTrace(__func__, 6, "(f, fit)[%d] is (%f, %f). resid is (%f)\n", + i, f->data.F32[i], fit->data.F32[i], resid->data.F64[i]); + } + } + } + } + + // XXX: Check error codes + stats = psVectorStats(stats, resid, NULL, mask, maskValue); + psTrace(__func__, 6, "Median is %f\n", stats->sampleMedian); + psTrace(__func__, 6, "Stdev is %f\n", stats->sampleStdev); + float minClipValue = -minClipSigma*stats->sampleStdev; + float maxClipValue = +maxClipSigma*stats->sampleStdev; + + // 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) || + (resid->data.F64[i] - stats->sampleMedian < minClipValue)) { + if (fit->type.type == PS_TYPE_F64) { + psTrace(__func__, 6, "Masking element %d (%f). resid->data.F64[%d] is %f\n", + i, fit->data.F64[i], i, resid->data.F64[i]); + } else { + psTrace(__func__, 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.U8[i] |= 0x01; + } + continue; + } + Nkeep++; + } + + psTrace(__func__, 6, "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); + } + + psTrace(__func__, 3, "---- %s() end ----\n", __func__); + return(poly); +} + + +/****************************************************************************** + ****************************************************************************** + 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. + +XXX: This routine needs to be written. Currently, this is simply a shell. We +can assume that all vectors have been converted to F64, that (f, x, y, z) are +non-null and F64. fErr may be NULL, but will be F64 is not. + *****************************************************************************/ +psPolynomial3D* VectorFitPolynomial3DOrd( + psPolynomial3D* myPoly, + const psVector* mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z) +{ + psTrace(__func__, 4, "---- VectorFitPolynomial3DOrd() begin ----\n"); + // These ASSERTS are redundant. + PS_ASSERT_POLY_NON_NULL(myPoly, NULL); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, NULL); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nY, NULL); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nZ, NULL); + + PS_ASSERT_VECTOR_NON_NULL(f, NULL); + PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, NULL); + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(y, fErr, NULL); + PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F64, NULL); + } + PS_ASSERT_VECTOR_NON_NULL(x, NULL); + PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL); + PS_ASSERT_VECTOR_NON_NULL(y, NULL); + PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL); + PS_ASSERT_VECTOR_NON_NULL(z, NULL); + PS_ASSERT_VECTOR_TYPE(z, PS_TYPE_F64, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, NULL); + if (mask != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, 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; + } else { + 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; + } + } + } + } + } + } + + if (0) { + // does the solution in place + // The matrices were overflowing, so I switched to LUD. + 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]); + } + } + } + } else { + // LUD version of the fit + psImage *ALUD = NULL; + psVector* outPerm = NULL; + psVector* coeffs = NULL; + + ALUD = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); + ALUD = psMatrixLUD(ALUD, &outPerm, A); + coeffs = psMatrixLUSolve(coeffs, ALUD, B, outPerm); + + // 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] = coeffs->data.F64[nx]; + myPoly->coeffErr[ix][iy][iz] = sqrt(A->data.F64[nx][nx]); + } + } + } + + psFree(ALUD); + psFree(coeffs); + psFree(outPerm); + } + 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(__func__, 4, "---- VectorFitPolynomial3DOrd() end ----\n"); + return (myPoly); +} + +/****************************************************************************** +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. + *****************************************************************************/ +psPolynomial3D *psVectorFitPolynomial3D( + psPolynomial3D *poly, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z) +{ + // Internal pointers for possibly NULL or mis-typed vectors. + psVector *x64 = NULL; + psVector *y64 = NULL; + psVector *z64 = NULL; + psVector *f64 = NULL; + psVector *fErr64 = NULL; + + PS_ASSERT_POLY_NON_NULL(poly, NULL); + PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, NULL); + + PS_ASSERT_VECTOR_NON_NULL(f, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL); + if (mask != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL); + } + PS_ASSERT_VECTOR_NON_NULL(x, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL); + // PS_ASSERT_VECTOR_TYPE(x, f->type.type, NULL); + PS_ASSERT_VECTOR_NON_NULL(y, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL); + // PS_ASSERT_VECTOR_TYPE(y, f->type.type, NULL); + PS_ASSERT_VECTOR_NON_NULL(z, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, NULL); + // PS_ASSERT_VECTOR_TYPE(z, f->type.type, NULL); + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL); + // PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, NULL); + } + + // + // Convert input vectors to F64 if necessary. + // + if (f->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(f64, f); + } else { + f64 = (psVector *) f; + } + if (x->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(x64, x); + } else { + x64 = (psVector *) x; + } + if (y->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(y64, y); + } else { + y64 = (psVector *) y; + } + + if (z->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(z64, z); + } else { + z64 = (psVector *) z; + } + + if (fErr != NULL) { + if (fErr->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(fErr64, fErr); + } else { + fErr64 = (psVector *) fErr; + } + } + + if (poly->type == PS_POLYNOMIAL_ORD) { + poly = VectorFitPolynomial3DOrd(poly, mask, maskValue, f64, fErr64, x64, y64, z64); + if (poly == NULL) { + psError(PS_ERR_UNKNOWN, true, "Could not fit polynomial. Returning NULL.\n"); + // Free psVectors that were created for NULL arguments. + if (f->type.type != PS_TYPE_F64) { + psFree(f64); + } + + if (x->type.type != PS_TYPE_F64) { + psFree(x64); + } + + if (y->type.type != PS_TYPE_F64) { + psFree(y64); + } + + if (z->type.type != PS_TYPE_F64) { + psFree(z64); + } + + if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) { + psFree(fErr64); + } + return(NULL); + } + } else if (poly->type == PS_POLYNOMIAL_CHEB) { + if (mask != NULL) { + psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring mask and maskValue with Chebyshev polynomials.\n"); + } + psLogMsg(__func__, PS_LOG_WARN, "WARNING: 3-D Chebyshev polynomial vector fitting has not been implemented. Returning NULL.\n"); + psFree(poly); + poly = NULL; + } else { + // Free psVectors that were created for NULL arguments. + if (f->type.type != PS_TYPE_F64) { + psFree(f64); + } + + if (x->type.type != PS_TYPE_F64) { + psFree(x64); + } + + if (y->type.type != PS_TYPE_F64) { + psFree(y64); + } + + if (z->type.type != PS_TYPE_F64) { + psFree(z64); + } + + if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) { + psFree(fErr64); + } + psError(PS_ERR_UNKNOWN, true, "Incorrect polynomial type. Returning NULL.\n"); + } + + + // Free psVectors that were created for NULL arguments. + if (f->type.type != PS_TYPE_F64) { + psFree(f64); + } + + if (x->type.type != PS_TYPE_F64) { + psFree(x64); + } + + if (y->type.type != PS_TYPE_F64) { + psFree(y64); + } + + if (z->type.type != PS_TYPE_F64) { + psFree(z64); + } + + if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) { + psFree(fErr64); + } + + return(poly); +} + +psPolynomial3D *psVectorClipFitPolynomial3D( + psPolynomial3D *poly, + psStats *stats, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z) +{ + psTrace(__func__, 3, "---- %s() begin ----\n", __func__); + PS_ASSERT_POLY_NON_NULL(poly, NULL); + PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, NULL); + PS_ASSERT_PTR_NON_NULL(stats, NULL); + PS_ASSERT_VECTOR_NON_NULL(mask, NULL); + PS_ASSERT_VECTOR_NON_NULL(f, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL); + if (mask != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL); + } + PS_ASSERT_VECTOR_NON_NULL(x, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL); + PS_ASSERT_VECTOR_TYPE(x, f->type.type, NULL); + + PS_ASSERT_VECTOR_NON_NULL(y, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL); + PS_ASSERT_VECTOR_TYPE(y, f->type.type, NULL); + + PS_ASSERT_VECTOR_NON_NULL(z, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, NULL); + PS_ASSERT_VECTOR_TYPE(z, f->type.type, NULL); + + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL); + PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, 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(f->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); + psTrace(__func__, 4, "stats->clipIter is %d\n", stats->clipIter); + psTrace(__func__, 4, "(minClipSigma, maxClipSigma) is (%.2f, %.2f)\n", minClipSigma, maxClipSigma); + + for (int N = 0; N < stats->clipIter; N++) { + psTrace(__func__, 6, "Loop iteration %d. Calling psVectorFitPolynomial1D()\n"); + int Nkeep = 0; + if (psTraceGetLevel(__func__) >= 6) { + if (mask != NULL) { + for (psS32 i = 0 ; i < mask->n ; i++) { + psTrace(__func__, 6, "mask[%d] is %d\n", i, mask->data.U8[i]); + } + } + } + + poly = psVectorFitPolynomial3D(poly, mask, maskValue, f, fErr, x, y, z); + // XXX: Check error codes + fit = psPolynomial3DEvalVector(poly, x, y, z); + 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(__func__) >= 6) { + if (mask != NULL) { + for (psS32 i = 0 ; i < mask->n ; i++) { + if (!((mask != NULL) && (mask->data.U8[i] & maskValue))) { + psTrace(__func__, 6, "(f, fit)[%d] is (%f, %f). resid is (%f)\n", + i, f->data.F32[i], fit->data.F32[i], resid->data.F64[i]); + } + } + } + } + + // XXX: Check error codes + stats = psVectorStats(stats, resid, NULL, mask, maskValue); + psTrace(__func__, 6, "Median is %f\n", stats->sampleMedian); + psTrace(__func__, 6, "Stdev is %f\n", stats->sampleStdev); + float minClipValue = -minClipSigma*stats->sampleStdev; + float maxClipValue = +maxClipSigma*stats->sampleStdev; + + // 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) || + (resid->data.F64[i] - stats->sampleMedian < minClipValue)) { + if (f->type.type == PS_TYPE_F64) { + psTrace(__func__, 6, "Masking element %d (%f). resid->data.F64[%d] is %f\n", + i, fit->data.F64[i], i, resid->data.F64[i]); + } else { + psTrace(__func__, 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.U8[i] |= 0x01; + } + continue; + } + Nkeep++; + } + + psTrace(__func__, 6, "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); + } + + psTrace(__func__, 3, "---- %s() end ----\n", __func__); + return(poly); +} + +/****************************************************************************** + ****************************************************************************** + 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. + +XXX: This routine needs to be written. Currently, this is simply a shell. We +can assume that all vectors have been converted to F64, that (f, x, y, z) are +non-null and F64. fErr may be NULL, but will be F64 is not. + *****************************************************************************/ +psPolynomial4D* VectorFitPolynomial4DOrd( + psPolynomial4D* myPoly, + const psVector* mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z, + const psVector *t) +{ + psTrace(__func__, 4, "---- %s() begin ----\n", __func__); + // These ASSERTS are redundant. + PS_ASSERT_POLY_NON_NULL(myPoly, NULL); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, NULL); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nY, NULL); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nZ, NULL); + PS_ASSERT_INT_NONNEGATIVE(myPoly->nT, NULL); + PS_ASSERT_VECTOR_NON_NULL(f, NULL); + PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, NULL); + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(y, fErr, NULL); + PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F64, NULL); + } + PS_ASSERT_VECTOR_NON_NULL(x, NULL); + PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL); + PS_ASSERT_VECTOR_NON_NULL(y, NULL); + PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL); + PS_ASSERT_VECTOR_NON_NULL(z, NULL); + PS_ASSERT_VECTOR_TYPE(z, PS_TYPE_F64, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, NULL); + PS_ASSERT_VECTOR_NON_NULL(t, NULL); + PS_ASSERT_VECTOR_TYPE(t, PS_TYPE_F64, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, t, NULL); + if (mask != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(y, mask, NULL); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, 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; + } + } + } + } + } + } + } + } + + + if (0) { + // does the solution in place + // XXX: The GaussJordan version was overflowing, so I'm using LUD. + 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]); + } + } + } + } + } else { + // LUD version of the fit + psImage *ALUD = NULL; + psVector* outPerm = NULL; + psVector* coeffs = NULL; + + ALUD = psImageAlloc(nTerm, nTerm, PS_TYPE_F64); + ALUD = psMatrixLUD(ALUD, &outPerm, A); + coeffs = psMatrixLUSolve(coeffs, ALUD, B, outPerm); + + // 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] = coeffs->data.F64[nx]; + myPoly->coeffErr[ix][iy][iz][it] = sqrt(A->data.F64[nx][nx]); + } + } + } + } + + psFree(ALUD); + psFree(coeffs); + psFree(outPerm); + + } + + 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(__func__, 4, "---- %s() end ----\n", __func__); + return (myPoly); +} + +/****************************************************************************** +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. + *****************************************************************************/ +psPolynomial4D *psVectorFitPolynomial4D( + psPolynomial4D *poly, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z, + const psVector *t) +{ + // Internal pointers for possibly NULL or mis-typed vectors. + psVector *x64 = NULL; + psVector *y64 = NULL; + psVector *z64 = NULL; + psVector *t64 = NULL; + psVector *f64 = NULL; + psVector *fErr64 = NULL; + + PS_ASSERT_POLY_NON_NULL(poly, NULL); + PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, NULL); + + PS_ASSERT_VECTOR_NON_NULL(f, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL); + if (mask != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL); + } + PS_ASSERT_VECTOR_NON_NULL(x, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL); + PS_ASSERT_VECTOR_NON_NULL(y, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL); + PS_ASSERT_VECTOR_NON_NULL(z, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, NULL); + PS_ASSERT_VECTOR_NON_NULL(t, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, t, NULL); + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, NULL); + } + + + // + // Convert input vector to F64 if necessary. + // + if (f->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(f64, f); + } else { + f64 = (psVector *) f; + } + if (x->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(x64, x); + } else { + x64 = (psVector *) x; + } + if (y->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(y64, y); + } else { + y64 = (psVector *) y; + } + if (z->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(z64, z); + } else { + z64 = (psVector *) z; + } + if (t->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(t64, t); + } else { + t64 = (psVector *) t; + } + // + // fErr + // + if (fErr != NULL) { + if (fErr->type.type != PS_TYPE_F64) { + PS_VECTOR_GEN_F64_FROM_F32(fErr64, fErr); + } else { + fErr64 = (psVector *) fErr; + } + } + + if (poly->type == PS_POLYNOMIAL_ORD) { + poly = VectorFitPolynomial4DOrd(poly, mask, maskValue, f64, fErr64, x64, y64, z64, t64); + if (poly == NULL) { + psError(PS_ERR_UNKNOWN, true, "Could not fit polynomial. Returning NULL.\n"); + // Free psVectors that were created for NULL arguments. + if (f->type.type != PS_TYPE_F64) { + psFree(f64); + } + + if (x->type.type != PS_TYPE_F64) { + psFree(x64); + } + + if (y->type.type != PS_TYPE_F64) { + psFree(y64); + } + + if (z->type.type != PS_TYPE_F64) { + psFree(z64); + } + + if (t->type.type != PS_TYPE_F64) { + psFree(t64); + } + + if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) { + psFree(fErr64); + } + return(NULL); + } + } else if (poly->type == PS_POLYNOMIAL_CHEB) { + if (mask != NULL) { + psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring mask and maskValue with Chebyshev polynomials.\n"); + } + psLogMsg(__func__, PS_LOG_WARN, "WARNING: 4-D Chebyshev polynomial vector fitting has not been implemented. Returning NULL.\n"); + psFree(poly); + poly = NULL; + } else { + // Free psVectors that were created for NULL arguments. + if (f->type.type != PS_TYPE_F64) { + psFree(f64); + } + + if (x->type.type != PS_TYPE_F64) { + psFree(x64); + } + + if (y->type.type != PS_TYPE_F64) { + psFree(y64); + } + + if (z->type.type != PS_TYPE_F64) { + psFree(z64); + } + + if (t->type.type != PS_TYPE_F64) { + psFree(t64); + } + + if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) { + psFree(fErr64); + } + psError(PS_ERR_UNKNOWN, true, "Incorrect polynomial type. Returning NULL.\n"); + } + + + // Free psVectors that were created for NULL arguments. + if (f->type.type != PS_TYPE_F64) { + psFree(f64); + } + + if (x->type.type != PS_TYPE_F64) { + psFree(x64); + } + + if (y->type.type != PS_TYPE_F64) { + psFree(y64); + } + + if (z->type.type != PS_TYPE_F64) { + psFree(z64); + } + + if (t->type.type != PS_TYPE_F64) { + psFree(t64); + } + + if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) { + psFree(fErr64); + } + + return(poly); +} + + +// XXX: Add F64 support. +psPolynomial4D *psVectorClipFitPolynomial4D( + psPolynomial4D *poly, + psStats *stats, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z, + const psVector *t) +{ + psTrace(__func__, 3, "---- %s() begin ----\n", __func__); + PS_ASSERT_POLY_NON_NULL(poly, NULL); + PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, NULL); + PS_ASSERT_PTR_NON_NULL(stats, NULL); + PS_ASSERT_VECTOR_NON_NULL(mask, NULL); + PS_ASSERT_VECTOR_NON_NULL(f, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL); + if (mask != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL); + } + PS_ASSERT_VECTOR_NON_NULL(x, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL); + PS_ASSERT_VECTOR_TYPE(x, f->type.type, NULL); + + PS_ASSERT_VECTOR_NON_NULL(y, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL); + PS_ASSERT_VECTOR_TYPE(y, f->type.type, NULL); + + PS_ASSERT_VECTOR_NON_NULL(z, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, NULL); + PS_ASSERT_VECTOR_TYPE(z, f->type.type, NULL); + + PS_ASSERT_VECTOR_NON_NULL(t, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(f, t, NULL); + PS_ASSERT_VECTOR_TYPE(t, f->type.type, NULL); + + if (fErr != NULL) { + PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL); + PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, 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(f->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); + psTrace(__func__, 4, "stats->clipIter is %d\n", stats->clipIter); + psTrace(__func__, 4, "(minClipSigma, maxClipSigma) is (%.2f, %.2f)\n", minClipSigma, maxClipSigma); + + for (int N = 0; N < stats->clipIter; N++) { + psTrace(__func__, 6, "Loop iteration %d. Calling psVectorFitPolynomial1D()\n"); + int Nkeep = 0; + if (psTraceGetLevel(__func__) >= 6) { + if (mask != NULL) { + for (psS32 i = 0 ; i < mask->n ; i++) { + psTrace(__func__, 6, "mask[%d] is %d\n", i, mask->data.U8[i]); + } + } + } + + poly = psVectorFitPolynomial4D (poly, mask, maskValue, f, fErr, x, y, z, t); + // XXX: Check error codes + fit = psPolynomial4DEvalVector (poly, x, y, z, t); + // XXX: This is coded differently for 1D, 2D. Consolidate. + 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]; + } + } + // XXX: Check error codes + + if (psTraceGetLevel(__func__) >= 6) { + if (mask != NULL) { + for (psS32 i = 0 ; i < mask->n ; i++) { + if (!((mask != NULL) && (mask->data.U8[i] & maskValue))) { + psTrace(__func__, 6, "(f, fit)[%d] is (%f, %f). resid is (%f)\n", + i, f->data.F32[i], fit->data.F32[i], resid->data.F64[i]); + } + } + } + } + + // XXX: Check error codes + stats = psVectorStats (stats, resid, NULL, mask, maskValue); + psTrace(__func__, 6, "Median is %f\n", stats->sampleMedian); + psTrace(__func__, 6, "Stdev is %f\n", stats->sampleStdev); + float minClipValue = -minClipSigma*stats->sampleStdev; + float maxClipValue = +maxClipSigma*stats->sampleStdev; + + // 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) || + (resid->data.F64[i] - stats->sampleMedian < minClipValue)) { + if (f->type.type == PS_TYPE_F64) { + psTrace(__func__, 6, "Masking element %d (%f). resid->data.F64[%d] is %f\n", + i, fit->data.F64[i], i, resid->data.F64[i]); + } else { + psTrace(__func__, 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.U8[i] |= 0x01; + } + continue; + } + + Nkeep++; + } + + psTrace(__func__, 6, "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); + } + + psTrace(__func__, 3, "---- %s() end ----\n", __func__); + return(poly); +} Index: /trunk/psLib/src/math/psMinimizePolyFit.h =================================================================== --- /trunk/psLib/src/math/psMinimizePolyFit.h (revision 6101) +++ /trunk/psLib/src/math/psMinimizePolyFit.h (revision 6101) @@ -0,0 +1,148 @@ +/** @file psMinimizePolyFit.c + * \brief basic minimization functions + * @ingroup Math + * + * This file will contain function prototypes for various + * 1-D polynomial fitting routines. + * + * @author GLG, MHPCC + * + * XXX: Must Doxygenate. + * + * @version $Revision: 1.1 $ $Name: not supported by cvs2svn $ + * @date $Date: 2006-01-21 02:43:31 $ + * + * Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii + * + */ + +#ifndef PS_MINIMIZE_POLYFIT_H +#define PS_MINIMIZE_POLYFIT_H + +/** \file psMinimize.h + * \brief minimization operations + * \ingroup Stats + */ +/** \addtogroup Stats + * \{ + */ + +#include "psVector.h" +#include "psMemory.h" +#include "psArray.h" +#include "psImage.h" +#include "psMatrix.h" +#include "psPolynomial.h" +#include "psSpline.h" +#include "psStats.h" +#include "psTrace.h" +#include "psError.h" +#include "psConstants.h" + +/** Derive a polynomial fit. + * + * psVectorFitPolynomial1d returns the polynomial that best fits the + * observations. The input parameters are a polynomial that specifies the + * fit order, myPoly, which will be altered and returned with the best-fit + * coefficients; and the observations, x, y and yErr. The independent + * variable list, x may be NULL, in which case the vector index is used. + * The dependent variable error, yErr may be null, in which case the solution + * is determined in the assumption that all data errors are equal. This + * function must be valid only for types psF32, psF64. + * + * @return psPolynomial1D* polynomial fit + */ + +psPolynomial1D *psVectorFitPolynomial1D( + psPolynomial1D *poly, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x +); + +psPolynomial2D *psVectorFitPolynomial2D( + psPolynomial2D *poly, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y +); + +psPolynomial3D *psVectorFitPolynomial3D( + psPolynomial3D *poly, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z +); + +psPolynomial4D *psVectorFitPolynomial4D( + psPolynomial4D *poly, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z, + const psVector *t +); + + +psPolynomial1D *psVectorClipFitPolynomial1D( + psPolynomial1D *poly, + psStats *stats, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x +); + +psPolynomial2D *psVectorClipFitPolynomial2D( + psPolynomial2D *poly, + psStats *stats, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y +); + +psPolynomial3D *psVectorClipFitPolynomial3D( + psPolynomial3D *poly, + psStats *stats, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z +); + +psPolynomial4D *psVectorClipFitPolynomial4D( + psPolynomial4D *poly, + psStats *stats, + const psVector *mask, + psMaskType maskValue, + const psVector *f, + const psVector *fErr, + const psVector *x, + const psVector *y, + const psVector *z, + const psVector *t +); + +/* \} */// End of MathGroup Functions + +#endif // #ifndef PS_MINIMIZE_POLYFIT_H + Index: /trunk/psLib/src/math/psMinimizePowell.c =================================================================== --- /trunk/psLib/src/math/psMinimizePowell.c (revision 6101) +++ /trunk/psLib/src/math/psMinimizePowell.c (revision 6101) @@ -0,0 +1,761 @@ +/** @file psMinimize.c + * \brief basic minimization functions + * @ingroup Math + * + * This file will contain functions to minimize an arbitrary function at + * a data point, fit an arbitrary function to a set of data points, and + * fit a 1-D polynomial to a set of data points. + * + * @author GLG, MHPCC + * + * @version $Revision: 1.1 $ $Name: not supported by cvs2svn $ + * @date $Date: 2006-01-21 02:43:31 $ + * + * Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii + * + * XXX: must follow coding name standards on local functions. + * XXX: put local functions in front. + * + */ +/*****************************************************************************/ +/* INCLUDE FILES */ +/*****************************************************************************/ +#include +#include +#include + +#include "psMinimizePowell.h" +#include "psStats.h" +#include "psImage.h" +#include "psImageStructManip.h" +#include "psLogMsg.h" +/*****************************************************************************/ +/* DEFINE STATEMENTS */ +/*****************************************************************************/ + +/*****************************************************************************/ +/* TYPE DEFINITIONS */ +/*****************************************************************************/ + +/*****************************************************************************/ +/* GLOBAL VARIABLES */ +/* XXX: Do these conform to code standard? */ +/*****************************************************************************/ +static psMinimizeChi2PowellFunc Chi2PowellFunc = NULL; +static psVector *myValue; +static psVector *myError; +/*****************************************************************************/ +/* FILE STATIC VARIABLES */ +/*****************************************************************************/ + +/*****************************************************************************/ +/* FUNCTION IMPLEMENTATION - LOCAL */ +/*****************************************************************************/ + +// This macro takes as input the vector BASE and adds a multiple of the vector +// LINE to it. We assume BASEMASK is non-null. +#define PS_VECTOR_ADD_MULTIPLE(BASE, BASEMASK, LINE, OUT, MUL) \ +for (psS32 i=0;in;i++) { \ + if (BASEMASK->data.U8[i] == 0) { \ + OUT->data.F32[i] = BASE->data.F32[i] + (MUL * LINE->data.F32[i]); \ + } else { \ + OUT->data.F32[i] = BASE->data.F32[i]; \ + } \ +} \ + +#define PS_VECTOR_F32_CHECK_ZERO_VECTOR(IN, BOOL_VAR) \ +BOOL_VAR = true; \ +for (psS32 i=0;in;i++) { \ + if (fabs(IN->data.F32[i]) >= FLT_EPSILON) { \ + BOOL_VAR = false; \ + break; \ + } \ +} \ + +#define PS_VECTOR_WITH_MASK_F32_CHECK_ZERO_VECTOR(IN, INMASK, BOOL_VAR) \ +BOOL_VAR = true; \ +for (psS32 i=0;in;i++) { \ + if ((INMASK->data.U8[i] == 0) && (fabs(IN->data.F32[i]) >= FLT_EPSILON)) { \ + BOOL_VAR = false; \ + break; \ + } \ +} \ + + +/****************************************************************************** +p_psDetermineBracket(): This routine takes as input an arbitrary function, +and the parameter to vary, and the line along which it must vary. This +function produces as output a bracket [a, b, c] such that +f(param + b * line) < f(param + a * line) +f(param + b * line) < f(param + c * line) +a < b < c + +Algorithm: + +XXX completely ad hoc: +start with the user-supplied starting parameter and +call that b. Calculate a/c as a fractional amount smaller/larger than b. +Repeat this process until a local minimum is found. + +XXX: +new algorithm: +start at x=0, expand in one direction until the function +decreases. Then you have two points in the bracket. Keep going until it +increases, or x is too large. If thst does not work, expand in the other +direction. + +XXX: +This is F32 only. Must add +F64 support (actually, make the defaults F64, +and convert F32 vectors to F64). + +XXX: +output bracket vector should be an input as well. +*****************************************************************************/ +psVector *p_psDetermineBracket( + psVector *params, + psVector *line, + const psVector *paramMask, + const psArray *coords, + psMinimizePowellFunc func) +{ + psF32 a = 0.0; + psF32 b = 0.0; + psF32 c = 0.0; + psF32 fa = 0.0; + psF32 fb = 0.0; + psF32 fc = 0.0; + psS32 iter = 100; + psF32 aDir = 0.0; + psF32 cDir = 0.0; + psF32 new_aDir = 0.0; + psF32 new_cDir = 0.0; + psVector *bracket = psVectorAlloc(3, PS_TYPE_F32); + psF32 stepSize = PS_DETERMINE_BRACKET_STEP_SIZE; + psVector *tmp = NULL; + psBool boolLineIsNull = true; + + psTrace(__func__, 4, "---- p_psDetermineBracket() begin ----\n"); + + // If the line vector is zero, then return NULL. + PS_VECTOR_WITH_MASK_F32_CHECK_ZERO_VECTOR(params, paramMask, boolLineIsNull); + if (boolLineIsNull == true) { + psTrace(__func__, 2, "p_psDetermineBracket() called with zero line vector.\n"); + psTrace(__func__, 4, "---- p_psDetermineBracket() end (NULL) ----\n"); + psFree(bracket); + return(NULL); + } + + tmp = psVectorAlloc(params->n, PS_TYPE_F32); + + b = 0; + a = -stepSize; + c = stepSize; + + PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmp, a); + fa = func(tmp, coords); + + PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmp, b); + fb = func(tmp, coords); + + PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmp, c); + fc = func(tmp, coords); + + if (fa < fb) { + aDir = -1; + } else { + aDir = 1; + } + + if (fc < fb) { + cDir = -1; + } else { + cDir = 1; + } + + psTrace(__func__, 6, "(a, b, c) is (%f %f %f) (fa, fb, fc) is (%f %f %f)\n", a, b, c, fa, fb, fc); + + while (iter > 0) { + psTrace(__func__, 6, "psDetermineBracket(): iteration %d\n", iter); + if ((fb < fa) && (fb < fc)) { + bracket->data.F32[0] = a; + bracket->data.F32[1] = b; + bracket->data.F32[2] = c; + psFree(tmp); + psTrace(__func__, 6, "---- p_psDetermineBracket() end ----\n"); + return(bracket); + } + stepSize*= (1.0 + stepSize); + a =- stepSize; + c =+ stepSize; + + PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmp, a); + fa = func(tmp, coords); + + PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmp, c); + fc = func(tmp, coords); + + psTrace(__func__, 6, "Iter(%d): (a, b, c) is (%f %f %f) (fa, fb, fc) is (%f %f %f)\n", iter, a, b, c, fa, fb, fc); + + if (fa < fb) { + new_aDir = -1; + } else { + new_aDir = 1; + } + + if (fc < fb) { + new_cDir = -1; + } else { + new_cDir = 1; + } + if ((new_aDir == 1) && (aDir == -1)) { + bracket->data.F32[0] = a; + bracket->data.F32[1] = b; + bracket->data.F32[2] = c; + psFree(tmp); + psTrace(__func__, 4, "---- p_psDetermineBracket() end ----\n"); + return(bracket); + } + + if ((new_cDir == 1) && (cDir == -1)) { + bracket->data.F32[0] = a; + bracket->data.F32[1] = b; + bracket->data.F32[2] = c; + psFree(tmp); + psTrace(__func__, 4, "---- p_psDetermineBracket() end ----\n"); + return(bracket); + } + aDir = new_aDir; + cDir = new_cDir; + iter--; + } + psFree(tmp); + psFree(bracket); + psTrace(__func__, 4, "---- p_psDetermineBracket() end (NULL) ----\n"); + return(NULL); +} + + +#define RETURN_FINAL_BRACKET(d) \ +if (a < c) { \ + bracket->data.F32[0] = a; \ + bracket->data.F32[1] = b; \ + bracket->data.F32[2] = c; \ +} else { \ + bracket->data.F32[0] = c; \ + bracket->data.F32[1] = b; \ + bracket->data.F32[2] = a; \ +} \ +psTrace(__func__, 4, "Final bracket (a, b, c) is (%f %f %f) (fa, fb, fc) is (%f %f %f)\n", a, b, c, fa, fb, fc); \ +psTrace(__func__, 4, "---- p_psDetermineBracket() end ----\n"); \ +return(bracket); \ + +#define PS_DETERMINE_BRACKET_MAX_ITERATIONS 100 +psVector *p_psDetermineBracket2( + psVector *params, + psVector *line, + const psVector *paramMask, + const psArray *coords, + psMinimizePowellFunc func) +{ + psF32 a = 0.0; + psF32 b = 0.0; + psF32 c = 0.0; + psF32 fa = 0.0; + psF32 fb = 0.0; + psF32 fc = 0.0; + psS32 iter = 0; + PS_VECTOR_GEN_STATIC_RECYCLED(tmp, params->n, PS_TYPE_F32); + psBool boolLineIsNull = true; + psF32 prevMin = 0.0; + psS32 countMin = 0; + + psTrace(__func__, 4, "---- p_psDetermineBracket() begin ----\n"); + + // If the line vector is zero, then return NULL. + PS_VECTOR_WITH_MASK_F32_CHECK_ZERO_VECTOR(params, paramMask, boolLineIsNull); + if (boolLineIsNull == true) { + psTrace(__func__, 2, "p_psDetermineBracket() called with zero line vector.\n"); + psTrace(__func__, 4, "---- p_psDetermineBracket() end (NULL) ----\n"); + return(NULL); + } + + // We determine in what x-direction does the function decrease. + a = 0.0; + fa = func(params, coords); + b = 0.5; + iter = 0; + do { + b*= (1.0 + PS_DETERMINE_BRACKET_STEP_SIZE); + PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmp, b); + fb = func(tmp, coords); + } while ((fabs(fb - fa) < FLT_EPSILON) && (iter++ < 100)); + + if (fb > fa) { + a = b; + fa = fb; + b = 0.0; + fb = func(params, coords); + } + c = b; + + // At this point we have (a, b) and we know that (fa >= fb). Initially, c=b; + // We keep stretching b out further from "a" until (fc > previous fc). If + // that happens, then we have our bracket. + psVector *bracket = psVectorAlloc(3, PS_TYPE_F32); + iter = 0; + while (iter < PS_DETERMINE_BRACKET_MAX_ITERATIONS) { + psTrace(__func__, 6, "psDetermineBracket(): iterationA %d\n", iter); + c+= (1.0 + PS_DETERMINE_BRACKET_STEP_SIZE) * (c - a); + + PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmp, c); + fc = func(tmp, coords); + + psTrace(__func__, 6, "Iteration(%d) (bracket): (a, b, c) is (%f %f %f) (fa, fb, fc) is (%f %f %f)\n", iter, a, b, c, fa, fb, fc); + + if ((fb < fa) && (fb < fc)) { + RETURN_FINAL_BRACKET(); + } else { + b = c; + fb = fc; + } + + // This code maintains a count of how many times the minimum fc has + // stayed the same. If it gets too high, we exit this loop. + if (fc == prevMin) { + countMin++; + } else { + countMin = 0; + } + prevMin = fc; + if (countMin == 10) { + RETURN_FINAL_BRACKET(); + } + + iter++; + } + + psFree(bracket); + psTrace(__func__, 4, "---- p_psDetermineBracket() end (NULL) (BAD) ----\n"); + return(NULL); +} + +/****************************************************************************** +This routine takes as input a possibly multi-dimensional function, along +with an initial guess at the parameters of that function and vector "line" +of the same size as the parameter vector. It will minimize the function +along that vector and returns the offset along that vector at which the +minimum is determined. + +XXX: This routine is not very efficient in terms of total evaluations of the +function. +XXX: This is F32 only (make it F64). +XXX: Since this is an internal function, many of the parameter checks are + redundant. +XXX: Don't modify the psMinimization argument. + *****************************************************************************/ +#define PS_LINEMIN_MAX_ITERATIONS 30 +psF32 p_psLineMin(psMinimization *min, + psVector *params, + psVector *line, + const psVector *paramMask, + const psArray *coords, + psMinimizePowellFunc func) +{ + PS_ASSERT_PTR_NON_NULL(min, NAN); + PS_ASSERT_VECTOR_NON_NULL(params, NAN); + PS_ASSERT_VECTOR_NON_EMPTY(params, NAN); + PS_ASSERT_VECTOR_TYPE(params, PS_TYPE_F32, NAN); + PS_ASSERT_VECTOR_NON_NULL(line, NAN); + PS_ASSERT_VECTOR_NON_EMPTY(line, NAN); + PS_ASSERT_VECTOR_TYPE(line, PS_TYPE_F32, NAN); + PS_ASSERT_VECTOR_NON_NULL(paramMask, NAN); + PS_ASSERT_VECTOR_NON_EMPTY(paramMask, NAN); + PS_ASSERT_VECTOR_TYPE(paramMask, PS_TYPE_U8, NAN); + PS_ASSERT_PTR_NON_NULL(coords, NAN); + PS_ASSERT_PTR_NON_NULL(func, NAN); + psVector *bracket; + psF32 a = 0.0; + psF32 b = 0.0; + psF32 c = 0.0; + psF32 n = 0.0; + psF32 fa = 0.0; + psF32 fb = 0.0; + psF32 fc = 0.0; + psF32 fn = 0.0; + psF32 mul = 0.0; + PS_VECTOR_GEN_STATIC_RECYCLED(tmpa, params->n, PS_TYPE_F32); + PS_VECTOR_GEN_STATIC_RECYCLED(tmpb, params->n, PS_TYPE_F32); + PS_VECTOR_GEN_STATIC_RECYCLED(tmpc, params->n, PS_TYPE_F32); + PS_VECTOR_GEN_STATIC_RECYCLED(tmpn, params->n, PS_TYPE_F32); + psS32 i = 0; + psS32 boolLineIsNull = true; + psS32 numIterations = 0; + + psTrace(__func__, 4, "---- p_psLineMin() begin ----\n"); + PS_VECTOR_F32_CHECK_ZERO_VECTOR(line, boolLineIsNull); + + if (boolLineIsNull == true) { + min->value = func(params, coords); + psTrace(__func__, 2, "p_psLineMin() called with zero line vector. Return 0.0. Function value is %f\n", min->value); + return(0.0); + } + + // XXX:Use psTraceGetLevel() + for (i=0;in;i++) { + psTrace(__func__, 6, + "(params, paramMask, line)[%d] is (%f %d %f)\n", i, + params->data.F32[i], + paramMask->data.U8[i], + line->data.F32[i]); + } + + bracket = p_psDetermineBracket2(params, line, paramMask, coords, func); + if (bracket == NULL) { + psError(PS_ERR_UNKNOWN, false, + "Could not bracket minimum. Returning NAN.\n"); + return(NAN); + } + numIterations = 0; + while (numIterations < PS_LINEMIN_MAX_ITERATIONS) { + numIterations++; + psTrace(__func__, 6, "p_psLineMin(): iteration %d\n", numIterations); + + a = bracket->data.F32[0]; + b = bracket->data.F32[1]; + c = bracket->data.F32[2]; + PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmpa, a); + PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmpb, b); + PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmpc, c); + fa = func(tmpa, coords); + fb = func(tmpb, coords); + fc = func(tmpc, coords); + psTrace(__func__, 6, "LineMin: f(%f %f %f) is (%f %f %f)\n", a, b, c, fa, fb, fc); + + // We determine which is the biggest segment in [a,b,c] then split + // that with the point n. + if ((b-a) > (c-b)) { + // This is the golden section formula + n = a + (0.69 * (b-a)); + for (i=0;in;i++) { + tmpn->data.F32[i] = params->data.F32[i] + (n * line->data.F32[i]); + } + fn = func(tmpn, coords); + + if (fn > fb) { + // a = n, b = b, c = c + bracket->data.F32[0] = n; + } else { + // a = a, b = n, c = b + bracket->data.F32[1] = n; + bracket->data.F32[2] = b; + } + } else { + n = b + (0.69 * (c-b)); + for (i=0;in;i++) { + tmpn->data.F32[i] = params->data.F32[i] + (n * line->data.F32[i]); + } + fn = func(tmpn, coords); + + if (fn > fb) { + // a = a, b = b, c = n + bracket->data.F32[2] = n; + } else { + // a = b, b = n, c = c + bracket->data.F32[0] = b; + bracket->data.F32[1] = n; + } + } + psTrace(__func__, 6, "LineMin: new bracket is (%f %f %f)\n", bracket->data.F32[0], bracket->data.F32[1], bracket->data.F32[2]); + + mul = bracket->data.F32[1]; + if ((fabs(a-b) < min->tol) && (fabs(b-c) < min->tol)) { + PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, params, mul); + min->value = func(params, coords); + psFree(bracket); + psTrace(__func__, 4, "---- p_psLineMin() end.a (%f) (%f) ----\n", mul, min->value); + return(mul); + } + } + + mul = bracket->data.F32[1]; + PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, params, mul); + min->value = func(params, coords); + psTrace(__func__, 4, "---- p_psLineMin() end.b (%f) %f ----\n", mul, min->value); + + psFree(bracket); + return(mul); +} + + +/****************************************************************************** +This routine must minimize a possibly multi-dimensional function. The +function to be minimized "func" is: + psF32 func(psVector *params, psArray *coords) +The "params" are the parameters of the function which are varied. The data +points at which the function is varied are in the argument "coords" which is +a psArray of psVectors: each vector represents a different coordinate. + +XXX: We do not use Brent's method. + +XXX: The SDR is silent about data types. F32 is implemented here. +Reimplement in F64, convert F32 vectors to F64. + *****************************************************************************/ +#define PS_MINIMIZE_POWELL_LINEMIN_MAX_ITERATIONS 20 +#define PS_MINIMIZE_POWELL_LINEMIN_ERROR_TOLERANCE 0.01 + +bool psMinimizePowell(psMinimization *min, + psVector *params, + const psVector *paramMask, + const psArray *coords, + psMinimizePowellFunc func) +{ + PS_ASSERT_PTR_NON_NULL(min, NULL); + PS_ASSERT_VECTOR_NON_NULL(params, NULL); + PS_ASSERT_VECTOR_NON_EMPTY(params, NULL); + PS_ASSERT_VECTOR_TYPE(params, PS_TYPE_F32, NULL); + PS_ASSERT_PTR_NON_NULL(coords, NULL); + PS_ASSERT_PTR_NON_NULL(func, NULL); + psS32 numDims = params->n; + PS_VECTOR_GEN_STATIC_RECYCLED(pQP, numDims, PS_TYPE_F32); + PS_VECTOR_GEN_STATIC_RECYCLED(u, numDims, PS_TYPE_F32); + PS_VECTOR_GEN_STATIC_RECYCLED(Q, numDims, PS_TYPE_F32); + psS32 i = 0; + psS32 j = 0; + psVector *myParamMask = NULL; + psMinimization dummyMin; + psF32 mul = 0.0; + psF32 baseFuncVal = 0.0; + psF32 currFuncVal = 0.0; + psS32 biggestIter = 0; + psF32 biggestDiff = 0.0; + psS32 iterationNumber = 0; + + psTrace(__func__, 4, "---- psMinimizePowell() begin ----\n"); + psTrace(__func__, 6, "min->maxIter is %d\n", min->maxIter); + psTrace(__func__, 6, "min->tol is %f\n", min->tol); + + if (paramMask == NULL) { + myParamMask = psVectorRecycle(myParamMask, params->n, PS_TYPE_U8); + p_psMemSetPersistent(myParamMask, true); + p_psMemSetPersistent(myParamMask->data.U8, true); + for (i=0;in;i++) { + myParamMask->data.U8[i] = 0; + } + } else { + myParamMask = (psVector *) paramMask; + } + PS_ASSERT_VECTORS_SIZE_EQUAL(params, myParamMask, NULL); + + // 1: Set v[i] to be the unit vectors for each dimension in params + psArray *v = psArrayAlloc(numDims); + for (i=0;idata[i]) = (psVector *) psVectorAlloc(numDims, PS_TYPE_F32); + for (j=0;jdata[i]))->data.F32[j] = 1.0; + } else { + ((psVector *) (v->data[i]))->data.F32[j] = 0.0; + } + } + } + + // 2: Set Q to be the initial params (P in the ADD) + for (i=0;idata.F32[i] = params->data.F32[i]; + } + + while (iterationNumber < min->maxIter) { + iterationNumber++; + psTrace(__func__, 6, "psMinimizePowell() iteration %d\n", iterationNumber); + + // 3: For each dimension in params, move Q only in the vector v[i] to + // minimize the function. + + baseFuncVal = func(Q, coords); + currFuncVal = baseFuncVal; + psTrace(__func__, 6, "Current function value is %f\n", currFuncVal); + + biggestDiff = 0; + biggestIter = 0; + for (i=0;idata.U8[i] == 0) { + P_PSMINIMIZATION_SET_MAXITER((&dummyMin),PS_MINIMIZE_POWELL_LINEMIN_MAX_ITERATIONS); + *(float*)&dummyMin.tol = PS_MINIMIZE_POWELL_LINEMIN_ERROR_TOLERANCE; + mul = p_psLineMin(&dummyMin, + Q, + ((psVector *) v->data[i]), + myParamMask, + coords, + func); + if (isnan(mul)) { + psError(PS_ERR_UNKNOWN, false, + "Could not perform line minimization. Returning FALSE.\n"); + psFree(v); + return(false); + } + psTrace(__func__, 6, "LineMin along dimension %d has multiple %f\n", i, mul); + + if (fabs(dummyMin.value - currFuncVal) > biggestDiff) { + biggestDiff = fabs(dummyMin.value - currFuncVal); + biggestIter = i; + } + currFuncVal = dummyMin.value; + } + } + psTrace(__func__, 6, "New function value is %f\n", currFuncVal); + + // 4: Set the vector u = Q - P + for (i=0;idata.U8[i] == 0) { + u->data.F32[i] = Q->data.F32[i] - params->data.F32[i]; + + psTrace(__func__, 6, "u[i]=Q[i]-P[i] (%f = %f - %f)\n", u->data.F32[i], + Q->data.F32[i], + params->data.F32[i]); + + } else { + u->data.F32[i] = 0.0; + } + } + + // 5: Move Q only in the direction u, and minimize the function. + for (i=0;idata.F32[i]); + } + + mul = p_psLineMin(&dummyMin, params, u, myParamMask, coords, func); + if (isnan(mul)) { + psError(PS_ERR_UNKNOWN, false, + "Could not perform line minimization. Returning FALSE.\n"); + psFree(v); + return(false); + } + + // 6: + if (dummyMin.value > currFuncVal) { + psFree(v); + min->iter = iterationNumber; + // XXX: Ensure that currFuncVal is the correct value to use here. + min->value = currFuncVal; + // XXX: ensure that the lastDelta should be 0.0. + min->lastDelta = 0.0; + psTrace(__func__, 4, "---- psMinimizePowell() end (1)(true) ----\n"); + return(true); + } + + for (i=0;idata.U8[i] == 0) { + pQP->data.F32[i] = (2 * Q->data.F32[i]) - params->data.F32[i]; + } else { + pQP->data.F32[i] = params->data.F32[i]; + } + } + psF32 fqp = func(pQP, coords); + psF32 term1 = (baseFuncVal - currFuncVal) - biggestDiff; + term1*= term1; + term1*= 2.0 * (baseFuncVal - (2.0 * currFuncVal) + fqp); + psF32 term2 = baseFuncVal - fqp; + term2*= term2 * biggestDiff; + if (term1 < term2) { + for (i=0;idata.U8[i] == 0) { + ((psVector *) v->data[biggestIter])->data.F32[i] = u->data.F32[i]; + } + } + } + + // 7: Set P to Q + for (i=0;idata.U8[i] == 0) { + params->data.F32[i] = Q->data.F32[i]; + } + } + + // 8: Go to step 3 until the change is less than some tolerance. + if (fabs(baseFuncVal - currFuncVal) <= min->tol) { + psFree(v); + // XXX: Ensure that currFuncVal is the correct value to use here. + min->value = currFuncVal; + min->iter = iterationNumber; + min->lastDelta = currFuncVal - baseFuncVal; + psTrace(__func__, 4, "---- psMinimizePowell() end (2) (true) ----\n"); + return(true); + } + } + + psFree(v); + min->iter = iterationNumber; + psTrace(__func__, 4, "---- psMinimizePowell() end (0) (false) ----\n"); + return(false); +} + + +/****************************************************************************** +This routine is to be used with the psMinimizeChi2Powell() function below. +and the psMinimizePowell() function above. + +The basic idea is calculate chi-squared for a set of params/coords/errors. +This functions uses global variables to receive the function pointer, the +data values, and the data errors. +XXX: This is F32 only. Must implement F64. + *****************************************************************************/ +static psF32 myPowellChi2Func(const psVector *params, + const psArray *coords) +{ + psTrace(__func__, 4, "---- myPowellChi2Func() begin ----\n"); + PS_ASSERT_VECTOR_NON_NULL(params, NAN); + PS_ASSERT_VECTOR_NON_EMPTY(params, NAN); + PS_ASSERT_VECTOR_NON_NULL(myValue, NAN); + PS_ASSERT_VECTOR_NON_EMPTY(myValue, NAN); + PS_ASSERT_PTR_NON_NULL(coords, NAN); + + psF32 chi2 = 0.0; + psF32 d; + psS32 i; + psVector *tmp; + + tmp = Chi2PowellFunc(params, coords); + if (myError == NULL) { + for (i=0;in;i++) { + d = (tmp->data.F32[i] - myValue->data.F32[i]); + chi2+= d * d; + } + } else { + for (i=0;in;i++) { + d = (tmp->data.F32[i] - myValue->data.F32[i]) / myError->data.F32[i]; + chi2+= d * d; + } + } + psFree(tmp); + psTrace(__func__, 4, "---- myPowellChi2Func() end (chi2 is %f) ----\n", chi2); + return(chi2); +} + + +/****************************************************************************** +This routine must minimize the chi-squared match of a set of data points and +values for a possibly multi-dimensional function. + +The basic idea is to use the psMinimizePowell() function defined above. In +order to do so, we defined above a function myPowellChi2Func() which takes +the "func" function and returns chi-squared over the params/coords/values. +We then use that function myPowellChi2Func() in the call to +psMinimizePowell(). + *****************************************************************************/ +bool psMinimizeChi2Powell(psMinimization *min, + psVector *params, + const psVector *paramMask, + const psArray *coords, + const psVector *value, + const psVector *error, + psMinimizeChi2PowellFunc model) +{ + myValue = (psVector *) value; + myError = (psVector *) error; + + Chi2PowellFunc = model; + + return(psMinimizePowell(min, params, paramMask, coords, myPowellChi2Func)); +} + Index: /trunk/psLib/src/math/psMinimizePowell.h =================================================================== --- /trunk/psLib/src/math/psMinimizePowell.h (revision 6101) +++ /trunk/psLib/src/math/psMinimizePowell.h (revision 6101) @@ -0,0 +1,93 @@ +/** @file psMinimizePowell.c + * \brief basic minimization functions + * @ingroup Math + * + * This file will contain function prototypes for various Powell + * chi-squared minimization routines. + * + * @author GLG, MHPCC + * + * @version $Revision: 1.1 $ $Name: not supported by cvs2svn $ + * @date $Date: 2006-01-21 02:43:31 $ + * + * Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii + * + */ + +#ifndef PS_MINIMIZE_POWELL_H +#define PS_MINIMIZE_POWELL_H + +/** \file psMinimize.h + * \brief minimization operations + * \ingroup Stats + */ +/** \addtogroup Stats + * \{ + */ + +#include "psMinimizeLMM.h" +#include "psVector.h" +#include "psMemory.h" +#include "psArray.h" +#include "psImage.h" +#include "psMatrix.h" +#include "psPolynomial.h" +#include "psSpline.h" +#include "psStats.h" +#include "psTrace.h" +#include "psError.h" +#include "psConstants.h" + +/** Specifies the format of a user-defined function that the general Powell + * minimizer routine will accept. + * + * @return float: the single float value of the function given the parameters + * and coordinate vectors. +*/ +typedef +float (*psMinimizePowellFunc)( + const psVector *params, ///< Parameters used to evaluate the function + const psArray *coords ///< Coordinates at which to evaluate +); + +/** Minimizes a specified function based on the Powell method. + * + * @return bool: True if successful. + */ +bool psMinimizePowell( + psMinimization *min, ///< Minimization specification + psVector *params, ///< "Best guess" for parameters that minimize func + const psVector *paramMask, ///< Parameters to be held fixed by minimizer + const psArray *coords, ///< Measurement coordinates + psMinimizePowellFunc func ///< Specified function +); + +/** Specifies the format of a user-defined function that the general Powell chi- + * squared minimizer routine will accept. + * + * @return psVector*: Calculated values given the parameters and coordinates. +*/ +typedef +psVector *(*psMinimizeChi2PowellFunc)( + const psVector *params, ///< Parameters used to evaluate the function + const psArray *coords ///< Coordinates at which to evaluate +); + +/** Minimizes a specified function based on the Powell chi-squared method. + * + * @return bool: True is successful. + */ +bool psMinimizeChi2Powell( + psMinimization *min, ///< Minimization specification + psVector *params, ///< "Best guess" for parameters that minimize func + const psVector *paramMask, ///< Parameters to be held fixed by minimizer + const psArray *coords, ///< Measurement coordinates + const psVector *value, ///< Measured values at the coordinates + const psVector *error, ///< Errors in the measure values (or NULL) + psMinimizeChi2PowellFunc model ///< Specified function +); + +/* \} */// End of MathGroup Functions + +#endif // #ifndef PS_MINIMIZE_POWELL_H + Index: /trunk/psLib/src/math/psPolynomial.c =================================================================== --- /trunk/psLib/src/math/psPolynomial.c (revision 6100) +++ /trunk/psLib/src/math/psPolynomial.c (revision 6101) @@ -7,6 +7,6 @@ * polynomials. It also contains a Gaussian functions. * -* @version $Revision: 1.134 $ $Name: not supported by cvs2svn $ -* @date $Date: 2005-12-19 23:58:47 $ +* @version $Revision: 1.135 $ $Name: not supported by cvs2svn $ +* @date $Date: 2006-01-21 02:43:31 $ * * Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii @@ -965,4 +965,6 @@ } +// XXX: The output of this routine is always psF64 while 1D and 2D are +// dependent on the input vectors. psVector *psPolynomial3DEvalVector(const psPolynomial3D *poly, const psVector *x, @@ -973,9 +975,9 @@ PS_ASSERT_POLY_NON_NULL(poly, NULL); PS_ASSERT_VECTOR_NON_NULL(x, NULL); - PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL); PS_ASSERT_VECTOR_NON_NULL(y, NULL); - PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL); PS_ASSERT_VECTOR_NON_NULL(z, NULL); - PS_ASSERT_VECTOR_TYPE(z, PS_TYPE_F64, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(z, NULL); psVector *tmp; @@ -994,10 +996,39 @@ // Evaluate polynomial - for (unsigned int i = 0; i < vecLen; i++) { - tmp->data.F64[i] = psPolynomial3DEval(poly, - x->data.F64[i], - y->data.F64[i], - z->data.F64[i]); - } + // XXX: Consult with IfA: is this how they want to handle multiple data types? + if ((x->type.type == PS_TYPE_F32) || (y->type.type == PS_TYPE_F32) || (z->type.type == PS_TYPE_F32)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial3DEval(poly, x->data.F32[i], y->data.F32[i], z->data.F32[i]); + } + } else if ((x->type.type == PS_TYPE_F32) || (y->type.type == PS_TYPE_F32) || (z->type.type == PS_TYPE_F64)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial3DEval(poly, x->data.F32[i], y->data.F32[i], z->data.F64[i]); + } + } else if ((x->type.type == PS_TYPE_F32) || (y->type.type == PS_TYPE_F64) || (z->type.type == PS_TYPE_F32)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial3DEval(poly, x->data.F32[i], y->data.F64[i], z->data.F32[i]); + } + } else if ((x->type.type == PS_TYPE_F32) || (y->type.type == PS_TYPE_F64) || (z->type.type == PS_TYPE_F64)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial3DEval(poly, x->data.F32[i], y->data.F64[i], z->data.F64[i]); + } + } else if ((x->type.type == PS_TYPE_F64) || (y->type.type == PS_TYPE_F32) || (z->type.type == PS_TYPE_F32)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial3DEval(poly, x->data.F64[i], y->data.F32[i], z->data.F32[i]); + } + } else if ((x->type.type == PS_TYPE_F64) || (y->type.type == PS_TYPE_F32) || (z->type.type == PS_TYPE_F64)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial3DEval(poly, x->data.F64[i], y->data.F32[i], z->data.F32[i]); + } + } else if ((x->type.type == PS_TYPE_F64) || (y->type.type == PS_TYPE_F64) || (z->type.type == PS_TYPE_F32)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial3DEval(poly, x->data.F64[i], y->data.F32[i], z->data.F32[i]); + } + } else if ((x->type.type == PS_TYPE_F64) || (y->type.type == PS_TYPE_F64) || (z->type.type == PS_TYPE_F64)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial3DEval(poly, x->data.F64[i], y->data.F32[i], z->data.F32[i]); + } + } + // Return output vector @@ -1033,11 +1064,11 @@ PS_ASSERT_POLY_NON_NULL(poly, NULL); PS_ASSERT_VECTOR_NON_NULL(x, NULL); - PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL); PS_ASSERT_VECTOR_NON_NULL(y, NULL); - PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL); PS_ASSERT_VECTOR_NON_NULL(z, NULL); - PS_ASSERT_VECTOR_TYPE(z, PS_TYPE_F64, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(z, NULL); PS_ASSERT_VECTOR_NON_NULL(t, NULL); - PS_ASSERT_VECTOR_TYPE(t, PS_TYPE_F64, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(t, NULL); psVector *tmp; @@ -1057,4 +1088,73 @@ // Allocoutput vector tmp = psVectorAlloc(vecLen, PS_TYPE_F64); + + // Evaluate polynomial + // XXX: Consult with IfA: is this how they want to handle multiple data types? + if ((x->type.type == PS_TYPE_F32) || (y->type.type == PS_TYPE_F32) || (z->type.type == PS_TYPE_F32) || (t->type.type == PS_TYPE_F32)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F32[i], y->data.F32[i], z->data.F32[i], t->data.F32[i]); + } + } else if ((x->type.type == PS_TYPE_F32) || (y->type.type == PS_TYPE_F32) || (z->type.type == PS_TYPE_F32) || (t->type.type == PS_TYPE_F64)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F32[i], y->data.F32[i], z->data.F32[i], t->data.F64[i]); + } + } else if ((x->type.type == PS_TYPE_F32) || (y->type.type == PS_TYPE_F32) || (z->type.type == PS_TYPE_F64) || (t->type.type == PS_TYPE_F32)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F32[i], y->data.F32[i], z->data.F64[i], t->data.F32[i]); + } + } else if ((x->type.type == PS_TYPE_F32) || (y->type.type == PS_TYPE_F32) || (z->type.type == PS_TYPE_F64) || (t->type.type == PS_TYPE_F64)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F32[i], y->data.F32[i], z->data.F64[i], t->data.F64[i]); + } + } else if ((x->type.type == PS_TYPE_F32) || (y->type.type == PS_TYPE_F64) || (z->type.type == PS_TYPE_F32) || (t->type.type == PS_TYPE_F32)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F32[i], y->data.F64[i], z->data.F32[i], t->data.F32[i]); + } + } else if ((x->type.type == PS_TYPE_F32) || (y->type.type == PS_TYPE_F64) || (z->type.type == PS_TYPE_F32) || (t->type.type == PS_TYPE_F64)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F32[i], y->data.F64[i], z->data.F32[i], t->data.F64[i]); + } + } else if ((x->type.type == PS_TYPE_F32) || (y->type.type == PS_TYPE_F64) || (z->type.type == PS_TYPE_F64) || (t->type.type == PS_TYPE_F32)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F32[i], y->data.F64[i], z->data.F64[i], t->data.F32[i]); + } + } else if ((x->type.type == PS_TYPE_F32) || (y->type.type == PS_TYPE_F64) || (z->type.type == PS_TYPE_F64) || (t->type.type == PS_TYPE_F64)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F32[i], y->data.F64[i], z->data.F64[i], t->data.F64[i]); + } + } else if ((x->type.type == PS_TYPE_F64) || (y->type.type == PS_TYPE_F32) || (z->type.type == PS_TYPE_F32) || (t->type.type == PS_TYPE_F32)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F64[i], y->data.F32[i], z->data.F32[i], t->data.F32[i]); + } + } else if ((x->type.type == PS_TYPE_F64) || (y->type.type == PS_TYPE_F32) || (z->type.type == PS_TYPE_F32) || (t->type.type == PS_TYPE_F64)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F64[i], y->data.F32[i], z->data.F32[i], t->data.F64[i]); + } + } else if ((x->type.type == PS_TYPE_F64) || (y->type.type == PS_TYPE_F32) || (z->type.type == PS_TYPE_F64) || (t->type.type == PS_TYPE_F32)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F64[i], y->data.F32[i], z->data.F64[i], t->data.F32[i]); + } + } else if ((x->type.type == PS_TYPE_F64) || (y->type.type == PS_TYPE_F32) || (z->type.type == PS_TYPE_F64) || (t->type.type == PS_TYPE_F64)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F64[i], y->data.F32[i], z->data.F64[i], t->data.F64[i]); + } + } else if ((x->type.type == PS_TYPE_F64) || (y->type.type == PS_TYPE_F64) || (z->type.type == PS_TYPE_F32) || (t->type.type == PS_TYPE_F32)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F64[i], y->data.F64[i], z->data.F32[i], t->data.F32[i]); + } + } else if ((x->type.type == PS_TYPE_F64) || (y->type.type == PS_TYPE_F64) || (z->type.type == PS_TYPE_F32) || (t->type.type == PS_TYPE_F64)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F64[i], y->data.F64[i], z->data.F32[i], t->data.F64[i]); + } + } else if ((x->type.type == PS_TYPE_F64) || (y->type.type == PS_TYPE_F64) || (z->type.type == PS_TYPE_F64) || (t->type.type == PS_TYPE_F32)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F64[i], y->data.F64[i], z->data.F64[i], t->data.F32[i]); + } + } else if ((x->type.type == PS_TYPE_F64) || (y->type.type == PS_TYPE_F64) || (z->type.type == PS_TYPE_F64) || (t->type.type == PS_TYPE_F64)) { + for (unsigned int i = 0; i < vecLen; i++) { + tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F64[i], y->data.F64[i], z->data.F64[i], t->data.F64[i]); + } + } + // Evaluate polynomial Index: /trunk/psLib/src/math/psStats.c =================================================================== --- /trunk/psLib/src/math/psStats.c (revision 6100) +++ /trunk/psLib/src/math/psStats.c (revision 6101) @@ -17,6 +17,6 @@ * * - * @version $Revision: 1.157 $ $Name: not supported by cvs2svn $ - * @date $Date: 2005-12-05 21:33:29 $ + * @version $Revision: 1.158 $ $Name: not supported by cvs2svn $ + * @date $Date: 2006-01-21 02:43:32 $ * * Copyright 2004 Maui High Performance Computing Center, University of Hawaii @@ -2279,4 +2279,5 @@ newStruct->clippedStdev = NAN; newStruct->clippedNvalues = -1; // XXX: This is never used + // XXX: Where do these values come from? newStruct->clipSigma = 3.0; newStruct->clipIter = 3;