IPP Software Navigation Tools IPP Links Communication Pan-STARRS Links

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

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

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

Legend:

Unmodified
Added
Removed

  • trunk/psModules/src/imcombine

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

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