Index: branches/pap/psModules/src/imcombine/pmSubtractionEquation.c =================================================================== --- branches/pap/psModules/src/imcombine/pmSubtractionEquation.c (revision 25841) +++ branches/pap/psModules/src/imcombine/pmSubtractionEquation.c (revision 25857) @@ -19,5 +19,4 @@ #define USE_VARIANCE // Include variance in equation? -//#define OLD_FUNCTIONS // Use old functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -25,5 +24,4 @@ ////////////////////////////////////////////////////////////////////////////////////////////////////////////// -#ifndef OLD_FUNCTIONS // Calculate the least-squares matrix and vector static bool calculateMatrixVector(psImage *matrix, // Least-squares matrix, updated @@ -401,370 +399,4 @@ } -#ifdef OLD_FUNCTIONS -// Calculate a single element of the least-squares matrix, with the polynomial expansions in one direction -static inline bool calculateMatrixElement1(psImage *matrix, // Matrix to calculate - int i, int j, // Coordinates of element - const psKernel *image1, // First image in multiplication - const psKernel *image2, // Second image in multiplication - const psKernel *variance, // Variance image - const psImage *polyValues, // Spatial polynomial values - int numKernels, // Number of kernel basis functions - int footprint, // (Half-)Size of stamp - int spatialOrder, // Maximum order of spatial variation - bool symmetric // Is the matrix symmetric? - ) -{ - double sum = calculateSumProduct(image1, image2, variance, footprint); // Sum of the image products - if (!isfinite(sum)) { - return false; - } - - // Generate the pseudo-convolutions from the spatial polynomial terms - for (int iyOrder = 0, iIndex = i; iyOrder <= spatialOrder; iyOrder++) { - for (int ixOrder = 0; ixOrder <= spatialOrder - iyOrder; ixOrder++, iIndex += numKernels) { - double convPoly = sum * polyValues->data.F64[iyOrder][ixOrder]; - - assert(iIndex < matrix->numRows && j < matrix->numCols); - - matrix->data.F64[iIndex][j] = convPoly; - if (symmetric) { - - assert(iIndex < matrix->numCols && j < matrix->numRows); - - matrix->data.F64[j][iIndex] = convPoly; - } - } - } - return true; -} - -// Calculate a single element of the least-squares matrix, with the polynomial expansions in both directions -static inline bool calculateMatrixElement2(psImage *matrix, // Matrix to calculate - int i, int j, // Coordinates of element - const psKernel *image1, // First image in multiplication - const psKernel *image2, // Second image in multiplication - const psKernel *variance, // Variance image - const psImage *polyValues, // Spatial polynomial values - int numKernels, // Number of kernel basis functions - int footprint, // (Half-)Size of stamp - int spatialOrder, // Maximum order of spatial variation - bool symmetric // Is the matrix symmetric? - ) -{ - double sum = calculateSumProduct(image1, image2, variance, footprint); // Sum of the image products - if (!isfinite(sum)) { - return false; - } - - // Generate the pseudo-convolutions from the spatial polynomial terms - for (int iyOrder = 0, iIndex = i; iyOrder <= spatialOrder; iyOrder++) { - for (int ixOrder = 0; ixOrder <= spatialOrder - iyOrder; ixOrder++, iIndex += numKernels) { - double iPoly = polyValues->data.F64[iyOrder][ixOrder]; // Value of polynomial - for (int jyOrder = 0, jIndex = j; jyOrder <= spatialOrder; jyOrder++) { - for (int jxOrder = 0; jxOrder <= spatialOrder - jyOrder; jxOrder++, jIndex += numKernels) { - double convPoly = sum * iPoly * polyValues->data.F64[jyOrder][jxOrder]; - - assert(iIndex < matrix->numRows && jIndex < matrix->numCols); - - matrix->data.F64[iIndex][jIndex] = convPoly; - if (symmetric) { - - assert(iIndex < matrix->numCols && jIndex < matrix->numRows); - - matrix->data.F64[jIndex][iIndex] = convPoly; - } - } - } - } - } - return true; -} - -// Calculate the square part of the matrix derived from multiplying convolutions -static bool calculateMatrixSquare(psImage *matrix, // Matrix to calculate - const psArray *convolutions1, // Convolutions for element 1 - const psArray *convolutions2, // Convolutions for element 2 - const psKernel *variance, // Variance image - const psImage *polyValues, // Polynomial values - int numKernels, // Number of kernel basis functions - int spatialOrder, // Order of spatial variation - int footprint // Half-size of stamp - ) -{ - bool symmetric = (convolutions1 == convolutions2 ? true : false); // Is matrix symmetric? - - for (int i = 0; i < numKernels; i++) { - psKernel *iConv = convolutions1->data[i]; // Convolution for i-th element - - for (int j = (symmetric ? i : 0); j < numKernels; j++) { - psKernel *jConv = convolutions2->data[j]; // Convolution for j-th element - - if (!calculateMatrixElement2(matrix, i, j, iConv, jConv, variance, polyValues, numKernels, - footprint, spatialOrder, symmetric)) { - psTrace("psModules.imcombine", 2, "Bad sumCC at %d, %d", i, j); - return false; - } - } - } - - return true; -} - -// Calculate least-squares matrix and vector -static bool calculateMatrix(psImage *matrix, // Matrix to calculate - const pmSubtractionKernels *kernels, // Kernel components - const psArray *convolutions, // Convolutions of source with kernels - const psKernel *input, // Input stamp, or NULL - const psKernel *variance, // Variance stamp - const psImage *polyValues, // Spatial polynomial values - int footprint, // (Half-)Size of stamp - bool normAndBG // Calculate normalisation and background terms? - ) -{ - int numKernels = kernels->num; // Number of kernel components - int spatialOrder = kernels->spatialOrder; // Maximum order of spatial variation - int numSpatial = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of spatial variation terms - int bgOrder = kernels->bgOrder; // Maximum order of background fit - int numBackground = normAndBG ? PM_SUBTRACTION_POLYTERMS(bgOrder) : 0; // Number of background terms - int numTerms = numKernels * numSpatial + (normAndBG ? 1 + numBackground : 0); // Total number of terms - assert(matrix); - assert(matrix->numCols == matrix->numRows); - assert(matrix->numCols == numTerms); - assert(convolutions && convolutions->n == numKernels); - assert(polyValues); - assert(!normAndBG || input); // If we want the normalisation and BG, then we need the input image - - // Square part of the matrix (convolution-convolution products) - if (!calculateMatrixSquare(matrix, convolutions, convolutions, variance, polyValues, numKernels, - spatialOrder, footprint)) { - return false; - } - - // XXX To support higher-order background model than simply constant, the below code needs to be updated. - if (normAndBG) { - int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation - int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background - - for (int i = 0; i < numKernels; i++) { - psKernel *conv = convolutions->data[i]; // Convolution for i-th element - - // Normalisation-convolution terms - if (!calculateMatrixElement1(matrix, i, normIndex, conv, input, variance, polyValues, numKernels, - footprint, spatialOrder, true)) { - psTrace("psModules.imcombine", 2, "Bad sumIC at %d", i); - return false; - } - - // Background-convolution terms - double sumC = 0.0; // Sum of the convolution - for (int y = - footprint; y <= footprint; y++) { - for (int x = - footprint; x <= footprint; x++) { - double value = conv->kernel[y][x]; -#ifdef USE_VARIANCE - value /= variance->kernel[y][x]; -#endif - sumC += value; - } - } - if (!isfinite(sumC)) { - psTrace("psModules.imcombine", 2, "Bad sumC at %d", i); - return false; - } - - for (int yOrder = 0, index = i; yOrder <= spatialOrder; yOrder++) { - for (int xOrder = 0; xOrder <= spatialOrder - yOrder; xOrder++, index += numKernels) { - double value = sumC * polyValues->data.F64[yOrder][xOrder]; - matrix->data.F64[index][bgIndex] = value; - matrix->data.F64[bgIndex][index] = value; - } - } - } - - // Background only, normalisation only, and background-normalisation terms - double sum1 = 0.0; // Sum of the weighting - double sumI = 0.0; // Sum of the input - double sumII = 0.0; // Sum of the input squared - for (int y = - footprint; y <= footprint; y++) { - for (int x = - footprint; x <= footprint; x++) { - double invNoise2 = 1.0; -#ifdef USE_VARIANCE - invNoise2 /= variance->kernel[y][x]; -#endif - double value = input->kernel[y][x] * invNoise2; - sumI += value; - sumII += value * input->kernel[y][x]; - sum1 += invNoise2; - } - } - if (!isfinite(sumI)) { - psTrace("psModules.imcombine", 2, "Bad sumI detected"); - return false; - } - if (!isfinite(sumII)) { - psTrace("psModules.imcombine", 2, "Bad sumII detected"); - return false; - } - if (!isfinite(sum1)) { - psTrace("psModules.imcombine", 2, "Bad sum1 detected"); - return false; - } - matrix->data.F64[normIndex][normIndex] = sumII; - matrix->data.F64[bgIndex][bgIndex] = sum1; - matrix->data.F64[normIndex][bgIndex] = sumI; - matrix->data.F64[bgIndex][normIndex] = sumI; - } - - return true; -} - - -// Calculate least-squares matrix and vector -static bool calculateVector(psVector *vector, // Vector to calculate, or NULL - const pmSubtractionKernels *kernels, // Kernel components - const psArray *convolutions, // Convolutions of source with kernels - const psKernel *input, // Input stamp, or NULL if !normAndBG - const psKernel *target, // Target stamp - const psKernel *variance, // Variance stamp - const psImage *polyValues, // Spatial polynomial values - int footprint, // (Half-)Size of stamp - bool normAndBG // Calculate normalisation and background terms? - ) -{ - int numKernels = kernels->num; // Number of kernel components - int spatialOrder = kernels->spatialOrder; // Maximum order of spatial variation - int numSpatial = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of spatial variation terms - int bgOrder = kernels->bgOrder; // Maximum order of background fit - int numBackground = normAndBG ? PM_SUBTRACTION_POLYTERMS(bgOrder) : 0; // Number of background terms - int numTerms = numKernels * numSpatial + (normAndBG ? 1 + numBackground : 0); // Total number of terms - assert(vector && vector->n == numTerms); - assert(convolutions && convolutions->n == numKernels); - assert(target); - assert(polyValues); - assert(!normAndBG || input); // If we want the normalisation and BG, then we need the input image - - // Convolution terms - for (int i = 0; i < numKernels; i++) { - psKernel *conv = convolutions->data[i]; // Convolution for i-th element - double sumTC = 0.0; // Sum of the target and convolution - for (int y = - footprint; y <= footprint; y++) { - for (int x = - footprint; x <= footprint; x++) { - double value = target->kernel[y][x] * conv->kernel[y][x]; -#ifdef USE_VARIANCE - value /= variance->kernel[y][x]; -#endif - sumTC += value; - } - } - if (!isfinite(sumTC)) { - psTrace("psModules.imcombine", 2, "Bad sumTC at %d", i); - return false; - } - for (int yOrder = 0, index = i; yOrder <= spatialOrder; yOrder++) { - for (int xOrder = 0; xOrder <= spatialOrder - yOrder; xOrder++, index += numKernels) { - vector->data.F64[index] = sumTC * polyValues->data.F64[yOrder][xOrder]; - } - } - } - - if (normAndBG) { - // Background terms - double sumT = 0.0; // Sum of the target - double sumIT = 0.0; // Sum of the input-target product - for (int y = - footprint; y <= footprint; y++) { - for (int x = - footprint; x <= footprint; x++) { - double value = target->kernel[y][x]; -#ifdef USE_VARIANCE - value /= variance->kernel[y][x]; -#endif - sumIT += value * input->kernel[y][x]; - sumT += value; - } - } - if (!isfinite(sumT)) { - psTrace("psModules.imcombine", 2, "Bad sumI detected"); - return false; - } - if (!isfinite(sumIT)) { - psTrace("psModules.imcombine", 2, "Bad sumIT detected"); - return false; - } - - int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation term - vector->data.F64[normIndex] = sumIT; - int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background term - vector->data.F64[bgIndex] = sumT; - } - - return true; -} - - - -// Calculate the cross-matrix, composed of convolutions of each image -// Note that the cross-matrix is NOT square -static bool calculateMatrixCross(psImage *matrix, // Matrix to calculate - const pmSubtractionKernels *kernels, // Kernel components - const psArray *convolutions1, // Convolutions of image 1 - const psArray *convolutions2, // Convolutions of image 2 - const psKernel *image1, // Image 1 stamp - const psKernel *variance, // Variance stamp - const psImage *polyValues, // Spatial polynomial values - int footprint // (Half-)Size of stamp - ) -{ - assert(matrix); - int numKernels = kernels->num; // Number of kernel components - int spatialOrder = kernels->spatialOrder; // Maximum order of spatial variation - int numSpatial = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of spatial polynomial terms - int numBackground = PM_SUBTRACTION_POLYTERMS(kernels->bgOrder); // Number of background terms - int numCols = numKernels * numSpatial + 1 + numBackground; // Number of columns - int numRows = numKernels * numSpatial; // Number of rows - assert(matrix->numCols == numCols && matrix->numRows == numRows); - assert(convolutions1 && convolutions1->n == numKernels); - assert(convolutions2 && convolutions2->n == numKernels); - - int normIndex, bgIndex; // Indices in matrix for normalisation and background terms - PM_SUBTRACTION_INDICES(normIndex, bgIndex, kernels); - - if (!calculateMatrixSquare(matrix, convolutions1, convolutions2, variance, polyValues, numKernels, - spatialOrder, footprint)) { - return false; - } - - for (int i = 0; i < numKernels; i++) { - // Normalisation - psKernel *conv = convolutions2->data[i]; // Convolution - if (!calculateMatrixElement1(matrix, i, normIndex, conv, image1, variance, polyValues, numKernels, - footprint, spatialOrder, false)) { - psTrace("psModules.imcombine", 2, "Bad sumIC at %d", i); - return false; - } - - // Background - double sumC = 0.0; // Sum of the weighting - for (int y = - footprint; y <= footprint; y++) { - for (int x = - footprint; x <= footprint; x++) { - double value = conv->kernel[y][x]; -#ifdef USE_VARIANCE - value /= variance->kernel[y][x]; -#endif - sumC += value; - } - } - if (!isfinite(sumC)) { - psTrace("psModules.imcombine", 2, "Bad sumC detected at %d", i); - return false; - } - for (int yOrder = 0, index = i; yOrder <= spatialOrder; yOrder++) { - for (int xOrder = 0; xOrder <= spatialOrder - yOrder; xOrder++, index += numKernels) { - matrix->data.F64[index][bgIndex] = sumC * polyValues->data.F64[yOrder][xOrder]; - } - } - } - - return true; -} -#endif - // Add in penalty term to least-squares vector static bool calculatePenalty(psVector *vector, // Vector to which to add in penalty term @@ -941,26 +573,12 @@ switch (kernels->mode) { case PM_SUBTRACTION_MODE_1: -#ifdef OLD_FUNCTIONS - status = calculateMatrix(stamp->matrix1, kernels, stamp->convolutions1, stamp->image1, - stamp->variance, polyValues, footprint, true); - status &= calculateVector(stamp->vector1, kernels, stamp->convolutions1, stamp->image1, - stamp->image2, stamp->variance, polyValues, footprint, true); -#else status = calculateMatrixVector(stamp->matrix1, stamp->vector1, stamp->image2, stamp->image1, stamp->variance, stamp->convolutions1, kernels, polyValues, footprint); -#endif break; case PM_SUBTRACTION_MODE_2: -#ifdef OLD_FUNCTIONS - status = calculateMatrix(stamp->matrix1, kernels, stamp->convolutions2, stamp->image2, - stamp->variance, polyValues, footprint, true); - status &= calculateVector(stamp->vector1, kernels, stamp->convolutions2, stamp->image2, - stamp->image1, stamp->variance, polyValues, footprint, true); -#else status = calculateMatrixVector(stamp->matrix1, stamp->vector1, stamp->image1, stamp->image2, stamp->variance, stamp->convolutions2, kernels, polyValues, footprint); -#endif break; case PM_SUBTRACTION_MODE_DUAL: @@ -975,22 +593,8 @@ psVectorInit(stamp->vector2, NAN); #endif -#ifdef OLD_FUNCTIONS - status = calculateMatrix(stamp->matrix1, kernels, stamp->convolutions1, stamp->image1, - stamp->variance, polyValues, footprint, true); - status &= calculateMatrix(stamp->matrix2, kernels, stamp->convolutions2, NULL, - stamp->variance, polyValues, footprint, false); - status &= calculateMatrixCross(stamp->matrixX, kernels, stamp->convolutions1, - stamp->convolutions2, stamp->image1, stamp->variance, polyValues, - footprint); - status &= calculateVector(stamp->vector1, kernels, stamp->convolutions1, stamp->image1, - stamp->image2, stamp->variance, polyValues, footprint, true); - status &= calculateVector(stamp->vector2, kernels, stamp->convolutions2, NULL, - stamp->image2, stamp->variance, polyValues, footprint, false); -#else status = calculateDualMatrixVector(stamp->matrix1, stamp->vector1, stamp->matrix2, stamp->vector2, stamp->matrixX, stamp->image1, stamp->image2, stamp->variance, stamp->convolutions1, stamp->convolutions2, kernels, polyValues, footprint); -#endif break; default: @@ -1291,156 +895,96 @@ // Pure matrix operations - // A * a = Ct * b + d // C * a = B * b + e // - // a = (Ct * Bi * C - A)i (Ct * Bi * e - d) - // b = Bi * (C * a - e) - psVector *a = psVectorRecycle(kernels->solution1, numParams, PS_TYPE_F64); - psVector *b = psVectorRecycle(kernels->solution2, numParams2, PS_TYPE_F64); -#ifdef TESTING - psVectorInit(a, NAN); - psVectorInit(b, NAN); -#endif - psImage *A = sumMatrix1; - psImage *B = sumMatrix2; - psImage *C = sumMatrixX; - psVector *d = sumVector1; - psVector *e = sumVector2; - - assert(a->n == numParams); - assert(b->n == numParams2); - assert(A->numRows == numParams && A->numCols == numParams); - assert(B->numRows == numParams2 && B->numCols == numParams2); - assert(C->numRows == numParams2 && C->numCols == numParams); - assert(d->n == numParams); - assert(e->n == numParams2); - - psImage *Bi = psMatrixInvert(NULL, B, NULL); - assert(Bi->numRows == numParams2 && Bi->numCols == numParams2); - psImage *Ct = psMatrixTranspose(NULL, C); - assert(Ct->numRows == numParams && Ct->numCols == numParams2); - - psImage *BiC = psMatrixMultiply(NULL, Bi, C); - assert(BiC->numRows == numParams2 && BiC->numCols == numParams); - psImage *CtBi = psMatrixMultiply(NULL, Ct, Bi); - assert(CtBi->numRows == numParams && CtBi->numCols == numParams2); - - psImage *CtBiC = psMatrixMultiply(NULL, Ct, BiC); - assert(CtBiC->numRows == numParams && CtBiC->numCols == numParams); - - psImage *F = (psImage*)psBinaryOp(NULL, CtBiC, "-", A); - assert(F->numRows == numParams && F->numCols == numParams); - float det = 0.0; - psImage *Fi = psMatrixInvert(NULL, F, &det); - assert(Fi->numRows == numParams && Fi->numCols == numParams); - psTrace("psModules.imcombine", 4, "Determinant of F: %f\n", det); - - psVector *g = psVectorAlloc(numParams, PS_TYPE_F64); -#ifdef TESTING - psVectorInit(g, NAN); -#endif - assert(CtBi->numRows == numParams && CtBi->numCols == numParams2); - assert(e->n == numParams2); - assert(d->n == numParams); + // Set F = ( A -Ct ; C -B ) + // Set g = ( a ; b ) + // Set h = ( d ; e ) + // So that we combine the above two equations: Fg = h + + int num = numParams + numParams2; // Number of params for new set + psImage *F = psImageAlloc(num, num, PS_TYPE_F64); + psVector *h = psVectorAlloc(num, PS_TYPE_F64); + for (int i = 0; i < numParams; i++) { - double value = 0.0; + h->data.F64[i] = sumVector1->data.F64[i]; + for (int j = 0; j < numParams; j++) { + F->data.F64[i][j] = sumMatrix1->data.F64[i][j]; + } for (int j = 0; j < numParams2; j++) { - value += CtBi->data.F64[i][j] * e->data.F64[j]; - } - g->data.F64[i] = value - d->data.F64[i]; - } - - assert(Fi->numRows == numParams && Fi->numCols == numParams); - assert(g->n == numParams); - for (int i = 0; i < numParams; i++) { - double value = 0.0; + F->data.F64[i][numParams + j] = - sumMatrixX->data.F64[j][i]; + } + } + for (int i = 0; i < numParams2; i++) { + h->data.F64[numParams + i] = sumVector2->data.F64[i]; for (int j = 0; j < numParams; j++) { - value += Fi->data.F64[i][j] * g->data.F64[j]; - } - a->data.F64[i] = value; - } - - psVector *h = psVectorAlloc(numParams2, PS_TYPE_F64); -#ifdef TESTING - psVectorInit(h, NAN); -#endif - assert(C->numRows == numParams2 && C->numCols == numParams); - assert(a->n == numParams); - assert(e->n == numParams2); - for (int i = 0; i < numParams2; i++) { - double value = 0.0; - for (int j = 0; j < numParams; j++) { - value += C->data.F64[i][j] * a->data.F64[j]; - } - h->data.F64[i] = value - e->data.F64[i]; - } - - assert(Bi->numRows == numParams2 && Bi->numCols == numParams2); - assert(h->n == numParams2); - for (int i = 0; i < numParams2; i++) { - double value = 0.0; + F->data.F64[numParams + i][j] = sumMatrixX->data.F64[i][j]; + } for (int j = 0; j < numParams2; j++) { - value += Bi->data.F64[i][j] * h->data.F64[j]; - } - b->data.F64[i] = value; - } - - -#if 1 - for (int i = 0; i < numParams; i++) { - double aVal1 = 0.0, bVal1 = 0.0; - for (int j = 0; j < numParams2; j++) { - aVal1 += A->data.F64[i][j] * a->data.F64[j]; - bVal1 += Ct->data.F64[i][j] * b->data.F64[j]; - } - bVal1 += d->data.F64[i]; - for (int j = numParams2; j < numParams; j++) { - aVal1 += A->data.F64[i][j] * a->data.F64[j]; - } - printf("%d: %lf\n", i, aVal1 - bVal1); - } - - for (int i = 0; i < numParams2; i++) { - double aVal2 = 0.0, bVal2 = 0.0; - for (int j = 0; j < numParams2; j++) { - aVal2 += C->data.F64[i][j] * a->data.F64[j]; - bVal2 += B->data.F64[i][j] * b->data.F64[j]; - } - bVal2 += e->data.F64[i]; - for (int j = numParams2; j < numParams; j++) { - aVal2 += C->data.F64[i][j] * a->data.F64[j]; - } - printf("%d: %lf\n", i, aVal2 - bVal2); - } -#endif + F->data.F64[numParams + i][numParams + j] = - sumMatrix2->data.F64[i][j]; + } + } + psFree(sumMatrix1); + psFree(sumMatrix2); + psFree(sumMatrixX); + psFree(sumVector1); + psFree(sumVector2); #ifdef TESTING { - psFits *fits = psFitsOpen("sumMatrix1.fits", "w"); - psFitsWriteImage(fits, NULL, sumMatrix1, 0, NULL); + psFits *fits = psFitsOpen("sumMatrix.fits", "w"); + psFitsWriteImage(fits, NULL, F, 0, NULL); psFitsClose(fits); } { - psFits *fits = psFitsOpen("sumMatrix2.fits", "w"); - psFitsWriteImage(fits, NULL, sumMatrix2, 0, NULL); + psImage *image = psImageAlloc(1, num, PS_TYPE_F64); + psFits *fits = psFitsOpen("sumVector.fits", "w"); + for (int i = 0; i < num; i++) { + image->data.F64[0][i] = h->data.F64[i]; + } + psFitsWriteImage(fits, NULL, image, 0, NULL); + psFree(image); psFitsClose(fits); } - { - psFits *fits = psFitsOpen("sumMatrixX.fits", "w"); - psFitsWriteImage(fits, NULL, sumMatrixX, 0, NULL); - psFitsClose(fits); - } - { - psFits *fits = psFitsOpen("sumFinverse.fits", "w"); - psFitsWriteImage(fits, NULL, Fi, 0, NULL); - psFitsClose(fits); - } -#endif - - kernels->solution1 = a; - kernels->solution2 = b; - - // XXXXX Free temporary matrices and vectors +#endif + + psVector *permutation = NULL; // Permutation vector, required for LU decomposition + psImage *luMatrix = psMatrixLUDecomposition(NULL, &permutation, F); + if (!luMatrix) { + psError(PS_ERR_UNKNOWN, true, "LU Decomposition of least-squares matrix failed.\n"); + psFree(F); + psFree(h); + psFree(luMatrix); + psFree(permutation); + return NULL; + } + psFree(F); + + psVector *g = psMatrixLUSolution(NULL, luMatrix, h, permutation); // Solution! + if (!g) { + psError(PS_ERR_UNKNOWN, true, "LU Decomposition of least-squares matrix failed.\n"); + psFree(h); + psFree(luMatrix); + psFree(permutation); + return NULL; + } + psFree(permutation); + psFree(luMatrix); + psFree(h); + + if (!kernels->solution1) { + kernels->solution1 = psVectorAlloc(numParams, PS_TYPE_F64); + } + if (!kernels->solution2) { + kernels->solution2 = psVectorAlloc(numParams2, PS_TYPE_F64); + } + + for (int i = 0; i < numParams; i++) { + kernels->solution1->data.F64[i] = g->data.F64[i]; + } + for (int i = 0; i < numParams2; i++) { + kernels->solution2->data.F64[i] = g->data.F64[numParams + i]; + } + psFree(g); }