IPP Software Navigation Tools IPP Links Communication Pan-STARRS Links

Ignore:
Timestamp:
Nov 2, 2009, 10:38:23 AM (17 years ago)
Author:
Paul Price
Message:

Merging PSF-matching code from branches/pap/ for dual convolution.

Location:
trunk/psModules/src/imcombine
Files:
5 edited

Legend:

Unmodified
Added
Removed

  • trunk/psModules/src/imcombine

  • trunk/psModules/src/imcombine/pmSubtraction.c

    r25279 r25999  
    2929#define PIXEL_LIST_BUFFER 100           // Number of entries to add to pixel list at a time
    3030#define MIN_SAMPLE_STATS    7           // Minimum number to use sample statistics; otherwise use quartiles
     31#define USE_SYS_ERR                   // Use systematic error image?
    3132
    3233//////////////////////////////////////////////////////////////////////////////////////////////////////////////
    … …  
    3637// Generate the kernel to apply to the variance from the normal kernel
    3738static psKernel *varianceKernel(psKernel *out, // Output kernel
    38                                 psKernel *normalKernel // Normal kernel
     39                                const psKernel *normalKernel // Normal kernel
    3940                                )
    4041{
    … …  
    4849
    4950    // Take the square of the normal kernel
    50     double sumNormal = 0.0, sumVariance = 0.0; // Sum of the normal and variance kernels
     51    double sumVariance = 0.0; // Sum of the variance kernels
    5152    for (int v = yMin; v <= yMax; v++) {
    5253        for (int u = xMin; u <= xMax; u++) {
    53             float value = normalKernel->kernel[v][u]; // Value of interest
    54             float value2 = PS_SQR(value); // Value squared
    55             sumNormal += value;
    56             sumVariance += value2;
    57             out->kernel[v][u] = value2;
     54            sumVariance += out->kernel[v][u] = PS_SQR(normalKernel->kernel[v][u]);
    5855        }
    5956    }
    … …  
    6562    return out;
    6663}
     64
     65// Contribute to an image of the solved kernel component for ISIS
     66static void solvedKernelISIS(psKernel *kernel, // Kernel, updated
     67                             const pmSubtractionKernels *kernels, // Kernel basis functions
     68                             float value,                         // Normalisation value for basis function
     69                             int index                  // Index of basis function of interest
     70    )
     71{
     72    int size = kernels->size;           // Kernel half-size
     73    psArray *preCalc = kernels->preCalc->data[index]; // Precalculated values
     74#if 0
     75    psVector *xKernel = preCalc->data[0]; // Kernel in x
     76    psVector *yKernel = preCalc->data[1]; // Kernel in y
     77    // Iterating over the kernel
     78    for (int y = 0, v = -size; v <= size; y++, v++) {
     79        float yValue = value * yKernel->data.F32[y];
     80        for (int x = 0, u = -size; u <= size; x++, u++) {
     81            kernel->kernel[v][u] +=  yValue * xKernel->data.F32[x];
     82        }
     83    }
     84    // Photometric scaling for even kernels only
     85    if (kernels->u->data.S32[i] % 2 == 0 && kernels->v->data.S32[i] % 2 == 0) {
     86        kernel->kernel[0][0] -= value;
     87    }
     88#else
     89    psKernel *k = preCalc->data[2]; // Kernel image
     90    for (int v = -size; v <= size; v++) {
     91        for (int u = -size; u <= size; u++) {
     92            kernel->kernel[v][u] +=  value * k->kernel[v][u];
     93        }
     94    }
     95#endif
     96
     97    return;
     98}
     99
    67100
    68101// Generate an image of the solved kernel
    … …  
    70103                              const pmSubtractionKernels *kernels, // Kernel basis functions
    71104                              const psImage *polyValues, // Spatial polynomial values
     105                              bool normalise,            // Add normalisation?
    72106                              bool wantDual // Want the dual (second) kernel?
    73107                              )
    … …  
    115149          case PM_SUBTRACTION_KERNEL_GUNK: {
    116150              if (i < kernels->inner) {
    117                   // Using pre-calculated function
    118                   psKernel *preCalc = kernels->preCalc->data[i]; // Precalculated values
    119                   // Iterating over the kernel
    120                   for (int v = -size; v <= size; v++) {
    121                       for (int u = -size; u <= size; u++) {
    122                           kernel->kernel[v][u] += preCalc->kernel[v][u] * value;
    123                           // Photometric scaling is built into the preCalc kernel --- no subtraction!
    124                       }
    125                   }
     151                  solvedKernelISIS(kernel, kernels, value, i);
    126152              } else {
    127153                  // Using delta function
    … …  
    134160          }
    135161          case PM_SUBTRACTION_KERNEL_ISIS: {
    136               psArray *preCalc = kernels->preCalc->data[i]; // Precalculated values
    137               psVector *xKernel = preCalc->data[0]; // Kernel in x
    138               psVector *yKernel = preCalc->data[1]; // Kernel in y
    139               // Iterating over the kernel
    140               for (int y = 0, v = -size; v <= size; y++, v++) {
    141                   for (int x = 0, u = -size; u <= size; x++, u++) {
    142                       kernel->kernel[v][u] +=  value * xKernel->data.F32[x] * yKernel->data.F32[y];
    143                   }
    144               }
    145               // Photometric scaling for even kernels only
    146               if (kernels->u->data.S32[i] % 2 == 0 && kernels->v->data.S32[i] % 2 == 0) {
    147                   kernel->kernel[0][0] -= value;
    148               }
     162              solvedKernelISIS(kernel, kernels, value, i);
    149163              break;
    150164          }
    … …  
    168182    }
    169183
    170     // Put in the normalisation component
    171     kernel->kernel[0][0] += (wantDual ? 1.0 : p_pmSubtractionSolutionNorm(kernels));
     184    if (normalise) {
     185        // Put in the normalisation component
     186        kernel->kernel[0][0] += (wantDual ? 1.0 : p_pmSubtractionSolutionNorm(kernels));
     187    }
    172188
    173189    return kernel;
    … …  
    339355    )
    340356{
    341     *kernelImage = solvedKernel(*kernelImage, kernels, polyValues, wantDual);
     357    *kernelImage = solvedKernel(*kernelImage, kernels, polyValues, true, wantDual);
    342358    if (variance || subMask) {
    343359        *kernelVariance = varianceKernel(*kernelVariance, *kernelImage);
    … …  
    413429}
    414430
     431#ifdef USE_SYS_ERR
    415432// Generate an image that can be used to track systematic errors
    416433static psImage *subtractionSysErrImage(const psImage *image, // Image from which to make sys err image
    … …  
    433450
    434451    return sys;
     452}
     453#endif
     454
     455// Convolve a stamp using an ISIS kernel basis function
     456static psKernel *convolveStampISIS(const psKernel *image, // Image to convolve
     457                                   const pmSubtractionKernels *kernels, // Kernel basis functions
     458                                   int index,                            // Index of basis function of interest
     459                                   int footprint                         // Half-size of stamp
     460    )
     461{
     462    psArray *preCalc = kernels->preCalc->data[index]; // Precalculated data
     463#if 1
     464    // Convolving using separable convolution
     465    psVector *xKernel = preCalc->data[0]; // Kernel in x
     466    psVector *yKernel = preCalc->data[1]; // Kernel in y
     467    int size = kernels->size;     // Size of kernel
     468
     469    // Convolve in x
     470    // Need to convolve a bit more than the footprint, for the y convolution
     471    int yMin = -size - footprint, yMax = size + footprint; // Range for y
     472    psKernel *temp = psKernelAlloc(yMin, yMax,
     473                                   -footprint, footprint); // Temporary convolution; NOTE: wrong way!
     474    for (int y = yMin; y <= yMax; y++) {
     475        for (int x = -footprint; x <= footprint; x++) {
     476            float value = 0.0;    // Value of convolved pixel
     477            int uMin = x - size, uMax = x + size; // Range for u
     478            psF32 *xKernelData = &xKernel->data.F32[xKernel->n - 1]; // Kernel values
     479            psF32 *imageData = &image->kernel[y][uMin]; // Image values
     480            for (int u = uMin; u <= uMax; u++, xKernelData--, imageData++) {
     481                value += *xKernelData * *imageData;
     482            }
     483            temp->kernel[x][y] = value; // NOTE: putting in wrong way!
     484        }
     485    }
     486
     487    // Convolve in y
     488    psKernel *convolved = psKernelAlloc(-footprint, footprint, -footprint, footprint);// Convolved image
     489    for (int x = -footprint; x <= footprint; x++) {
     490        for (int y = -footprint; y <= footprint; y++) {
     491            float value = 0.0;    // Value of convolved pixel
     492            int vMin = y - size, vMax = y + size; // Range for v
     493            psF32 *yKernelData = &yKernel->data.F32[yKernel->n - 1]; // Kernel values
     494            psF32 *imageData = &temp->kernel[x][vMin]; // Image values; NOTE: wrong way!
     495            for (int v = vMin; v <= vMax; v++, yKernelData--, imageData++) {
     496                value += *yKernelData * *imageData;
     497            }
     498            convolved->kernel[y][x] = value;
     499        }
     500    }
     501    psFree(temp);
     502
     503    // Photometric scaling for even kernels only
     504    if (kernels->u->data.S32[index] % 2 == 0 && kernels->v->data.S32[index] % 2 == 0) {
     505        convolveSub(convolved, image, footprint);
     506    }
     507    return convolved;
     508#else
     509    // Convolving using precalculated kernel
     510    return p_pmSubtractionConvolveStampPrecalc(image, preCalc->data[2]);
     511#endif
    435512}
    436513
    … …  
    456533    psKernel *kernel;                   // Kernel to use
    457534    if (!preKernel) {
    458         kernel = solvedKernel(NULL, kernels, polyValues, wantDual);
     535        kernel = solvedKernel(NULL, kernels, polyValues, true, wantDual);
    459536    } else {
    460537        kernel = psMemIncrRefCounter(preKernel);
    … …  
    586663          if (index < kernels->inner) {
    587664              // Photometric scaling is already built in to the precalculated kernel
    588               return p_pmSubtractionConvolveStampPrecalc(image, kernels->preCalc->data[index]);
     665              return convolveStampISIS(image, kernels, index, footprint);
    589666          }
    590667          // Using delta function
    … …  
    596673      }
    597674      case PM_SUBTRACTION_KERNEL_ISIS: {
    598           psArray *preCalc = kernels->preCalc->data[index]; // Precalculated values
    599           psVector *xKernel = preCalc->data[0]; // Kernel in x
    600           psVector *yKernel = preCalc->data[1]; // Kernel in y
    601           int size = kernels->size;     // Size of kernel
    602 
    603           // Convolve in x
    604           // Need to convolve a bit more than the footprint, for the y convolution
    605           int yMin = -size - footprint, yMax = size + footprint; // Range for y
    606           psKernel *temp = psKernelAlloc(yMin, yMax,
    607                                          -footprint, footprint); // Temporary convolution; NOTE: wrong way!
    608           for (int y = yMin; y <= yMax; y++) {
    609               for (int x = -footprint; x <= footprint; x++) {
    610                   float value = 0.0;    // Value of convolved pixel
    611                   int uMin = x - size, uMax = x + size; // Range for u
    612                   psF32 *xKernelData = &xKernel->data.F32[xKernel->n - 1]; // Kernel values
    613                   psF32 *imageData = &image->kernel[y][uMin]; // Image values
    614                   for (int u = uMin; u <= uMax; u++, xKernelData--, imageData++) {
    615                       value += *xKernelData * *imageData;
    616                   }
    617                   temp->kernel[x][y] = value; // NOTE: putting in wrong way!
    618               }
    619           }
    620 
    621           // Convolve in y
    622           psKernel *convolved = psKernelAlloc(-footprint, footprint, -footprint, footprint);// Convolved image
    623           for (int x = -footprint; x <= footprint; x++) {
    624               for (int y = -footprint; y <= footprint; y++) {
    625                   float value = 0.0;    // Value of convolved pixel
    626                   int vMin = y - size, vMax = y + size; // Range for v
    627                   psF32 *yKernelData = &yKernel->data.F32[yKernel->n - 1]; // Kernel values
    628                   psF32 *imageData = &temp->kernel[x][vMin]; // Image values; NOTE: wrong way!
    629                   for (int v = vMin; v <= vMax; v++, yKernelData--, imageData++) {
    630                       value += *yKernelData * *imageData;
    631                   }
    632                   convolved->kernel[y][x] = value;
    633               }
    634           }
    635           psFree(temp);
    636 
    637           // Photometric scaling for even kernels only
    638           if (kernels->u->data.S32[index] % 2 == 0 && kernels->v->data.S32[index] % 2 == 0) {
    639               convolveSub(convolved, image, footprint);
    640           }
    641           return convolved;
     675          return convolveStampISIS(image, kernels, index, footprint);
    642676      }
    643677      case PM_SUBTRACTION_KERNEL_RINGS: {
    … …  
    901935
    902936    psImage *polyValues = p_pmSubtractionPolynomial(NULL, kernels->spatialOrder, x, y); // Solved polynomial
    903     psKernel *kernel = solvedKernel(NULL, kernels, polyValues, wantDual); // The appropriate kernel
     937    psKernel *kernel = solvedKernel(NULL, kernels, polyValues, true, wantDual); // The appropriate kernel
    904938    psFree(polyValues);
    905939
    … …  
    932966    psImage *polyValues = p_pmSubtractionPolynomial(NULL, kernels->spatialOrder, x, y);
    933967
    934     psKernel *kernel = solvedKernel(NULL, kernels, polyValues, wantDual); // The appropriate kernel
     968    psKernel *kernel = solvedKernel(NULL, kernels, polyValues, true, wantDual); // The appropriate kernel
    935969    psFree(polyValues);
    936970
    … …  
    949983}
    950984
    951 #if 0
     985#if 1
    952986psArray *pmSubtractionKernelSolutions(const pmSubtractionKernels *kernels, float x, float y, bool wantDual)
    953987{
    … …  
    957991    PS_ASSERT_FLOAT_WITHIN_RANGE(y, -1.0, 1.0, NULL);
    958992
    959     psArray *images = psArrayAlloc(kernels->solution1->n - 1); // Images of each kernel to return
    960     psVector *fakeSolution = psVectorAlloc(kernels->solution1->n, PS_TYPE_F64); // Fake solution vector
    961     psVectorInit(fakeSolution, 0.0);
    962 
    963     for (int i = 0; i < kernels->solution1->n - 1; i++) {
    964         fakeSolution->data.F64[i] = kernels->solution1->data.F64[i];
    965         images->data[i] = pmSubtractionKernelImage(kernels, x, y, wantDual);
    966         fakeSolution->data.F64[i] = 0.0;
    967     }
    968 
    969     psFree(fakeSolution);
     993    psVector *solution = wantDual ? kernels->solution2 : kernels->solution1; // Solution of interest
     994    psVector *backup = psVectorCopy(NULL, solution, PS_TYPE_F64);  // Backup version
     995
     996    int num = wantDual ? solution->n - 1 : solution->n; // Number of kernel basis functions
     997
     998    psImage *polyValues = p_pmSubtractionPolynomial(NULL, kernels->spatialOrder, x, y); // Solved polynomial
     999    psArray *images = psArrayAlloc(num + 1); // Images of each kernel to return
     1000
     1001    // The whole kernel
     1002    {
     1003        psKernel *kernel = solvedKernel(NULL, kernels, polyValues, true, wantDual); // The appropriate kernel
     1004        images->data[0] = psMemIncrRefCounter(kernel->image);
     1005        psFree(kernel);
     1006    }
     1007
     1008    // The parts
     1009    psVectorInit(solution, 0.0);
     1010    for (int i = 0; i < num; i++) {
     1011        solution->data.F64[i] = backup->data.F64[i];
     1012        psKernel *kernel = solvedKernel(NULL, kernels, polyValues, false, wantDual); // The appropriate kernel
     1013#if 0
     1014        int size = kernels->size;
     1015        double sum = 0.0;
     1016        for (int v = -size; v <= size; v++) {
     1017            for (int u = -size; u <= size; u++) {
     1018                sum += kernel->kernel[v][u];
     1019            }
     1020        }
     1021        fprintf(stderr, "Kernel %d: %lf\n", i, sum);
     1022#endif
     1023        images->data[i + 1] = psMemIncrRefCounter(kernel->image);
     1024        psFree(kernel);
     1025        solution->data.F64[i] = 0.0;
     1026    }
     1027    psFree(polyValues);
     1028    psVectorCopy(solution, backup, PS_TYPE_F64);
     1029    psFree(backup);
    9701030
    9711031    return images;
    … …  
    11291189        if (!out1->image) {
    11301190            out1->image = psImageAlloc(numCols, numRows, PS_TYPE_F32);
    1131             // XXX if (threaded) {
    1132             // XXX     psMutexInit(out1->image);
    1133             // XXX }
    11341191        }
    11351192        if (ro1->variance) {
    11361193            if (!out1->variance) {
    11371194                out1->variance = psImageAlloc(numCols, numRows, PS_TYPE_F32);
    1138                 // XXX if (threaded) {
    1139                 // XXX     psMutexInit(out1->variance);
    1140                 // XXX }
    11411195            }
    11421196            psImageInit(out1->variance, 0.0);
    … …  
    11461200        if (!out2->image) {
    11471201            out2->image = psImageAlloc(numCols, numRows, PS_TYPE_F32);
    1148             // XXX if (threaded) {
    1149             // XXX     psMutexInit(out2->image);
    1150             // XXX }
    11511202        }
    11521203        if (ro2->variance) {
    11531204            if (!out2->variance) {
    11541205                out2->variance = psImageAlloc(numCols, numRows, PS_TYPE_F32);
    1155                 // XXX if (threaded) {
    1156                 // XXX     psMutexInit(out2->variance);
    1157                 // XXX }
    11581206            }
    11591207            psImageInit(out2->variance, 0.0);
    … …  
    11621210    psImage *convMask = NULL;           // Convolved mask image (common to inputs 1 and 2)
    11631211    if (subMask) {
    1164         // XXX if (threaded) {
    1165         // XXX     psMutexInit(subMask);
    1166         // XXX }
    11671212        if (kernels->mode == PM_SUBTRACTION_MODE_1 || kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
    11681213            if (!out1->mask) {
    … …  
    11881233
    11891234    psImage *sys1 = NULL, *sys2 = NULL; // Systematic error images
     1235#ifdef USE_SYS_ERR
    11901236    if (kernels->mode == PM_SUBTRACTION_MODE_1 || kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
    11911237        sys1 = subtractionSysErrImage(ro1->image, sysError);
    1192         // XXX if (threaded && sys1) {
    1193         // XXX     psMutexInit(sys1);
    1194         // XXX }
    11951238    }
    11961239    if (kernels->mode == PM_SUBTRACTION_MODE_2 || kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
    11971240        sys2 = subtractionSysErrImage(ro2->image, sysError);
    1198         // XXX if (threaded && sys2) {
    1199         // XXX     psMutexInit(sys2);
    1200         // XXX }
    1201     }
     1241    }
     1242#endif
    12021243
    12031244    int size = kernels->size;           // Half-size of kernel
    … …  
    12481289                psArrayAdd(args, 1, (pmReadout*)ro1); // Casting away const
    12491290                psArrayAdd(args, 1, (pmReadout*)ro2); // Casting away const
    1250                 // Since adding to the array can impact the reference count, we need to lock
    1251                 // XXX if (sys1) {
    1252                 // XXX     psMutexLock(sys1);
    1253                 // XXX }
    12541291                psArrayAdd(args, 1, sys1);
    1255                 // XXX if (sys1) {
    1256                 // XXX     psMutexUnlock(sys1);
    1257                 // XXX }
    1258                 // XXX if (sys2) {
    1259                 // XXX     psMutexLock(sys2);
    1260                 // XXX }
    12611292                psArrayAdd(args, 1, sys2);
    1262                 // XXX if (sys2) {
    1263                 // XXX     psMutexUnlock(sys2);
    1264                 // XXX }
    1265                 // XXX if (subMask) {
    1266                 // XXX     psMutexLock(subMask);
    1267                 // XXX }
    12681293                psArrayAdd(args, 1, subMask);
    1269                 // XXX if (subMask) {
    1270                 // XXX     psMutexUnlock(subMask);
    1271                 // XXX }
    12721294                PS_ARRAY_ADD_SCALAR(args, maskBad, PS_TYPE_IMAGE_MASK);
    12731295                PS_ARRAY_ADD_SCALAR(args, maskPoor, PS_TYPE_IMAGE_MASK);
    … …  
    13081330            psFree(job);
    13091331        }
    1310 
    1311         // XXX if (subMask) {
    1312         // XXX     psMutexDestroy(subMask);
    1313         // XXX }
    1314         // XXX if (sys1) {
    1315         // XXX     psMutexDestroy(sys1);
    1316         // XXX }
    1317         // XXX if (sys2) {
    1318         // XXX     psMutexDestroy(sys2);
    1319         // XXX }
    13201332    }
    13211333    psImageConvolveSetThreads(oldThreads);

  • trunk/psModules/src/imcombine/pmSubtractionAnalysis.c

    r25279 r25999  
    1616#define KERNEL_MOSAIC 2                 // Half-number of kernel instances in the mosaic image
    1717
     18//#define TESTING
    1819
    1920bool pmSubtractionAnalysis(psMetadata *analysis, psMetadata *header,
    … …  
    117118
    118119
    119 #if 0
     120#ifdef TESTING
    120121    // Generate images of the kernel components
    121122    {
    … …  
    128129        }
    129130        psArray *kernelImages = pmSubtractionKernelSolutions(kernels, 0.0, 0.0, false);
    130         psFits *kernelFile = psFitsOpen("kernels.fits", "w");
     131        psFits *kernelFile = psFitsOpen("kernels1.fits", "w");
     132        (void)psFitsWriteImageCube(kernelFile, header, kernelImages, NULL);
     133        psFitsClose(kernelFile);
     134        psFree(kernelImages);
     135        psFree(header);
     136    }
     137    if (kernels->solution2) {
     138        psMetadata *header = psMetadataAlloc(); // Header
     139        for (int i = 0; i < kernels->solution2->n; i++) {
     140            psString name = NULL;       // Header keyword
     141            psStringAppend(&name, "SOLN%04d", i);
     142            psMetadataAddF64(header, PS_LIST_TAIL, name, 0, NULL, kernels->solution2->data.F64[i]);
     143            psFree(name);
     144        }
     145        psArray *kernelImages = pmSubtractionKernelSolutions(kernels, 0.0, 0.0, true);
     146        psFits *kernelFile = psFitsOpen("kernels2.fits", "w");
    131147        (void)psFitsWriteImageCube(kernelFile, header, kernelImages, NULL);
    132148        psFitsClose(kernelFile);

  • trunk/psModules/src/imcombine/pmSubtractionEquation.c

    r24844 r25999  
    1515#include "pmSubtractionVisual.h"
    1616
    17 // #define TESTING                         // TESTING output for debugging; may not work with threads!
     17//#define TESTING                         // TESTING output for debugging; may not work with threads!
    1818
    1919#define USE_VARIANCE                    // Include variance in equation?
     20
    2021
    2122//////////////////////////////////////////////////////////////////////////////////////////////////////////////
    … …  
    2324//////////////////////////////////////////////////////////////////////////////////////////////////////////////
    2425
    25 // Calculate the sum over a stamp product
    26 static inline double calculateSumProduct(const psKernel *image1, // First image in multiplication
    27                                          const psKernel *image2, // Second image in multiplication
    28                                          const psKernel *variance, // Variance image
    29                                          int footprint // (Half-)Size of stamp
    30     )
     26// Calculate the least-squares matrix and vector
     27static bool calculateMatrixVector(psImage *matrix, // Least-squares matrix, updated
     28                                  psVector *vector, // Least-squares vector, updated
     29                                  const psKernel *input, // Input image (target)
     30                                  const psKernel *reference, // Reference image (convolution source)
     31                                  const psKernel *variance,  // Variance image
     32                                  const psArray *convolutions,         // Convolutions for each kernel
     33                                  const pmSubtractionKernels *kernels, // Kernels
     34                                  const psImage *polyValues, // Spatial polynomial values
     35                                  int footprint // (Half-)Size of stamp
     36                                  )
    3137{
    32     double sum = 0.0;                   // Sum of the image products
     38    // (I - R * sum_i a_i k_i - g) (R * k_j) = 0
     39    // I C_j = sum_i C_i C_j
     40
     41    // Background: C_i = 1.0
     42    // Normalisation: C_i = R
     43
     44    int numKernels = kernels->num;                      // Number of kernels
     45    int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
     46    int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
     47    int spatialOrder = kernels->spatialOrder;       // Order of spatial variation
     48    int numPoly = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of polynomial terms
     49    double poly[numPoly];                                 // Polynomial terms
     50    double poly2[numPoly][numPoly];                       // Polynomial-polynomial values
     51
     52    // Evaluate polynomial-polynomial terms
     53    for (int iyOrder = 0, iIndex = 0; iyOrder <= spatialOrder; iyOrder++) {
     54        for (int ixOrder = 0; ixOrder <= spatialOrder - iyOrder; ixOrder++, iIndex++) {
     55            double iPoly = polyValues->data.F64[iyOrder][ixOrder]; // Value of polynomial
     56            poly[iIndex] = iPoly;
     57            for (int jyOrder = 0, jIndex = 0; jyOrder <= spatialOrder; jyOrder++) {
     58                for (int jxOrder = 0; jxOrder <= spatialOrder - jyOrder; jxOrder++, jIndex++) {
     59                    double jPoly = polyValues->data.F64[jyOrder][jxOrder];
     60                    poly2[iIndex][jIndex] = iPoly * jPoly;
     61                }
     62            }
     63        }
     64    }
     65
     66
     67    for (int i = 0; i < numKernels; i++) {
     68        psKernel *iConv = convolutions->data[i]; // Convolution for index i
     69        for (int j = i; j < numKernels; j++) {
     70            psKernel *jConv = convolutions->data[j]; // Convolution for index j
     71
     72            double sumCC = 0.0;         // Sum of convolution products
     73            for (int y = - footprint; y <= footprint; y++) {
     74                for (int x = - footprint; x <= footprint; x++) {
     75                    double cc = iConv->kernel[y][x] * jConv->kernel[y][x];
     76#ifdef USE_VARIANCE
     77                    cc /= variance->kernel[y][x];
     78#endif
     79                    sumCC += cc;
     80                }
     81            }
     82
     83            // Spatial variation
     84            for (int iTerm = 0, iIndex = i; iTerm < numPoly; iTerm++, iIndex += numKernels) {
     85                for (int jTerm = 0, jIndex = j; jTerm < numPoly; jTerm++, jIndex += numKernels) {
     86                    double value = sumCC * poly2[iTerm][jTerm];
     87                    matrix->data.F64[iIndex][jIndex] = value;
     88                    matrix->data.F64[jIndex][iIndex] = value;
     89                }
     90            }
     91        }
     92
     93        double sumRC = 0.0;             // Sum of the reference-convolution products
     94        double sumIC = 0.0;             // Sum of the input-convolution products
     95        double sumC = 0.0;              // Sum of the convolution
     96        for (int y = - footprint; y <= footprint; y++) {
     97            for (int x = - footprint; x <= footprint; x++) {
     98                float conv = iConv->kernel[y][x];
     99                float in = input->kernel[y][x];
     100                float ref = reference->kernel[y][x];
     101                double ic = in * conv;
     102                double rc = ref * conv;
     103                double c = conv;
     104#ifdef USE_VARIANCE
     105                float varVal = 1.0 / variance->kernel[y][x];
     106                ic *= varVal;
     107                rc *= varVal;
     108                c *= varVal;
     109#endif
     110                sumIC += ic;
     111                sumRC += rc;
     112                sumC += c;
     113            }
     114        }
     115        // Spatial variation
     116        for (int iTerm = 0, iIndex = i; iTerm < numPoly; iTerm++, iIndex += numKernels) {
     117            double normTerm = sumRC * poly[iTerm];
     118            double bgTerm = sumC * poly[iTerm];
     119            matrix->data.F64[iIndex][normIndex] = normTerm;
     120            matrix->data.F64[normIndex][iIndex] = normTerm;
     121            matrix->data.F64[iIndex][bgIndex] = bgTerm;
     122            matrix->data.F64[bgIndex][iIndex] = bgTerm;
     123            vector->data.F64[iIndex] = sumIC * poly[iTerm];
     124        }
     125    }
     126
     127    double sumRR = 0.0;                 // Sum of the reference product
     128    double sumIR = 0.0;                 // Sum of the input-reference product
     129    double sum1 = 0.0;                  // Sum of the background
     130    double sumR = 0.0;                  // Sum of the reference
     131    double sumI = 0.0;                  // Sum of the input
    33132    for (int y = - footprint; y <= footprint; y++) {
    34133        for (int x = - footprint; x <= footprint; x++) {
    35             double value = image1->kernel[y][x] * image2->kernel[y][x];
     134            double in = input->kernel[y][x];
     135            double ref = reference->kernel[y][x];
     136            double ir = in * ref;
     137            double rr = PS_SQR(ref);
     138            double one = 1.0;
    36139#ifdef USE_VARIANCE
    37             value /= variance->kernel[y][x];
    38 #endif
    39             sum += value;
    40         }
    41     }
    42     return sum;
    43 }
    44 
    45 // Calculate a single element of the least-squares matrix, with the polynomial expansions in one direction
    46 static inline bool calculateMatrixElement1(psImage *matrix, // Matrix to calculate
    47                                            int i, int j, // Coordinates of element
    48                                            const psKernel *image1, // First image in multiplication
    49                                            const psKernel *image2, // Second image in multiplication
    50                                            const psKernel *variance, // Variance image
    51                                            const psImage *polyValues, // Spatial polynomial values
    52                                            int numKernels, // Number of kernel basis functions
    53                                            int footprint, // (Half-)Size of stamp
    54                                            int spatialOrder, // Maximum order of spatial variation
    55                                            bool symmetric // Is the matrix symmetric?
    56     )
    57 {
    58     double sum = calculateSumProduct(image1, image2, variance, footprint); // Sum of the image products
    59     if (!isfinite(sum)) {
    60         return false;
    61     }
    62 
    63     // Generate the pseudo-convolutions from the spatial polynomial terms
    64     for (int iyOrder = 0, iIndex = i; iyOrder <= spatialOrder; iyOrder++) {
    65         for (int ixOrder = 0; ixOrder <= spatialOrder - iyOrder; ixOrder++, iIndex += numKernels) {
    66             double convPoly = sum * polyValues->data.F64[iyOrder][ixOrder];
    67 
    68             assert(iIndex < matrix->numRows && j < matrix->numCols);
    69 
    70             matrix->data.F64[iIndex][j] = convPoly;
    71             if (symmetric) {
    72 
    73                 assert(iIndex < matrix->numCols && j < matrix->numRows);
    74 
    75                 matrix->data.F64[j][iIndex] = convPoly;
    76             }
    77         }
    78     }
     140            float varVal = 1.0 / variance->kernel[y][x];
     141            rr *= varVal;
     142            ir *= varVal;
     143            in *= varVal;
     144            ref *= varVal;
     145            one *= varVal;
     146#endif
     147            sumRR += rr;
     148            sumIR += ir;
     149            sumR += ref;
     150            sumI += in;
     151            sum1 += one;
     152        }
     153    }
     154    matrix->data.F64[normIndex][normIndex] = sumRR;
     155    matrix->data.F64[bgIndex][bgIndex] = sum1;
     156    matrix->data.F64[normIndex][bgIndex] = matrix->data.F64[bgIndex][normIndex] = sumR;
     157    vector->data.F64[normIndex] = sumIR;
     158    vector->data.F64[bgIndex] = sumI;
     159
    79160    return true;
    80161}
    81162
    82 // Calculate a single element of the least-squares matrix, with the polynomial expansions in both directions
    83 static inline bool calculateMatrixElement2(psImage *matrix, // Matrix to calculate
    84                                            int i, int j, // Coordinates of element
    85                                            const psKernel *image1, // First image in multiplication
    86                                            const psKernel *image2, // Second image in multiplication
    87                                            const psKernel *variance, // Variance image
    88                                            const psImage *polyValues, // Spatial polynomial values
    89                                            int numKernels, // Number of kernel basis functions
    90                                            int footprint, // (Half-)Size of stamp
    91                                            int spatialOrder, // Maximum order of spatial variation
    92                                            bool symmetric // Is the matrix symmetric?
    93     )
     163// Calculate the least-squares matrix and vector for dual convolution
     164static bool calculateDualMatrixVector(psImage *matrix1, // Least-squares matrix, updated
     165                                      psVector *vector1, // Least-squares vector, updated
     166                                      psImage *matrix2,  // Least-squares matrix, updated
     167                                      psVector *vector2, // Least-squares vector, updated
     168                                      psImage *matrixX,  // Cross-matrix
     169                                      const psKernel *image1, // Image 1
     170                                      const psKernel *image2, // Image 2
     171                                      const psKernel *variance,  // Variance image
     172                                      const psArray *convolutions1, // Convolutions of image 1 for each kernel
     173                                      const psArray *convolutions2, // Convolutions of image 2 for each kernel
     174                                      const pmSubtractionKernels *kernels, // Kernels
     175                                      const psImage *polyValues, // Spatial polynomial values
     176                                      int footprint // (Half-)Size of stamp
     177                                      )
    94178{
    95     double sum = calculateSumProduct(image1, image2, variance, footprint); // Sum of the image products
    96     if (!isfinite(sum)) {
    97         return false;
    98     }
    99 
    100     // Generate the pseudo-convolutions from the spatial polynomial terms
    101     for (int iyOrder = 0, iIndex = i; iyOrder <= spatialOrder; iyOrder++) {
    102         for (int ixOrder = 0; ixOrder <= spatialOrder - iyOrder; ixOrder++, iIndex += numKernels) {
     179    // A_ij = A_i A_j
     180    // B_ij = B_i B_j
     181    // C_ij = A_i B_j
     182    // d_i = A_i I_2
     183    // e_i = B_i I_2
     184
     185    // A_i = I_1 * k_i
     186    // B_i = I_2 * k_i
     187
     188    // Background: A_i = 1.0
     189    // Normalisation: A_i = I_1
     190
     191    int numKernels = kernels->num;                      // Number of kernels
     192    int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
     193    int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
     194    int spatialOrder = kernels->spatialOrder;       // Order of spatial variation
     195    int numPoly = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of polynomial terms
     196    double poly[numPoly];                                 // Polynomial terms
     197    double poly2[numPoly][numPoly];                       // Polynomial-polynomial values
     198
     199    // Evaluate polynomial-polynomial terms
     200    for (int iyOrder = 0, iIndex = 0; iyOrder <= spatialOrder; iyOrder++) {
     201        for (int ixOrder = 0; ixOrder <= spatialOrder - iyOrder; ixOrder++, iIndex++) {
    103202            double iPoly = polyValues->data.F64[iyOrder][ixOrder]; // Value of polynomial
    104             for (int jyOrder = 0, jIndex = j; jyOrder <= spatialOrder; jyOrder++) {
    105                 for (int jxOrder = 0; jxOrder <= spatialOrder - jyOrder; jxOrder++, jIndex += numKernels) {
    106                     double convPoly = sum * iPoly * polyValues->data.F64[jyOrder][jxOrder];
    107 
    108                     assert(iIndex < matrix->numRows && jIndex < matrix->numCols);
    109 
    110                     matrix->data.F64[iIndex][jIndex] = convPoly;
    111                     if (symmetric) {
    112 
    113                         assert(iIndex < matrix->numCols && jIndex < matrix->numRows);
    114 
    115                         matrix->data.F64[jIndex][iIndex] = convPoly;
    116                     }
    117                 }
    118             }
    119         }
    120     }
     203            poly[iIndex] = iPoly;
     204            for (int jyOrder = 0, jIndex = 0; jyOrder <= spatialOrder; jyOrder++) {
     205                for (int jxOrder = 0; jxOrder <= spatialOrder - jyOrder; jxOrder++, jIndex++) {
     206                    double jPoly = polyValues->data.F64[jyOrder][jxOrder];
     207                    poly2[iIndex][jIndex] = iPoly * jPoly;
     208                }
     209            }
     210        }
     211    }
     212
     213
     214    for (int i = 0; i < numKernels; i++) {
     215        psKernel *iConv1 = convolutions1->data[i]; // Convolution 1 for index i
     216        psKernel *iConv2 = convolutions2->data[i]; // Convolution 2 for index i
     217        for (int j = i; j < numKernels; j++) {
     218            psKernel *jConv1 = convolutions1->data[j]; // Convolution 1 for index j
     219            psKernel *jConv2 = convolutions2->data[j]; // Convolution 2 for index j
     220
     221            double sumAA = 0.0;         // Sum of convolution products for matrix A
     222            double sumBB = 0.0;         // Sum of convolution products for matrix B
     223            double sumAB = 0.0;         // Sum of convolution products for matrix C
     224            for (int y = - footprint; y <= footprint; y++) {
     225                for (int x = - footprint; x <= footprint; x++) {
     226                    double aa = iConv1->kernel[y][x] * jConv1->kernel[y][x];
     227                    double bb = iConv2->kernel[y][x] * jConv2->kernel[y][x];
     228                    double ab = iConv1->kernel[y][x] * jConv2->kernel[y][x];
     229#ifdef USE_VARIANCE
     230                    aa /= variance->kernel[y][x];
     231                    bb /= variance->kernel[y][x];
     232                    ab /= variance->kernel[y][x];
     233#endif
     234                    sumAA += aa;
     235                    sumBB += bb;
     236                    sumAB += ab;
     237                }
     238            }
     239
     240            // Spatial variation
     241            for (int iTerm = 0, iIndex = i; iTerm < numPoly; iTerm++, iIndex += numKernels) {
     242                for (int jTerm = 0, jIndex = j; jTerm < numPoly; jTerm++, jIndex += numKernels) {
     243                    double aa = sumAA * poly2[iTerm][jTerm];
     244                    double bb = sumBB * poly2[iTerm][jTerm];
     245                    double ab = sumAB * poly2[iTerm][jTerm];
     246                    matrix1->data.F64[iIndex][jIndex] = aa;
     247                    matrix1->data.F64[jIndex][iIndex] = aa;
     248                    matrix2->data.F64[iIndex][jIndex] = bb;
     249                    matrix2->data.F64[jIndex][iIndex] = bb;
     250                    matrixX->data.F64[iIndex][jIndex] = ab;
     251                }
     252            }
     253        }
     254        for (int j = 0; j < i; j++) {
     255            psKernel *jConv2 = convolutions2->data[j]; // Convolution 2 for index j
     256            double sumAB = 0.0;         // Sum of convolution products for matrix C
     257            for (int y = - footprint; y <= footprint; y++) {
     258                for (int x = - footprint; x <= footprint; x++) {
     259                    double ab = iConv1->kernel[y][x] * jConv2->kernel[y][x];
     260#ifdef USE_VARIANCE
     261                    ab /= variance->kernel[y][x];
     262#endif
     263                    sumAB += ab;
     264                }
     265            }
     266
     267            // Spatial variation
     268            for (int iTerm = 0, iIndex = i; iTerm < numPoly; iTerm++, iIndex += numKernels) {
     269                for (int jTerm = 0, jIndex = j; jTerm < numPoly; jTerm++, jIndex += numKernels) {
     270                    double ab = sumAB * poly2[iTerm][jTerm];
     271                    matrixX->data.F64[iIndex][jIndex] = ab;
     272                }
     273            }
     274        }
     275
     276        double sumAI2 = 0.0;            // Sum of A.I_2 products (for vector 1)
     277        double sumBI2 = 0.0;            // Sum of B.I_2 products (for vector 2)
     278        double sumAI1 = 0.0;            // Sum of A.I_1 products (for matrix 1, normalisation)
     279        double sumA = 0.0;              // Sum of A (for matrix 1, background)
     280        double sumBI1 = 0.0;            // Sum of B.I_1 products (for matrix X, normalisation)
     281        double sumB = 0.0;              // Sum of B products (for matrix X, background)
     282        double sumI2 = 0.0;             // Sum of I_2 (for vector 1, background)
     283        double sumI1I2 = 0.0;           // Sum of I_1.I_2 (for vector 1, normalisation)
     284        for (int y = - footprint; y <= footprint; y++) {
     285            for (int x = - footprint; x <= footprint; x++) {
     286                float a = iConv1->kernel[y][x];
     287                float b = iConv2->kernel[y][x];
     288                float i1 = image1->kernel[y][x];
     289                float i2 = image2->kernel[y][x];
     290
     291                double ai2 = a * i2;
     292                double bi2 = b * i2;
     293                double ai1 = a * i1;
     294                double bi1 = b * i1;
     295                double i1i2 = i1 * i2;
     296
     297#ifdef USE_VARIANCE
     298                float varVal = 1.0 / variance->kernel[y][x];
     299                ai2 *= varVal;
     300                bi2 *= varVal;
     301                ai1 *= varVal;
     302                bi1 *= varVal;
     303                i1i2 *= varVal;
     304                a *= varVal;
     305                b *= varVal;
     306                i2 *= varVal;
     307#endif
     308
     309                sumAI2 += ai2;
     310                sumBI2 += bi2;
     311                sumAI1 += ai1;
     312                sumA += a;
     313                sumBI1 += bi1;
     314                sumB += b;
     315                sumI2 += i2;
     316                sumI1I2 += i1i2;
     317            }
     318        }
     319        // Spatial variation
     320        for (int iTerm = 0, iIndex = i; iTerm < numPoly; iTerm++, iIndex += numKernels) {
     321            double ai2 = sumAI2 * poly[iTerm];
     322            double bi2 = sumBI2 * poly[iTerm];
     323            double ai1 = sumAI1 * poly[iTerm];
     324            double a = sumA * poly[iTerm];
     325            double bi1 = sumBI1 * poly[iTerm];
     326            double b = sumB * poly[iTerm];
     327
     328            matrix1->data.F64[iIndex][normIndex] = ai1;
     329            matrix1->data.F64[normIndex][iIndex] = ai1;
     330            matrix1->data.F64[iIndex][bgIndex] = a;
     331            matrix1->data.F64[bgIndex][iIndex] = a;
     332            vector1->data.F64[iIndex] = ai2;
     333            vector2->data.F64[iIndex] = bi2;
     334            matrixX->data.F64[iIndex][normIndex] = bi1;
     335            matrixX->data.F64[iIndex][bgIndex] = b;
     336        }
     337    }
     338
     339    double sumI1 = 0.0;                 // Sum of I_1 (for matrix 1, background-normalisation)
     340    double sumI1I1 = 0.0;               // Sum of I_1^2 (for matrix 1, normalisation-normalisation)
     341    double sum1 = 0.0;                  // Sum of 1 (for matrix 1, background-background)
     342    double sumI2 = 0.0;                 // Sum of I_2 (for vector 1, background)
     343    double sumI1I2 = 0.0;               // Sum of I_1.I_2 (for vector 1, normalisation)
     344    for (int y = - footprint; y <= footprint; y++) {
     345        for (int x = - footprint; x <= footprint; x++) {
     346            float i1 = image1->kernel[y][x];
     347            float i2 = image2->kernel[y][x];
     348
     349            double i1i1 = i1 * i1;
     350            double one = 1.0;
     351            double i1i2 = i1 * i2;
     352
     353#ifdef USE_VARIANCE
     354            float varVal = 1.0 / variance->kernel[y][x];
     355            i1 *= varVal;
     356            i1i1 *= varVal;
     357            one *= varVal;
     358            i2 *= varVal;
     359            i1i2 *= varVal;
     360#endif
     361
     362            sumI1 += i1;
     363            sumI1I1 += i1i1;
     364            sum1 += one;
     365            sumI2 += i2;
     366            sumI1I2 += i1i2;
     367        }
     368    }
     369    matrix1->data.F64[bgIndex][normIndex] = sumI1;
     370    matrix1->data.F64[normIndex][bgIndex] = sumI1;
     371    matrix1->data.F64[normIndex][normIndex] = sumI1I1;
     372    matrix1->data.F64[bgIndex][bgIndex] = sum1;
     373    vector1->data.F64[bgIndex] = sumI2;
     374    vector1->data.F64[normIndex] = sumI1I2;
     375
    121376    return true;
    122377}
    123378
    124 // Calculate the square part of the matrix derived from multiplying convolutions
    125 static bool calculateMatrixSquare(psImage *matrix, // Matrix to calculate
    126                                   const psArray *convolutions1, // Convolutions for element 1
    127                                   const psArray *convolutions2, // Convolutions for element 2
    128                                   const psKernel *variance, // Variance image
    129                                   const psImage *polyValues, // Polynomial values
    130                                   int numKernels, // Number of kernel basis functions
    131                                   int spatialOrder, // Order of spatial variation
    132                                   int footprint // Half-size of stamp
     379// Merge dual matrices and vectors into single matrix equation
     380// Have: Aa = Ct.b + d
     381// Have: Ca = Bb + e
     382// Set: F = ( A -Ct ;  C -B )
     383// Set: g = ( a ; b )
     384// Set: h = ( d ; e )
     385// So that we combine the above two equations: Fg = h
     386static bool calculateEquationDual(psImage **outMatrix,
     387                                  psVector **outVector,
     388                                  const psImage *sumMatrix1,
     389                                  const psImage *sumMatrix2,
     390                                  const psImage *sumMatrixX,
     391                                  const psVector *sumVector1,
     392                                  const psVector *sumVector2
    133393                                  )
    134394{
    135     bool symmetric = (convolutions1 == convolutions2 ? true : false); // Is matrix symmetric?
    136 
    137     for (int i = 0; i < numKernels; i++) {
    138         psKernel *iConv = convolutions1->data[i]; // Convolution for i-th element
    139 
    140         for (int j = (symmetric ? i : 0); j < numKernels; j++) {
    141             psKernel *jConv = convolutions2->data[j]; // Convolution for j-th element
    142 
    143             if (!calculateMatrixElement2(matrix, i, j, iConv, jConv, variance, polyValues, numKernels,
    144                                          footprint, spatialOrder, symmetric)) {
    145                 psTrace("psModules.imcombine", 2, "Bad sumCC at %d, %d", i, j);
    146                 return false;
    147             }
     395    psAssert(sumMatrix1 && sumMatrix2 && sumMatrixX, "Require input matrices");
     396    psAssert(sumVector1 && sumVector2, "Require input vectors");
     397    int num1 = sumVector1->n;   // Number of parameters in first set
     398    int num2 = sumVector2->n;   // Number of parameters in second set
     399    int num = num1 + num2;      // Number of parameters in new set
     400
     401    psAssert(sumMatrix1->type.type == PS_TYPE_F64 &&
     402             sumMatrix2->type.type == PS_TYPE_F64 &&
     403             sumMatrixX->type.type == PS_TYPE_F64 &&
     404             sumVector1->type.type == PS_TYPE_F64 &&
     405             sumVector2->type.type == PS_TYPE_F64,
     406             "Require input type is F64");
     407
     408    psAssert(outMatrix, "Require output matrix");
     409    psAssert(outVector, "Require output vector");
     410    if (!*outMatrix) {
     411        *outMatrix = psImageAlloc(num, num, PS_TYPE_F64);
     412    }
     413    if (!*outVector) {
     414        *outVector = psVectorAlloc(num, PS_TYPE_F64);
     415    }
     416    psImage *matrix = *outMatrix;
     417    psVector *vector = *outVector;
     418
     419    psAssert(sumMatrix1->numCols == num1 && sumMatrix1->numRows == num1, "Require size NxN");
     420    psAssert(sumMatrix2->numCols == num2 && sumMatrix2->numRows == num2, "Require size MxM");
     421    psAssert(sumMatrixX->numCols == num1 && sumMatrixX->numRows == num2, "Require size MxN");
     422
     423    memcpy(vector->data.F64, sumVector1->data.F64, num1 * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
     424    memcpy(&vector->data.F64[num1], sumVector2->data.F64, num2 * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
     425
     426    for (int i = 0; i < num1; i++) {
     427        memcpy(matrix->data.F64[i], sumMatrix1->data.F64[i], num1 * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
     428        for (int j = 0, k = num1; j < num2; j++, k++) {
     429            matrix->data.F64[i][k] = - sumMatrixX->data.F64[j][i];
     430        }
     431    }
     432    for (int i1 = 0, i2 = num1; i1 < num2; i1++, i2++) {
     433        memcpy(matrix->data.F64[i2], sumMatrixX->data.F64[i1], num1 * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
     434        for (int j = 0, k = num1; j < num2; j++, k++) {
     435            matrix->data.F64[i2][k] = - sumMatrix2->data.F64[i1][j];
    148436        }
    149437    }
    … …  
    152440}
    153441
    154 // Calculate least-squares matrix and vector
    155 static bool calculateMatrix(psImage *matrix, // Matrix to calculate
    156                             const pmSubtractionKernels *kernels, // Kernel components
    157                             const psArray *convolutions, // Convolutions of source with kernels
    158                             const psKernel *input, // Input stamp, or NULL
    159                             const psKernel *variance, // Variance stamp
    160                             const psImage *polyValues, // Spatial polynomial values
    161                             int footprint, // (Half-)Size of stamp
    162                             bool normAndBG // Calculate normalisation and background terms?
    163     )
    164 {
    165     int numKernels = kernels->num;      // Number of kernel components
    166     int spatialOrder = kernels->spatialOrder; // Maximum order of spatial variation
    167     int numSpatial = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of spatial variation terms
    168     int bgOrder = kernels->bgOrder;     // Maximum order of background fit
    169     int numBackground = normAndBG ? PM_SUBTRACTION_POLYTERMS(bgOrder) : 0; // Number of background terms
    170     int numTerms = numKernels * numSpatial + (normAndBG ? 1 + numBackground : 0); // Total number of terms
    171     assert(matrix);
    172     assert(matrix->numCols == matrix->numRows);
    173     assert(matrix->numCols == numTerms);
    174     assert(convolutions && convolutions->n == numKernels);
    175     assert(polyValues);
    176     assert(!normAndBG || input);        // If we want the normalisation and BG, then we need the input image
    177 
    178     // Square part of the matrix (convolution-convolution products)
    179     if (!calculateMatrixSquare(matrix, convolutions, convolutions, variance, polyValues, numKernels,
    180                                spatialOrder, footprint)) {
    181         return false;
    182     }
    183 
    184     // XXX To support higher-order background model than simply constant, the below code needs to be updated.
    185     if (normAndBG) {
    186         int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
    187         int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
    188 
    189         for (int i = 0; i < numKernels; i++) {
    190             psKernel *conv = convolutions->data[i]; // Convolution for i-th element
    191 
    192             // Normalisation-convolution terms
    193             if (!calculateMatrixElement1(matrix, i, normIndex, conv, input, variance, polyValues, numKernels,
    194                                          footprint, spatialOrder, true)) {
    195                 psTrace("psModules.imcombine", 2, "Bad sumIC at %d", i);
    196                 return false;
    197             }
    198 
    199             // Background-convolution terms
    200             double sumC = 0.0;          // Sum of the convolution
    201             for (int y = - footprint; y <= footprint; y++) {
    202                 for (int x = - footprint; x <= footprint; x++) {
    203                     double value = conv->kernel[y][x];
    204 #ifdef USE_VARIANCE
    205                     value /= variance->kernel[y][x];
    206 #endif
    207                     sumC += value;
    208                 }
    209             }
    210             if (!isfinite(sumC)) {
    211                 psTrace("psModules.imcombine", 2, "Bad sumC at %d", i);
    212                 return false;
    213             }
    214 
    215             for (int yOrder = 0, index = i; yOrder <= spatialOrder; yOrder++) {
    216                 for (int xOrder = 0; xOrder <= spatialOrder - yOrder; xOrder++, index += numKernels) {
    217                     double value = sumC * polyValues->data.F64[yOrder][xOrder];
    218                     matrix->data.F64[index][bgIndex] = value;
    219                     matrix->data.F64[bgIndex][index] = value;
    220                 }
    221             }
    222         }
    223 
    224         // Background only, normalisation only, and background-normalisation terms
    225         double sum1 = 0.0;              // Sum of the weighting
    226         double sumI = 0.0;              // Sum of the input
    227         double sumII = 0.0;             // Sum of the input squared
    228         for (int y = - footprint; y <= footprint; y++) {
    229             for (int x = - footprint; x <= footprint; x++) {
    230                 double invNoise2 = 1.0;
    231 #ifdef USE_VARIANCE
    232                 invNoise2 /= variance->kernel[y][x];
    233 #endif
    234                 double value = input->kernel[y][x] * invNoise2;
    235                 sumI += value;
    236                 sumII += value * input->kernel[y][x];
    237                 sum1 += invNoise2;
    238             }
    239         }
    240         if (!isfinite(sumI)) {
    241             psTrace("psModules.imcombine", 2, "Bad sumI detected");
    242             return false;
    243         }
    244         if (!isfinite(sumII)) {
    245             psTrace("psModules.imcombine", 2, "Bad sumII detected");
    246             return false;
    247         }
    248         if (!isfinite(sum1)) {
    249             psTrace("psModules.imcombine", 2, "Bad sum1 detected");
    250             return false;
    251         }
    252         matrix->data.F64[normIndex][normIndex] = sumII;
    253         matrix->data.F64[bgIndex][bgIndex] = sum1;
    254         matrix->data.F64[normIndex][bgIndex] = sumI;
    255         matrix->data.F64[bgIndex][normIndex] = sumI;
    256     }
    257 
    258     return true;
    259 }
    260 
    261 
    262 // Calculate least-squares matrix and vector
    263 static bool calculateVector(psVector *vector, // Vector to calculate, or NULL
    264                             const pmSubtractionKernels *kernels, // Kernel components
    265                             const psArray *convolutions, // Convolutions of source with kernels
    266                             const psKernel *input, // Input stamp, or NULL if !normAndBG
    267                             const psKernel *target, // Target stamp
    268                             const psKernel *variance, // Variance stamp
    269                             const psImage *polyValues, // Spatial polynomial values
    270                             int footprint, // (Half-)Size of stamp
    271                             bool normAndBG // Calculate normalisation and background terms?
    272     )
    273 {
    274     int numKernels = kernels->num;      // Number of kernel components
    275     int spatialOrder = kernels->spatialOrder; // Maximum order of spatial variation
    276     int numSpatial = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of spatial variation terms
    277     int bgOrder = kernels->bgOrder;     // Maximum order of background fit
    278     int numBackground = normAndBG ? PM_SUBTRACTION_POLYTERMS(bgOrder) : 0; // Number of background terms
    279     int numTerms = numKernels * numSpatial + (normAndBG ? 1 + numBackground : 0); // Total number of terms
    280     assert(vector && vector->n == numTerms);
    281     assert(convolutions && convolutions->n == numKernels);
    282     assert(target);
    283     assert(polyValues);
    284     assert(!normAndBG || input);       // If we want the normalisation and BG, then we need the input image
    285 
    286     // Convolution terms
    287     for (int i = 0; i < numKernels; i++) {
    288         psKernel *conv = convolutions->data[i]; // Convolution for i-th element
    289         double sumTC = 0.0;          // Sum of the target and convolution
    290         for (int y = - footprint; y <= footprint; y++) {
    291             for (int x = - footprint; x <= footprint; x++) {
    292                 double value = target->kernel[y][x] * conv->kernel[y][x];
    293 #ifdef USE_VARIANCE
    294                 value /= variance->kernel[y][x];
    295 #endif
    296                 sumTC += value;
    297             }
    298         }
    299         if (!isfinite(sumTC)) {
    300             psTrace("psModules.imcombine", 2, "Bad sumTC at %d", i);
    301             return false;
    302         }
    303         for (int yOrder = 0, index = i; yOrder <= spatialOrder; yOrder++) {
    304             for (int xOrder = 0; xOrder <= spatialOrder - yOrder; xOrder++, index += numKernels) {
    305                 vector->data.F64[index] = sumTC * polyValues->data.F64[yOrder][xOrder];
    306             }
    307         }
    308     }
    309 
    310     if (normAndBG) {
    311         // Background terms
    312         double sumT = 0.0;              // Sum of the target
    313         double sumIT = 0.0;             // Sum of the input-target product
    314         for (int y = - footprint; y <= footprint; y++) {
    315             for (int x = - footprint; x <= footprint; x++) {
    316                 double value = target->kernel[y][x];
    317 #ifdef USE_VARIANCE
    318                 value /= variance->kernel[y][x];
    319 #endif
    320                 sumIT += value * input->kernel[y][x];
    321                 sumT += value;
    322             }
    323         }
    324         if (!isfinite(sumT)) {
    325             psTrace("psModules.imcombine", 2, "Bad sumI detected");
    326             return false;
    327         }
    328         if (!isfinite(sumIT)) {
    329             psTrace("psModules.imcombine", 2, "Bad sumIT detected");
    330             return false;
    331         }
    332 
    333         int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation term
    334         vector->data.F64[normIndex] = sumIT;
    335         int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background term
    336         vector->data.F64[bgIndex] = sumT;
    337     }
    338 
    339     return true;
    340 }
    341 
    342 
    343 
    344 // Calculate the cross-matrix, composed of convolutions of each image
    345 // Note that the cross-matrix is NOT square
    346 static bool calculateMatrixCross(psImage *matrix, // Matrix to calculate
    347                                  const pmSubtractionKernels *kernels, // Kernel components
    348                                  const psArray *convolutions1, // Convolutions of image 1
    349                                  const psArray *convolutions2, // Convolutions of image 2
    350                                  const psKernel *image1, // Image 1 stamp
    351                                  const psKernel *variance, // Variance stamp
    352                                  const psImage *polyValues, // Spatial polynomial values
    353                                  int footprint // (Half-)Size of stamp
    354                                  )
    355 {
    356     assert(matrix);
    357     int numKernels = kernels->num;      // Number of kernel components
    358     int spatialOrder = kernels->spatialOrder; // Maximum order of spatial variation
    359     int numSpatial = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of spatial polynomial terms
    360     int numBackground = PM_SUBTRACTION_POLYTERMS(kernels->bgOrder); // Number of background terms
    361     int numCols = numKernels * numSpatial + 1 + numBackground; // Number of columns
    362     int numRows = numKernels * numSpatial; // Number of rows
    363     assert(matrix->numCols == numCols && matrix->numRows == numRows);
    364     assert(convolutions1 && convolutions1->n == numKernels);
    365     assert(convolutions2 && convolutions2->n == numKernels);
    366 
    367     int normIndex, bgIndex;             // Indices in matrix for normalisation and background terms
    368     PM_SUBTRACTION_INDICES(normIndex, bgIndex, kernels);
    369 
    370     if (!calculateMatrixSquare(matrix, convolutions1, convolutions2, variance, polyValues, numKernels,
    371                                spatialOrder, footprint)) {
    372         return false;
    373     }
    374 
    375     for (int i = 0; i < numKernels; i++) {
    376         // Normalisation
    377         psKernel *conv = convolutions2->data[i]; // Convolution
    378         if (!calculateMatrixElement1(matrix, i, normIndex, conv, image1, variance, polyValues, numKernels,
    379                                      footprint, spatialOrder, false)) {
    380             psTrace("psModules.imcombine", 2, "Bad sumIC at %d", i);
    381             return false;
    382         }
    383 
    384         // Background
    385         double sumC = 0.0;              // Sum of the weighting
    386         for (int y = - footprint; y <= footprint; y++) {
    387             for (int x = - footprint; x <= footprint; x++) {
    388                 double value = conv->kernel[y][x];
    389 #ifdef USE_VARIANCE
    390                 value /= variance->kernel[y][x];
    391 #endif
    392                 sumC += value;
    393             }
    394         }
    395         if (!isfinite(sumC)) {
    396             psTrace("psModules.imcombine", 2, "Bad sumC detected at %d", i);
    397             return false;
    398         }
    399         for (int yOrder = 0, index = i; yOrder <= spatialOrder; yOrder++) {
    400             for (int xOrder = 0; xOrder <= spatialOrder - yOrder; xOrder++, index += numKernels) {
    401                 matrix->data.F64[index][bgIndex] = sumC * polyValues->data.F64[yOrder][xOrder];
    402             }
    403         }
    404     }
    405 
    406     return true;
    407 }
    408 
    409442
    410443// Add in penalty term to least-squares vector
    411 static bool calculatePenalty(psVector *vector, // Vector to which to add in penalty term
    412                              const pmSubtractionKernels *kernels // Kernel parameters
     444static bool calculatePenalty(psImage *matrix,                     // Matrix to which to add in penalty term
     445                             psVector *vector,                    // Vector to which to add in penalty term
     446                             const pmSubtractionKernels *kernels, // Kernel parameters
     447                             float norm                           // Normalisation
    413448    )
    414449{
    … …  
    423458        for (int yOrder = 0, index = i; yOrder <= spatialOrder; yOrder++) {
    424459            for (int xOrder = 0; xOrder <= spatialOrder - yOrder; xOrder++, index += numKernels) {
    425                 vector->data.F64[index] -= penalties->data.F32[i];
     460                // Contribution to chi^2: a_i^2 P_i
     461                matrix->data.F64[index][index] -= norm * penalties->data.F32[i];
    426462            }
    427463        }
    … …  
    582618    switch (kernels->mode) {
    583619      case PM_SUBTRACTION_MODE_1:
    584         status = calculateMatrix(stamp->matrix1, kernels, stamp->convolutions1, stamp->image1,
    585                                  stamp->variance, polyValues, footprint, true);
    586         status &= calculateVector(stamp->vector1, kernels, stamp->convolutions1, stamp->image1,
    587                                   stamp->image2, stamp->variance, polyValues, footprint, true);
     620        status = calculateMatrixVector(stamp->matrix1, stamp->vector1, stamp->image2, stamp->image1,
     621                                       stamp->variance, stamp->convolutions1, kernels, polyValues,
     622                                       footprint);
    588623        break;
    589624      case PM_SUBTRACTION_MODE_2:
    590         status = calculateMatrix(stamp->matrix1, kernels, stamp->convolutions2, stamp->image2,
    591                                  stamp->variance, polyValues, footprint, true);
    592         status &= calculateVector(stamp->vector1, kernels, stamp->convolutions2, stamp->image2,
    593                                   stamp->image1, stamp->variance, polyValues, footprint, true);
     625        status = calculateMatrixVector(stamp->matrix1, stamp->vector1, stamp->image1, stamp->image2,
     626                                       stamp->variance, stamp->convolutions2, kernels, polyValues,
     627                                       footprint);
    594628        break;
    595629      case PM_SUBTRACTION_MODE_DUAL:
    … …  
    604638        psVectorInit(stamp->vector2, NAN);
    605639#endif
    606         status  = calculateMatrix(stamp->matrix1, kernels, stamp->convolutions1, stamp->image1,
    607                                   stamp->variance, polyValues, footprint, true);
    608         status &= calculateMatrix(stamp->matrix2, kernels, stamp->convolutions2, NULL,
    609                                   stamp->variance, polyValues, footprint, false);
    610         status &= calculateMatrixCross(stamp->matrixX, kernels, stamp->convolutions1,
    611                                        stamp->convolutions2, stamp->image1, stamp->variance, polyValues,
    612                                        footprint);
    613         status &= calculateVector(stamp->vector1, kernels, stamp->convolutions1, stamp->image1,
    614                                   stamp->image2, stamp->variance, polyValues, footprint, true);
    615         status &= calculateVector(stamp->vector2, kernels, stamp->convolutions2, NULL,
    616                                   stamp->image2, stamp->variance, polyValues, footprint, false);
     640        status = calculateDualMatrixVector(stamp->matrix1, stamp->vector1, stamp->matrix2, stamp->vector2,
     641                                           stamp->matrixX, stamp->image1, stamp->image2, stamp->variance,
     642                                           stamp->convolutions1, stamp->convolutions2, kernels, polyValues,
     643                                           footprint);
    617644        break;
    618645      default:
    … …  
    629656
    630657#ifdef TESTING
    631     if (psTraceGetLevel("psModules.imcombine.equation") >= 10) {
     658    {
    632659        psString matrixName = NULL;
    633660        psStringAppend(&matrixName, "matrix1_%d.fits", index);
    … …  
    825852#endif
    826853
    827         calculatePenalty(sumVector, kernels);
     854#if 0
     855        int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background
     856        calculatePenalty(sumMatrix, sumVector, kernels, sumMatrix->data.F64[bgIndex][bgIndex]);
     857#endif
    828858
    829859#ifdef TESTING
    … …  
    909939            }
    910940        }
    911         calculatePenalty(sumVector1, kernels);
    912         calculatePenalty(sumVector2, kernels);
    913 
    914         // Pure matrix operations
    915 
    916         // A * a = Ct * b + d
    917         // C * a = B  * b + e
    918         //
    919         // a = (Ct * Bi * C - A)i (Ct * Bi * e - d)
    920         // b = Bi * (C * a - e)
    921         psVector *a = psVectorRecycle(kernels->solution1, numParams, PS_TYPE_F64);
    922         psVector *b = psVectorRecycle(kernels->solution2, numParams2, PS_TYPE_F64);
    923 #ifdef TESTING
    924         psVectorInit(a, NAN);
    925         psVectorInit(b, NAN);
    926 #endif
    927         psImage *A = sumMatrix1;
    928         psImage *B = sumMatrix2;
    929         psImage *C = sumMatrixX;
    930         psVector *d = sumVector1;
    931         psVector *e = sumVector2;
    932 
    933         assert(a->n == numParams);
    934         assert(b->n == numParams2);
    935         assert(A->numRows == numParams && A->numCols == numParams);
    936         assert(B->numRows == numParams2 && B->numCols == numParams2);
    937         assert(C->numRows == numParams2 && C->numCols == numParams);
    938         assert(d->n == numParams);
    939         assert(e->n == numParams2);
    940 
    941         psImage *Bi = psMatrixInvert(NULL, B, NULL);
    942         assert(Bi->numRows == numParams2 && Bi->numCols == numParams2);
    943         psImage *Ct = psMatrixTranspose(NULL, C);
    944         assert(Ct->numRows == numParams && Ct->numCols == numParams2);
    945 
    946         psImage *BiC = psMatrixMultiply(NULL, Bi, C);
    947         assert(BiC->numRows == numParams2 && BiC->numCols == numParams);
    948         psImage *CtBi = psMatrixMultiply(NULL, Ct, Bi);
    949         assert(CtBi->numRows == numParams && CtBi->numCols == numParams2);
    950 
    951         psImage *CtBiC = psMatrixMultiply(NULL, Ct, BiC);
    952         assert(CtBiC->numRows == numParams && CtBiC->numCols == numParams);
    953 
    954         psImage *F = (psImage*)psBinaryOp(NULL, CtBiC, "-", A);
    955         assert(F->numRows == numParams && F->numCols == numParams);
    956         float det = 0.0;
    957         psImage *Fi = psMatrixInvert(NULL, F, &det);
    958         assert(Fi->numRows == numParams && Fi->numCols == numParams);
    959         psTrace("psModules.imcombine", 4, "Determinant of F: %f\n", det);
    960 
    961         psVector *g = psVectorAlloc(numParams, PS_TYPE_F64);
    962 #ifdef TESTING
    963         psVectorInit(g, NAN);
    964 #endif
    965         assert(CtBi->numRows == numParams && CtBi->numCols == numParams2);
    966         assert(e->n == numParams2);
    967         assert(d->n == numParams);
    968         for (int i = 0; i < numParams; i++) {
    969             double value = 0.0;
    970             for (int j = 0; j < numParams2; j++) {
    971                 value += CtBi->data.F64[i][j] * e->data.F64[j];
    972             }
    973             g->data.F64[i] = value - d->data.F64[i];
    974         }
    975 
    976         assert(Fi->numRows == numParams && Fi->numCols == numParams);
    977         assert(g->n == numParams);
    978         for (int i = 0; i < numParams; i++) {
    979             double value = 0.0;
    980             for (int j = 0; j < numParams; j++) {
    981                 value += Fi->data.F64[i][j] * g->data.F64[j];
    982             }
    983             a->data.F64[i] = value;
    984         }
    985 
    986         psVector *h = psVectorAlloc(numParams2, PS_TYPE_F64);
    987 #ifdef TESTING
    988         psVectorInit(h, NAN);
    989 #endif
    990         assert(C->numRows == numParams2 && C->numCols == numParams);
    991         assert(a->n == numParams);
    992         assert(e->n == numParams2);
    993         for (int i = 0; i < numParams2; i++) {
    994             double value = 0.0;
    995             for (int j = 0; j < numParams; j++) {
    996                 value += C->data.F64[i][j] * a->data.F64[j];
    997             }
    998             h->data.F64[i] = value - e->data.F64[i];
    999         }
    1000 
    1001         assert(Bi->numRows == numParams2 && Bi->numCols == numParams2);
    1002         assert(h->n == numParams2);
    1003         for (int i = 0; i < numParams2; i++) {
    1004             double value = 0.0;
    1005             for (int j = 0; j < numParams2; j++) {
    1006                 value += Bi->data.F64[i][j] * h->data.F64[j];
    1007             }
    1008             b->data.F64[i] = value;
    1009         }
    1010 
    1011 
    1012 #if 0
    1013         for (int i = 0; i < numParams; i++) {
    1014             double aVal1 = 0.0, bVal1 = 0.0;
    1015             for (int j = 0; j < numParams2; j++) {
    1016                 aVal1 += A->data.F64[i][j] * a->data.F64[j];
    1017                 bVal1 += Ct->data.F64[i][j] * b->data.F64[j];
    1018             }
    1019             bVal1 += d->data.F64[i];
    1020             for (int j = numParams2; j < numParams; j++) {
    1021                 aVal1 += A->data.F64[i][j] * a->data.F64[j];
    1022             }
    1023             printf("%d: %lf\n", i, aVal1 - bVal1);
    1024         }
    1025 
    1026         for (int i = 0; i < numParams2; i++) {
    1027             double aVal2 = 0.0, bVal2 = 0.0;
    1028             for (int j = 0; j < numParams2; j++) {
    1029                 aVal2 += C->data.F64[i][j] * a->data.F64[j];
    1030                 bVal2 += B->data.F64[i][j] * b->data.F64[j];
    1031             }
    1032             bVal2 += e->data.F64[i];
    1033             for (int j = numParams2; j < numParams; j++) {
    1034                 aVal2 += C->data.F64[i][j] * a->data.F64[j];
    1035             }
    1036             printf("%d: %lf\n", i, aVal2 - bVal2);
    1037         }
    1038 #endif
     941
     942        int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background
     943        calculatePenalty(sumMatrix1, sumVector1, kernels, sumMatrix1->data.F64[bgIndex][bgIndex]);
     944        calculatePenalty(sumMatrix2, sumVector2, kernels, -sumMatrix1->data.F64[bgIndex][bgIndex]);
     945
     946        psImage *sumMatrix = NULL;      // Combined matrix
     947        psVector *sumVector = NULL;     // Combined vector
     948        calculateEquationDual(&sumMatrix, &sumVector, sumMatrix1, sumMatrix2,
     949                              sumMatrixX, sumVector1, sumVector2);
    1039950
    1040951#ifdef TESTING
    1041952        {
    1042             psFits *fits = psFitsOpen("sumMatrix1.fits", "w");
    1043             psFitsWriteImage(fits, NULL, sumMatrix1, 0, NULL);
     953            psFits *fits = psFitsOpen("sumMatrix.fits", "w");
     954            psFitsWriteImage(fits, NULL, sumMatrix, 0, NULL);
    1044955            psFitsClose(fits);
    1045956        }
    1046957        {
    1047             psFits *fits = psFitsOpen("sumMatrix2.fits", "w");
    1048             psFitsWriteImage(fits, NULL, sumMatrix2, 0, NULL);
     958            psImage *image = psImageAlloc(1, numParams + numParams2, PS_TYPE_F64);
     959            psFits *fits = psFitsOpen("sumVector.fits", "w");
     960            for (int i = 0; i < numParams + numParams2; i++) {
     961                image->data.F64[0][i] = sumVector->data.F64[i];
     962            }
     963            psFitsWriteImage(fits, NULL, image, 0, NULL);
     964            psFree(image);
    1049965            psFitsClose(fits);
    1050966        }
     967#endif
     968
     969        psVector *solution = NULL;                       // Solution to equation!
    1051970        {
    1052             psFits *fits = psFitsOpen("sumMatrixX.fits", "w");
    1053             psFitsWriteImage(fits, NULL, sumMatrixX, 0, NULL);
     971            solution = psMatrixSolveSVD(solution, sumMatrix, sumVector);
     972            if (!solution) {
     973                psError(PS_ERR_UNKNOWN, false, "SVD solution of least-squares equation failed.\n");
     974                psFree(sumMatrix);
     975                psFree(sumVector);
     976                return NULL;
     977            }
     978
     979            int numSpatial = PM_SUBTRACTION_POLYTERMS(kernels->spatialOrder); // Number of spatial variations
     980            int numKernels = kernels->num; // Number of kernel basis functions
     981
     982            // Remove a kernel basis for image 1 from the equation
     983#define MASK_BASIS_1(INDEX)                                             \
     984            {                                                           \
     985                for (int j = 0, index = INDEX; j < numSpatial; j++, index += numKernels) { \
     986                    for (int k = 0; k < numParams2; k++) {              \
     987                        sumMatrix1->data.F64[k][index] = 0.0;           \
     988                        sumMatrix1->data.F64[index][k] = 0.0;           \
     989                        sumMatrixX->data.F64[k][index] = 0.0;           \
     990                    }                                                   \
     991                    sumMatrix1->data.F64[bgIndex][index] = 0.0;         \
     992                    sumMatrix1->data.F64[index][bgIndex] = 0.0;         \
     993                    sumMatrix1->data.F64[normIndex][index] = 0.0;       \
     994                    sumMatrix1->data.F64[index][normIndex] = 0.0;       \
     995                    sumMatrix1->data.F64[index][index] = 1.0;           \
     996                    sumVector1->data.F64[index] = 0.0;                  \
     997                }                                                       \
     998            }
     999
     1000            // Remove a kernel basis for image 2 from the equation
     1001#define MASK_BASIS_2(INDEX)                                             \
     1002            {                                                           \
     1003                for (int j = 0, index = INDEX; j < numSpatial; j++, index += numKernels) { \
     1004                    for (int k = 0; k < numParams2; k++) {              \
     1005                        sumMatrix2->data.F64[k][index] = 0.0;           \
     1006                        sumMatrix2->data.F64[index][k] = 0.0;           \
     1007                        sumMatrixX->data.F64[index][k] = 0.0;           \
     1008                    }                                                   \
     1009                    sumMatrix2->data.F64[index][index] = 1.0;           \
     1010                    sumMatrixX->data.F64[index][normIndex] = 0.0;       \
     1011                    sumMatrixX->data.F64[index][bgIndex] = 0.0;         \
     1012                    sumVector2->data.F64[index] = 0.0;                  \
     1013                }                                                       \
     1014            }
     1015
     1016            #define TOL 1.0e-5
     1017            int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
     1018            double norm = solution->data.F64[normIndex];        // Normalisation
     1019            double thresh = norm * TOL;                         // Threshold for low parameters
     1020            for (int i = 0; i < numKernels; i++) {
     1021                // Getting 0th order parameter value.  In the presence of spatial variation, the actual value
     1022                // of the parameter will vary over the image.  We are in effect getting the value in the
     1023                // centre of the image.  If we use different polynomial functions (e.g., Chebyshev), we may
     1024                // have to change this to properly determine the value of the parameter at the centre.
     1025                double param1 = solution->data.F64[i],
     1026                    param2 = solution->data.F64[numParams + i]; // Parameters of interest
     1027                bool mask1 = false, mask2 = false;              // Masked the parameter?
     1028                if (fabs(param1) < thresh) {
     1029                    psTrace("psModules.imcombine", 7, "Parameter %d: 1 below threshold\n", i);
     1030                    MASK_BASIS_1(i);
     1031                    mask1 = true;
     1032                }
     1033                if (fabs(param2) < thresh) {
     1034                    psTrace("psModules.imcombine", 7, "Parameter %d: 2 below threshold\n", i);
     1035                    MASK_BASIS_2(i);
     1036                    mask2 = true;
     1037                }
     1038
     1039                if (!mask1 && !mask2) {
     1040                    if (fabs(param1) < fabs(param2)) {
     1041                        psTrace("psModules.imcombine", 7, "Parameter %d: 1 < 2\n", i);
     1042                        MASK_BASIS_1(i);
     1043                    } else {
     1044                        psTrace("psModules.imcombine", 7, "Parameter %d: 2 < 1\n", i);
     1045                        MASK_BASIS_2(i);
     1046                    }
     1047                }
     1048            }
     1049        }
     1050
     1051        calculateEquationDual(&sumMatrix, &sumVector, sumMatrix1, sumMatrix2,
     1052                              sumMatrixX, sumVector1, sumVector2);
     1053
     1054#ifdef TESTING
     1055        {
     1056            psFits *fits = psFitsOpen("sumMatrixFix.fits", "w");
     1057            psFitsWriteImage(fits, NULL, sumMatrix, 0, NULL);
    10541058            psFitsClose(fits);
    10551059        }
    10561060        {
    1057             psFits *fits = psFitsOpen("sumFinverse.fits", "w");
    1058             psFitsWriteImage(fits, NULL, Fi, 0, NULL);
     1061            psImage *image = psImageAlloc(1, numParams + numParams2, PS_TYPE_F64);
     1062            psFits *fits = psFitsOpen("sumVectorFix.fits", "w");
     1063            for (int i = 0; i < numParams + numParams2; i++) {
     1064                image->data.F64[0][i] = sumVector->data.F64[i];
     1065            }
     1066            psFitsWriteImage(fits, NULL, image, 0, NULL);
     1067            psFree(image);
    10591068            psFitsClose(fits);
    10601069        }
    10611070#endif
    10621071
    1063         kernels->solution1 = a;
    1064         kernels->solution2 = b;
    1065 
    1066         // XXXXX Free temporary matrices and vectors
     1072        solution = psMatrixSolveSVD(solution, sumMatrix, sumVector);
     1073        if (!solution) {
     1074            psError(PS_ERR_UNKNOWN, false, "SVD solution of least-squares equation failed.\n");
     1075            psFree(sumMatrix);
     1076            psFree(sumVector);
     1077            return NULL;
     1078        }
     1079
     1080        psFree(sumMatrix1);
     1081        psFree(sumMatrix2);
     1082        psFree(sumMatrixX);
     1083        psFree(sumVector1);
     1084        psFree(sumVector2);
     1085
     1086        psFree(sumMatrix);
     1087        psFree(sumVector);
     1088
     1089#ifdef TESTING
     1090        {
     1091            psImage *image = psImageAlloc(1, numParams + numParams2, PS_TYPE_F64);
     1092            psFits *fits = psFitsOpen("solnVector.fits", "w");
     1093            for (int i = 0; i < numParams + numParams2; i++) {
     1094                image->data.F64[0][i] = solution->data.F64[i];
     1095            }
     1096            psFitsWriteImage(fits, NULL, image, 0, NULL);
     1097            psFree(image);
     1098            psFitsClose(fits);
     1099        }
     1100#endif
     1101
     1102        if (!kernels->solution1) {
     1103            kernels->solution1 = psVectorAlloc(numParams, PS_TYPE_F64);
     1104        }
     1105        if (!kernels->solution2) {
     1106            kernels->solution2 = psVectorAlloc(numParams2, PS_TYPE_F64);
     1107        }
     1108
     1109        memcpy(kernels->solution1->data.F64, solution->data.F64, numParams * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
     1110        memcpy(kernels->solution2->data.F64, &solution->data.F64[numParams],
     1111               numParams2 * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
     1112
     1113        psFree(solution);
    10671114
    10681115    }

  • trunk/psModules/src/imcombine/pmSubtractionKernels.c

    r25120 r25999  
    8181    kernels->penalties = psVectorRealloc(kernels->penalties, start + numNew);
    8282    kernels->inner = start;
     83    kernels->num += numNew;
    8384
    8485    // Generate a set of kernels for each (u,v)
    … …  
    9495            kernels->v->data.S32[index] = v;
    9596            kernels->preCalc->data[index] = NULL;
    96             kernels->penalties->data.F32[index] = kernels->penalty * (PS_SQR(u) + PS_SQR(v));
     97            kernels->penalties->data.F32[index] = kernels->penalty * PS_SQR(PS_SQR(u) + PS_SQR(v));
    9798
    9899            psTrace("psModules.imcombine", 7, "Kernel %d: %d %d\n", index, u, v);
    99100        }
    100101    }
     102
     103    kernels->widths->n = start + numNew;
     104    kernels->u->n = start + numNew;
     105    kernels->v->n = start + numNew;
     106    kernels->preCalc->n = start + numNew;
     107    kernels->penalties->n = start + numNew;
    101108
    102109    return true;
    … …  
    140147        for (int uOrder = 0; uOrder <= orders->data.S32[i]; uOrder++) {
    141148            for (int vOrder = 0; vOrder <= orders->data.S32[i] - uOrder; vOrder++, index++) {
    142                 psArray *preCalc = psArrayAlloc(2); // Array to hold precalculated values
     149                psArray *preCalc = psArrayAlloc(3); // Array to hold precalculated values
    143150                psVector *xKernel = preCalc->data[0] = subtractionKernelISIS(sigma, uOrder, size); // x Kernel
    144151                psVector *yKernel = preCalc->data[1] = subtractionKernelISIS(sigma, vOrder, size); // y Kernel
     152                psKernel *kernel = preCalc->data[2] = psKernelAlloc(-size, size, -size, size);      // Kernel
    145153
    146154                // Calculate moments
    … …  
    149157                    for (int u = -size, x = 0; u <= size; u++, x++) {
    150158                        double value = xKernel->data.F32[x] * yKernel->data.F32[y]; // Value of kernel
    151                         moment += value * (PS_SQR(u) + PS_SQR(v));
     159                        kernel->kernel[v][u] = value;
     160                        moment += value * PS_SQR((PS_SQR(u) + PS_SQR(v)));
    152161                    }
    153162                }
    … …  
    164173                    psBinaryOp(xKernel, xKernel, "*", psScalarAlloc(sum, PS_TYPE_F32));
    165174                    psBinaryOp(yKernel, yKernel, "*", psScalarAlloc(sum, PS_TYPE_F32));
     175                    psBinaryOp(kernel->image, kernel->image, "*", psScalarAlloc(PS_SQR(sum), PS_TYPE_F32));
     176                    kernel->kernel[0][0] -= 1.0;
    166177                    moment *= PS_SQR(sum);
    167178                }
     179
     180
     181#if 0
     182                double sum = 0.0;   // Sum of kernel component
     183                for (int v = -size; v <= size; v++) {
     184                    for (int u = -size; u <= size; u++) {
     185                        sum += kernel->kernel[v][u];
     186                    }
     187                }
     188                fprintf(stderr, "%d sum: %lf\n", index, sum);
     189#endif
    168190
    169191                kernels->widths->data.F32[index] = fwhms->data.F32[i];
    … …  
    456478    PS_ASSERT_INT_LESS_THAN(inner, size, NULL);
    457479
    458     // XXX GUNK doesn't seem to work --- doesn't add the POIS components, or at least, they're not noticed
    459 
    460480    pmSubtractionKernels *kernels = p_pmSubtractionKernelsRawISIS(size, spatialOrder, fwhms, orders,
    461481                                                                  penalty, mode); // Kernels
     482    kernels->type = PM_SUBTRACTION_KERNEL_GUNK;
    462483    psStringPrepend(&kernels->description, "GUNK=");
    463484    psStringAppend(&kernels->description, "+POIS(%d,%d)", inner, spatialOrder);
    … …  
    577598                                    poly->data.F32[j] = polyVal;
    578599                                    norm += polyVal;
    579                                     moment += polyVal * (PS_SQR(u) + PS_SQR(v));
     600                                    moment += polyVal * PS_SQR(PS_SQR(u) + PS_SQR(v));
    580601
    581602                                    psVectorExtend(uCoords, RINGS_BUFFER, 1);
    … …  
    603624                        psBinaryOp(poly, poly, "*", psScalarAlloc(1.0 / norm, PS_TYPE_F32));
    604625                    }
    605 //                    moment /= norm;
     626                    moment /= norm;
    606627                }
    607628
Note: See TracChangeset for help on using the changeset viewer.