IPP Software Navigation Tools IPP Links Communication Pan-STARRS Links

Changeset 25857


Ignore:
Timestamp:
Oct 15, 2009, 3:12:27 PM (17 years ago)
Author:
Paul Price
Message:

Got rid of old matrix/vector calculation --- new code does the same, and is easier to understand.
Implementing cleaner solution for dual convolution: combine all matrices and vectors into single matrix equation: subject to less roundoff problems, works the same.

File:
1 edited

Legend:

Unmodified
Added
Removed

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

    r25841 r25857  
    1919#define USE_VARIANCE                    // Include variance in equation?
    2020
    21 //#define OLD_FUNCTIONS                   // Use old functions
    2221
    2322//////////////////////////////////////////////////////////////////////////////////////////////////////////////
    … …  
    2524//////////////////////////////////////////////////////////////////////////////////////////////////////////////
    2625
    27 #ifndef OLD_FUNCTIONS
    2826// Calculate the least-squares matrix and vector
    2927static bool calculateMatrixVector(psImage *matrix, // Least-squares matrix, updated
    … …  
    401399}
    402400
    403 #ifdef OLD_FUNCTIONS
    404 // Calculate a single element of the least-squares matrix, with the polynomial expansions in one direction
    405 static inline bool calculateMatrixElement1(psImage *matrix, // Matrix to calculate
    406                                            int i, int j, // Coordinates of element
    407                                            const psKernel *image1, // First image in multiplication
    408                                            const psKernel *image2, // Second image in multiplication
    409                                            const psKernel *variance, // Variance image
    410                                            const psImage *polyValues, // Spatial polynomial values
    411                                            int numKernels, // Number of kernel basis functions
    412                                            int footprint, // (Half-)Size of stamp
    413                                            int spatialOrder, // Maximum order of spatial variation
    414                                            bool symmetric // Is the matrix symmetric?
    415     )
    416 {
    417     double sum = calculateSumProduct(image1, image2, variance, footprint); // Sum of the image products
    418     if (!isfinite(sum)) {
    419         return false;
    420     }
    421 
    422     // Generate the pseudo-convolutions from the spatial polynomial terms
    423     for (int iyOrder = 0, iIndex = i; iyOrder <= spatialOrder; iyOrder++) {
    424         for (int ixOrder = 0; ixOrder <= spatialOrder - iyOrder; ixOrder++, iIndex += numKernels) {
    425             double convPoly = sum * polyValues->data.F64[iyOrder][ixOrder];
    426 
    427             assert(iIndex < matrix->numRows && j < matrix->numCols);
    428 
    429             matrix->data.F64[iIndex][j] = convPoly;
    430             if (symmetric) {
    431 
    432                 assert(iIndex < matrix->numCols && j < matrix->numRows);
    433 
    434                 matrix->data.F64[j][iIndex] = convPoly;
    435             }
    436         }
    437     }
    438     return true;
    439 }
    440 
    441 // Calculate a single element of the least-squares matrix, with the polynomial expansions in both directions
    442 static inline bool calculateMatrixElement2(psImage *matrix, // Matrix to calculate
    443                                            int i, int j, // Coordinates of element
    444                                            const psKernel *image1, // First image in multiplication
    445                                            const psKernel *image2, // Second image in multiplication
    446                                            const psKernel *variance, // Variance image
    447                                            const psImage *polyValues, // Spatial polynomial values
    448                                            int numKernels, // Number of kernel basis functions
    449                                            int footprint, // (Half-)Size of stamp
    450                                            int spatialOrder, // Maximum order of spatial variation
    451                                            bool symmetric // Is the matrix symmetric?
    452     )
    453 {
    454     double sum = calculateSumProduct(image1, image2, variance, footprint); // Sum of the image products
    455     if (!isfinite(sum)) {
    456         return false;
    457     }
    458 
    459     // Generate the pseudo-convolutions from the spatial polynomial terms
    460     for (int iyOrder = 0, iIndex = i; iyOrder <= spatialOrder; iyOrder++) {
    461         for (int ixOrder = 0; ixOrder <= spatialOrder - iyOrder; ixOrder++, iIndex += numKernels) {
    462             double iPoly = polyValues->data.F64[iyOrder][ixOrder]; // Value of polynomial
    463             for (int jyOrder = 0, jIndex = j; jyOrder <= spatialOrder; jyOrder++) {
    464                 for (int jxOrder = 0; jxOrder <= spatialOrder - jyOrder; jxOrder++, jIndex += numKernels) {
    465                     double convPoly = sum * iPoly * polyValues->data.F64[jyOrder][jxOrder];
    466 
    467                     assert(iIndex < matrix->numRows && jIndex < matrix->numCols);
    468 
    469                     matrix->data.F64[iIndex][jIndex] = convPoly;
    470                     if (symmetric) {
    471 
    472                         assert(iIndex < matrix->numCols && jIndex < matrix->numRows);
    473 
    474                         matrix->data.F64[jIndex][iIndex] = convPoly;
    475                     }
    476                 }
    477             }
    478         }
    479     }
    480     return true;
    481 }
    482 
    483 // Calculate the square part of the matrix derived from multiplying convolutions
    484 static bool calculateMatrixSquare(psImage *matrix, // Matrix to calculate
    485                                   const psArray *convolutions1, // Convolutions for element 1
    486                                   const psArray *convolutions2, // Convolutions for element 2
    487                                   const psKernel *variance, // Variance image
    488                                   const psImage *polyValues, // Polynomial values
    489                                   int numKernels, // Number of kernel basis functions
    490                                   int spatialOrder, // Order of spatial variation
    491                                   int footprint // Half-size of stamp
    492                                   )
    493 {
    494     bool symmetric = (convolutions1 == convolutions2 ? true : false); // Is matrix symmetric?
    495 
    496     for (int i = 0; i < numKernels; i++) {
    497         psKernel *iConv = convolutions1->data[i]; // Convolution for i-th element
    498 
    499         for (int j = (symmetric ? i : 0); j < numKernels; j++) {
    500             psKernel *jConv = convolutions2->data[j]; // Convolution for j-th element
    501 
    502             if (!calculateMatrixElement2(matrix, i, j, iConv, jConv, variance, polyValues, numKernels,
    503                                          footprint, spatialOrder, symmetric)) {
    504                 psTrace("psModules.imcombine", 2, "Bad sumCC at %d, %d", i, j);
    505                 return false;
    506             }
    507         }
    508     }
    509 
    510     return true;
    511 }
    512 
    513 // Calculate least-squares matrix and vector
    514 static bool calculateMatrix(psImage *matrix, // Matrix to calculate
    515                             const pmSubtractionKernels *kernels, // Kernel components
    516                             const psArray *convolutions, // Convolutions of source with kernels
    517                             const psKernel *input, // Input stamp, or NULL
    518                             const psKernel *variance, // Variance stamp
    519                             const psImage *polyValues, // Spatial polynomial values
    520                             int footprint, // (Half-)Size of stamp
    521                             bool normAndBG // Calculate normalisation and background terms?
    522     )
    523 {
    524     int numKernels = kernels->num;      // Number of kernel components
    525     int spatialOrder = kernels->spatialOrder; // Maximum order of spatial variation
    526     int numSpatial = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of spatial variation terms
    527     int bgOrder = kernels->bgOrder;     // Maximum order of background fit
    528     int numBackground = normAndBG ? PM_SUBTRACTION_POLYTERMS(bgOrder) : 0; // Number of background terms
    529     int numTerms = numKernels * numSpatial + (normAndBG ? 1 + numBackground : 0); // Total number of terms
    530     assert(matrix);
    531     assert(matrix->numCols == matrix->numRows);
    532     assert(matrix->numCols == numTerms);
    533     assert(convolutions && convolutions->n == numKernels);
    534     assert(polyValues);
    535     assert(!normAndBG || input);        // If we want the normalisation and BG, then we need the input image
    536 
    537     // Square part of the matrix (convolution-convolution products)
    538     if (!calculateMatrixSquare(matrix, convolutions, convolutions, variance, polyValues, numKernels,
    539                                spatialOrder, footprint)) {
    540         return false;
    541     }
    542 
    543     // XXX To support higher-order background model than simply constant, the below code needs to be updated.
    544     if (normAndBG) {
    545         int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
    546         int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
    547 
    548         for (int i = 0; i < numKernels; i++) {
    549             psKernel *conv = convolutions->data[i]; // Convolution for i-th element
    550 
    551             // Normalisation-convolution terms
    552             if (!calculateMatrixElement1(matrix, i, normIndex, conv, input, variance, polyValues, numKernels,
    553                                          footprint, spatialOrder, true)) {
    554                 psTrace("psModules.imcombine", 2, "Bad sumIC at %d", i);
    555                 return false;
    556             }
    557 
    558             // Background-convolution terms
    559             double sumC = 0.0;          // Sum of the convolution
    560             for (int y = - footprint; y <= footprint; y++) {
    561                 for (int x = - footprint; x <= footprint; x++) {
    562                     double value = conv->kernel[y][x];
    563 #ifdef USE_VARIANCE
    564                     value /= variance->kernel[y][x];
    565 #endif
    566                     sumC += value;
    567                 }
    568             }
    569             if (!isfinite(sumC)) {
    570                 psTrace("psModules.imcombine", 2, "Bad sumC at %d", i);
    571                 return false;
    572             }
    573 
    574             for (int yOrder = 0, index = i; yOrder <= spatialOrder; yOrder++) {
    575                 for (int xOrder = 0; xOrder <= spatialOrder - yOrder; xOrder++, index += numKernels) {
    576                     double value = sumC * polyValues->data.F64[yOrder][xOrder];
    577                     matrix->data.F64[index][bgIndex] = value;
    578                     matrix->data.F64[bgIndex][index] = value;
    579                 }
    580             }
    581         }
    582 
    583         // Background only, normalisation only, and background-normalisation terms
    584         double sum1 = 0.0;              // Sum of the weighting
    585         double sumI = 0.0;              // Sum of the input
    586         double sumII = 0.0;             // Sum of the input squared
    587         for (int y = - footprint; y <= footprint; y++) {
    588             for (int x = - footprint; x <= footprint; x++) {
    589                 double invNoise2 = 1.0;
    590 #ifdef USE_VARIANCE
    591                 invNoise2 /= variance->kernel[y][x];
    592 #endif
    593                 double value = input->kernel[y][x] * invNoise2;
    594                 sumI += value;
    595                 sumII += value * input->kernel[y][x];
    596                 sum1 += invNoise2;
    597             }
    598         }
    599         if (!isfinite(sumI)) {
    600             psTrace("psModules.imcombine", 2, "Bad sumI detected");
    601             return false;
    602         }
    603         if (!isfinite(sumII)) {
    604             psTrace("psModules.imcombine", 2, "Bad sumII detected");
    605             return false;
    606         }
    607         if (!isfinite(sum1)) {
    608             psTrace("psModules.imcombine", 2, "Bad sum1 detected");
    609             return false;
    610         }
    611         matrix->data.F64[normIndex][normIndex] = sumII;
    612         matrix->data.F64[bgIndex][bgIndex] = sum1;
    613         matrix->data.F64[normIndex][bgIndex] = sumI;
    614         matrix->data.F64[bgIndex][normIndex] = sumI;
    615     }
    616 
    617     return true;
    618 }
    619 
    620 
    621 // Calculate least-squares matrix and vector
    622 static bool calculateVector(psVector *vector, // Vector to calculate, or NULL
    623                             const pmSubtractionKernels *kernels, // Kernel components
    624                             const psArray *convolutions, // Convolutions of source with kernels
    625                             const psKernel *input, // Input stamp, or NULL if !normAndBG
    626                             const psKernel *target, // Target stamp
    627                             const psKernel *variance, // Variance stamp
    628                             const psImage *polyValues, // Spatial polynomial values
    629                             int footprint, // (Half-)Size of stamp
    630                             bool normAndBG // Calculate normalisation and background terms?
    631     )
    632 {
    633     int numKernels = kernels->num;      // Number of kernel components
    634     int spatialOrder = kernels->spatialOrder; // Maximum order of spatial variation
    635     int numSpatial = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of spatial variation terms
    636     int bgOrder = kernels->bgOrder;     // Maximum order of background fit
    637     int numBackground = normAndBG ? PM_SUBTRACTION_POLYTERMS(bgOrder) : 0; // Number of background terms
    638     int numTerms = numKernels * numSpatial + (normAndBG ? 1 + numBackground : 0); // Total number of terms
    639     assert(vector && vector->n == numTerms);
    640     assert(convolutions && convolutions->n == numKernels);
    641     assert(target);
    642     assert(polyValues);
    643     assert(!normAndBG || input);       // If we want the normalisation and BG, then we need the input image
    644 
    645     // Convolution terms
    646     for (int i = 0; i < numKernels; i++) {
    647         psKernel *conv = convolutions->data[i]; // Convolution for i-th element
    648         double sumTC = 0.0;          // Sum of the target and convolution
    649         for (int y = - footprint; y <= footprint; y++) {
    650             for (int x = - footprint; x <= footprint; x++) {
    651                 double value = target->kernel[y][x] * conv->kernel[y][x];
    652 #ifdef USE_VARIANCE
    653                 value /= variance->kernel[y][x];
    654 #endif
    655                 sumTC += value;
    656             }
    657         }
    658         if (!isfinite(sumTC)) {
    659             psTrace("psModules.imcombine", 2, "Bad sumTC at %d", i);
    660             return false;
    661         }
    662         for (int yOrder = 0, index = i; yOrder <= spatialOrder; yOrder++) {
    663             for (int xOrder = 0; xOrder <= spatialOrder - yOrder; xOrder++, index += numKernels) {
    664                 vector->data.F64[index] = sumTC * polyValues->data.F64[yOrder][xOrder];
    665             }
    666         }
    667     }
    668 
    669     if (normAndBG) {
    670         // Background terms
    671         double sumT = 0.0;              // Sum of the target
    672         double sumIT = 0.0;             // Sum of the input-target product
    673         for (int y = - footprint; y <= footprint; y++) {
    674             for (int x = - footprint; x <= footprint; x++) {
    675                 double value = target->kernel[y][x];
    676 #ifdef USE_VARIANCE
    677                 value /= variance->kernel[y][x];
    678 #endif
    679                 sumIT += value * input->kernel[y][x];
    680                 sumT += value;
    681             }
    682         }
    683         if (!isfinite(sumT)) {
    684             psTrace("psModules.imcombine", 2, "Bad sumI detected");
    685             return false;
    686         }
    687         if (!isfinite(sumIT)) {
    688             psTrace("psModules.imcombine", 2, "Bad sumIT detected");
    689             return false;
    690         }
    691 
    692         int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation term
    693         vector->data.F64[normIndex] = sumIT;
    694         int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background term
    695         vector->data.F64[bgIndex] = sumT;
    696     }
    697 
    698     return true;
    699 }
    700 
    701 
    702 
    703 // Calculate the cross-matrix, composed of convolutions of each image
    704 // Note that the cross-matrix is NOT square
    705 static bool calculateMatrixCross(psImage *matrix, // Matrix to calculate
    706                                  const pmSubtractionKernels *kernels, // Kernel components
    707                                  const psArray *convolutions1, // Convolutions of image 1
    708                                  const psArray *convolutions2, // Convolutions of image 2
    709                                  const psKernel *image1, // Image 1 stamp
    710                                  const psKernel *variance, // Variance stamp
    711                                  const psImage *polyValues, // Spatial polynomial values
    712                                  int footprint // (Half-)Size of stamp
    713                                  )
    714 {
    715     assert(matrix);
    716     int numKernels = kernels->num;      // Number of kernel components
    717     int spatialOrder = kernels->spatialOrder; // Maximum order of spatial variation
    718     int numSpatial = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of spatial polynomial terms
    719     int numBackground = PM_SUBTRACTION_POLYTERMS(kernels->bgOrder); // Number of background terms
    720     int numCols = numKernels * numSpatial + 1 + numBackground; // Number of columns
    721     int numRows = numKernels * numSpatial; // Number of rows
    722     assert(matrix->numCols == numCols && matrix->numRows == numRows);
    723     assert(convolutions1 && convolutions1->n == numKernels);
    724     assert(convolutions2 && convolutions2->n == numKernels);
    725 
    726     int normIndex, bgIndex;             // Indices in matrix for normalisation and background terms
    727     PM_SUBTRACTION_INDICES(normIndex, bgIndex, kernels);
    728 
    729     if (!calculateMatrixSquare(matrix, convolutions1, convolutions2, variance, polyValues, numKernels,
    730                                spatialOrder, footprint)) {
    731         return false;
    732     }
    733 
    734     for (int i = 0; i < numKernels; i++) {
    735         // Normalisation
    736         psKernel *conv = convolutions2->data[i]; // Convolution
    737         if (!calculateMatrixElement1(matrix, i, normIndex, conv, image1, variance, polyValues, numKernels,
    738                                      footprint, spatialOrder, false)) {
    739             psTrace("psModules.imcombine", 2, "Bad sumIC at %d", i);
    740             return false;
    741         }
    742 
    743         // Background
    744         double sumC = 0.0;              // Sum of the weighting
    745         for (int y = - footprint; y <= footprint; y++) {
    746             for (int x = - footprint; x <= footprint; x++) {
    747                 double value = conv->kernel[y][x];
    748 #ifdef USE_VARIANCE
    749                 value /= variance->kernel[y][x];
    750 #endif
    751                 sumC += value;
    752             }
    753         }
    754         if (!isfinite(sumC)) {
    755             psTrace("psModules.imcombine", 2, "Bad sumC detected at %d", i);
    756             return false;
    757         }
    758         for (int yOrder = 0, index = i; yOrder <= spatialOrder; yOrder++) {
    759             for (int xOrder = 0; xOrder <= spatialOrder - yOrder; xOrder++, index += numKernels) {
    760                 matrix->data.F64[index][bgIndex] = sumC * polyValues->data.F64[yOrder][xOrder];
    761             }
    762         }
    763     }
    764 
    765     return true;
    766 }
    767 #endif
    768 
    769401// Add in penalty term to least-squares vector
    770402static bool calculatePenalty(psVector *vector, // Vector to which to add in penalty term
    … …  
    941573    switch (kernels->mode) {
    942574      case PM_SUBTRACTION_MODE_1:
    943 #ifdef OLD_FUNCTIONS
    944         status = calculateMatrix(stamp->matrix1, kernels, stamp->convolutions1, stamp->image1,
    945                                  stamp->variance, polyValues, footprint, true);
    946         status &= calculateVector(stamp->vector1, kernels, stamp->convolutions1, stamp->image1,
    947                                   stamp->image2, stamp->variance, polyValues, footprint, true);
    948 #else
    949575        status = calculateMatrixVector(stamp->matrix1, stamp->vector1, stamp->image2, stamp->image1,
    950576                                       stamp->variance, stamp->convolutions1, kernels, polyValues,
    951577                                       footprint);
    952 #endif
    953578        break;
    954579      case PM_SUBTRACTION_MODE_2:
    955 #ifdef OLD_FUNCTIONS
    956         status = calculateMatrix(stamp->matrix1, kernels, stamp->convolutions2, stamp->image2,
    957                                  stamp->variance, polyValues, footprint, true);
    958         status &= calculateVector(stamp->vector1, kernels, stamp->convolutions2, stamp->image2,
    959                                   stamp->image1, stamp->variance, polyValues, footprint, true);
    960 #else
    961580        status = calculateMatrixVector(stamp->matrix1, stamp->vector1, stamp->image1, stamp->image2,
    962581                                       stamp->variance, stamp->convolutions2, kernels, polyValues,
    963582                                       footprint);
    964 #endif
    965583        break;
    966584      case PM_SUBTRACTION_MODE_DUAL:
    … …  
    975593        psVectorInit(stamp->vector2, NAN);
    976594#endif
    977 #ifdef OLD_FUNCTIONS
    978         status  = calculateMatrix(stamp->matrix1, kernels, stamp->convolutions1, stamp->image1,
    979                                   stamp->variance, polyValues, footprint, true);
    980         status &= calculateMatrix(stamp->matrix2, kernels, stamp->convolutions2, NULL,
    981                                   stamp->variance, polyValues, footprint, false);
    982         status &= calculateMatrixCross(stamp->matrixX, kernels, stamp->convolutions1,
    983                                        stamp->convolutions2, stamp->image1, stamp->variance, polyValues,
    984                                        footprint);
    985         status &= calculateVector(stamp->vector1, kernels, stamp->convolutions1, stamp->image1,
    986                                   stamp->image2, stamp->variance, polyValues, footprint, true);
    987         status &= calculateVector(stamp->vector2, kernels, stamp->convolutions2, NULL,
    988                                   stamp->image2, stamp->variance, polyValues, footprint, false);
    989 #else
    990595        status = calculateDualMatrixVector(stamp->matrix1, stamp->vector1, stamp->matrix2, stamp->vector2,
    991596                                           stamp->matrixX, stamp->image1, stamp->image2, stamp->variance,
    992597                                           stamp->convolutions1, stamp->convolutions2, kernels, polyValues,
    993598                                           footprint);
    994 #endif
    995599        break;
    996600      default:
    … …  
    1291895
    1292896        // Pure matrix operations
    1293 
    1294897        // A * a = Ct * b + d
    1295898        // C * a = B  * b + e
    1296899        //
    1297         // a = (Ct * Bi * C - A)i (Ct * Bi * e - d)
    1298         // b = Bi * (C * a - e)
    1299         psVector *a = psVectorRecycle(kernels->solution1, numParams, PS_TYPE_F64);
    1300         psVector *b = psVectorRecycle(kernels->solution2, numParams2, PS_TYPE_F64);
    1301 #ifdef TESTING
    1302         psVectorInit(a, NAN);
    1303         psVectorInit(b, NAN);
    1304 #endif
    1305         psImage *A = sumMatrix1;
    1306         psImage *B = sumMatrix2;
    1307         psImage *C = sumMatrixX;
    1308         psVector *d = sumVector1;
    1309         psVector *e = sumVector2;
    1310 
    1311         assert(a->n == numParams);
    1312         assert(b->n == numParams2);
    1313         assert(A->numRows == numParams && A->numCols == numParams);
    1314         assert(B->numRows == numParams2 && B->numCols == numParams2);
    1315         assert(C->numRows == numParams2 && C->numCols == numParams);
    1316         assert(d->n == numParams);
    1317         assert(e->n == numParams2);
    1318 
    1319         psImage *Bi = psMatrixInvert(NULL, B, NULL);
    1320         assert(Bi->numRows == numParams2 && Bi->numCols == numParams2);
    1321         psImage *Ct = psMatrixTranspose(NULL, C);
    1322         assert(Ct->numRows == numParams && Ct->numCols == numParams2);
    1323 
    1324         psImage *BiC = psMatrixMultiply(NULL, Bi, C);
    1325         assert(BiC->numRows == numParams2 && BiC->numCols == numParams);
    1326         psImage *CtBi = psMatrixMultiply(NULL, Ct, Bi);
    1327         assert(CtBi->numRows == numParams && CtBi->numCols == numParams2);
    1328 
    1329         psImage *CtBiC = psMatrixMultiply(NULL, Ct, BiC);
    1330         assert(CtBiC->numRows == numParams && CtBiC->numCols == numParams);
    1331 
    1332         psImage *F = (psImage*)psBinaryOp(NULL, CtBiC, "-", A);
    1333         assert(F->numRows == numParams && F->numCols == numParams);
    1334         float det = 0.0;
    1335         psImage *Fi = psMatrixInvert(NULL, F, &det);
    1336         assert(Fi->numRows == numParams && Fi->numCols == numParams);
    1337         psTrace("psModules.imcombine", 4, "Determinant of F: %f\n", det);
    1338 
    1339         psVector *g = psVectorAlloc(numParams, PS_TYPE_F64);
    1340 #ifdef TESTING
    1341         psVectorInit(g, NAN);
    1342 #endif
    1343         assert(CtBi->numRows == numParams && CtBi->numCols == numParams2);
    1344         assert(e->n == numParams2);
    1345         assert(d->n == numParams);
     900        // Set F = ( A -Ct ;  C -B )
     901        // Set g = ( a ; b )
     902        // Set h = ( d ; e )
     903        // So that we combine the above two equations: Fg = h
     904
     905        int num = numParams + numParams2; // Number of params for new set
     906        psImage *F = psImageAlloc(num, num, PS_TYPE_F64);
     907        psVector *h = psVectorAlloc(num, PS_TYPE_F64);
     908
    1346909        for (int i = 0; i < numParams; i++) {
    1347             double value = 0.0;
     910            h->data.F64[i] = sumVector1->data.F64[i];
     911            for (int j = 0; j < numParams; j++) {
     912                F->data.F64[i][j] = sumMatrix1->data.F64[i][j];
     913            }
    1348914            for (int j = 0; j < numParams2; j++) {
    1349                 value += CtBi->data.F64[i][j] * e->data.F64[j];
    1350             }
    1351             g->data.F64[i] = value - d->data.F64[i];
    1352         }
    1353 
    1354         assert(Fi->numRows == numParams && Fi->numCols == numParams);
    1355         assert(g->n == numParams);
    1356         for (int i = 0; i < numParams; i++) {
    1357             double value = 0.0;
     915                F->data.F64[i][numParams + j] = - sumMatrixX->data.F64[j][i];
     916            }
     917        }
     918        for (int i = 0; i < numParams2; i++) {
     919            h->data.F64[numParams + i] = sumVector2->data.F64[i];
    1358920            for (int j = 0; j < numParams; j++) {
    1359                 value += Fi->data.F64[i][j] * g->data.F64[j];
    1360             }
    1361             a->data.F64[i] = value;
    1362         }
    1363 
    1364         psVector *h = psVectorAlloc(numParams2, PS_TYPE_F64);
    1365 #ifdef TESTING
    1366         psVectorInit(h, NAN);
    1367 #endif
    1368         assert(C->numRows == numParams2 && C->numCols == numParams);
    1369         assert(a->n == numParams);
    1370         assert(e->n == numParams2);
    1371         for (int i = 0; i < numParams2; i++) {
    1372             double value = 0.0;
    1373             for (int j = 0; j < numParams; j++) {
    1374                 value += C->data.F64[i][j] * a->data.F64[j];
    1375             }
    1376             h->data.F64[i] = value - e->data.F64[i];
    1377         }
    1378 
    1379         assert(Bi->numRows == numParams2 && Bi->numCols == numParams2);
    1380         assert(h->n == numParams2);
    1381         for (int i = 0; i < numParams2; i++) {
    1382             double value = 0.0;
     921                F->data.F64[numParams + i][j] = sumMatrixX->data.F64[i][j];
     922            }
    1383923            for (int j = 0; j < numParams2; j++) {
    1384                 value += Bi->data.F64[i][j] * h->data.F64[j];
    1385             }
    1386             b->data.F64[i] = value;
    1387         }
    1388 
    1389 
    1390 #if 1
    1391         for (int i = 0; i < numParams; i++) {
    1392             double aVal1 = 0.0, bVal1 = 0.0;
    1393             for (int j = 0; j < numParams2; j++) {
    1394                 aVal1 += A->data.F64[i][j] * a->data.F64[j];
    1395                 bVal1 += Ct->data.F64[i][j] * b->data.F64[j];
    1396             }
    1397             bVal1 += d->data.F64[i];
    1398             for (int j = numParams2; j < numParams; j++) {
    1399                 aVal1 += A->data.F64[i][j] * a->data.F64[j];
    1400             }
    1401             printf("%d: %lf\n", i, aVal1 - bVal1);
    1402         }
    1403 
    1404         for (int i = 0; i < numParams2; i++) {
    1405             double aVal2 = 0.0, bVal2 = 0.0;
    1406             for (int j = 0; j < numParams2; j++) {
    1407                 aVal2 += C->data.F64[i][j] * a->data.F64[j];
    1408                 bVal2 += B->data.F64[i][j] * b->data.F64[j];
    1409             }
    1410             bVal2 += e->data.F64[i];
    1411             for (int j = numParams2; j < numParams; j++) {
    1412                 aVal2 += C->data.F64[i][j] * a->data.F64[j];
    1413             }
    1414             printf("%d: %lf\n", i, aVal2 - bVal2);
    1415         }
    1416 #endif
     924                F->data.F64[numParams + i][numParams + j] = - sumMatrix2->data.F64[i][j];
     925            }
     926        }
     927        psFree(sumMatrix1);
     928        psFree(sumMatrix2);
     929        psFree(sumMatrixX);
     930        psFree(sumVector1);
     931        psFree(sumVector2);
    1417932
    1418933#ifdef TESTING
    1419934        {
    1420             psFits *fits = psFitsOpen("sumMatrix1.fits", "w");
    1421             psFitsWriteImage(fits, NULL, sumMatrix1, 0, NULL);
     935            psFits *fits = psFitsOpen("sumMatrix.fits", "w");
     936            psFitsWriteImage(fits, NULL, F, 0, NULL);
    1422937            psFitsClose(fits);
    1423938        }
    1424939        {
    1425             psFits *fits = psFitsOpen("sumMatrix2.fits", "w");
    1426             psFitsWriteImage(fits, NULL, sumMatrix2, 0, NULL);
     940            psImage *image = psImageAlloc(1, num, PS_TYPE_F64);
     941            psFits *fits = psFitsOpen("sumVector.fits", "w");
     942            for (int i = 0; i < num; i++) {
     943                image->data.F64[0][i] = h->data.F64[i];
     944            }
     945            psFitsWriteImage(fits, NULL, image, 0, NULL);
     946            psFree(image);
    1427947            psFitsClose(fits);
    1428948        }
    1429         {
    1430             psFits *fits = psFitsOpen("sumMatrixX.fits", "w");
    1431             psFitsWriteImage(fits, NULL, sumMatrixX, 0, NULL);
    1432             psFitsClose(fits);
    1433         }
    1434         {
    1435             psFits *fits = psFitsOpen("sumFinverse.fits", "w");
    1436             psFitsWriteImage(fits, NULL, Fi, 0, NULL);
    1437             psFitsClose(fits);
    1438         }
    1439 #endif
    1440 
    1441         kernels->solution1 = a;
    1442         kernels->solution2 = b;
    1443 
    1444         // XXXXX Free temporary matrices and vectors
     949#endif
     950
     951        psVector *permutation = NULL;       // Permutation vector, required for LU decomposition
     952        psImage *luMatrix = psMatrixLUDecomposition(NULL, &permutation, F);
     953        if (!luMatrix) {
     954            psError(PS_ERR_UNKNOWN, true, "LU Decomposition of least-squares matrix failed.\n");
     955            psFree(F);
     956            psFree(h);
     957            psFree(luMatrix);
     958            psFree(permutation);
     959            return NULL;
     960        }
     961        psFree(F);
     962
     963        psVector *g = psMatrixLUSolution(NULL, luMatrix, h, permutation); // Solution!
     964        if (!g) {
     965            psError(PS_ERR_UNKNOWN, true, "LU Decomposition of least-squares matrix failed.\n");
     966            psFree(h);
     967            psFree(luMatrix);
     968            psFree(permutation);
     969            return NULL;
     970        }
     971        psFree(permutation);
     972        psFree(luMatrix);
     973        psFree(h);
     974
     975        if (!kernels->solution1) {
     976            kernels->solution1 = psVectorAlloc(numParams, PS_TYPE_F64);
     977        }
     978        if (!kernels->solution2) {
     979            kernels->solution2 = psVectorAlloc(numParams2, PS_TYPE_F64);
     980        }
     981
     982        for (int i = 0; i < numParams; i++) {
     983            kernels->solution1->data.F64[i] = g->data.F64[i];
     984        }
     985        for (int i = 0; i < numParams2; i++) {
     986            kernels->solution2->data.F64[i] = g->data.F64[numParams + i];
     987        }
     988        psFree(g);
    1445989
    1446990    }
Note: See TracChangeset for help on using the changeset viewer.