IPP Software Navigation Tools IPP Links Communication Pan-STARRS Links

Ignore:
Timestamp:
May 3, 2010, 8:45:22 AM (16 years ago)
Author:
eugene
Message:

updates from trunk

Location:
branches/simmosaic_branches
Files:
3 edited

Legend:

Unmodified
Added
Removed

  • branches/simmosaic_branches

  • branches/simmosaic_branches/psModules

  • branches/simmosaic_branches/psModules/src/imcombine/pmSubtractionEquation.c

    r24844 r27839  
    77#include <pslib.h>
    88
     9#include "pmErrorCodes.h"
    910#include "pmSubtraction.h"
    1011#include "pmSubtractionKernels.h"
    … …  
    1516#include "pmSubtractionVisual.h"
    1617
    17 // #define TESTING                         // TESTING output for debugging; may not work with threads!
    18 
    19 #define USE_VARIANCE                    // Include variance in equation?
     18//#define TESTING                         // TESTING output for debugging; may not work with threads!
     19
     20//#define USE_WEIGHT                      // Include weight (1/variance) in equation?
     21//#define USE_WINDOW                      // Include weight (1/variance) in equation?
     22
    2023
    2124//////////////////////////////////////////////////////////////////////////////////////////////////////////////
    … …  
    2326//////////////////////////////////////////////////////////////////////////////////////////////////////////////
    2427
    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     )
     28// Calculate the least-squares matrix and vector
     29static bool calculateMatrixVector(psImage *matrix, // Least-squares matrix, updated
     30                                  psVector *vector, // Least-squares vector, updated
     31                                  double *norm,     // Normalisation, updated
     32                                  const psKernel *input, // Input image (target)
     33                                  const psKernel *reference, // Reference image (convolution source)
     34                                  const psKernel *weight,  // Weight image
     35                                  const psKernel *window,  // Window image
     36                                  const psArray *convolutions,         // Convolutions for each kernel
     37                                  const pmSubtractionKernels *kernels, // Kernels
     38                                  const psImage *polyValues, // Spatial polynomial values
     39                                  int footprint, // (Half-)Size of stamp
     40                                  int normWindow, // Window (half-)size for normalisation measurement
     41                                  const pmSubtractionEquationCalculationMode mode
     42                                  )
    3143{
    32     double sum = 0.0;                   // Sum of the image products
     44    // (I - R * sum_i a_i k_i - g) (R * k_j) = 0
     45    // I C_j = sum_i C_i C_j
     46
     47    // Background: C_i = 1.0
     48    // Normalisation: C_i = R
     49
     50    int numKernels = kernels->num;                      // Number of kernels
     51    int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
     52    int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
     53    int spatialOrder = kernels->spatialOrder;       // Order of spatial variation
     54    int numPoly = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of polynomial terms
     55    double poly[numPoly];                                 // Polynomial terms
     56    double poly2[numPoly][numPoly];                       // Polynomial-polynomial values
     57
     58    // Evaluate polynomial-polynomial terms
     59    // XXX we can skip this if we are not calculating kernel coeffs
     60    for (int iyOrder = 0, iIndex = 0; iyOrder <= spatialOrder; iyOrder++) {
     61        for (int ixOrder = 0; ixOrder <= spatialOrder - iyOrder; ixOrder++, iIndex++) {
     62            double iPoly = polyValues->data.F64[iyOrder][ixOrder]; // Value of polynomial
     63            poly[iIndex] = iPoly;
     64            for (int jyOrder = 0, jIndex = 0; jyOrder <= spatialOrder; jyOrder++) {
     65                for (int jxOrder = 0; jxOrder <= spatialOrder - jyOrder; jxOrder++, jIndex++) {
     66                    double jPoly = polyValues->data.F64[jyOrder][jxOrder];
     67                    poly2[iIndex][jIndex] = iPoly * jPoly;
     68                }
     69            }
     70        }
     71    }
     72
     73    // initialize the matrix and vector for NOP on all coeffs.  we only fill in the coeffs we
     74    // choose to calculate
     75    psImageInit(matrix, 0.0);
     76    psVectorInit(vector, 1.0);
     77    for (int i = 0; i < matrix->numCols; i++) {
     78        matrix->data.F64[i][i] = 1.0;
     79    }
     80
     81    // the order of the elements in the matrix and vector is:
     82    // [kernel 0, x^0 y^0][kernel 1 x^0 y^0]...[kernel N, x^0 y^0]
     83    // [kernel 0, x^1 y^0][kernel 1 x^1 y^0]...[kernel N, x^1 y^0]
     84    // [kernel 0, x^n y^m][kernel 1 x^n y^m]...[kernel N, x^n y^m]
     85    // normalization
     86    // bg 0, bg 1, bg 2 (only 0 is currently used?)
     87
     88    for (int i = 0; i < numKernels; i++) {
     89        psKernel *iConv = convolutions->data[i]; // Convolution for index i
     90        for (int j = i; j < numKernels; j++) {
     91            psKernel *jConv = convolutions->data[j]; // Convolution for index j
     92
     93            double sumCC = 0.0;         // Sum of convolution products
     94            for (int y = - footprint; y <= footprint; y++) {
     95                for (int x = - footprint; x <= footprint; x++) {
     96                    double cc = iConv->kernel[y][x] * jConv->kernel[y][x];
     97                    if (weight) {
     98                        cc *= weight->kernel[y][x];
     99                    }
     100                    if (window) {
     101                        cc *= window->kernel[y][x];
     102                    }
     103                    sumCC += cc;
     104                }
     105            }
     106
     107            // Spatial variation of kernel coeffs
     108            if (mode & PM_SUBTRACTION_EQUATION_KERNELS) {
     109                for (int iTerm = 0, iIndex = i; iTerm < numPoly; iTerm++, iIndex += numKernels) {
     110                    for (int jTerm = 0, jIndex = j; jTerm < numPoly; jTerm++, jIndex += numKernels) {
     111                        double value = sumCC * poly2[iTerm][jTerm];
     112                        matrix->data.F64[iIndex][jIndex] = value;
     113                        matrix->data.F64[jIndex][iIndex] = value;
     114                    }
     115                }
     116            }
     117        }
     118
     119        double sumRC = 0.0;             // Sum of the reference-convolution products
     120        double sumIC = 0.0;             // Sum of the input-convolution products
     121        double sumC = 0.0;              // Sum of the convolution
     122        for (int y = - footprint; y <= footprint; y++) {
     123            for (int x = - footprint; x <= footprint; x++) {
     124                float conv = iConv->kernel[y][x];
     125                float in = input->kernel[y][x];
     126                float ref = reference->kernel[y][x];
     127                double ic = in * conv;
     128                double rc = ref * conv;
     129                double c = conv;
     130                if (weight) {
     131                    float wtVal = weight->kernel[y][x];
     132                    ic *= wtVal;
     133                    rc *= wtVal;
     134                    c *= wtVal;
     135                }
     136                if (window) {
     137                    float winVal = window->kernel[y][x];
     138                    ic *= winVal;
     139                    rc *= winVal;
     140                    c  *= winVal;
     141                }
     142                sumIC += ic;
     143                sumRC += rc;
     144                sumC += c;
     145            }
     146        }
     147        // Spatial variation
     148        for (int iTerm = 0, iIndex = i; iTerm < numPoly; iTerm++, iIndex += numKernels) {
     149            double normTerm = sumRC * poly[iTerm];
     150            double bgTerm = sumC * poly[iTerm];
     151            if ((mode & PM_SUBTRACTION_EQUATION_NORM) && (mode & PM_SUBTRACTION_EQUATION_KERNELS)) {
     152                matrix->data.F64[iIndex][normIndex] = normTerm;
     153                matrix->data.F64[normIndex][iIndex] = normTerm;
     154            }
     155            if ((mode & PM_SUBTRACTION_EQUATION_BG) && (mode & PM_SUBTRACTION_EQUATION_KERNELS)) {
     156                matrix->data.F64[iIndex][bgIndex] = bgTerm;
     157                matrix->data.F64[bgIndex][iIndex] = bgTerm;
     158            }
     159            if (mode & PM_SUBTRACTION_EQUATION_KERNELS) {
     160                vector->data.F64[iIndex] = sumIC * poly[iTerm];
     161                if (!(mode & PM_SUBTRACTION_EQUATION_NORM)) {
     162                    // subtract norm * sumRC * poly[iTerm]
     163                    psAssert (kernels->solution1, "programming error: define solution first!");
     164                    int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
     165                    double norm = fabs(kernels->solution1->data.F64[normIndex]);  // Normalisation
     166                    vector->data.F64[iIndex] -= norm * normTerm;
     167                }
     168            }
     169        }
     170    }
     171
     172    double sumRR = 0.0;                 // Sum of the reference product
     173    double sumIR = 0.0;                 // Sum of the input-reference product
     174    double sum1 = 0.0;                  // Sum of the background
     175    double sumR = 0.0;                  // Sum of the reference
     176    double sumI = 0.0;                  // Sum of the input
     177    double normI1 = 0.0, normI2 = 0.0;  // Sum of I_1 and I_2 within the normalisation window
    33178    for (int y = - footprint; y <= footprint; y++) {
    34179        for (int x = - footprint; x <= footprint; x++) {
    35             double value = image1->kernel[y][x] * image2->kernel[y][x];
    36 #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     )
     180            double in = input->kernel[y][x];
     181            double ref = reference->kernel[y][x];
     182            double ir = in * ref;
     183            double rr = PS_SQR(ref);
     184            double one = 1.0;
     185
     186            if (PS_SQR(x) + PS_SQR(y) <= PS_SQR(normWindow)) {
     187                normI1 += ref;
     188                normI2 += in;
     189            }
     190
     191            if (weight) {
     192                float wtVal = weight->kernel[y][x];
     193                rr *= wtVal;
     194                ir *= wtVal;
     195                in *= wtVal;
     196                ref *= wtVal;
     197                one *= wtVal;
     198            }
     199            if (window) {
     200                float  winVal = window->kernel[y][x];
     201                rr      *= winVal;
     202                ir      *= winVal;
     203                in      *= winVal;
     204                ref *= winVal;
     205                one *= winVal;
     206            }
     207            sumRR += rr;
     208            sumIR += ir;
     209            sumR += ref;
     210            sumI += in;
     211            sum1 += one;
     212        }
     213    }
     214
     215    *norm = normI2 / normI1;
     216
     217    if (mode & PM_SUBTRACTION_EQUATION_NORM) {
     218        matrix->data.F64[normIndex][normIndex] = sumRR;
     219        vector->data.F64[normIndex] = sumIR;
     220        // subtract sum over kernels * kernel solution
     221    }
     222    if (mode & PM_SUBTRACTION_EQUATION_BG) {
     223        matrix->data.F64[bgIndex][bgIndex] = sum1;
     224        vector->data.F64[bgIndex] = sumI;
     225    }
     226    if ((mode & PM_SUBTRACTION_EQUATION_NORM) && (mode & PM_SUBTRACTION_EQUATION_BG)) {
     227        matrix->data.F64[normIndex][bgIndex] = sumR;
     228        matrix->data.F64[bgIndex][normIndex] = sumR;
     229    }
     230
     231    // check for any NAN values in the result, skip if found:
     232    for (int iy = 0; iy < matrix->numRows; iy++) {
     233        for (int ix = 0; ix < matrix->numCols; ix++) {
     234            if (!isfinite(matrix->data.F64[iy][ix])) {
     235                fprintf (stderr, "WARNING: NAN in matrix\n");
     236                return false;
     237            }
     238        }
     239    }
     240    for (int ix = 0; ix < vector->n; ix++) {
     241        if (!isfinite(vector->data.F64[ix])) {
     242            fprintf (stderr, "WARNING: NAN in vector\n");
     243            return false;
     244        }
     245    }
     246
     247    return true;
     248}
     249
     250
     251// Calculate the least-squares matrix and vector for dual convolution
     252static bool calculateDualMatrixVector(psImage *matrix, // Least-squares matrix, updated
     253                                      psVector *vector, // Least-squares vector, updated
     254                                      double *norm,     // Normalisation, updated
     255                                      const psKernel *image1, // Image 1
     256                                      const psKernel *image2, // Image 2
     257                                      const psKernel *weight,  // Weight image
     258                                      const psKernel *window,  // Window image
     259                                      const psArray *convolutions1, // Convolutions of image 1 for each kernel
     260                                      const psArray *convolutions2, // Convolutions of image 2 for each kernel
     261                                      const pmSubtractionKernels *kernels, // Kernels
     262                                      const psImage *polyValues, // Spatial polynomial values
     263                                      int footprint, // (Half-)Size of stamp
     264                                      int normWindow, // Window (half-)size for normalisation measurement
     265                                      const pmSubtractionEquationCalculationMode mode
     266                                      )
    57267{
    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     }
    79     return true;
    80 }
    81 
    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     )
    94 {
    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) {
     268    int numKernels = kernels->num;                      // Number of kernels
     269    int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
     270    int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
     271    int spatialOrder = kernels->spatialOrder;       // Order of spatial variation
     272    int numPoly = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of polynomial terms
     273    double poly[numPoly];                                 // Polynomial terms
     274    double poly2[numPoly][numPoly];                       // Polynomial-polynomial values
     275
     276    int numBackground = PM_SUBTRACTION_POLYTERMS(kernels->bgOrder); // Number of background terms
     277    int numParams = numKernels * numPoly + 1 + numBackground;       // Number of regular parameters
     278    int numParams2 = numKernels * numPoly;                          // Number of additional parameters for dual
     279    int numDual = numParams + numParams2;                           // Total number of parameters for dual
     280
     281    psAssert(matrix &&
     282             matrix->type.type == PS_TYPE_F64 &&
     283             matrix->numCols == numDual &&
     284             matrix->numRows == numDual,
     285             "Least-squares matrix is bad.");
     286    psAssert(vector &&
     287             vector->type.type == PS_TYPE_F64 &&
     288             vector->n == numDual,
     289             "Least-squares vector is bad.");
     290
     291    // Evaluate polynomial-polynomial terms
     292    for (int iyOrder = 0, iIndex = 0; iyOrder <= spatialOrder; iyOrder++) {
     293        for (int ixOrder = 0; ixOrder <= spatialOrder - iyOrder; ixOrder++, iIndex++) {
    103294            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     }
    121     return true;
    122 }
    123 
    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
    133                                   )
    134 {
    135     bool symmetric = (convolutions1 == convolutions2 ? true : false); // Is matrix symmetric?
     295            poly[iIndex] = iPoly;
     296            for (int jyOrder = 0, jIndex = 0; jyOrder <= spatialOrder; jyOrder++) {
     297                for (int jxOrder = 0; jxOrder <= spatialOrder - jyOrder; jxOrder++, jIndex++) {
     298                    double jPoly = polyValues->data.F64[jyOrder][jxOrder];
     299                    poly2[iIndex][jIndex] = iPoly * jPoly;
     300                }
     301            }
     302        }
     303    }
     304
     305
     306    // initialize the matrix and vector for NOP on all coeffs.  we only fill in the coeffs we
     307    // choose to calculate
     308    psImageInit(matrix, 0.0);
     309    psVectorInit(vector, 1.0);
     310    for (int i = 0; i < matrix->numCols; i++) {
     311        matrix->data.F64[i][i] = 1.0;
     312    }
    136313
    137314    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             }
    148         }
    149     }
    150 
    151     return true;
    152 }
    153 
    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
     315        psKernel *iConv1 = convolutions1->data[i]; // Convolution 1 for index i
     316        psKernel *iConv2 = convolutions2->data[i]; // Convolution 2 for index i
     317        for (int j = i; j < numKernels; j++) {
     318            psKernel *jConv1 = convolutions1->data[j]; // Convolution 1 for index j
     319            psKernel *jConv2 = convolutions2->data[j]; // Convolution 2 for index j
     320
     321            double sumAA = 0.0;         // Sum of convolution products between image 1
     322            double sumBB = 0.0;         // Sum of convolution products between image 2
     323            double sumAB = 0.0;         // Sum of convolution products across images 1 and 2
    201324            for (int y = - footprint; y <= footprint; y++) {
    202325                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
     326                    double aa = iConv1->kernel[y][x] * jConv1->kernel[y][x];
     327                    double bb = iConv2->kernel[y][x] * jConv2->kernel[y][x];
     328                    double ab = iConv1->kernel[y][x] * jConv2->kernel[y][x];
     329                    if (weight) {
     330                        float wtVal = weight->kernel[y][x];
     331                        aa *= wtVal;
     332                        bb *= wtVal;
     333                        ab *= wtVal;
     334                    }
     335                    if (window) {
     336                        float wtVal = window->kernel[y][x];
     337                        aa *= wtVal;
     338                        bb *= wtVal;
     339                        ab *= wtVal;
     340                    }
     341                    sumAA += aa;
     342                    sumBB += bb;
     343                    sumAB += ab;
     344                }
     345            }
     346
     347            // Spatial variation of kernel coeffs
     348            if (mode & PM_SUBTRACTION_EQUATION_KERNELS) {
     349                for (int iTerm = 0, iIndex = i; iTerm < numPoly; iTerm++, iIndex += numKernels) {
     350                    for (int jTerm = 0, jIndex = j; jTerm < numPoly; jTerm++, jIndex += numKernels) {
     351                        double aa = sumAA * poly2[iTerm][jTerm];
     352                        double bb = sumBB * poly2[iTerm][jTerm];
     353                        double ab = sumAB * poly2[iTerm][jTerm];
     354
     355                        matrix->data.F64[iIndex][jIndex] = aa;
     356                        matrix->data.F64[jIndex][iIndex] = aa;
     357
     358                        matrix->data.F64[iIndex + numParams][jIndex + numParams] = bb;
     359                        matrix->data.F64[jIndex + numParams][iIndex + numParams] = bb;
     360
     361                        matrix->data.F64[iIndex][jIndex + numParams] = ab;
     362                        matrix->data.F64[jIndex + numParams][iIndex] = ab;
     363                    }
     364                }
     365            }
     366        }
     367        for (int j = 0; j < i; j++) {
     368            psKernel *jConv2 = convolutions2->data[j]; // Convolution 2 for index j
     369            double sumAB = 0.0;         // Sum of convolution products for matrix C
     370            for (int y = - footprint; y <= footprint; y++) {
     371                for (int x = - footprint; x <= footprint; x++) {
     372                    double ab = iConv1->kernel[y][x] * jConv2->kernel[y][x];
     373                    if (weight) {
     374                        ab *= weight->kernel[y][x];
     375                    }
     376                    if (window) {
     377                        ab *= window->kernel[y][x];
     378                    }
     379                    sumAB += ab;
     380                }
     381            }
     382
     383            // Spatial variation of kernel coeffs
     384            if (mode & PM_SUBTRACTION_EQUATION_KERNELS) {
     385                for (int iTerm = 0, iIndex = i; iTerm < numPoly; iTerm++, iIndex += numKernels) {
     386                    for (int jTerm = 0, jIndex = j; jTerm < numPoly; jTerm++, jIndex += numKernels) {
     387                        double ab = sumAB * poly2[iTerm][jTerm];
     388                        matrix->data.F64[iIndex][jIndex + numParams] = ab;
     389                        matrix->data.F64[jIndex + numParams][iIndex] = ab;
     390                    }
     391                }
     392            }
     393        }
     394
     395        double sumAI2 = 0.0;            // Sum of A.I_2 products (for vector)
     396        double sumBI2 = 0.0;            // Sum of B.I_2 products (for vector)
     397        double sumAI1 = 0.0;            // Sum of A.I_1 products (for matrix, normalisation)
     398        double sumA = 0.0;              // Sum of A (for matrix, background)
     399        double sumBI1 = 0.0;            // Sum of B.I_1 products (for matrix, normalisation)
     400        double sumB = 0.0;              // Sum of B products (for matrix, background)
     401        double sumI2 = 0.0;             // Sum of I_2 (for vector, background)
    228402        for (int y = - footprint; y <= footprint; y++) {
    229403            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");
     404                double a = iConv1->kernel[y][x];
     405                double b = iConv2->kernel[y][x];
     406                float i1 = image1->kernel[y][x];
     407                float i2 = image2->kernel[y][x];
     408
     409                double ai2 = a * i2;
     410                double bi2 = b * i2;
     411                double ai1 = a * i1;
     412                double bi1 = b * i1;
     413
     414                if (weight) {
     415                    float wtVal = weight->kernel[y][x];
     416                    ai2 *= wtVal;
     417                    bi2 *= wtVal;
     418                    ai1 *= wtVal;
     419                    bi1 *= wtVal;
     420                    a *= wtVal;
     421                    b *= wtVal;
     422                    i2 *= wtVal;
     423                }
     424                if (window) {
     425                    float wtVal = window->kernel[y][x];
     426                    ai2 *= wtVal;
     427                    bi2 *= wtVal;
     428                    ai1 *= wtVal;
     429                    bi1 *= wtVal;
     430                    a *= wtVal;
     431                    b *= wtVal;
     432                    i2 *= wtVal;
     433                }
     434                sumAI2 += ai2;
     435                sumBI2 += bi2;
     436                sumAI1 += ai1;
     437                sumA += a;
     438                sumBI1 += bi1;
     439                sumB += b;
     440                sumI2 += i2;
     441            }
     442        }
     443        // Spatial variation
     444        for (int iTerm = 0, iIndex = i; iTerm < numPoly; iTerm++, iIndex += numKernels) {
     445            double ai2 = sumAI2 * poly[iTerm];
     446            double bi2 = sumBI2 * poly[iTerm];
     447            double ai1 = sumAI1 * poly[iTerm];
     448            double a   = sumA * poly[iTerm];
     449            double bi1 = sumBI1 * poly[iTerm];
     450            double b   = sumB * poly[iTerm];
     451
     452            if ((mode & PM_SUBTRACTION_EQUATION_NORM) && (mode & PM_SUBTRACTION_EQUATION_KERNELS)) {
     453                matrix->data.F64[iIndex][normIndex] = ai1;
     454                matrix->data.F64[normIndex][iIndex] = ai1;
     455                matrix->data.F64[iIndex + numParams][normIndex] = bi1;
     456                matrix->data.F64[normIndex][iIndex + numParams] = bi1;
     457            }
     458            if ((mode & PM_SUBTRACTION_EQUATION_BG) && (mode & PM_SUBTRACTION_EQUATION_KERNELS)) {
     459                matrix->data.F64[iIndex][bgIndex] = a;
     460                matrix->data.F64[bgIndex][iIndex] = a;
     461                matrix->data.F64[iIndex + numParams][bgIndex] = b;
     462                matrix->data.F64[bgIndex][iIndex + numParams] = b;
     463            }
     464            if (mode & PM_SUBTRACTION_EQUATION_KERNELS) {
     465                vector->data.F64[iIndex] = ai2;
     466                vector->data.F64[iIndex + numParams] = bi2;
     467                if (!(mode & PM_SUBTRACTION_EQUATION_NORM)) {
     468                    // subtract norm * sumRC * poly[iTerm]
     469                    psAssert (kernels->solution1, "programming error: define solution first!");
     470                    int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
     471                    double norm = fabs(kernels->solution1->data.F64[normIndex]);  // Normalisation
     472                    vector->data.F64[iIndex] -= norm * ai1;
     473                    vector->data.F64[iIndex + numParams] -= norm * bi1;
     474                }
     475            }
     476        }
     477    }
     478
     479    double sumI1 = 0.0;                 // Sum of I_1 (for matrix, background-normalisation)
     480    double sumI1I1 = 0.0;               // Sum of I_1^2 (for matrix, normalisation-normalisation)
     481    double sum1 = 0.0;                  // Sum of 1 (for matrix, background-background)
     482    double sumI2 = 0.0;                 // Sum of I_2 (for vector, background)
     483    double sumI1I2 = 0.0;               // Sum of I_1.I_2 (for vector, normalisation)
     484    double normI1 = 0.0, normI2 = 0.0;  // Sum of I_1 and I_2 within the normalisation window
     485    for (int y = - footprint; y <= footprint; y++) {
     486        for (int x = - footprint; x <= footprint; x++) {
     487            double i1 = image1->kernel[y][x];
     488            double i2 = image2->kernel[y][x];
     489
     490            double i1i1 = i1 * i1;
     491            double one = 1.0;
     492            double i1i2 = i1 * i2;
     493
     494            if (PS_SQR(x) + PS_SQR(y) <= PS_SQR(normWindow)) {
     495                normI1 += i1;
     496                normI2 += i2;
     497            }
     498
     499            if (weight) {
     500                float wtVal = weight->kernel[y][x];
     501                i1 *= wtVal;
     502                i1i1 *= wtVal;
     503                one *= wtVal;
     504                i2 *= wtVal;
     505                i1i2 *= wtVal;
     506            }
     507            if (window) {
     508                float wtVal = window->kernel[y][x];
     509                i1 *= wtVal;
     510                i1i1 *= wtVal;
     511                one *= wtVal;
     512                i2 *= wtVal;
     513                i1i2 *= wtVal;
     514            }
     515            sumI1 += i1;
     516            sumI1I1 += i1i1;
     517            sum1 += one;
     518            sumI2 += i2;
     519            sumI1I2 += i1i2;
     520        }
     521    }
     522
     523    *norm = normI2 / normI1;
     524
     525    if (mode & PM_SUBTRACTION_EQUATION_NORM) {
     526        matrix->data.F64[normIndex][normIndex] = sumI1I1;
     527        vector->data.F64[normIndex] = sumI1I2;
     528    }
     529    if (mode & PM_SUBTRACTION_EQUATION_BG) {
     530        matrix->data.F64[bgIndex][bgIndex] = sum1;
     531        vector->data.F64[bgIndex] = sumI2;
     532    }
     533    if ((mode & PM_SUBTRACTION_EQUATION_NORM) && (mode & PM_SUBTRACTION_EQUATION_BG)) {
     534        matrix->data.F64[bgIndex][normIndex] = sumI1;
     535        matrix->data.F64[normIndex][bgIndex] = sumI1;
     536    }
     537
     538    // check for any NAN values in the result, skip if found:
     539    for (int iy = 0; iy < matrix->numRows; iy++) {
     540        for (int ix = 0; ix < matrix->numCols; ix++) {
     541            if (!isfinite(matrix->data.F64[iy][ix])) {
     542                fprintf (stderr, "WARNING: NAN in matrix\n");
     543                return false;
     544            }
     545        }
     546    }
     547    for (int ix = 0; ix < vector->n; ix++) {
     548        if (!isfinite(vector->data.F64[ix])) {
     549            fprintf (stderr, "WARNING: NAN in vector\n");
    242550            return false;
    243551        }
    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     }
     552    }
     553
    257554
    258555    return true;
    259556}
    260557
    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 
    409 
     558#if 1
    410559// 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
     560bool calculatePenalty(psImage *matrix,                     // Matrix to which to add in penalty term
     561                             psVector *vector,                    // Vector to which to add in penalty term
     562                             const pmSubtractionKernels *kernels, // Kernel parameters
     563                             float norm                           // Normalisation
    413564    )
    414565{
    … …  
    420571    int spatialOrder = kernels->spatialOrder; // Order of spatial variations
    421572    int numKernels = kernels->num; // Number of kernel components
     573    int numSpatial = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of spatial variations
     574    int numParams = numKernels * numSpatial;                 // Number of kernel parameters
     575
     576    // order is :
     577    // [p_0,x_0,y_0 p_1,x_0,y_0, p_2,x_0,y_0]
     578    // [p_0,x_1,y_0 p_1,x_1,y_0, p_2,x_1,y_0]
     579    // [p_0,x_0,y_1 p_1,x_0,y_1, p_2,x_0,y_1]
     580    // [norm]
     581    // [bg]
     582    // [q_0,x_0,y_0 q_1,x_0,y_0, q_2,x_0,y_0]
     583    // [q_0,x_1,y_0 q_1,x_1,y_0, q_2,x_1,y_0]
     584    // [q_0,x_0,y_1 q_1,x_0,y_1, q_2,x_0,y_1]
     585
    422586    for (int i = 0; i < numKernels; i++) {
    423587        for (int yOrder = 0, index = i; yOrder <= spatialOrder; yOrder++) {
    424588            for (int xOrder = 0; xOrder <= spatialOrder - yOrder; xOrder++, index += numKernels) {
    425                 vector->data.F64[index] -= penalties->data.F32[i];
     589                // Contribution to chi^2: a_i^2 P_i
     590                psAssert(isfinite(penalties->data.F32[i]), "Invalid penalty");
     591                matrix->data.F64[index][index] += norm * penalties->data.F32[i];
     592                if (kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
     593                    matrix->data.F64[index + numParams + 2][index + numParams + 2] += norm * penalties->data.F32[i];
     594                    // matrix[i][i] is ~ (k_i * I_1)(k_i * I_1)
     595                    // penalties scale with second moments
     596                    //
     597                }
    426598            }
    427599        }
    … …  
    430602    return true;
    431603}
     604# endif
    432605
    433606//////////////////////////////////////////////////////////////////////////////////////////////////////////////
    … …  
    439612// Calculate the value of a polynomial, specified by coefficients and polynomial values
    440613double p_pmSubtractionCalculatePolynomial(const psVector *coeff, // Coefficients
    441                                                  const psImage *polyValues, // Polynomial values
    442                                                  int order, // Order of polynomials
    443                                                  int index, // Index at which to begin
    444                                                  int step // Step between subsequent indices
    445                                                  )
     614                                          const psImage *polyValues, // Polynomial values
     615                                          int order, // Order of polynomials
     616                                          int index, // Index at which to begin
     617                                          int step // Step between subsequent indices
     618                                          )
    446619{
    447620    double sum = 0.0;                   // Value of the polynomial sum
    … …  
    458631
    459632double p_pmSubtractionSolutionCoeff(const pmSubtractionKernels *kernels, const psImage *polyValues,
    460                                            int index, bool wantDual)
     633                                    int index, bool wantDual)
    461634{
    462635#if 0
    … …  
    511684    const pmSubtractionKernels *kernels = job->args->data[1]; // Kernels
    512685    int index = PS_SCALAR_VALUE(job->args->data[2], S32); // Stamp index
    513 
    514     return pmSubtractionCalculateEquationStamp(stamps, kernels, index);
     686    pmSubtractionEquationCalculationMode mode  = PS_SCALAR_VALUE(job->args->data[3], S32); // calculation model
     687
     688    return pmSubtractionCalculateEquationStamp(stamps, kernels, index, mode);
    515689}
    516690
    517691bool pmSubtractionCalculateEquationStamp(pmSubtractionStampList *stamps, const pmSubtractionKernels *kernels,
    518                                          int index)
     692                                         int index, const pmSubtractionEquationCalculationMode mode)
    519693{
    520694    PM_ASSERT_SUBTRACTION_STAMP_LIST_NON_NULL(stamps, false);
    … …  
    529703    int numBackground = PM_SUBTRACTION_POLYTERMS(kernels->bgOrder); // Number of background terms
    530704
     705    // numKernels is the number of unique kernel images (one for each Gaussian modified by a specific polynomial).
     706    // = \sum_i^N_Gaussians [(order + 1) * (order + 2) / 2], eg for 1 Gauss and 1st order, numKernels = 3
     707
    531708    // Total number of parameters to solve for: coefficient of each kernel basis function, multipled by the
    532709    // number of coefficients for the spatial polynomial, normalisation and a constant background offset.
    533710    int numParams = numKernels * numSpatial + 1 + numBackground;
     711    if (kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
     712        // An additional image is convolved
     713        numParams += numKernels * numSpatial;
     714    }
    534715
    535716    pmSubtractionStamp *stamp = stamps->stamps->data[index]; // Stamp of interest
    536717    psAssert(stamp->status == PM_SUBTRACTION_STAMP_CALCULATE, "We only operate on stamps with this state.");
    537718
    538     // Generate convolutions
     719    // Generate convolutions: these are generated once and saved
    539720    if (!pmSubtractionConvolveStamp(stamp, kernels, footprint)) {
    540         psError(PS_ERR_UNKNOWN, false, "Unable to convolve stamp %d.", index);
     721        psError(psErrorCodeLast(), false, "Unable to convolve stamp %d.", index);
    541722        return NULL;
    542723    }
    … …  
    566747#endif
    567748
     749    // XXX visualize the set of convolved stamps
     750
    568751    psImage *polyValues = p_pmSubtractionPolynomial(NULL, spatialOrder,
    569752                                                    stamp->xNorm, stamp->yNorm); // Polynomial terms
    570753
    571     bool new = stamp->vector1 ? false : true; // Is this a new run?
     754    bool new = stamp->vector ? false : true; // Is this a new run?
    572755    if (new) {
    573         stamp->matrix1 = psImageAlloc(numParams, numParams, PS_TYPE_F64);
    574         stamp->vector1 = psVectorAlloc(numParams, PS_TYPE_F64);
     756        stamp->matrix = psImageAlloc(numParams, numParams, PS_TYPE_F64);
     757        stamp->vector = psVectorAlloc(numParams, PS_TYPE_F64);
    575758    }
    576759#ifdef TESTING
    577     psImageInit(stamp->matrix1, NAN);
    578     psVectorInit(stamp->vector1, NAN);
     760    psImageInit(stamp->matrix, NAN);
     761    psVectorInit(stamp->vector, NAN);
    579762#endif
    580763
    581764    bool status;                    // Status of least-squares matrix/vector calculation
     765
     766    psKernel *weight = NULL;
     767    psKernel *window = NULL;
     768
     769#ifdef USE_WEIGHT
     770    weight = stamp->weight;
     771#endif
     772#ifdef USE_WINDOW
     773    window = stamps->window;
     774#endif
     775
    582776    switch (kernels->mode) {
    583777      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);
     778        status = calculateMatrixVector(stamp->matrix, stamp->vector, &stamp->norm, stamp->image2, stamp->image1,
     779                                       weight, window, stamp->convolutions1, kernels,
     780                                       polyValues, footprint, stamps->normWindow, mode);
    588781        break;
    589782      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);
     783        status = calculateMatrixVector(stamp->matrix, stamp->vector, &stamp->norm, stamp->image1, stamp->image2,
     784                                       weight, window, stamp->convolutions2, kernels,
     785                                       polyValues, footprint, stamps->normWindow, mode);
    594786        break;
    595787      case PM_SUBTRACTION_MODE_DUAL:
    596         if (new) {
    597             stamp->matrix2 = psImageAlloc(numKernels * numSpatial, numKernels * numSpatial, PS_TYPE_F64);
    598             stamp->matrixX = psImageAlloc(numParams, numKernels * numSpatial, PS_TYPE_F64);
    599             stamp->vector2 = psVectorAlloc(numKernels * numSpatial, PS_TYPE_F64);
    600         }
    601 #ifdef TESTING
    602         psImageInit(stamp->matrix2, NAN);
    603         psImageInit(stamp->matrixX, NAN);
    604         psVectorInit(stamp->vector2, NAN);
    605 #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);
     788        status = calculateDualMatrixVector(stamp->matrix, stamp->vector, &stamp->norm,
     789                                           stamp->image1, stamp->image2,
     790                                           weight, window, stamp->convolutions1, stamp->convolutions2,
     791                                           kernels, polyValues, footprint, stamps->normWindow, mode);
    617792        break;
    618793      default:
    … …  
    623798        stamp->status = PM_SUBTRACTION_STAMP_REJECTED;
    624799        psWarning("Rejecting stamp %d (%d,%d) because of bad equation",
    625                   index, (int)(stamp->x + 0.5), (int)(stamp->y + 0.5));
     800                  index, (int)(stamp->x - 0.5), (int)(stamp->y - 0.5));
    626801    } else {
    627802        stamp->status = PM_SUBTRACTION_STAMP_USED;
    … …  
    629804
    630805#ifdef TESTING
    631     if (psTraceGetLevel("psModules.imcombine.equation") >= 10) {
     806    {
    632807        psString matrixName = NULL;
    633         psStringAppend(&matrixName, "matrix1_%d.fits", index);
     808        psStringAppend(&matrixName, "matrix_%d.fits", index);
    634809        psFits *matrixFile = psFitsOpen(matrixName, "w");
    635810        psFree(matrixName);
    636         psFitsWriteImage(matrixFile, NULL, stamp->matrix1, 0, NULL);
     811        psFitsWriteImage(matrixFile, NULL, stamp->matrix, 0, NULL);
    637812        psFitsClose(matrixFile);
    638813
    639814        matrixName = NULL;
    640         psStringAppend(&matrixName, "vector1_%d.fits", index);
    641         psImage *dummy = psImageAlloc(stamp->vector1->n, 1, PS_TYPE_F64);
    642         memcpy(dummy->data.F64[0], stamp->vector1->data.F64,
    643                PSELEMTYPE_SIZEOF(PS_TYPE_F64) * stamp->vector1->n);
     815        psStringAppend(&matrixName, "vector_%d.fits", index);
     816        psImage *dummy = psImageAlloc(stamp->vector->n, 1, PS_TYPE_F64);
     817        memcpy(dummy->data.F64[0], stamp->vector->data.F64,
     818               PSELEMTYPE_SIZEOF(PS_TYPE_F64) * stamp->vector->n);
    644819        matrixFile = psFitsOpen(matrixName, "w");
    645820        psFree(matrixName);
    … …  
    647822        psFree(dummy);
    648823        psFitsClose(matrixFile);
    649 
    650         if (stamp->vector2) {
    651             matrixName = NULL;
    652             psStringAppend(&matrixName, "vector2_%d.fits", index);
    653             dummy = psImageAlloc(stamp->vector2->n, 1, PS_TYPE_F64);
    654             memcpy(dummy->data.F64[0], stamp->vector2->data.F64,
    655                    PSELEMTYPE_SIZEOF(PS_TYPE_F64) * stamp->vector2->n);
    656             matrixFile = psFitsOpen(matrixName, "w");
    657             psFree(matrixName);
    658             psFitsWriteImage(matrixFile, NULL, dummy, 0, NULL);
    659             psFree(dummy);
    660             psFitsClose(matrixFile);
    661         }
    662 
    663         if (kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
    664             matrixName = NULL;
    665             psStringAppend(&matrixName, "matrix2_%d.fits", index);
    666             matrixFile = psFitsOpen(matrixName, "w");
    667             psFree(matrixName);
    668             psFitsWriteImage(matrixFile, NULL, stamp->matrix2, 0, NULL);
    669             psFitsClose(matrixFile);
    670 
    671             matrixName = NULL;
    672             psStringAppend(&matrixName, "matrixX_%d.fits", index);
    673             matrixFile = psFitsOpen(matrixName, "w");
    674             psFree(matrixName);
    675             psFitsWriteImage(matrixFile, NULL, stamp->matrixX, 0, NULL);
    676             psFitsClose(matrixFile);
    677         }
    678824    }
    679825#endif
    … …  
    684830}
    685831
    686 bool pmSubtractionCalculateEquation(pmSubtractionStampList *stamps, const pmSubtractionKernels *kernels)
     832bool pmSubtractionCalculateEquation(pmSubtractionStampList *stamps, const pmSubtractionKernels *kernels,
     833                                    const pmSubtractionEquationCalculationMode mode)
    687834{
    688835    PM_ASSERT_SUBTRACTION_STAMP_LIST_NON_NULL(stamps, false);
    … …  
    699846        }
    700847
     848        if ((stamp->x <= 0.0) && (stamp->y <= 0.0)) {
     849            psAbort ("bad stamp");
     850        }
     851        if (!isfinite(stamp->x) && !isfinite(stamp->y)) {
     852            psAbort ("bad stamp");
     853        }
     854
    701855        if (pmSubtractionThreaded()) {
    702856            psThreadJob *job = psThreadJobAlloc("PSMODULES_SUBTRACTION_CALCULATE_EQUATION");
    … …  
    704858            psArrayAdd(job->args, 1, (pmSubtractionKernels*)kernels); // Casting away const to put on array
    705859            PS_ARRAY_ADD_SCALAR(job->args, i, PS_TYPE_S32);
     860            PS_ARRAY_ADD_SCALAR(job->args, mode, PS_TYPE_S32);
    706861            if (!psThreadJobAddPending(job)) {
    707862                psFree(job);
    … …  
    710865            psFree(job);
    711866        } else {
    712             pmSubtractionCalculateEquationStamp(stamps, kernels, i);
     867            pmSubtractionCalculateEquationStamp(stamps, kernels, i, mode);
    713868        }
    714869    }
    715870
    716871    if (!psThreadPoolWait(true)) {
    717         psError(PS_ERR_UNKNOWN, false, "Error waiting for threads.");
     872        psError(psErrorCodeLast(), false, "Error waiting for threads.");
    718873        return false;
    719874    }
    720875
    721876    pmSubtractionVisualPlotLeastSquares(stamps);
     877    pmSubtractionVisualShowKernels((pmSubtractionKernels  *)kernels);
     878    pmSubtractionVisualShowBasis(stamps);
    722879
    723880    psLogMsg("psModules.imcombine", PS_LOG_INFO, "Calculate equation: %f sec",
    … …  
    728885}
    729886
    730 bool pmSubtractionSolveEquation(pmSubtractionKernels *kernels, const pmSubtractionStampList *stamps)
     887// private functions used on pmSubtractionSolveEquation
     888bool psVectorWriteFile (char *filename, const psVector *vector);
     889bool psFitsWriteImageSimple (char *filename, psImage *image, psMetadata *header);
     890
     891psImage *p_pmSubSolve_wUt (psVector *w, psImage *U);
     892psImage *p_pmSubSolve_VwUt (psImage *V, psImage *wUt);
     893
     894bool p_pmSubSolve_SetWeights (psVector *wApply, psVector *w, psVector *wMask);
     895
     896bool p_pmSubSolve_UtB (psVector **UtB, psImage *U, psVector *B);
     897bool p_pmSubSolve_wUtB (psVector **wUtB, psVector *w, psVector *UtB);
     898bool p_pmSubSolve_VwUtB (psVector **VwUtB, psImage *V, psVector *wUtB);
     899
     900bool p_pmSubSolve_Ax (psVector **B, psImage *A, psVector *x);
     901bool p_pmSubSolve_VdV (double *value, psVector *x, psVector *y);
     902bool p_pmSubSolve_y2 (double *y2, pmSubtractionKernels *kernels, const pmSubtractionStampList *stamps);
     903
     904psImage *p_pmSubSolve_Xvar (psImage *V, psVector *w);
     905
     906double p_pmSubSolve_ChiSquare (pmSubtractionKernels *kernels, const pmSubtractionStampList *stamps);
     907
     908bool pmSubtractionSolveEquation(pmSubtractionKernels *kernels,
     909                                const pmSubtractionStampList *stamps,
     910                                const pmSubtractionEquationCalculationMode mode)
    731911{
    732912    PM_ASSERT_SUBTRACTION_KERNELS_NON_NULL(kernels, false);
    … …  
    734914
    735915    // Check inputs
    736     int numParams = -1;                // Number of parameters
    737     int numParams2 = 0;                // Number of parameters for part solution (DUAL mode)
     916    int numKernels = kernels->num;      // Number of kernel basis functions
     917    int numSpatial = PM_SUBTRACTION_POLYTERMS(kernels->spatialOrder); // Number of spatial variations
     918    int numBackground = PM_SUBTRACTION_POLYTERMS(kernels->bgOrder); // Number of background terms
     919    int numParams = numKernels * numSpatial + 1 + numBackground;    // Number of parameters being solved for
     920    int numSolution1 = numParams, numSolution2 = 0;                 // Number of parameters for each solution
     921    if (kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
     922        // An additional image is convolved
     923        numSolution2 = numKernels * numSpatial;
     924        numParams += numSolution2;
     925    }
     926
    738927    for (int i = 0; i < stamps->num; i++) {
    739928        pmSubtractionStamp *stamp = stamps->stamps->data[i]; // Stamp of interest
    … …  
    743932        }
    744933
    745         PS_ASSERT_VECTOR_NON_NULL(stamp->vector1, false);
    746         if (numParams == -1) {
    747             numParams = stamp->vector1->n;
    748         }
    749         PS_ASSERT_VECTOR_SIZE(stamp->vector1, (long)numParams, false);
    750         PS_ASSERT_VECTOR_TYPE(stamp->vector1, PS_TYPE_F64, false);
    751         PS_ASSERT_IMAGE_NON_NULL(stamp->matrix1, false);
    752         PS_ASSERT_IMAGE_SIZE(stamp->matrix1, numParams, numParams, false);
    753         PS_ASSERT_IMAGE_TYPE(stamp->matrix1, PS_TYPE_F64, false);
    754 
    755         if (kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
    756             PS_ASSERT_IMAGE_NON_NULL(stamp->matrix2, false);
    757             PS_ASSERT_IMAGE_NON_NULL(stamp->matrixX, false);
    758             if (numParams2 == 0) {
    759                 numParams2 = stamp->matrix2->numCols;
    760             }
    761             PS_ASSERT_IMAGE_SIZE(stamp->matrix2, numParams2, numParams2, false);
    762             PS_ASSERT_IMAGE_SIZE(stamp->matrixX, numParams, numParams2, false);
    763             PS_ASSERT_IMAGE_TYPE(stamp->matrix2, PS_TYPE_F64, false);
    764             PS_ASSERT_IMAGE_TYPE(stamp->matrixX, PS_TYPE_F64, false);
    765             PS_ASSERT_VECTOR_NON_NULL(stamp->vector2, false);
    766             PS_ASSERT_VECTOR_SIZE(stamp->vector2, (long)numParams2, false);
    767             PS_ASSERT_VECTOR_TYPE(stamp->vector2, PS_TYPE_F64, false);
    768         }
    769     }
    770     if (numParams == -1) {
    771         psError(PS_ERR_BAD_PARAMETER_VALUE, true, "No suitable stamps found.");
    772         return NULL;
     934        PS_ASSERT_VECTOR_NON_NULL(stamp->vector, false);
     935        PS_ASSERT_VECTOR_SIZE(stamp->vector, (long)numParams, false);
     936        PS_ASSERT_VECTOR_TYPE(stamp->vector, PS_TYPE_F64, false);
     937        PS_ASSERT_IMAGE_NON_NULL(stamp->matrix, false);
     938        PS_ASSERT_IMAGE_SIZE(stamp->matrix, numParams, numParams, false);
     939        PS_ASSERT_IMAGE_TYPE(stamp->matrix, PS_TYPE_F64, false);
    773940    }
    774941
    … …  
    786953        psVectorInit(sumVector, 0.0);
    787954        psImageInit(sumMatrix, 0.0);
     955
     956        psVector *norms = psVectorAllocEmpty(stamps->num, PS_TYPE_F64); // Normalisations
     957
    788958        int numStamps = 0;              // Number of good stamps
    789959        for (int i = 0; i < stamps->num; i++) {
    790960            pmSubtractionStamp *stamp = stamps->stamps->data[i]; // Stamp of interest
    791 
    792961            if (stamp->status == PM_SUBTRACTION_STAMP_USED) {
    793 
    794 #ifdef TESTING
    795               // XXX double-check for NAN in data:
    796                 for (int iy = 0; iy < stamp->matrix1->numRows; iy++) {
    797                     for (int ix = 0; ix < stamp->matrix1->numCols; ix++) {
    798                         if (!isfinite(stamp->matrix1->data.F64[iy][ix])) {
    799                             fprintf (stderr, "WARNING: NAN in matrix1\n");
    800                         }
    801                     }
    802                 }
    803                 for (int ix = 0; ix < stamp->vector1->n; ix++) {
    804                     if (!isfinite(stamp->vector1->data.F64[ix])) {
    805                         fprintf (stderr, "WARNING: NAN in vector1\n");
    806                     }
    807                 }
    808 #endif
    809 
    810                 (void)psBinaryOp(sumMatrix, sumMatrix, "+", stamp->matrix1);
    811                 (void)psBinaryOp(sumVector, sumVector, "+", stamp->vector1);
     962                (void)psBinaryOp(sumMatrix, sumMatrix, "+", stamp->matrix);
     963                (void)psBinaryOp(sumVector, sumVector, "+", stamp->vector);
     964                psVectorAppend(norms, stamp->norm);
    812965                pmSubtractionStampPrint(ds9, stamp->x, stamp->y, stamps->footprint, "green");
    813966                numStamps++;
    … …  
    817970        }
    818971
     972#if 0
     973        int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background
     974        calculatePenalty(sumMatrix, sumVector, kernels, sumMatrix->data.F64[bgIndex][bgIndex]);
     975#endif
     976
     977        psVector *solution = NULL;                       // Solution to equation!
     978        solution = psVectorAlloc(numParams, PS_TYPE_F64);
     979        psVectorInit(solution, 0);
     980
     981#if 0
     982        // Regular, straight-forward solution
     983        solution = psMatrixSolveSVD(solution, sumMatrix, sumVector, NAN);
     984#else
     985        {
     986            // Solve normalisation and background separately
     987            int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
     988            int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background
     989
     990            psStats *stats = psStatsAlloc(PS_STAT_ROBUST_MEDIAN); // Statistics for norm
     991            if (!psVectorStats(stats, norms, NULL, NULL, 0)) {
     992                psError(PM_ERR_DATA, false, "Unable to determine median normalisation");
     993                psFree(stats);
     994                psFree(sumMatrix);
     995                psFree(sumVector);
     996                psFree(norms);
     997                return false;
     998            }
     999
     1000            double normValue = stats->robustMedian;
     1001            // double bgValue = 0.0;
     1002
     1003            psFree(stats);
     1004
    8191005#ifdef TESTING
    820         for (int ix = 0; ix < sumVector->n; ix++) {
    821             if (!isfinite(sumVector->data.F64[ix])) {
    822                 fprintf (stderr, "WARNING: NAN in vector1\n");
    823             }
    824         }
    825 #endif
    826 
    827         calculatePenalty(sumVector, kernels);
    828 
    829 #ifdef TESTING
    830         for (int ix = 0; ix < sumVector->n; ix++) {
    831             if (!isfinite(sumVector->data.F64[ix])) {
    832                 fprintf (stderr, "WARNING: NAN in vector1\n");
    833             }
    834         }
    835         {
    836             psImage *inverse = psMatrixInvert(NULL, sumMatrix, NULL);
    837             psFits *fits = psFitsOpen("matrixInv.fits", "w");
    838             psFitsWriteImage(fits, NULL, inverse, 0, NULL);
    839             psFitsClose(fits);
    840             psFree(inverse);
    841         }
    842         {
    843             psImage *X = psMatrixInvert(NULL, sumMatrix, NULL);
    844             psImage *Xt = psMatrixTranspose(NULL, X);
    845             psImage *XtX = psMatrixMultiply(NULL, Xt, X);
    846             psFits *fits = psFitsOpen("matrixErr.fits", "w");
    847             psFitsWriteImage(fits, NULL, XtX, 0, NULL);
    848             psFitsClose(fits);
    849             psFree(X);
    850             psFree(Xt);
    851             psFree(XtX);
    852         }
    853 #endif
    854 
    855         psVector *permutation = NULL;       // Permutation vector, required for LU decomposition
    856         psImage *luMatrix = psMatrixLUDecomposition(NULL, &permutation, sumMatrix);
     1006            fprintf(stderr, "Norm: %lf\n", normValue);
     1007#endif
     1008            // Solve kernel components
     1009            for (int i = 0; i < numSolution1; i++) {
     1010                sumVector->data.F64[i] -= normValue * sumMatrix->data.F64[normIndex][i];
     1011
     1012                sumMatrix->data.F64[i][normIndex] = 0.0;
     1013                sumMatrix->data.F64[normIndex][i] = 0.0;
     1014            }
     1015            sumVector->data.F64[bgIndex] -= normValue * sumMatrix->data.F64[normIndex][bgIndex];
     1016            sumMatrix->data.F64[bgIndex][normIndex] = 0.0;
     1017            sumMatrix->data.F64[normIndex][bgIndex] = 0.0;
     1018
     1019            sumMatrix->data.F64[normIndex][normIndex] = 1.0;
     1020            sumVector->data.F64[normIndex] = 0.0;
     1021
     1022            solution = psMatrixSolveSVD(solution, sumMatrix, sumVector, NAN);
     1023
     1024            solution->data.F64[normIndex] = normValue;
     1025        }
     1026# endif
     1027
     1028        if (!kernels->solution1) {
     1029            kernels->solution1 = psVectorAlloc(sumVector->n, PS_TYPE_F64);
     1030            psVectorInit(kernels->solution1, 0.0);
     1031        }
     1032
     1033        // only update the solutions that we chose to calculate:
     1034        if (mode & PM_SUBTRACTION_EQUATION_NORM) {
     1035            int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
     1036            kernels->solution1->data.F64[normIndex] = solution->data.F64[normIndex];
     1037        }
     1038        if (mode & PM_SUBTRACTION_EQUATION_BG) {
     1039            int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
     1040            kernels->solution1->data.F64[bgIndex] = solution->data.F64[bgIndex];
     1041        }
     1042        if (mode & PM_SUBTRACTION_EQUATION_KERNELS) {
     1043            int numKernels = kernels->num;
     1044            int spatialOrder = kernels->spatialOrder;       // Order of spatial variation
     1045            int numPoly = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of polynomial terms
     1046            for (int i = 0; i < numKernels * numPoly; i++) {
     1047                kernels->solution1->data.F64[i] = solution->data.F64[i];
     1048            }
     1049        }
     1050
     1051        psFree(norms);
     1052        psFree(solution);
     1053        psFree(sumVector);
    8571054        psFree(sumMatrix);
    858         if (!luMatrix) {
    859             psError(PS_ERR_UNKNOWN, true, "LU Decomposition of least-squares matrix failed.\n");
    860             psFree(sumVector);
    861             psFree(luMatrix);
    862             psFree(permutation);
    863             return NULL;
    864         }
    865         kernels->solution1 = psMatrixLUSolution(kernels->solution1, luMatrix, sumVector, permutation);
    8661055
    8671056#ifdef TESTING
    … …  
    8691058        for (int ix = 0; ix < kernels->solution1->n; ix++) {
    8701059            if (!isfinite(kernels->solution1->data.F64[ix])) {
    871                 fprintf (stderr, "WARNING: NAN in vector1\n");
    872             }
    873         }
    874 #endif
    875 
    876         psFree(sumVector);
    877         psFree(luMatrix);
    878         psFree(permutation);
    879         if (!kernels->solution1) {
    880             psError(PS_ERR_UNKNOWN, true, "Failed to solve the least-squares system.\n");
    881             return NULL;
    882         }
     1060                fprintf (stderr, "WARNING: NAN in vector\n");
     1061            }
     1062        }
     1063#endif
     1064
    8831065    } else {
    8841066        // Dual convolution solution
    8851067
    8861068        // Accumulation of stamp matrices/vectors
    887         psImage *sumMatrix1 = psImageAlloc(numParams, numParams, PS_TYPE_F64);
    888         psImage *sumMatrix2 = psImageAlloc(numParams2, numParams2, PS_TYPE_F64);
    889         psImage *sumMatrixX = psImageAlloc(numParams, numParams2, PS_TYPE_F64);
    890         psVector *sumVector1 = psVectorAlloc(numParams, PS_TYPE_F64);
    891         psVector *sumVector2 = psVectorAlloc(numParams, PS_TYPE_F64);
    892         psImageInit(sumMatrix1, 0.0);
    893         psImageInit(sumMatrix2, 0.0);
    894         psImageInit(sumMatrixX, 0.0);
    895         psVectorInit(sumVector1, 0.0);
    896         psVectorInit(sumVector2, 0.0);
     1069        psImage *sumMatrix = psImageAlloc(numParams, numParams, PS_TYPE_F64);
     1070        psVector *sumVector = psVectorAlloc(numParams, PS_TYPE_F64);
     1071        psImageInit(sumMatrix, 0.0);
     1072        psVectorInit(sumVector, 0.0);
     1073
     1074        psVector *norms = psVectorAllocEmpty(stamps->num, PS_TYPE_F64); // Normalisations
    8971075
    8981076        int numStamps = 0;              // Number of good stamps
    … …  
    9001078            pmSubtractionStamp *stamp = stamps->stamps->data[i]; // Stamp of interest
    9011079            if (stamp->status == PM_SUBTRACTION_STAMP_USED) {
    902                 (void)psBinaryOp(sumMatrix1, sumMatrix1, "+", stamp->matrix1);
    903                 (void)psBinaryOp(sumMatrix2, sumMatrix2, "+", stamp->matrix2);
    904                 (void)psBinaryOp(sumMatrixX, sumMatrixX, "+", stamp->matrixX);
    905                 (void)psBinaryOp(sumVector1, sumVector1, "+", stamp->vector1);
    906                 (void)psBinaryOp(sumVector2, sumVector2, "+", stamp->vector2);
     1080                (void)psBinaryOp(sumMatrix, sumMatrix, "+", stamp->matrix);
     1081                (void)psBinaryOp(sumVector, sumVector, "+", stamp->vector);
     1082
     1083                psVectorAppend(norms, stamp->norm);
     1084
    9071085                pmSubtractionStampPrint(ds9, stamp->x, stamp->y, stamps->footprint, "green");
    9081086                numStamps++;
    9091087            }
    9101088        }
    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);
     1089
    9231090#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);
     1091        psFitsWriteImageSimple ("sumMatrix.fits", sumMatrix, NULL);
     1092        psVectorWriteFile("sumVector.dat", sumVector);
     1093#endif
     1094
     1095#if 1
     1096        // int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background
     1097        // calculatePenalty(sumMatrix, sumVector, kernels, sumMatrix->data.F64[bgIndex][bgIndex]);
     1098
     1099        int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
     1100        calculatePenalty(sumMatrix, sumVector, kernels, sumMatrix->data.F64[normIndex][normIndex] / 1000.0);
     1101#endif
     1102
     1103        psVector *solution = NULL;                       // Solution to equation!
     1104        solution = psVectorAlloc(numParams, PS_TYPE_F64);
     1105        psVectorInit(solution, 0);
     1106
     1107#if 0
     1108        // Regular, straight-forward solution
     1109        solution = psMatrixSolveSVD(solution, sumMatrix, sumVector, NAN);
     1110#else
     1111        {
     1112            // Solve normalisation and background separately
     1113            int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
     1114            int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background
     1115
     1116#if 0
     1117            psImage *normMatrix = psImageAlloc(2, 2, PS_TYPE_F64);
     1118            psVector *normVector = psVectorAlloc(2, PS_TYPE_F64);
     1119
     1120            normMatrix->data.F64[0][0] = sumMatrix->data.F64[normIndex][normIndex];
     1121            normMatrix->data.F64[1][1] = sumMatrix->data.F64[bgIndex][bgIndex];
     1122            normMatrix->data.F64[0][1] = normMatrix->data.F64[1][0] = sumMatrix->data.F64[normIndex][bgIndex];
     1123
     1124            normVector->data.F64[0] = sumVector->data.F64[normIndex];
     1125            normVector->data.F64[1] = sumVector->data.F64[bgIndex];
     1126
     1127            psVector *normSolution = psMatrixSolveSVD(NULL, normMatrix, normVector, NAN);
     1128
     1129            double normValue = normSolution->data.F64[0];
     1130            double bgValue = normSolution->data.F64[1];
     1131
     1132            psFree(normMatrix);
     1133            psFree(normVector);
     1134            psFree(normSolution);
     1135#endif
     1136
     1137            psStats *stats = psStatsAlloc(PS_STAT_ROBUST_MEDIAN); // Statistics for norm
     1138            if (!psVectorStats(stats, norms, NULL, NULL, 0)) {
     1139                psError(PM_ERR_DATA, false, "Unable to determine median normalisation");
     1140                psFree(stats);
     1141                psFree(sumMatrix);
     1142                psFree(sumVector);
     1143                psFree(norms);
     1144                return false;
     1145            }
     1146
     1147            double normValue = stats->robustMedian;
     1148
     1149            psFree(stats);
     1150
    9621151#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);
     1152            fprintf(stderr, "Norm: %lf\n", normValue);
     1153#endif
     1154
     1155            // Solve kernel components
     1156            for (int i = 0; i < numSolution2; i++) {
     1157                sumVector->data.F64[i] -= normValue * sumMatrix->data.F64[normIndex][i];
     1158                sumVector->data.F64[i + numSolution1] -= normValue * sumMatrix->data.F64[normIndex][i + numSolution1];
     1159
     1160                sumMatrix->data.F64[i][normIndex] = 0.0;
     1161                sumMatrix->data.F64[normIndex][i] = 0.0;
     1162
     1163                sumMatrix->data.F64[i + numSolution1][normIndex] = 0.0;
     1164                sumMatrix->data.F64[normIndex][i + numSolution1] = 0.0;
     1165            }
     1166            sumVector->data.F64[bgIndex] -= normValue * sumMatrix->data.F64[normIndex][bgIndex];
     1167            sumMatrix->data.F64[bgIndex][normIndex] = 0.0;
     1168            sumMatrix->data.F64[normIndex][bgIndex] = 0.0;
     1169
     1170            sumMatrix->data.F64[normIndex][normIndex] = 1.0;
     1171
     1172            sumVector->data.F64[normIndex] = 0.0;
     1173
     1174            solution = psMatrixSolveSVD(solution, sumMatrix, sumVector, NAN);
     1175
     1176            solution->data.F64[normIndex] = normValue;
     1177        }
     1178#endif
     1179
     1180
    9871181#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
    1039 
    1040 #ifdef TESTING
    1041         {
    1042             psFits *fits = psFitsOpen("sumMatrix1.fits", "w");
    1043             psFitsWriteImage(fits, NULL, sumMatrix1, 0, NULL);
    1044             psFitsClose(fits);
    1045         }
    1046         {
    1047             psFits *fits = psFitsOpen("sumMatrix2.fits", "w");
    1048             psFitsWriteImage(fits, NULL, sumMatrix2, 0, NULL);
    1049             psFitsClose(fits);
    1050         }
    1051         {
    1052             psFits *fits = psFitsOpen("sumMatrixX.fits", "w");
    1053             psFitsWriteImage(fits, NULL, sumMatrixX, 0, NULL);
    1054             psFitsClose(fits);
    1055         }
    1056         {
    1057             psFits *fits = psFitsOpen("sumFinverse.fits", "w");
    1058             psFitsWriteImage(fits, NULL, Fi, 0, NULL);
    1059             psFitsClose(fits);
    1060         }
    1061 #endif
    1062 
    1063         kernels->solution1 = a;
    1064         kernels->solution2 = b;
    1065 
    1066         // XXXXX Free temporary matrices and vectors
     1182        for (int i = 0; i < solution->n; i++) {
     1183            fprintf(stderr, "Dual solution %d: %lf\n", i, solution->data.F64[i]);
     1184        }
     1185#endif
     1186
     1187        psFree(sumMatrix);
     1188        psFree(sumVector);
     1189
     1190        psFree(norms);
     1191
     1192        if (!kernels->solution1) {
     1193            kernels->solution1 = psVectorAlloc(numSolution1, PS_TYPE_F64);
     1194            psVectorInit (kernels->solution1, 0.0);
     1195        }
     1196        if (!kernels->solution2) {
     1197            kernels->solution2 = psVectorAlloc(numSolution2, PS_TYPE_F64);
     1198            psVectorInit (kernels->solution2, 0.0);
     1199        }
     1200
     1201        // only update the solutions that we chose to calculate:
     1202        if (mode & PM_SUBTRACTION_EQUATION_NORM) {
     1203            int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
     1204            kernels->solution1->data.F64[normIndex] = solution->data.F64[normIndex];
     1205        }
     1206        if (mode & PM_SUBTRACTION_EQUATION_BG) {
     1207            int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
     1208            kernels->solution1->data.F64[bgIndex] = solution->data.F64[bgIndex];
     1209        }
     1210        if (mode & PM_SUBTRACTION_EQUATION_KERNELS) {
     1211            int numKernels = kernels->num;
     1212            for (int i = 0; i < numKernels * numSpatial; i++) {
     1213                // XXX fprintf (stderr, "keep\n");
     1214                kernels->solution1->data.F64[i] = solution->data.F64[i];
     1215                kernels->solution2->data.F64[i] = solution->data.F64[i + numSolution1];
     1216            }
     1217        }
     1218
     1219
     1220        memcpy(kernels->solution1->data.F64, solution->data.F64,
     1221               numSolution1 * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
     1222        memcpy(kernels->solution2->data.F64, &solution->data.F64[numSolution1],
     1223               numSolution2 * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
     1224
     1225        psFree(solution);
    10671226
    10681227    }
    … …  
    10831242     }
    10841243
    1085     pmSubtractionVisualPlotLeastSquares((pmSubtractionStampList *) stamps); //casting away const
     1244    // pmSubtractionVisualPlotLeastSquares((pmSubtractionStampList *) stamps); //casting away const
    10861245    return true;
    10871246}
    10881247
     1248bool pmSubtractionResidualStats(psVector *fSigRes, psVector *fMaxRes, psVector *fMinRes, psKernel *target, psKernel *source, psKernel *residual, double norm, int footprint) {
     1249
     1250    // XXX measure some useful stats on the residuals
     1251    float sum = 0.0;
     1252    float peak = 0.0;
     1253    for (int y = - footprint; y <= footprint; y++) {
     1254        for (int x = - footprint; x <= footprint; x++) {
     1255            sum += 0.5*(target->kernel[y][x] + source->kernel[y][x] * norm);
     1256            peak = PS_MAX(peak, 0.5*(target->kernel[y][x] + source->kernel[y][x] * norm));
     1257        }
     1258    }
     1259
     1260    // only count pixels with more than X% of the source flux
     1261    // calculate stdev(dflux)
     1262    float dflux1 = 0.0;
     1263    float dflux2 = 0.0;
     1264    int npix = 0;
     1265
     1266    float dmax = 0.0;
     1267    float dmin = 0.0;
     1268
     1269    for (int y = - footprint; y <= footprint; y++) {
     1270        for (int x = - footprint; x <= footprint; x++) {
     1271            float dflux = 0.5*(target->kernel[y][x] + source->kernel[y][x] * norm);
     1272            if (dflux < 0.02*sum) continue;
     1273            dflux1 += residual->kernel[y][x];
     1274            dflux2 += PS_SQR(residual->kernel[y][x]);
     1275            dmax = PS_MAX(residual->kernel[y][x], dmax);
     1276            dmin = PS_MIN(residual->kernel[y][x], dmin);
     1277            npix ++;
     1278        }
     1279    }
     1280    float sigma = sqrt(dflux2 / npix - PS_SQR(dflux1/npix));
     1281    if (!isfinite(sum))  return false;
     1282    if (!isfinite(dmax)) return false;
     1283    if (!isfinite(dmin)) return false;
     1284    if (!isfinite(peak)) return false;
     1285
     1286    // fprintf (stderr, "sum: %f, peak: %f, sigma: %f, fsigma: %f, fmax: %f, fmin: %f\n", sum, peak, sigma, sigma/sum, dmax/peak, dmin/peak);
     1287    psVectorAppend(fSigRes, sigma/sum);
     1288    psVectorAppend(fMaxRes, dmax/peak);
     1289    psVectorAppend(fMinRes, dmin/peak);
     1290    return true;
     1291}
     1292
    10891293psVector *pmSubtractionCalculateDeviations(pmSubtractionStampList *stamps,
    1090                                            const pmSubtractionKernels *kernels)
     1294                                           pmSubtractionKernels *kernels)
    10911295{
    10921296    PM_ASSERT_SUBTRACTION_STAMP_LIST_NON_NULL(stamps, NULL);
    … …  
    11031307    psKernel *residual = psKernelAlloc(-footprint, footprint, -footprint, footprint); // Residual image
    11041308
     1309    // set up holding images for the visualization
     1310    pmSubtractionVisualShowFitInit (stamps);
     1311
     1312    psVector *fSigRes = psVectorAllocEmpty(stamps->num, PS_TYPE_F32);
     1313    psVector *fMinRes = psVectorAllocEmpty(stamps->num, PS_TYPE_F32);
     1314    psVector *fMaxRes = psVectorAllocEmpty(stamps->num, PS_TYPE_F32);
     1315
     1316    // we want to save the residual images for the 9 brightest stamps.
     1317    // identify the 9 brightest stamps
     1318    psVector *keepStamps  = psVectorAlloc(stamps->num, PS_TYPE_S32);
     1319    psVectorInit (keepStamps, 0);
     1320    {
     1321        psVector *flux  = psVectorAlloc(stamps->num, PS_TYPE_F32);
     1322        psVectorInit (flux, 0.0);
     1323
     1324        for (int i = 0; i < stamps->num; i++) {
     1325            pmSubtractionStamp *stamp = stamps->stamps->data[i];
     1326            if (!isfinite(stamp->flux)) continue;
     1327            flux->data.F32[i] = stamp->flux;
     1328        }
     1329
     1330        psVector *index = psVectorSortIndex(NULL, flux);
     1331        for (int i = 0; (i < stamps->num) && (i < 9); i++) {
     1332            int n = stamps->num - i - 1;
     1333            keepStamps->data.S32[index->data.S32[n]] = 1;
     1334        }
     1335        psFree (flux);
     1336        psFree (index);
     1337
     1338        // this function is called multiple times in the iteration, but
     1339        // we only know after the interation is done if we will try again.
     1340        // therefore we must save the sample each time, and blow away the old one
     1341        // if it exists.
     1342        psFree (kernels->sampleStamps);
     1343        kernels->sampleStamps = psArrayAllocEmpty(9);
     1344    }
     1345
     1346    psString log = psStringCopy("Deviations:\n");               // Log message with deviations
    11051347    for (int i = 0; i < stamps->num; i++) {
    11061348        pmSubtractionStamp *stamp = stamps->stamps->data[i]; // The stamp of interest
    … …  
    11161358
    11171359        // Calculate residuals
    1118         psKernel *variance = stamp->variance; // Variance postage stamp
     1360        psKernel *weight = stamp->weight; // Weight postage stamp
    11191361        psImageInit(residual->image, 0.0);
    11201362        if (kernels->mode != PM_SUBTRACTION_MODE_DUAL) {
    … …  
    11331375                                                          false); // Kernel image
    11341376                if (!image) {
    1135                     psError(PS_ERR_UNKNOWN, false, "Unable to generate image of kernel.");
     1377                    psError(psErrorCodeLast(), false, "Unable to generate image of kernel.");
    11361378                    return false;
    11371379                }
    … …  
    11621404                for (int y = - footprint; y <= footprint; y++) {
    11631405                    for (int x = - footprint; x <= footprint; x++) {
    1164                         residual->kernel[y][x] -= convolution->kernel[y][x] * coefficient;
     1406                        residual->kernel[y][x] += convolution->kernel[y][x] * coefficient;
    11651407                    }
    11661408                }
    11671409            }
     1410
     1411            // XXX visualize the target, source, convolution and residual
     1412            pmSubtractionVisualShowFitAddStamp (target, source, residual, background, norm, i);
     1413
    11681414            for (int y = - footprint; y <= footprint; y++) {
    11691415                for (int x = - footprint; x <= footprint; x++) {
    1170                     residual->kernel[y][x] += target->kernel[y][x] - background - source->kernel[y][x] * norm;
    1171                 }
    1172             }
     1416                    residual->kernel[y][x] += background + source->kernel[y][x] * norm - target->kernel[y][x];
     1417                }
     1418            }
     1419
     1420            if (keepStamps->data.S32[i]) {
     1421                psImage *sample = psImageCopy(NULL, residual->image, PS_TYPE_F32);
     1422                psArrayAdd (kernels->sampleStamps, 9, sample);
     1423                psFree (sample);
     1424            }
     1425
     1426            pmSubtractionResidualStats(fSigRes, fMaxRes, fMinRes, target, source, residual, norm, footprint);
     1427
    11731428        } else {
    11741429            // Dual convolution
    … …  
    11861441                for (int y = - footprint; y <= footprint; y++) {
    11871442                    for (int x = - footprint; x <= footprint; x++) {
    1188                         residual->kernel[y][x] += conv2->kernel[y][x] * coeff2 - conv1->kernel[y][x] * coeff1;
     1443                        residual->kernel[y][x] += conv2->kernel[y][x] * coeff2 + conv1->kernel[y][x] * coeff1;
    11891444                    }
    11901445                }
    11911446            }
     1447
     1448            // XXX visualize the target, source, convolution and residual
     1449            pmSubtractionVisualShowFitAddStamp (image2, image1, residual, background, norm, i);
     1450
    11921451            for (int y = - footprint; y <= footprint; y++) {
    11931452                for (int x = - footprint; x <= footprint; x++) {
    1194                     residual->kernel[y][x] += image2->kernel[y][x] - background - image1->kernel[y][x] * norm;
    1195                 }
    1196             }
     1453                    residual->kernel[y][x] += background + image1->kernel[y][x] * norm - image2->kernel[y][x];
     1454                }
     1455            }
     1456            if (keepStamps->data.S32[i]) {
     1457                psImage *sample = psImageCopy(NULL, residual->image, PS_TYPE_F32);
     1458                psArrayAdd (kernels->sampleStamps, 9, sample);
     1459                psFree (sample);
     1460            }
     1461
     1462            pmSubtractionResidualStats(fSigRes, fMaxRes, fMinRes, image1, image2, residual, norm, footprint);
    11971463        }
    11981464
    … …  
    12001466        for (int y = - footprint; y <= footprint; y++) {
    12011467            for (int x = - footprint; x <= footprint; x++) {
    1202                 double dev = PS_SQR(residual->kernel[y][x]) / variance->kernel[y][x];
     1468                double dev = PS_SQR(residual->kernel[y][x]) * weight->kernel[y][x];
    12031469                deviation += dev;
    12041470#ifdef TESTING
    … …  
    12091475        deviations->data.F32[i] = devNorm * deviation;
    12101476        psTrace("psModules.imcombine", 5, "Deviation for stamp %d (%d,%d): %f\n",
    1211                 i, (int)(stamp->x + 0.5), (int)(stamp->y + 0.5), deviations->data.F32[i]);
     1477                i, (int)(stamp->x - 0.5), (int)(stamp->y - 0.5), deviations->data.F32[i]);
     1478        psStringAppend(&log, "Stamp %d (%d,%d): %f\n",
     1479                       i, (int)(stamp->x - 0.5), (int)(stamp->y - 0.5), deviations->data.F32[i]);
    12121480        if (!isfinite(deviations->data.F32[i])) {
    12131481            stamp->status = PM_SUBTRACTION_STAMP_REJECTED;
    12141482            psTrace("psModules.imcombine", 5,
    12151483                    "Rejecting stamp %d (%d,%d) because of non-finite deviation\n",
    1216                     i, (int)(stamp->x + 0.5), (int)(stamp->y + 0.5));
     1484                    i, (int)(stamp->x - 0.5), (int)(stamp->y - 0.5));
    12171485            continue;
    12181486        }
    … …  
    12431511            psFitsClose(fits);
    12441512        }
    1245         if (stamp->variance) {
     1513        if (stamp->weight) {
    12461514            psString filename = NULL;
    1247             psStringAppend(&filename, "stamp_variance_%03d.fits", i);
     1515            psStringAppend(&filename, "stamp_weight_%03d.fits", i);
    12481516            psFits *fits = psFitsOpen(filename, "w");
    12491517            psFree(filename);
    1250             psFitsWriteImage(fits, NULL, stamp->variance->image, 0, NULL);
     1518            psFitsWriteImage(fits, NULL, stamp->weight->image, 0, NULL);
    12511519            psFitsClose(fits);
    12521520        }
    … …  
    12541522
    12551523    }
     1524
     1525    psFree(keepStamps);
     1526
     1527    psLogMsg("psModules.imcombine", PS_LOG_DETAIL, "%s", log);
     1528    psFree(log);
     1529
     1530    // calculate and report the normalization and background for the image center
     1531    {
     1532        polyValues = p_pmSubtractionPolynomial(polyValues, kernels->spatialOrder, 0.0, 0.0);
     1533        double norm = p_pmSubtractionSolutionNorm(kernels); // Normalisation
     1534        double background = p_pmSubtractionSolutionBackground(kernels, polyValues);// Difference in background
     1535        psLogMsg("psModules.imcombine", PS_LOG_INFO, "normalization: %f, background: %f", norm, background);
     1536
     1537        pmSubtractionVisualShowFit(norm);
     1538        pmSubtractionVisualPlotFit(kernels);
     1539
     1540        psStats *stats = psStatsAlloc(PS_STAT_ROBUST_MEDIAN | PS_STAT_ROBUST_STDEV);
     1541        psVectorStats (stats, fSigRes, NULL, NULL, 0);
     1542        kernels->fSigResMean = stats->robustMedian;
     1543        kernels->fSigResStdev = stats->robustStdev;
     1544
     1545        psStatsInit (stats);
     1546        psVectorStats (stats, fMaxRes, NULL, NULL, 0);
     1547        kernels->fMaxResMean = stats->robustMedian;
     1548        kernels->fMaxResStdev = stats->robustStdev;
     1549
     1550        psStatsInit (stats);
     1551        psVectorStats (stats, fMinRes, NULL, NULL, 0);
     1552        kernels->fMinResMean = stats->robustMedian;
     1553        kernels->fMinResStdev = stats->robustStdev;
     1554
     1555        // XXX save these values somewhere
     1556        psLogMsg("psModules.imcombine", PS_LOG_INFO, "fSigma: %f +/- %f, fMaxRes: %f +/- %f, fMinRes: %f +/- %f",
     1557                 kernels->fSigResMean, kernels->fSigResStdev,
     1558                 kernels->fMaxResMean, kernels->fMaxResStdev,
     1559                 kernels->fMinResMean, kernels->fMinResStdev);
     1560
     1561        psFree (fSigRes);
     1562        psFree (fMaxRes);
     1563        psFree (fMinRes);
     1564        psFree (stats);
     1565    }
     1566
    12561567    psFree(residual);
    12571568    psFree(polyValues);
    12581569
     1570
    12591571    return deviations;
    12601572}
     1573
     1574// we are supplied U, not Ut; w represents a diagonal matrix (also, we apply 1/w instead of w)
     1575psImage *p_pmSubSolve_wUt (psVector *w, psImage *U) {
     1576
     1577    psAssert (w->n == U->numCols, "w and U dimensions do not match");
     1578
     1579    // wUt has dimensions transposed relative to Ut.
     1580    psImage *wUt = psImageAlloc (U->numRows, U->numCols, PS_TYPE_F64);
     1581    psImageInit (wUt, 0.0);
     1582
     1583    for (int i = 0; i < wUt->numCols; i++) {
     1584        for (int j = 0; j < wUt->numRows; j++) {
     1585            if (!isfinite(w->data.F64[j])) continue;
     1586            if (w->data.F64[j] == 0.0) continue;
     1587            wUt->data.F64[j][i] = U->data.F64[i][j] / w->data.F64[j];
     1588        }
     1589    }
     1590    return wUt;
     1591}
     1592
     1593// XXX this is just standard matrix multiplication: use psMatrixMultiply?
     1594psImage *p_pmSubSolve_VwUt (psImage *V, psImage *wUt) {
     1595
     1596    psAssert (V->numCols == wUt->numRows, "matrix dimensions do not match");
     1597
     1598    psImage *Ainv = psImageAlloc (wUt->numCols, V->numRows, PS_TYPE_F64);
     1599
     1600    for (int i = 0; i < Ainv->numCols; i++) {
     1601        for (int j = 0; j < Ainv->numRows; j++) {
     1602            double sum = 0.0;
     1603            for (int k = 0; k < V->numCols; k++) {
     1604                sum += V->data.F64[j][k] * wUt->data.F64[k][i];
     1605            }
     1606            Ainv->data.F64[j][i] = sum;
     1607        }
     1608    }
     1609    return Ainv;
     1610}
     1611
     1612// we are supplied U, not Ut
     1613bool p_pmSubSolve_UtB (psVector **UtB, psImage *U, psVector *B) {
     1614
     1615    psAssert (U->numRows == B->n, "U and B dimensions do not match");
     1616
     1617    UtB[0] = psVectorRecycle (UtB[0], U->numCols, PS_TYPE_F64);
     1618
     1619    for (int i = 0; i < U->numCols; i++) {
     1620        double sum = 0.0;
     1621        for (int j = 0; j < U->numRows; j++) {
     1622            sum += B->data.F64[j] * U->data.F64[j][i];
     1623        }
     1624        UtB[0]->data.F64[i] = sum;
     1625    }
     1626    return true;
     1627}
     1628
     1629// w is diagonal
     1630bool p_pmSubSolve_wUtB (psVector **wUtB, psVector *w, psVector *UtB) {
     1631
     1632    psAssert (w->n == UtB->n, "w and UtB dimensions do not match");
     1633
     1634    // wUt has dimensions transposed relative to Ut.
     1635    wUtB[0] = psVectorRecycle (wUtB[0], w->n, PS_TYPE_F64);
     1636    psVectorInit (wUtB[0], 0.0);
     1637
     1638    for (int i = 0; i < w->n; i++) {
     1639        if (!isfinite(w->data.F64[i])) continue;
     1640        if (w->data.F64[i] == 0.0) continue;
     1641        wUtB[0]->data.F64[i] = UtB->data.F64[i] / w->data.F64[i];
     1642    }
     1643    return true;
     1644}
     1645
     1646// this is basically matrix * vector
     1647bool p_pmSubSolve_VwUtB (psVector **VwUtB, psImage *V, psVector *wUtB) {
     1648
     1649    psAssert (V->numCols == wUtB->n, "V and wUtB dimensions do not match");
     1650
     1651    VwUtB[0] = psVectorRecycle (*VwUtB, V->numRows, PS_TYPE_F64);
     1652
     1653    for (int j = 0; j < V->numRows; j++) {
     1654        double sum = 0.0;
     1655        for (int i = 0; i < V->numCols; i++) {
     1656            sum += V->data.F64[j][i] * wUtB->data.F64[i];
     1657        }
     1658        VwUtB[0]->data.F64[j] = sum;
     1659    }
     1660    return true;
     1661}
     1662
     1663// this is basically matrix * vector
     1664bool p_pmSubSolve_Ax (psVector **B, psImage *A, psVector *x) {
     1665
     1666    psAssert (A->numCols == x->n, "A and x dimensions do not match");
     1667
     1668    B[0] = psVectorRecycle (*B, A->numRows, PS_TYPE_F64);
     1669
     1670    for (int j = 0; j < A->numRows; j++) {
     1671        double sum = 0.0;
     1672        for (int i = 0; i < A->numCols; i++) {
     1673            sum += A->data.F64[j][i] * x->data.F64[i];
     1674        }
     1675        B[0]->data.F64[j] = sum;
     1676    }
     1677    return true;
     1678}
     1679
     1680// this is basically Vector * vector
     1681bool p_pmSubSolve_VdV (double *value, psVector *x, psVector *y) {
     1682
     1683    psAssert (x->n == y->n, "x and y dimensions do not match");
     1684
     1685    double sum = 0.0;
     1686    for (int i = 0; i < x->n; i++) {
     1687        sum += x->data.F64[i] * y->data.F64[i];
     1688    }
     1689    *value = sum;
     1690    return true;
     1691}
     1692
     1693bool p_pmSubSolve_y2 (double *y2, pmSubtractionKernels *kernels, const pmSubtractionStampList *stamps) {
     1694
     1695    int footprint = stamps->footprint; // Half-size of stamps
     1696
     1697    double sum = 0.0;
     1698    for (int i = 0; i < stamps->num; i++) {
     1699
     1700        pmSubtractionStamp *stamp = stamps->stamps->data[i];
     1701        if (stamp->status != PM_SUBTRACTION_STAMP_USED) continue;
     1702
     1703        psKernel *weight = NULL;
     1704        psKernel *window = NULL;
     1705        psKernel *input = NULL;
     1706
     1707#ifdef USE_WEIGHT
     1708        weight = stamp->weight;
     1709#endif
     1710#ifdef USE_WINDOW
     1711        window = stamps->window;
     1712#endif
     1713
     1714        switch (kernels->mode) {
     1715            // MODE_1 : convolve image 1 to match image 2 (and vice versa)
     1716          case PM_SUBTRACTION_MODE_1:
     1717            input = stamp->image2;
     1718            break;
     1719          case PM_SUBTRACTION_MODE_2:
     1720            input = stamp->image1;
     1721            break;
     1722          default:
     1723            psAbort ("programming error");
     1724        }
     1725
     1726        for (int y = - footprint; y <= footprint; y++) {
     1727            for (int x = - footprint; x <= footprint; x++) {
     1728                double in = input->kernel[y][x];
     1729                double value = in*in;
     1730                if (weight) {
     1731                    float wtVal = weight->kernel[y][x];
     1732                    value *= wtVal;
     1733                }
     1734                if (window) {
     1735                    float  winVal = window->kernel[y][x];
     1736                    value *= winVal;
     1737                }
     1738                sum += value;
     1739            }
     1740        }
     1741    }
     1742    *y2 = sum;
     1743    return true;
     1744}
     1745
     1746double p_pmSubSolve_ChiSquare (pmSubtractionKernels *kernels, const pmSubtractionStampList *stamps) {
     1747
     1748    int footprint = stamps->footprint; // Half-size of stamps
     1749    int numKernels = kernels->num;      // Number of kernels
     1750
     1751    double sum = 0.0;
     1752
     1753    psKernel *residual = psKernelAlloc(-footprint, footprint, -footprint, footprint); // Residual image
     1754    psImageInit(residual->image, 0.0);
     1755
     1756    psImage *polyValues = NULL;         // Polynomial values
     1757
     1758    for (int i = 0; i < stamps->num; i++) {
     1759
     1760        pmSubtractionStamp *stamp = stamps->stamps->data[i];
     1761        if (stamp->status != PM_SUBTRACTION_STAMP_USED) continue;
     1762
     1763        psKernel *weight = NULL;
     1764        psKernel *window = NULL;
     1765        psKernel *target = NULL;
     1766        psKernel *source = NULL;
     1767
     1768        psArray *convolutions = NULL;
     1769
     1770#ifdef USE_WEIGHT
     1771        weight = stamp->weight;
     1772#endif
     1773#ifdef USE_WINDOW
     1774        window = stamps->window;
     1775#endif
     1776
     1777        switch (kernels->mode) {
     1778            // MODE_1 : convolve image 1 to match image 2 (and vice versa)
     1779          case PM_SUBTRACTION_MODE_1:
     1780            target = stamp->image2;
     1781            source = stamp->image1;
     1782            convolutions = stamp->convolutions1;
     1783            break;
     1784          case PM_SUBTRACTION_MODE_2:
     1785            target = stamp->image1;
     1786            source = stamp->image2;
     1787            convolutions = stamp->convolutions2;
     1788            break;
     1789          default:
     1790            psAbort ("programming error");
     1791        }
     1792
     1793        // Calculate coefficients of the kernel basis functions
     1794        polyValues = p_pmSubtractionPolynomial(polyValues, kernels->spatialOrder, stamp->xNorm, stamp->yNorm);
     1795        double norm = p_pmSubtractionSolutionNorm(kernels); // Normalisation
     1796        double background = p_pmSubtractionSolutionBackground(kernels, polyValues);// Difference in background
     1797
     1798        psImageInit(residual->image, 0.0);
     1799        for (int j = 0; j < numKernels; j++) {
     1800            psKernel *convolution = convolutions->data[j]; // Convolution
     1801            double coefficient = p_pmSubtractionSolutionCoeff(kernels, polyValues, j, false); // Coefficient
     1802            for (int y = - footprint; y <= footprint; y++) {
     1803                for (int x = - footprint; x <= footprint; x++) {
     1804                    residual->kernel[y][x] -= convolution->kernel[y][x] * coefficient;
     1805                }
     1806            }
     1807        }
     1808
     1809        for (int y = - footprint; y <= footprint; y++) {
     1810            for (int x = - footprint; x <= footprint; x++) {
     1811                double resid = target->kernel[y][x] - background - source->kernel[y][x] * norm + residual->kernel[y][x];
     1812                double value = PS_SQR(resid);
     1813                if (weight) {
     1814                    float wtVal = weight->kernel[y][x];
     1815                    value *= wtVal;
     1816                }
     1817                if (window) {
     1818                    float  winVal = window->kernel[y][x];
     1819                    value *= winVal;
     1820                }
     1821                sum += value;
     1822            }
     1823        }
     1824    }
     1825    psFree (polyValues);
     1826    psFree (residual);
     1827
     1828    return sum;
     1829}
     1830
     1831bool p_pmSubSolve_SetWeights (psVector *wApply, psVector *w, psVector *wMask) {
     1832
     1833    for (int i = 0; i < w->n; i++) {
     1834        wApply->data.F64[i] = wMask->data.U8[i] ? 0.0 : w->data.F64[i];
     1835    }
     1836    return true;
     1837}
     1838
     1839// we are supplied V and w; w represents a diagonal matrix (also, we apply 1/w instead of w)
     1840psImage *p_pmSubSolve_Xvar (psImage *V, psVector *w) {
     1841
     1842    psAssert (w->n == V->numCols, "w and U dimensions do not match");
     1843
     1844    psImage *Vn = psImageAlloc (V->numCols, V->numRows, PS_TYPE_F64);
     1845    psImageInit (Vn, 0.0);
     1846
     1847    // generate Vn = V * w^{-1}
     1848    for (int j = 0; j < Vn->numRows; j++) {
     1849        for (int i = 0; i < Vn->numCols; i++) {
     1850            if (!isfinite(w->data.F64[i])) continue;
     1851            if (w->data.F64[i] == 0.0) continue;
     1852            Vn->data.F64[j][i] = V->data.F64[j][i] / w->data.F64[i];
     1853        }
     1854    }
     1855
     1856    psImage *Xvar = psImageAlloc (V->numCols, V->numRows, PS_TYPE_F64);
     1857    psImageInit (Xvar, 0.0);
     1858
     1859    // generate Xvar = Vn * Vn^T
     1860    for (int j = 0; j < Vn->numRows; j++) {
     1861        for (int i = 0; i < Vn->numCols; i++) {
     1862            double sum = 0.0;
     1863            for (int k = 0; k < Vn->numCols; k++) {
     1864                sum += Vn->data.F64[k][i]*Vn->data.F64[k][j];
     1865            }
     1866            Xvar->data.F64[j][i] = sum;
     1867        }
     1868    }
     1869    return Xvar;
     1870}
     1871
     1872// I get confused by the index values between the image vs matrix usage:  In terms
     1873// of the elements of an image A(x,y) = A->data.F64[y][x] = A_x,y, a matrix
     1874// multiplication is: A_k,j * B_i,k = C_i,j
     1875
     1876
     1877bool psFitsWriteImageSimple (char *filename, psImage *image, psMetadata *header) {
     1878
     1879    psFits *fits = psFitsOpen(filename, "w");
     1880    psFitsWriteImage(fits, header, image, 0, NULL);
     1881    psFitsClose(fits);
     1882
     1883    return true;
     1884}
     1885
     1886bool psVectorWriteFile (char *filename, const psVector *vector) {
     1887
     1888    FILE *f = fopen (filename, "w");
     1889    int fd = fileno(f);
     1890    p_psVectorPrint (fd, vector, "unnamed");
     1891    fclose (f);
     1892
     1893    return true;
     1894}
     1895
     1896
     1897# if 0
     1898
     1899#ifdef TESTING
     1900        psFitsWriteImageSimple("A.fits", sumMatrix, NULL);
     1901        psVectorWriteFile ("B.dat", sumVector);
     1902#endif
     1903
     1904# define SVD_ANALYSIS 0
     1905# define COEFF_SIG 0.0
     1906# define SVD_TOL 0.0
     1907
     1908        // Use SVD to determine the kernel coeffs (and validate)
     1909        if (SVD_ANALYSIS) {
     1910
     1911            // We have sumVector and sumMatrix.  we are trying to solve the following equation:
     1912            // sumMatrix * x = sumVector.
     1913
     1914            // we can use any standard matrix inversion to solve this.  However, the basis
     1915            // functions in general have substantial correlation, so that the solution may be
     1916            // somewhat poorly determined or unstable.  If not numerically ill-conditioned, the
     1917            // system of equations may be statistically ill-conditioned.  Noise in the image
     1918            // will drive insignificant, but correlated, terms in the solution.  To avoid these
     1919            // problems, we can use SVD to identify numerically unconstrained values and to
     1920            // avoid statistically badly determined value.
     1921
     1922            // A = sumMatrix, B = sumVector
     1923            // SVD: A = U w V^T  -> A^{-1} = V (1/w) U^T
     1924            // x = V (1/w) (U^T B)
     1925            // \sigma_x = sqrt(diag(A^{-1}))
     1926            // solve for x and A^{-1} to get x & dx
     1927            // identify the elements of (1/w) that are nan (1/0.0) -> set to 0.0
     1928            // identify the elements of x that are insignificant (x / dx < 1.0? < 0.5?) -> set to 0.0
     1929
     1930            // If I use the SVD trick to re-condition the matrix, I need to break out the
     1931            // kernel and normalization terms from the background term.
     1932            // XXX is this true?  or was this due to an error in the analysis?
     1933
     1934            int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
     1935
     1936            // now pull out the kernel elements into their own square matrix
     1937            psImage  *kernelMatrix = psImageAlloc  (sumMatrix->numCols - 1, sumMatrix->numRows - 1, PS_TYPE_F64);
     1938            psVector *kernelVector = psVectorAlloc (sumMatrix->numCols - 1, PS_TYPE_F64);
     1939
     1940            for (int ix = 0, kx = 0; ix < sumMatrix->numCols; ix++) {
     1941                if (ix == bgIndex) continue;
     1942                for (int iy = 0, ky = 0; iy < sumMatrix->numRows; iy++) {
     1943                    if (iy == bgIndex) continue;
     1944                    kernelMatrix->data.F64[ky][kx] = sumMatrix->data.F64[iy][ix];
     1945                    ky++;
     1946                }
     1947                kernelVector->data.F64[kx] = sumVector->data.F64[ix];
     1948                kx++;
     1949            }
     1950
     1951            psImage *U = NULL;
     1952            psImage *V = NULL;
     1953            psVector *w = NULL;
     1954            if (!psMatrixSVD (&U, &w, &V, kernelMatrix)) {
     1955                psError(psErrorCodeLast(), false, "failed to perform SVD on sumMatrix\n");
     1956                return NULL;
     1957            }
     1958
     1959            // calculate A_inverse:
     1960            // Ainv = V * w * U^T
     1961            psImage *wUt  = p_pmSubSolve_wUt (w, U);
     1962            psImage *Ainv = p_pmSubSolve_VwUt (V, wUt);
     1963            psImage *Xvar = NULL;
     1964            psFree (wUt);
     1965
     1966# ifdef TESTING
     1967            // kernel terms:
     1968            for (int i = 0; i < w->n; i++) {
     1969                fprintf (stderr, "w: %f\n", w->data.F64[i]);
     1970            }
     1971# endif
     1972            // loop over w adding in more and more of the values until chisquare is no longer
     1973            // dropping significantly.
     1974            // XXX this does not seem to work very well: we seem to need all terms even for
     1975            // simple cases...
     1976
     1977            psVector *Xsvd = NULL;
     1978            {
     1979                psVector *Ax = NULL;
     1980                psVector *UtB = NULL;
     1981                psVector *wUtB = NULL;
     1982
     1983                psVector *wApply = psVectorAlloc(w->n, PS_TYPE_F64);
     1984                psVector *wMask = psVectorAlloc(w->n, PS_TYPE_U8);
     1985                psVectorInit (wMask, 1); // start by masking everything
     1986
     1987                double chiSquareLast = NAN;
     1988                int maxWeight = 0;
     1989
     1990                double Axx, Bx, y2;
     1991
     1992                // XXX this is an attempt to exclude insignificant modes.
     1993                // it was not successful with the ISIS kernel set: removing even
     1994                // the least significant mode leaves additional ringing / noise
     1995                // because the terms are so coupled.
     1996                for (int k = 0; false && (k < w->n); k++) {
     1997
     1998                    // unmask the k-th weight
     1999                    wMask->data.U8[k] = 0;
     2000                    p_pmSubSolve_SetWeights(wApply, w, wMask);
     2001
     2002                    // solve for x:
     2003                    // x = V * w * (U^T * B)
     2004                    p_pmSubSolve_UtB (&UtB, U, kernelVector);
     2005                    p_pmSubSolve_wUtB (&wUtB, wApply, UtB);
     2006                    p_pmSubSolve_VwUtB (&Xsvd, V, wUtB);
     2007
     2008                    // chi-square for this system of equations:
     2009                    // chi-square = sum over terms of: (Ax - B)*x - b*x - y^2
     2010                    // y^2 = \sum_stamps \sum_pixels input->kernel[y][x]^2
     2011                    p_pmSubSolve_Ax (&Ax, kernelMatrix, Xsvd);
     2012                    p_pmSubSolve_VdV (&Axx, Ax, Xsvd);
     2013                    p_pmSubSolve_VdV (&Bx, kernelVector, Xsvd);
     2014                    p_pmSubSolve_y2 (&y2, kernels, stamps);
     2015
     2016                    // apparently, this works (compare with the brute force value below
     2017                    double chiSquare = Axx - 2.0*Bx + y2;
     2018                    double deltaChi = (k == 0) ? chiSquare : chiSquareLast - chiSquare;
     2019                    chiSquareLast = chiSquare;
     2020
     2021                    // fprintf (stderr, "chi square = %f, delta: %f\n", chiSquare, deltaChi);
     2022                    if (k && !maxWeight && (deltaChi < 1.0)) {
     2023                        maxWeight = k;
     2024                    }
     2025                }
     2026
     2027                // keep all terms or we get extra ringing
     2028                maxWeight = w->n;
     2029                psVectorInit (wMask, 1);
     2030                for (int k = 0; k < maxWeight; k++) {
     2031                    wMask->data.U8[k] = 0;
     2032                }
     2033                p_pmSubSolve_SetWeights(wApply, w, wMask);
     2034
     2035                // solve for x:
     2036                // x = V * w * (U^T * B)
     2037                p_pmSubSolve_UtB (&UtB, U, kernelVector);
     2038                p_pmSubSolve_wUtB (&wUtB, wApply, UtB);
     2039                p_pmSubSolve_VwUtB (&Xsvd, V, wUtB);
     2040
     2041                // chi-square for this system of equations:
     2042                // chi-square = sum over terms of: (Ax - B)*x - b*x - y^2
     2043                // y^2 = \sum_stamps \sum_pixels input->kernel[y][x]^2
     2044                p_pmSubSolve_Ax (&Ax, kernelMatrix, Xsvd);
     2045                p_pmSubSolve_VdV (&Axx, Ax, Xsvd);
     2046                p_pmSubSolve_VdV (&Bx, kernelVector, Xsvd);
     2047                p_pmSubSolve_y2 (&y2, kernels, stamps);
     2048
     2049                // apparently, this works (compare with the brute force value below
     2050                double chiSquare = Axx - 2.0*Bx + y2;
     2051                psLogMsg ("psModules.imcombine", PS_LOG_INFO, "model kernel with %d terms; chi square = %f\n", maxWeight, chiSquare);
     2052
     2053                // re-calculate A^{-1} to get new variances:
     2054                // Ainv = V * w * U^T
     2055                // XXX since we keep all terms, this is identical to Ainv
     2056                psImage *wUt  = p_pmSubSolve_wUt (wApply, U);
     2057                Xvar = p_pmSubSolve_VwUt (V, wUt);
     2058                psFree (wUt);
     2059
     2060                psFree (Ax);
     2061                psFree (UtB);
     2062                psFree (wUtB);
     2063                psFree (wApply);
     2064                psFree (wMask);
     2065            }
     2066
     2067            // copy the kernel solutions to the full solution vector:
     2068            solution = psVectorAlloc(sumVector->n, PS_TYPE_F64);
     2069            solutionErr = psVectorAlloc(sumVector->n, PS_TYPE_F64);
     2070
     2071            for (int ix = 0, kx = 0; ix < sumVector->n; ix++) {
     2072                if (ix == bgIndex) {
     2073                    solution->data.F64[ix] = 0;
     2074                    solutionErr->data.F64[ix] = 0.001;
     2075                    continue;
     2076                }
     2077                solutionErr->data.F64[ix] = sqrt(Ainv->data.F64[kx][kx]);
     2078                solution->data.F64[ix] = Xsvd->data.F64[kx];
     2079                kx++;
     2080            }
     2081
     2082            psFree (kernelMatrix);
     2083            psFree (kernelVector);
     2084
     2085            psFree (U);
     2086            psFree (V);
     2087            psFree (w);
     2088
     2089            psFree (Ainv);
     2090            psFree (Xsvd);
     2091        } else {
     2092            psVector *permutation = NULL;       // Permutation vector, required for LU decomposition
     2093            psImage *luMatrix = psMatrixLUDecomposition(NULL, &permutation, sumMatrix);
     2094            if (!luMatrix) {
     2095                psError(PM_ERR_DATA, true, "LU Decomposition of least-squares matrix failed.\n");
     2096                psFree(solution);
     2097                psFree(sumVector);
     2098                psFree(sumMatrix);
     2099                psFree(luMatrix);
     2100                psFree(permutation);
     2101                return NULL;
     2102            }
     2103
     2104            solution = psMatrixLUSolution(NULL, luMatrix, sumVector, permutation);
     2105            psFree(luMatrix);
     2106            psFree(permutation);
     2107            if (!solution) {
     2108                psError(PM_ERR_DATA, true, "Failed to solve the least-squares system.\n");
     2109                psFree(solution);
     2110                psFree(sumVector);
     2111                psFree(sumMatrix);
     2112                return NULL;
     2113            }
     2114
     2115            // XXX LUD does not provide A^{-1}?  fake the error for now
     2116            solutionErr = psVectorAlloc(sumVector->n, PS_TYPE_F64);
     2117            for (int ix = 0; ix < sumVector->n; ix++) {
     2118                solutionErr->data.F64[ix] = 0.1*solution->data.F64[ix];
     2119            }
     2120        }
     2121
     2122        if (!kernels->solution1) {
     2123            kernels->solution1 = psVectorAlloc (sumVector->n, PS_TYPE_F64);
     2124            psVectorInit (kernels->solution1, 0.0);
     2125        }
     2126
     2127        // only update the solutions that we chose to calculate:
     2128        if (mode & PM_SUBTRACTION_EQUATION_NORM) {
     2129            int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
     2130            kernels->solution1->data.F64[normIndex] = solution->data.F64[normIndex];
     2131        }
     2132        if (mode & PM_SUBTRACTION_EQUATION_BG) {
     2133            int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
     2134            kernels->solution1->data.F64[bgIndex] = solution->data.F64[bgIndex];
     2135        }
     2136        if (mode & PM_SUBTRACTION_EQUATION_KERNELS) {
     2137            int numKernels = kernels->num;
     2138            int spatialOrder = kernels->spatialOrder;       // Order of spatial variation
     2139            int numPoly = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of polynomial terms
     2140            for (int i = 0; i < numKernels * numPoly; i++) {
     2141                // XXX fprintf (stderr, "%f +/- %f (%f) -> ", solution->data.F64[i], solutionErr->data.F64[i], fabs(solution->data.F64[i]/solutionErr->data.F64[i]));
     2142                if (fabs(solution->data.F64[i] / solutionErr->data.F64[i]) < COEFF_SIG) {
     2143                    // XXX fprintf (stderr, "drop\n");
     2144                    kernels->solution1->data.F64[i] = 0.0;
     2145                } else {
     2146                    // XXX fprintf (stderr, "keep\n");
     2147                    kernels->solution1->data.F64[i] = solution->data.F64[i];
     2148                }
     2149            }
     2150        }
     2151        // double chiSquare = p_pmSubSolve_ChiSquare (kernels, stamps);
     2152        // fprintf (stderr, "chi square Brute = %f\n", chiSquare);
     2153
     2154        psFree(solution);
     2155        psFree(sumVector);
     2156        psFree(sumMatrix);
     2157# endif
     2158
     2159#ifdef TESTING
     2160              // XXX double-check for NAN in data:
     2161                for (int iy = 0; iy < stamp->matrix->numRows; iy++) {
     2162                    for (int ix = 0; ix < stamp->matrix->numCols; ix++) {
     2163                        if (!isfinite(stamp->matrix->data.F64[iy][ix])) {
     2164                            fprintf (stderr, "WARNING: NAN in matrix\n");
     2165                        }
     2166                    }
     2167                }
     2168                for (int ix = 0; ix < stamp->vector->n; ix++) {
     2169                    if (!isfinite(stamp->vector->data.F64[ix])) {
     2170                        fprintf (stderr, "WARNING: NAN in vector\n");
     2171                    }
     2172                }
     2173#endif
     2174
     2175#ifdef TESTING
     2176        for (int ix = 0; ix < sumVector->n; ix++) {
     2177            if (!isfinite(sumVector->data.F64[ix])) {
     2178                fprintf (stderr, "WARNING: NAN in vector\n");
     2179            }
     2180        }
     2181#endif
     2182
     2183#ifdef TESTING
     2184        for (int ix = 0; ix < sumVector->n; ix++) {
     2185            if (!isfinite(sumVector->data.F64[ix])) {
     2186                fprintf (stderr, "WARNING: NAN in vector\n");
     2187            }
     2188        }
     2189        {
     2190            psImage *inverse = psMatrixInvert(NULL, sumMatrix, NULL);
     2191            psFitsWriteImageSimple("matrixInv.fits", inverse, NULL);
     2192            psFree(inverse);
     2193        }
     2194        {
     2195            psImage *X = psMatrixInvert(NULL, sumMatrix, NULL);
     2196            psImage *Xt = psMatrixTranspose(NULL, X);
     2197            psImage *XtX = psMatrixMultiply(NULL, Xt, X);
     2198            psFitsWriteImageSimple("matrixErr.fits", XtX, NULL);
     2199            psFree(X);
     2200            psFree(Xt);
     2201            psFree(XtX);
     2202        }
     2203#endif
     2204
Note: See TracChangeset for help on using the changeset viewer.