/****************************************************************************** * LevenMar.c - Levenberg-marquardt and Gauss-Jordan elimination methods. * * This imlemetation is based on the algorithm described in "Numerical * * recipies" Cambridge University Press, 1986, Section 2.1 and Section 14.4. * ******************************************************************************* * (C) Gershon Elber, Technion, Israel Institute of Technology * ******************************************************************************* * Written by Ronen Lev, November 2002 * ******************************************************************************/ #include "irit_sm.h" #include "misc_loc.h" #define IRIT_GAUUS_JORDAN_SWAP_INC(type, x, y) \ { type temp = (x); x++ = (y); y++ = temp; } static unsigned int MaxLevenbergIterations = 1000; static RealType MaxLevenbergLambda = IRIT_INFNTY; static void CalcAlphaBetaSig1Aux(RealType **X, RealType Y[], unsigned NumberOfDataElements, RealType ModelParams[], IritLevenEvalFuncType *ShapeFunc, RealType *Alpha, RealType Beta[], unsigned NumberOfModelParams, RealType *ChiSqr, RealType *AuxMem); static void CalcAlphaBetaAux(RealType **X, RealType Y[], RealType Sigma[], unsigned int NumberOfDataElements, RealType ModelParams[], IritLevenEvalFuncType *ShapeFunc, RealType Alpha[], RealType Beta[], unsigned int NumberOfModelParams, RealType *ChiSqr, RealType *AuxMem); static int LevenMinProccessAux(RealType **X, RealType Y[], RealType Sigma[], unsigned NumberOfDataElements, RealType ModelParams[], IritLevenEvalFuncType *ShapeFunc, IritLevenIsModelValidFuncType *ModelValidatorFunc, RealType Alpha[], RealType Beta[], unsigned NumberOfModelParams, RealType Lambda, RealType *CurChiSqr, RealType *AuxMem); static int LevenMinProccessSig1Aux(RealType **X, RealType Y[], unsigned NumberOfDataElements, RealType ModelParams[], IritLevenEvalFuncType *ShapeFunc, IritLevenIsModelValidFuncType *ModelValidatorFunc, RealType Alpha[], RealType Beta[], unsigned NumberOfModelParams, RealType Lambda, RealType *CurChiSqr, RealType *AuxMem); /***************************************************************************** * DESCRIPTION: M * This function modifies the maximum number of iteration when calculating M * Levenberg-Marquardt. M * * * PARAMETERS: M * NewVal: The new maximum number of iterations. M * * * RETURN VALUE: M * unsigned: The old maximum number of iterations. M * * * SEE ALSO: M * IritLevenMarMinSig1, IritLevenMarMin M * * * KEYWORDS: M * IritLevenMarSetMaxIterations M *****************************************************************************/ unsigned IritLevenMarSetMaxIterations(unsigned NewVal) { unsigned OldVal = MaxLevenbergIterations; MaxLevenbergIterations = NewVal; return OldVal; } /***************************************************************************** * DESCRIPTION: * * This function calculates the Alpha, Beta and ChiSqr of the data. * * This function is same as CalcAlphaBetaAux except it assumes the * * sigma is 1 for all data points. * * * * PARAMETERS: * * X: Pointer to a list of data. * * Y: Pointer to the list of expected results. * * NumberOfDataElements: The number of data elements in X & Y. * * ModelParams: The model params to be checked. * * ShapeFunc: The shape function. * * Alpha: The result Alpha matrix. * * Beta: The result Beta Vector. * * NumberOfModelParams: The number of model params. * * ChiSqr: The sum of the squared distance. * * AuxMem: An auxiliry allocated vercor of RealType of size * * NumberOfModelParams. * * * * RETURN VALUE: * * void * *****************************************************************************/ static void CalcAlphaBetaSig1Aux(RealType **X, RealType Y[], unsigned NumberOfDataElements, RealType ModelParams[], IritLevenEvalFuncType *ShapeFunc, RealType *Alpha, RealType Beta[], unsigned NumberOfModelParams, RealType *ChiSqr, RealType *AuxMem) { unsigned int i, j, DataNum; RealType *DyDParam = AuxMem; /* Initilize Alpha and Beta. */ ZAP_MEM(Beta, NumberOfModelParams * sizeof(RealType)); ZAP_MEM(Alpha, NumberOfModelParams * NumberOfModelParams * sizeof(RealType)); *ChiSqr = 0; for (DataNum = 0; DataNum < NumberOfDataElements; ++DataNum) { unsigned int CurModelParam; RealType ResultingY, Dy; ShapeFunc(X[DataNum], ModelParams, &ResultingY, DyDParam); Dy = Y[DataNum] - ResultingY; for (CurModelParam = 0; CurModelParam < NumberOfModelParams; ++CurModelParam) { for (i = 0; i <= CurModelParam; ++i) Alpha[CurModelParam * NumberOfModelParams + i] += DyDParam[CurModelParam] * DyDParam[i]; Beta[CurModelParam] += DyDParam[CurModelParam] * Dy; } *ChiSqr += (RealType)(SQR(Dy)); } /* Fill the symmetric portion. */ for (i = 1; i < NumberOfModelParams; ++i) for (j = 0; j < i; ++j) Alpha[j * NumberOfModelParams + i] = Alpha[i * NumberOfModelParams + j]; } /***************************************************************************** * DESCRIPTION: * * This function calculates the Alpha, Beta and ChiSqr of the data. * * * * PARAMETERS: * * X: Pointer to a list of data. * * Y: Pointer to the list of expected results. * * Sigma: Pointer to the expected variance vector. * * NumberOfDataElements: The number of data elements in X, Y & Sigma. * * ModelParams: The model params to be checked. * * ShapeFunc: The shape function. * * Alpha: The result Alpha matrix. * * Beta: The result Beta Vector. * * NumberOfModelParams: The number of model params. * * ChiSqr: The sum of the squared distance. * * AuxMem: An auxiliry allocated vercor of RealType of size * * NumberOfModelParams. * * * * RETURN VALUE: * * void * *****************************************************************************/ static void CalcAlphaBetaAux(RealType **X, RealType Y[], RealType Sigma[], unsigned int NumberOfDataElements, RealType ModelParams[], IritLevenEvalFuncType *ShapeFunc, RealType Alpha[], RealType Beta[], unsigned int NumberOfModelParams, RealType *ChiSqr, RealType *AuxMem) { unsigned int i, j, DataNum; RealType *DyDParam = AuxMem; /* Initilize Alpha and Beta. */ ZAP_MEM(Beta, NumberOfModelParams * sizeof(RealType)); ZAP_MEM(Alpha, NumberOfModelParams * NumberOfModelParams * sizeof(RealType)); *ChiSqr = 0; for (DataNum = 0; DataNum < NumberOfDataElements; ++DataNum) { unsigned int CurModelParam; RealType ResultingY, Dy, DSig2; ShapeFunc(X[DataNum], ModelParams, &ResultingY, DyDParam); Dy = Y[DataNum] - ResultingY; DSig2 = 1.0 / SQR(Sigma[DataNum]); for (CurModelParam = 0; CurModelParam < NumberOfModelParams; ++CurModelParam) { RealType WeightedDiff = DyDParam[CurModelParam] / DSig2; for (i = 0; i <= CurModelParam; ++i) Alpha[CurModelParam * NumberOfModelParams + i] += WeightedDiff * DyDParam[i]; Beta[CurModelParam] += WeightedDiff * Dy; } *ChiSqr += (RealType) (SQR(Dy) * DSig2); } /* Fill the symmetric portion. */ for (i = 1; i < NumberOfModelParams; ++i) for (j = 0; j < i; ++j) Alpha[j * NumberOfModelParams + i] = Alpha[i * NumberOfModelParams + j]; } /***************************************************************************** * DESCRIPTION: * * This function attempts to improve the current Model Params. * * If the attempt is successful it also updates Alpha and Beta. * * * * PARAMETERS: * * X: Pointer to the list of the data. * * Y: Pointer to the list of expected results. * * Sigma: Pointer to the expected variance vector. * * NumberOfDataElements: The Number of data elements in X, Y & Sigma. * * ModelParams: Pointer to the current model params (may be updated). * * ShapeFunc: The shape function. * * ModelValidatorFunc: The model validator. * * Alpha: The result alpha matrix (may be updated). * * Beta: The results Beta vector (may be updated). * * NumberOfModelParams: The number of model params. * * Lambda: Current Lambda. * * CurChiSqr: Current ChiSqr (may be updated). * * AuxMem: An auxiliry allocated vector of RealType of size * * NumberOfModelParams * (NumberOfModelParams + 3). * * * * RETURN VALUE: * * int: TRUE if a better model has been found, FALSE otherwise. * *****************************************************************************/ static int LevenMinProccessAux(RealType **X, RealType Y[], RealType Sigma[], unsigned NumberOfDataElements, RealType ModelParams[], IritLevenEvalFuncType *ShapeFunc, IritLevenIsModelValidFuncType *ModelValidatorFunc, RealType Alpha[], RealType Beta[], unsigned NumberOfModelParams, RealType Lambda, RealType *CurChiSqr, RealType *AuxMem) { unsigned int i, Result = FALSE; RealType Multiplier, NewChiSqr, *NewAlpha, *NewBeta, *NewModelParams; Multiplier = 1.0f + Lambda; NewAlpha = AuxMem; GEN_COPY(NewAlpha, Alpha, sizeof(RealType) * NumberOfModelParams * NumberOfModelParams); NewBeta = &AuxMem[NumberOfModelParams * NumberOfModelParams]; GEN_COPY(NewBeta, Beta, sizeof(RealType) * NumberOfModelParams); NewModelParams = &AuxMem[NumberOfModelParams * NumberOfModelParams + NumberOfModelParams]; for (i = 0; i < NumberOfModelParams; ++i) NewAlpha[i + NumberOfModelParams * i] *= Multiplier; IritGaussJordan(NewAlpha, NewBeta, NumberOfModelParams, 1); for (i = 0; i < NumberOfModelParams; ++i) NewModelParams[i] = ModelParams[i] + NewBeta[i]; CalcAlphaBetaAux(X, Y, Sigma, NumberOfDataElements, NewModelParams, ShapeFunc, NewAlpha, NewBeta, NumberOfModelParams, &NewChiSqr, &AuxMem[NumberOfModelParams * NumberOfModelParams + 2 * NumberOfModelParams]); if ((NewChiSqr < *CurChiSqr) && ((ModelValidatorFunc == NULL) || ((ModelValidatorFunc != NULL) && (ModelValidatorFunc(NewModelParams) == TRUE)))) { GEN_COPY(Alpha, NewAlpha, sizeof(RealType) * NumberOfModelParams * NumberOfModelParams); GEN_COPY(Beta, NewBeta, sizeof(RealType) * NumberOfModelParams); GEN_COPY(ModelParams, NewModelParams, sizeof(RealType) * NumberOfModelParams); *CurChiSqr = NewChiSqr; Result = TRUE; } return Result; } /***************************************************************************** * DESCRIPTION: * * This function attempts to improve the current Model Params. * * If the attempt is successful it also updates Alpha and Beta. * * This function is similar to LevenMinProccessAux except that sigma=1. * * * * PARAMETERS: * * X: Pointer to the list of the data. * * Y: Pointer to the list of expected results. * * NumberOfDataElements: The Number of data elements in X, Y & Sigma. * * ModelParams: Pointer to the current model params (may be updated). * * ShapeFunc: The shape function. * * ModelValidatorFunc: The model validator. * * Alpha: The result alpha matrix (may be updated). * * Beta: The results Beta vector (may be updated). * * NumberOfModelParams: The number of model params. * * Lambda: Current Lambda. * * CurChiSqr: Current ChiSqr (may be updated). * * AuxMem: An auxiliry allocated vector of RealType of size. * * NumberOfModelParams * (NumberOfModelParams + 3). * * * * RETURN VALUE: * * int: TRUE if a better model has been found, FALSE otherwise. * *****************************************************************************/ static int LevenMinProccessSig1Aux(RealType **X, RealType Y[], unsigned NumberOfDataElements, RealType ModelParams[], IritLevenEvalFuncType *ShapeFunc, IritLevenIsModelValidFuncType *ModelValidatorFunc, RealType Alpha[], RealType Beta[], unsigned NumberOfModelParams, RealType Lambda, RealType *CurChiSqr, RealType *AuxMem) { unsigned int i; int Result = FALSE; RealType Multiplier, NewChiSqr, *NewAlpha, *NewBeta, *NewModelParams; Multiplier = 1.0f + Lambda; NewAlpha = AuxMem; GEN_COPY(NewAlpha, Alpha, sizeof(RealType) * NumberOfModelParams * NumberOfModelParams); NewBeta = &AuxMem[NumberOfModelParams * NumberOfModelParams]; GEN_COPY(NewBeta, Beta, sizeof(RealType) * NumberOfModelParams); NewModelParams = &AuxMem[NumberOfModelParams * NumberOfModelParams + NumberOfModelParams]; for (i = 0; i < NumberOfModelParams; ++i) NewAlpha[i + NumberOfModelParams * i] *= Multiplier; IritGaussJordan(NewAlpha, NewBeta, NumberOfModelParams, 1); for (i = 0; i < NumberOfModelParams; ++i) NewModelParams[i] = ModelParams[i] + NewBeta[i]; CalcAlphaBetaSig1Aux(X, Y, NumberOfDataElements, NewModelParams, ShapeFunc, NewAlpha, NewBeta, NumberOfModelParams, &NewChiSqr, &AuxMem[NumberOfModelParams * NumberOfModelParams +2 * NumberOfModelParams]); if ((NewChiSqr < *CurChiSqr) && ((ModelValidatorFunc == NULL) || ((ModelValidatorFunc != NULL) && (ModelValidatorFunc(NewModelParams) == TRUE)))) { GEN_COPY(Alpha, NewAlpha, sizeof(RealType) * NumberOfModelParams * NumberOfModelParams); GEN_COPY(Beta, NewBeta, sizeof(RealType) * NumberOfModelParams); GEN_COPY(ModelParams, NewModelParams, sizeof(RealType) * NumberOfModelParams); *CurChiSqr = NewChiSqr; Result = TRUE; } return Result; } /***************************************************************************** * DESCRIPTION: M * This function calculates the levenberg-marquardt minimization of the M * Specified function M * * * PARAMETERS: M * X: Pointer to a list of data. M * Y: Pointer to the list of expected results. M * Sigma: Pointer to the expected variance vector. M * NumberOfDataElements: The number of data elements in X, Y & Sigma. M * ModelParams: The model params to be checked. M * ShapeFunc: The shape function. M * ProtectionFunc: The numerical protection function. M * ModelValidatorFunc: The model validator. M * NumberOfModelParams: The number of model params. M * Tolerance: If the error is smaller then Tolerance return. M * * * RETURN VALUE: M * RealType: The squared sum of the error. M * * * SEE ALSO: M * IritLevenMarMinSig1, IritLevenMarSetMaxIterations M * * * KEYWORDS: M * IritLevenMarMin M *****************************************************************************/ RealType IritLevenMarMin(RealType **X, RealType Y[], RealType Sigma[], unsigned NumberOfDataElements, RealType ModelParams[], IritLevenEvalFuncType *ShapeFunc, IritLevenNumerProtectionFuncType *ProtectionFunc, IritLevenIsModelValidFuncType *ModelValidatorFunc, unsigned NumberOfModelParams, RealType Tolerance) { unsigned int IterNum = 0; RealType *Alpha, *Beta, *AuxMem, Lambda, CurChiSqr; Alpha = (RealType *) IritMalloc(sizeof(RealType) * NumberOfModelParams * NumberOfModelParams); Beta = (RealType *) IritMalloc(sizeof(RealType) * NumberOfModelParams); AuxMem = (RealType *) IritMalloc(sizeof(RealType) * NumberOfModelParams * (3 + NumberOfModelParams)); if ((Alpha == NULL) || (Beta == NULL) || (AuxMem == NULL)) { IRIT_FATAL_ERROR("Unable to allocate memory."); } CalcAlphaBetaAux(X, Y, Sigma, NumberOfDataElements, ModelParams, ShapeFunc, Alpha, Beta, NumberOfModelParams, &CurChiSqr, AuxMem); Lambda = 0.001; while ((IterNum < MaxLevenbergIterations) && (CurChiSqr > Tolerance) && (Lambda < MaxLevenbergLambda)) { int TryResult = LevenMinProccessAux(X, Y, Sigma, NumberOfDataElements, ModelParams, ShapeFunc, ModelValidatorFunc, Alpha, Beta, NumberOfModelParams, Lambda, &CurChiSqr, AuxMem); if (TryResult) Lambda *= 0.1; else Lambda *= 10.0; if (ProtectionFunc != NULL) ProtectionFunc(ModelParams); } IritFree(Alpha); IritFree(Beta); IritFree(AuxMem); return CurChiSqr; } /***************************************************************************** * DESCRIPTION: M * This function calculates the levenberg-marquardt minimization of the M * Specified function M * This function is similar to LevenMin except that sigma is allways 1 M * * * PARAMETERS: M * X: Pointer to a list of data. M * Y: Pointer to the list of expected results. M * NumberOfDataElements: The number of data elements in X, Y & Sigma. M * ModelParams: The model params to be checked. M * ShapeFunc: The shape function. M * ProtectionFunc: The numerical protection function. M * ModelValidatorFunc: The model validator. M * NumberOfModelParams: The number of model params. M * Tolerance: If the error is smaller then Tolerance return. M * * * RETURN VALUE: M * RealType: The squared sum of the error. M * * * SEE ALSO: M * IritLevenMarMin, IritLevenMarSetMaxIterations M * * * KEYWORDS: M * IritLevenMarMinSig1 M *****************************************************************************/ RealType IritLevenMarMinSig1(RealType **X, RealType Y[], unsigned NumberOfDataElements, RealType ModelParams[], IritLevenEvalFuncType *ShapeFunc, IritLevenNumerProtectionFuncType *ProtectionFunc, IritLevenIsModelValidFuncType *ModelValidatorFunc, unsigned NumberOfModelParams, RealType Tolerance) { unsigned int IterNum = 0; RealType *Alpha, *Beta, *AuxMem, Lambda, CurChiSqr; Alpha = (RealType *) IritMalloc(sizeof(RealType) * NumberOfModelParams * NumberOfModelParams); Beta = (RealType *) IritMalloc(sizeof(RealType) * NumberOfModelParams); AuxMem = (RealType *) IritMalloc(sizeof(RealType) * NumberOfModelParams * ( 3 + NumberOfModelParams)); if ((Alpha == NULL) || (Beta == NULL) || (AuxMem == NULL)) { IRIT_FATAL_ERROR("Unable to allocate memory."); } CalcAlphaBetaSig1Aux(X, Y, NumberOfDataElements, ModelParams, ShapeFunc, Alpha, Beta, NumberOfModelParams, &CurChiSqr, AuxMem); Lambda = 0.001; while ((IterNum < MaxLevenbergIterations) && (CurChiSqr > Tolerance) && (Lambda < MaxLevenbergLambda)){ int TryResult = LevenMinProccessSig1Aux(X, Y, NumberOfDataElements, ModelParams, ShapeFunc, ModelValidatorFunc, Alpha, Beta, NumberOfModelParams, Lambda, &CurChiSqr, AuxMem); ++IterNum; if (TryResult) Lambda *= 0.1; else Lambda *= 10.0; if (ProtectionFunc != NULL) ProtectionFunc(ModelParams); } IritFree(Alpha); IritFree(Beta); IritFree(AuxMem); return CurChiSqr; } /***************************************************************************** * DESCRIPTION: M * This functions solves the linear equation Ax=B, using the Gauss-Jordan M * elimination algorithm. M * * * PARAMETERS: M * A: A matrix of N*N elements, is invalid on exit. M * B: A matrix of N*M elements, contains the result on exit. M * N: Described above. M * M: Described above. M * * * RETURN VALUE: M * int: TRUE on success, FALSE if singular. M * * * SEE ALSO: M * M * * * KEYWORDS: M * IritGaussJordan, Gauss-Jordan elimination M *****************************************************************************/ int IritGaussJordan(RealType *A, RealType *B, unsigned N, unsigned M) { unsigned int i, j, k, CurPivot, CurRow; unsigned char *SolvedColumnVec, *c; RealType *p, *q; SolvedColumnVec = (unsigned char *) IritMalloc(sizeof(unsigned char) * N); ZAP_MEM(SolvedColumnVec, sizeof(unsigned char) * N); for (i = 0; i < N; ++i) { RealType Divisor, Biggest = 0.0; /* An unreachable value to allow checking if CurPivot has been set. */ CurPivot = N; CurRow = 0; /* To prevent a warning. */ for (j = 0; j < N; ++j) { if (SolvedColumnVec[j] != 1) { c = SolvedColumnVec; p = &A[j * N]; for (k = 0; k < N; ++k, p++) { if (*c++ == 0 && FABS(*p) > Biggest) { CurPivot = k; CurRow = j; Biggest = FABS(*p); } } } } if (CurPivot == N) { IritFree(SolvedColumnVec); return FALSE; } ++SolvedColumnVec[CurPivot]; if (CurPivot != CurRow) { p = &A[CurPivot * N]; q = &A[CurRow * N]; for (j = 0; j++ < N; ) IRIT_GAUUS_JORDAN_SWAP_INC(RealType, *p, *q); p = &B[CurPivot * M]; q = &B[CurRow * M]; for (j = 0; j++ < M; ) IRIT_GAUUS_JORDAN_SWAP_INC(RealType, *p, *q); } if (APX_EQ(A[CurPivot + CurPivot * N], 0.0)) { IritFree(SolvedColumnVec); return FALSE; } Divisor = 1.0 / A[CurPivot + CurPivot * N]; A[CurPivot + CurPivot * N] = 1.0; for (j = 0; j < N; ++j) if (j != CurPivot) A[j + CurPivot * N] *= Divisor; for (j = 0; j < M; ++j) B[j + CurPivot * M] *= Divisor; for (j = 0; j < N; ++j) if (j != CurPivot) { RealType LineMultiplier = A[CurPivot + j * N]; p = &A[j * N]; q = &A[CurPivot * N]; for (k = 0; k++ < N; ) *p++ -= LineMultiplier * *q++; p = &B[j * M]; q = &B[CurPivot * M]; for (k = 0; k++ < M; ) *p++ -= LineMultiplier * *q++; } } IritFree(SolvedColumnVec); return TRUE; }