IPP Software Navigation Tools IPP Links Communication Pan-STARRS Links

Changeset 26001


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

Merging branches/pap for dual convolution.

Location:
trunk/psLib
Files:
3 edited

Legend:

Unmodified
Added
Removed

  • trunk/psLib

  • trunk/psLib/src/math/psMatrix.c

    r24122 r26001  
    9494LHS_NAME.data  = RHS_NAME;
    9595
    96 
    97 /*****************************************************************************/
    98 /* FILE STATIC FUNCTIONS                                                     */
    99 /*****************************************************************************/
    100 
    101 static void  psVectorToGslVector(gsl_vector *outGslVector, const psVector *inVector);
    102 static void gslVectorToPsVector(psVector *outVector, gsl_vector *inGslVector);
    103 static void  psImageToGslMatrix(gsl_matrix *outGslMatrix, const psImage *inImage);
    104 static void gslMatrixToPsImage(psImage *outImage, gsl_matrix *inGslMatrix);
     96////////////////////////////////////////////////////////////////////////////////
     97// Conversion functions
     98////////////////////////////////////////////////////////////////////////////////
     99
     100// gsl_vector holds *doubles*, so we can directly copy F64, but need to convert F32
    105101
    106102/** Static function to copy psF32 or psF64 vector data to a GSL vector */
    107 static void  psVectorToGslVector(gsl_vector *outGslVector,
    108                                  const psVector *inVector)
    109 {
    110     psU32 i = 0;
    111     psU32 n = 0;
    112 
    113 
    114     n = inVector->n;
    115     for(i=0; i<n; i++) {
    116         if(inVector->type.type == PS_TYPE_F32) {
    117             outGslVector->data[i] = (psF64)inVector->data.F32[i];
     103static void vectorPStoGSL(gsl_vector *out, const psVector *in)
     104{
     105    psAssert(out->size == in->n, "Sizes don't match!");
     106
     107    long n = in->n;                     // Size of input
     108    switch (in->type.type) {
     109      case PS_TYPE_F32:
     110        for (long i = 0; i < n; i++) {
     111            out->data[i] = in->data.F32[i];
     112        }
     113        break;
     114      case PS_TYPE_F64:
     115        memcpy(out->data, in->data.F64, n * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
     116        break;
     117      default:
     118        psAbort("Unsupported vector type: %x\n", in->type.type);
     119    }
     120    return;
     121}
     122
     123/** Static function to copy GSL vector data to a psF32 or psF64 vector */
     124static void vectorGSLtoPS(psVector *out, const gsl_vector *in)
     125{
     126    psAssert(in->size == out->n, "Sizes don't match!");
     127
     128    long n = out->n;                    // Size of output
     129    switch (out->type.type) {
     130      case PS_TYPE_F32:
     131        for (long i = 0; i < n; i++) {
     132            out->data.F32[i] = in->data[i];
     133        }
     134        break;
     135      case PS_TYPE_F64:
     136        memcpy(out->data.F64, in->data, n * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
     137        break;
     138      default:
     139        psAbort("Unsupported vector type: %x\n", out->type.type);
     140    }
     141    return;
     142}
     143
     144
     145/** Static function to copy psF32 or psF64 image data to a GSL matrix */
     146static void matrixPStoGSL(gsl_matrix *out, const psImage *in)
     147{
     148    psAssert(out->size1 == in->numRows && out->size2 == in->numCols, "Sizes don't match!");
     149
     150    int numCols = in->numCols, numRows = in->numRows; // Size of matrix
     151    switch (in->type.type) {
     152      case PS_TYPE_F32:
     153        for (int y = 0, i = 0; y < numRows; y++) {
     154            for (int x = 0; x < numCols; x++, i++) {
     155                out->data[i] = in->data.F32[y][x];
     156            }
     157        }
     158        break;
     159      case PS_TYPE_F64:
     160        if (in->parent|| out->tda != out->size1) {
     161            for (int y = 0, i = 0; y < numRows; y++, i += numCols) {
     162                memcpy(&out->data[i], in->data.F64[y], numCols * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
     163            }
    118164        } else {
    119             outGslVector->data[i] = inVector->data.F64[i];
    120         }
    121     }
    122 }
    123 
    124 /** Static function to copy GSL vector data to a psF32 or psF64 vector */
    125 static void gslVectorToPsVector(psVector *outVector,
    126                                 gsl_vector *inGslVector)
    127 {
    128     psU32 i = 0;
    129     psU32 n = 0;
    130 
    131 
    132     n = outVector->n;
    133     for(i=0; i<n; i++) {
    134         if(outVector->type.type == PS_TYPE_F32) {
    135             outVector->data.F32[i] = (psF32)inGslVector->data[i];
     165            memcpy(out->data, in->p_rawDataBuffer, numCols * numRows * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
     166        }
     167        break;
     168      default:
     169        psAbort("Unsupported vector type: %x\n", in->type.type);
     170    }
     171    return;
     172}
     173
     174/** Static function to copy GSL matrix data to a psF32 or psF64 image */
     175static void matrixGSLtoPS(psImage *out, const gsl_matrix *in)
     176{
     177    psAssert(in->size1 == out->numRows && in->size2 == out->numCols, "Sizes don't match!");
     178
     179    int numCols = out->numCols, numRows = out->numRows; // Size of matrix
     180    switch (out->type.type) {
     181      case PS_TYPE_F32:
     182        for (int y = 0, i = 0; y < numRows; y++) {
     183            for (int x = 0; x < numCols; x++, i++) {
     184                out->data.F32[y][x] = in->data[i];
     185            }
     186        }
     187        break;
     188      case PS_TYPE_F64:
     189        if (out->parent || in->tda != in->size1) {
     190            for (int y = 0, i = 0; y < numRows; y++, i += numCols) {
     191                memcpy(out->data.F64[y], &in->data[i], numCols * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
     192            }
    136193        } else {
    137             outVector->data.F64[i] = inGslVector->data[i];
    138         }
    139     }
    140 }
    141 
    142 /** Static function to copy psF32 or psF64 image data to a GSL matrix */
    143 static void  psImageToGslMatrix(gsl_matrix *outGslMatrix,
    144                                 const psImage *inImage)
    145 {
    146     psU32 i = 0;
    147     psU32 j = 0;
    148     psU32 numRows = 0;
    149     psU32 numCols = 0;
    150 
    151 
    152     numRows = inImage->numRows;
    153     numCols = inImage->numCols;
    154     if(inImage->type.type == PS_TYPE_F32) {
    155         for(i=0; i<numRows; i++) {
    156             for(j=0; j<numCols; j++) {
    157                 outGslMatrix->data[i*numCols+j] = inImage->data.F32[i][j];
    158             }
    159         }
    160     } else {
    161         for(i=0; i<numRows; i++) {
    162             for(j=0; j<numCols; j++) {
    163                 outGslMatrix->data[i*numCols+j] = inImage->data.F64[i][j];
    164             }
    165         }
    166     }
    167 }
    168 
    169 /** Static function to copy GSL matrix data to a psF32 or psF64 image */
    170 static void gslMatrixToPsImage(psImage *outImage,
    171                                gsl_matrix *inGslMatrix)
    172 {
    173     psU32 i = 0;
    174     psU32 j = 0;
    175     psU32 numRows = 0;
    176     psU32 numCols = 0;
    177 
    178 
    179     numRows = outImage->numRows;
    180     numCols = outImage->numCols;
    181     if(outImage->type.type == PS_TYPE_F32) {
    182         for(i=0; i<numRows; i++) {
    183             for(j=0; j<numCols; j++) {
    184                 outImage->data.F32[i][j] = inGslMatrix->data[i*numCols+j];
    185             }
    186         }
    187     } else {
    188         for(i=0; i<numRows; i++) {
    189             for(j=0; j<numCols; j++) {
    190                 outImage->data.F64[i][j] = inGslMatrix->data[i*numCols+j];
    191             }
    192         }
    193     }
     194            memcpy(out->p_rawDataBuffer, in->data, numCols * numRows * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
     195        }
     196        break;
     197      default:
     198        psAbort("Unsupported vector type: %x\n", out->type.type);
     199    }
     200    return;
    194201}
    195202
    … …  
    245252
    246253    // Copy psImage data into GSL matrix data
    247     psImageToGslMatrix(lu, in);
     254    matrixPStoGSL(lu, in);
    248255
    249256    // Calculate LU decomposition
    … …  
    251258
    252259    // Copy GSL matrix data to psImage data
    253     gslMatrixToPsImage(out, lu);
     260    matrixGSLtoPS(out, lu);
    254261
    255262    // Free GSL data
    … …  
    293300    // Initialize GSL data
    294301    lu = gsl_matrix_alloc(numRows, numCols);
    295     psImageToGslMatrix(lu, LU);
     302    matrixPStoGSL(lu, LU);
    296303    b = gsl_vector_alloc(RHS->n);
    297     psVectorToGslVector(b, RHS);
     304    vectorPStoGSL(b, RHS);
    298305    x = gsl_vector_alloc(RHS->n);
    299306
    … …  
    306313
    307314    // Copy GSL vector data to psVector data
    308     gslVectorToPsVector(out, x);
     315    vectorGSLtoPS(out, x);
    309316
    310317    // Free GSL data
    … …  
    341348    // Initialize GSL data
    342349    lu = gsl_matrix_alloc(numRows, numCols);
    343     psImageToGslMatrix(lu, LU);
     350    matrixPStoGSL(lu, LU);
    344351
    345352    permGSL.size = perm->n;
    … …  
    352359
    353360    // Copy GSL vector data to psVector data
    354     gslMatrixToPsImage(out, inverse);
     361    matrixGSLtoPS(out, inverse);
    355362
    356363    // Free GSL data
    … …  
    379386    switch (a->type.type) {
    380387      case PS_TYPE_F32: {
    381           psF32 **values = a->data.F32; /* Dereference */
    382           int numCols = a->numCols, numRows = a->numRows; /* Size of matrix */
    383           for (int i = 0; i < numRows; i++) {
    384               for (int j = 0; j < numCols; j++) {
    385                   if (!isfinite(values[i][j])) {
    386                       // psError(PS_ERR_BAD_PARAMETER_VALUE, 3,
    387                       // "Input matrix contains non-finite elements: matrix[%d][%d] is %.2f\n",
    388                       // i, j, values[i][j]);
    389                       return false;
    390                   }
    391               }
    392           }
    393           break;
     388          psF32 **values = a->data.F32; /* Dereference */
     389          int numCols = a->numCols, numRows = a->numRows; /* Size of matrix */
     390          for (int i = 0; i < numRows; i++) {
     391              for (int j = 0; j < numCols; j++) {
     392                  if (!isfinite(values[i][j])) {
     393                      // psError(PS_ERR_BAD_PARAMETER_VALUE, 3,
     394                      // "Input matrix contains non-finite elements: matrix[%d][%d] is %.2f\n",
     395                      // i, j, values[i][j]);
     396                      return false;
     397                  }
     398              }
     399          }
     400          break;
    394401      }
    395402      case PS_TYPE_F64: {
    396           psF64 **values = a->data.F64; /* Dereference */
    397           int numCols = a->numCols, numRows = a->numRows; /* Size of matrix */
    398           for (int i = 0; i < numRows; i++) {
    399               for (int j = 0; j < numCols; j++) {
    400                   if (!isfinite(values[i][j])) {
    401                       // psError(PS_ERR_BAD_PARAMETER_VALUE, 3,
    402                       // "Input matrix contains non-finite elements: matrix[%d][%d] is %.2f\n",
    403                       // i, j, values[i][j]);
    404                       return false;
    405                   }
    406               }
    407           }
    408           break;
     403          psF64 **values = a->data.F64; /* Dereference */
     404          int numCols = a->numCols, numRows = a->numRows; /* Size of matrix */
     405          for (int i = 0; i < numRows; i++) {
     406              for (int j = 0; j < numCols; j++) {
     407                  if (!isfinite(values[i][j])) {
     408                      // psError(PS_ERR_BAD_PARAMETER_VALUE, 3,
     409                      // "Input matrix contains non-finite elements: matrix[%d][%d] is %.2f\n",
     410                      // i, j, values[i][j]);
     411                      return false;
     412                  }
     413              }
     414          }
     415          break;
    409416      }
    410         // MATRIX_CHECK_NONFINITE_CASE(F32, a);
    411         // MATRIX_CHECK_NONFINITE_CASE(F64, a);
     417        // MATRIX_CHECK_NONFINITE_CASE(F32, a);
     418        // MATRIX_CHECK_NONFINITE_CASE(F64, a);
    412419      default:
    413         psAbort("Should never get here.");
     420        psAbort("Should never get here.");
    414421    }
    415422
    … …  
    471478    switch (a->type.type) {
    472479      case PS_TYPE_F32: {
    473           psF32 **values = a->data.F32; /* Dereference */
    474           int numCols = a->numCols, numRows = a->numRows; /* Size of matrix */
    475           for (int i = 0; i < numRows; i++) {
    476               for (int j = 0; j < numCols; j++) {
    477                   if (!isfinite(values[i][j])) {
    478                       return false;
    479                   }
    480               }
    481           }
    482           break;
     480          psF32 **values = a->data.F32; /* Dereference */
     481          int numCols = a->numCols, numRows = a->numRows; /* Size of matrix */
     482          for (int i = 0; i < numRows; i++) {
     483              for (int j = 0; j < numCols; j++) {
     484                  if (!isfinite(values[i][j])) {
     485                      return false;
     486                  }
     487              }
     488          }
     489          break;
    483490      }
    484491      case PS_TYPE_F64: {
    485           psF64 **values = a->data.F64; /* Dereference */
    486           int numCols = a->numCols, numRows = a->numRows; /* Size of matrix */
    487           for (int i = 0; i < numRows; i++) {
    488               for (int j = 0; j < numCols; j++) {
    489                   if (!isfinite(values[i][j])) {
    490                       return false;
    491                   }
    492               }
    493           }
    494           break;
     492          psF64 **values = a->data.F64; /* Dereference */
     493          int numCols = a->numCols, numRows = a->numRows; /* Size of matrix */
     494          for (int i = 0; i < numRows; i++) {
     495              for (int j = 0; j < numCols; j++) {
     496                  if (!isfinite(values[i][j])) {
     497                      return false;
     498                  }
     499              }
     500          }
     501          break;
    495502      }
    496503      default:
    497         psAbort("Should never get here.");
    498     }
    499  
     504        psAbort("Should never get here.");
     505    }
     506
    500507    // Following the algorithm laid out by Press et al., we loop along the matrix diagonal, but
    501508    // we do not operate on the diagonal elements in order.  Instead, we are looking for the
    … …  
    511518
    512519    if (a->type.type == PS_TYPE_F32) {
    513         psF32 **A = a->data.F32;
    514         psF32  *B = b->data.F32;
    515         int *colIndex = colIndexV->data.S32;
    516         int *rowIndex = rowIndexV->data.S32;
    517         int *pivot    = pivotV->data.S32;
    518         psF32 growth = 1.0;
    519 
    520         for (int diag = 0; diag < nSquare; diag++) {
    521 
    522             psF32 maxval = 0.0;
    523             int maxrow = 0;
    524             int maxcol = 0;
    525 
    526             // search for the next pivot
    527             for (int row = 0; row < nSquare; row++) {
    528                 if (!isfinite(A[row][diag])) goto escape;
    529 
    530                 // if we have already operated on this row (pivot[row] is true), skip it
    531                 if (pivot[row]) continue;
    532 
    533                 // if we have not yet operated on this row (pivot[row] is false), look for pivot for this row
    534                 for (int col = 0; col < nSquare; col++) {
    535                     if (pivot[col]) continue;
    536                     if (fabs (A[row][col]) < maxval) continue;
    537                     maxval = fabs (A[row][col]);
    538                     maxrow = row;
    539                     maxcol = col;
    540                 }
    541             }
    542 
    543             // if pivot[maxcol] is set, we have already done this row: this implies a singular matrix
    544             if (pivot[maxcol]) goto escape;
    545             pivot[maxcol] = 1;
    546 
    547             // if the selected pivot is off the diagonal, do a row swap
    548             if (maxrow != maxcol) {
    549                 for (int col = 0; col < nSquare; col++) PS_SWAP (A[maxrow][col], A[maxcol][col]);
    550                 PS_SWAP (B[maxrow], B[maxcol]);
    551             }
    552             rowIndex[diag] = maxrow;
    553             colIndex[diag] = maxcol;
    554             if (A[maxcol][maxcol] == 0.0) goto escape;
    555             // Kahan replaces the 0.0 pivot with epsilon*(largest element in column) + underFlow.
    556             // Here we are going to raise an error if the dynamic range is too large.
    557 
    558             /* rescale by pivot reciprocal */
    559             psF32 tmpval = 1.0 / A[maxcol][maxcol];
    560             A[maxcol][maxcol] = 1.0;
    561             for (int col = 0; col < nSquare; col++) A[maxcol][col] *= tmpval;
    562             B[maxcol] *= tmpval;
    563 
    564             // check for ill-conditioned matrix: measure the pivot growth and trigger on over/under flow
    565             growth *= tmpval;
    566             psTrace ("psLib.math", 4, "growth : %e\n", growth);
    567             if (fabs(growth) > MAX_RANGE) goto escape;
    568 
    569             /* adjust the elements above the pivot */
    570             for (int row = 0; row < nSquare; row++) {
    571                 if (row == maxcol) continue;
    572                 tmpval = A[row][maxcol];
    573                 A[row][maxcol] = 0.0;
    574                 for (int col = 0; col < nSquare; col++) A[row][col] -= A[maxcol][col]*tmpval;
    575                 B[row] -= B[maxcol]*tmpval;
    576             }
    577         }
    578 
    579         // swap back the inverse matrix based on the row swaps above
    580         for (int col = nSquare - 1; col >= 0; col--) {
    581             if (rowIndex[col] != colIndex[col]) {
    582                 for (int row = 0; row < nSquare; row++) PS_SWAP (A[row][rowIndex[col]], A[row][colIndex[col]]);
    583             }
    584         }
     520        psF32 **A = a->data.F32;
     521        psF32  *B = b->data.F32;
     522        int *colIndex = colIndexV->data.S32;
     523        int *rowIndex = rowIndexV->data.S32;
     524        int *pivot    = pivotV->data.S32;
     525        psF32 growth = 1.0;
     526
     527        for (int diag = 0; diag < nSquare; diag++) {
     528
     529            psF32 maxval = 0.0;
     530            int maxrow = 0;
     531            int maxcol = 0;
     532
     533            // search for the next pivot
     534            for (int row = 0; row < nSquare; row++) {
     535                if (!isfinite(A[row][diag])) goto escape;
     536
     537                // if we have already operated on this row (pivot[row] is true), skip it
     538                if (pivot[row]) continue;
     539
     540                // if we have not yet operated on this row (pivot[row] is false), look for pivot for this row
     541                for (int col = 0; col < nSquare; col++) {
     542                    if (pivot[col]) continue;
     543                    if (fabs (A[row][col]) < maxval) continue;
     544                    maxval = fabs (A[row][col]);
     545                    maxrow = row;
     546                    maxcol = col;
     547                }
     548            }
     549
     550            // if pivot[maxcol] is set, we have already done this row: this implies a singular matrix
     551            if (pivot[maxcol]) goto escape;
     552            pivot[maxcol] = 1;
     553
     554            // if the selected pivot is off the diagonal, do a row swap
     555            if (maxrow != maxcol) {
     556                for (int col = 0; col < nSquare; col++) PS_SWAP (A[maxrow][col], A[maxcol][col]);
     557                PS_SWAP (B[maxrow], B[maxcol]);
     558            }
     559            rowIndex[diag] = maxrow;
     560            colIndex[diag] = maxcol;
     561            if (A[maxcol][maxcol] == 0.0) goto escape;
     562            // Kahan replaces the 0.0 pivot with epsilon*(largest element in column) + underFlow.
     563            // Here we are going to raise an error if the dynamic range is too large.
     564
     565            /* rescale by pivot reciprocal */
     566            psF32 tmpval = 1.0 / A[maxcol][maxcol];
     567            A[maxcol][maxcol] = 1.0;
     568            for (int col = 0; col < nSquare; col++) A[maxcol][col] *= tmpval;
     569            B[maxcol] *= tmpval;
     570
     571            // check for ill-conditioned matrix: measure the pivot growth and trigger on over/under flow
     572            growth *= tmpval;
     573            psTrace ("psLib.math", 4, "growth : %e\n", growth);
     574            if (fabs(growth) > MAX_RANGE) goto escape;
     575
     576            /* adjust the elements above the pivot */
     577            for (int row = 0; row < nSquare; row++) {
     578                if (row == maxcol) continue;
     579                tmpval = A[row][maxcol];
     580                A[row][maxcol] = 0.0;
     581                for (int col = 0; col < nSquare; col++) A[row][col] -= A[maxcol][col]*tmpval;
     582                B[row] -= B[maxcol]*tmpval;
     583            }
     584        }
     585
     586        // swap back the inverse matrix based on the row swaps above
     587        for (int col = nSquare - 1; col >= 0; col--) {
     588            if (rowIndex[col] != colIndex[col]) {
     589                for (int row = 0; row < nSquare; row++) PS_SWAP (A[row][rowIndex[col]], A[row][colIndex[col]]);
     590            }
     591        }
    585592    } else {
    586         psF64 **A = a->data.F64;
    587         psF64  *B = b->data.F64;
    588         int *colIndex = colIndexV->data.S32;
    589         int *rowIndex = rowIndexV->data.S32;
    590         int *pivot    = pivotV->data.S32;
    591         psF64 growth = 1.0;
    592 
    593         for (int diag = 0; diag < nSquare; diag++) {
    594 
    595             psF64 maxval = 0.0;
    596             int maxrow = 0;
    597             int maxcol = 0;
    598 
    599             // search for the next pivot
    600             for (int row = 0; row < nSquare; row++) {
    601                 if (!isfinite(A[row][diag])) goto escape;
    602 
    603                 // if we have already operated on this row (pivot[row] is true), skip it
    604                 if (pivot[row]) continue;
    605 
    606                 // if we have not yet operated on this row (pivot[row] is false), look for pivot for this row
    607                 for (int col = 0; col < nSquare; col++) {
    608                     if (pivot[col]) continue;
    609                     if (fabs (A[row][col]) < maxval) continue;
    610                     maxval = fabs (A[row][col]);
    611                     maxrow = row;
    612                     maxcol = col;
    613                 }
    614             }
    615 
    616             // if pivot[maxcol] is set, we have already done this row: this implies a singular matrix
    617             if (pivot[maxcol]) goto escape;
    618             pivot[maxcol] = 1;
    619 
    620             // if the selected pivot is off the diagonal, do a row swap
    621             if (maxrow != maxcol) {
    622                 for (int col = 0; col < nSquare; col++) PS_SWAP (A[maxrow][col], A[maxcol][col]);
    623                 PS_SWAP (B[maxrow], B[maxcol]);
    624             }
    625             rowIndex[diag] = maxrow;
    626             colIndex[diag] = maxcol;
    627             if (A[maxcol][maxcol] == 0.0) goto escape;
    628             // Kahan replaces the 0.0 pivot with epsilon*(largest element in column) + underFlow.
    629             // Here we are going to raise an error if the dynamic range is too large.
    630 
    631             /* rescale by pivot reciprocal */
    632             psF64 tmpval = 1.0 / A[maxcol][maxcol];
    633             A[maxcol][maxcol] = 1.0;
    634             for (int col = 0; col < nSquare; col++) A[maxcol][col] *= tmpval;
    635             B[maxcol] *= tmpval;
    636 
    637             // check for ill-conditioned matrix: measure the pivot growth and trigger on over/under flow
    638             growth *= tmpval;
    639             psTrace ("psLib.math", 4, "growth : %e\n", growth);
    640             if (fabs(growth) > MAX_RANGE) goto escape;
    641 
    642             /* adjust the elements above the pivot */
    643             for (int row = 0; row < nSquare; row++) {
    644                 if (row == maxcol) continue;
    645                 tmpval = A[row][maxcol];
    646                 A[row][maxcol] = 0.0;
    647                 for (int col = 0; col < nSquare; col++) A[row][col] -= A[maxcol][col]*tmpval;
    648                 B[row] -= B[maxcol]*tmpval;
    649             }
    650         }
    651 
    652         // swap back the inverse matrix based on the row swaps above
    653         for (int col = nSquare - 1; col >= 0; col--) {
    654             if (rowIndex[col] != colIndex[col]) {
    655                 for (int row = 0; row < nSquare; row++) PS_SWAP (A[row][rowIndex[col]], A[row][colIndex[col]]);
    656             }
    657         }
     593        psF64 **A = a->data.F64;
     594        psF64  *B = b->data.F64;
     595        int *colIndex = colIndexV->data.S32;
     596        int *rowIndex = rowIndexV->data.S32;
     597        int *pivot    = pivotV->data.S32;
     598        psF64 growth = 1.0;
     599
     600        for (int diag = 0; diag < nSquare; diag++) {
     601
     602            psF64 maxval = 0.0;
     603            int maxrow = 0;
     604            int maxcol = 0;
     605
     606            // search for the next pivot
     607            for (int row = 0; row < nSquare; row++) {
     608                if (!isfinite(A[row][diag])) goto escape;
     609
     610                // if we have already operated on this row (pivot[row] is true), skip it
     611                if (pivot[row]) continue;
     612
     613                // if we have not yet operated on this row (pivot[row] is false), look for pivot for this row
     614                for (int col = 0; col < nSquare; col++) {
     615                    if (pivot[col]) continue;
     616                    if (fabs (A[row][col]) < maxval) continue;
     617                    maxval = fabs (A[row][col]);
     618                    maxrow = row;
     619                    maxcol = col;
     620                }
     621            }
     622
     623            // if pivot[maxcol] is set, we have already done this row: this implies a singular matrix
     624            if (pivot[maxcol]) goto escape;
     625            pivot[maxcol] = 1;
     626
     627            // if the selected pivot is off the diagonal, do a row swap
     628            if (maxrow != maxcol) {
     629                for (int col = 0; col < nSquare; col++) PS_SWAP (A[maxrow][col], A[maxcol][col]);
     630                PS_SWAP (B[maxrow], B[maxcol]);
     631            }
     632            rowIndex[diag] = maxrow;
     633            colIndex[diag] = maxcol;
     634            if (A[maxcol][maxcol] == 0.0) goto escape;
     635            // Kahan replaces the 0.0 pivot with epsilon*(largest element in column) + underFlow.
     636            // Here we are going to raise an error if the dynamic range is too large.
     637
     638            /* rescale by pivot reciprocal */
     639            psF64 tmpval = 1.0 / A[maxcol][maxcol];
     640            A[maxcol][maxcol] = 1.0;
     641            for (int col = 0; col < nSquare; col++) A[maxcol][col] *= tmpval;
     642            B[maxcol] *= tmpval;
     643
     644            // check for ill-conditioned matrix: measure the pivot growth and trigger on over/under flow
     645            growth *= tmpval;
     646            psTrace ("psLib.math", 4, "growth : %e\n", growth);
     647            if (fabs(growth) > MAX_RANGE) goto escape;
     648
     649            /* adjust the elements above the pivot */
     650            for (int row = 0; row < nSquare; row++) {
     651                if (row == maxcol) continue;
     652                tmpval = A[row][maxcol];
     653                A[row][maxcol] = 0.0;
     654                for (int col = 0; col < nSquare; col++) A[row][col] -= A[maxcol][col]*tmpval;
     655                B[row] -= B[maxcol]*tmpval;
     656            }
     657        }
     658
     659        // swap back the inverse matrix based on the row swaps above
     660        for (int col = nSquare - 1; col >= 0; col--) {
     661            if (rowIndex[col] != colIndex[col]) {
     662                for (int row = 0; row < nSquare; row++) PS_SWAP (A[row][rowIndex[col]], A[row][colIndex[col]]);
     663            }
     664        }
    658665    }
    659666
    … …  
    701708    lu = gsl_matrix_alloc(numRows, numCols);
    702709    inv = gsl_matrix_alloc(numRows, numCols);
    703     psImageToGslMatrix(lu, in);
     710    matrixPStoGSL(lu, in);
    704711
    705712    // Invert data and calculate determinant
    … …  
    708715    if (determinant) {
    709716      // XXX this is getting the wrong value: is it the wrong calculation?
    710       // it disagrees with the results of 
     717      // it disagrees with the results of
    711718      // det = (psF32)gsl_linalg_LU_det(lu, signum);
    712719      // used in psMatrixDeterminatn
    … …  
    716723
    717724    // Copy GSL matrix data to psImage data
    718     gslMatrixToPsImage(out, inv);
     725    matrixGSLtoPS(out, inv);
    719726
    720727    // Free GSL structs
    … …  
    749756    perm = gsl_permutation_alloc(numRows);
    750757    lu = gsl_matrix_alloc(numRows, numCols);
    751     psImageToGslMatrix(lu, in);
     758    matrixPStoGSL(lu, in);
    752759
    753760    // Calculate determinant
    754761    gsl_linalg_LU_decomp(lu, perm, &signum);
    755     det = (psF32)gsl_linalg_LU_det(lu, signum);
     762    det = (psF32)gsl_linalg_LU_lndet(lu);
    756763
    757764    // Free GSL structs
    … …  
    877884
    878885    inGSL = gsl_matrix_alloc(numRows, numCols);
    879     psImageToGslMatrix(inGSL, in);
     886    matrixPStoGSL(inGSL, in);
    880887    outGSL = gsl_matrix_alloc(numRows, numCols);
    881888
    … …  
    892899
    893900    // Copy GSL matrix data to psImage data
    894     gslMatrixToPsImage(out, outGSL);
     901    matrixGSLtoPS(out, outGSL);
    895902
    896903    // Free GSL structs
    … …  
    10261033}
    10271034
     1035psVector *psMatrixSolveSVD(psVector *out, const psImage *matrix, const psVector *vector)
     1036{
     1037    #define psMatrixSolveSVD_EXIT {psFree(out); return NULL; }
     1038    PS_ASSERT_GENERAL_IMAGE_NON_NULL(matrix, psMatrixSolveSVD_EXIT);
     1039    PS_CHECK_DIMEN_AND_TYPE(matrix, PS_DIMEN_IMAGE, psMatrixSolveSVD_EXIT);
     1040    PS_ASSERT_GENERAL_VECTOR_NON_NULL(vector, psMatrixSolveSVD_EXIT);
     1041    PS_CHECK_DIMEN_AND_TYPE(vector, PS_DIMEN_VECTOR, psMatrixSolveSVD_EXIT);
     1042
     1043    int numCols = matrix->numCols, numRows = matrix->numRows; // Size of matrix
     1044
     1045    // Decompose matrix: A = U S V^T
     1046    gsl_matrix *A = gsl_matrix_alloc(numRows, numCols); // Input matrix in GSL-speak; becomes matrix U
     1047    gsl_matrix *V = gsl_matrix_alloc(numCols, numCols); // Untransposed matrix V
     1048    gsl_vector *S = gsl_vector_alloc(numCols);          // Singular values
     1049    gsl_vector *work = gsl_vector_alloc(numCols);       // Work space for GSL
     1050
     1051    matrixPStoGSL(A, matrix);
     1052
     1053    int gslStatus = 0;                  // Status of GSL
     1054    if ((gslStatus = gsl_linalg_SV_decomp(A, V, S, work))) {
     1055        const char *err = gsl_strerror(gslStatus);
     1056        psError(PS_ERR_UNKNOWN, true, "Unable to decompose matrix: %s", err);
     1057        gsl_matrix_free(A);
     1058        gsl_matrix_free(V);
     1059        gsl_vector_free(S);
     1060        gsl_vector_free(work);
     1061        return NULL;
     1062    }
     1063    gsl_vector_free(work);
     1064
     1065    // Solve system (or minimise least-squares if overconstrained): Ax = b
     1066    gsl_vector *b = gsl_vector_alloc(numCols); // Vector b
     1067    gsl_vector *x = gsl_vector_alloc(numCols); // Solution
     1068
     1069    vectorPStoGSL(b, vector);
     1070
     1071    if ((gslStatus = gsl_linalg_SV_solve(A, V, S, b, x))) {
     1072        const char *err = gsl_strerror(gslStatus);
     1073        psError(PS_ERR_UNKNOWN, true, "Unable to solve matrix equation: %s", err);
     1074        gsl_matrix_free(A);
     1075        gsl_matrix_free(V);
     1076        gsl_vector_free(S);
     1077        gsl_vector_free(b);
     1078        gsl_vector_free(x);
     1079        return NULL;
     1080    }
     1081
     1082    gsl_matrix_free(A);
     1083    gsl_matrix_free(V);
     1084    gsl_vector_free(S);
     1085    gsl_vector_free(b);
     1086
     1087    out = psVectorRecycle(out, numCols, PS_TYPE_F64);
     1088
     1089    vectorGSLtoPS(out, x);
     1090    gsl_vector_free(x);
     1091
     1092    return out;
     1093}
     1094
     1095
     1096
    10281097// This code supplied by Andy Becker (becker@astro.washington.edu)
    10291098psImage *psMatrixSVD(psImage* evec, psVector* eval, const psImage* in)
    … …  
    10501119
    10511120    // Copy psImage data into GSL matrix data
    1052     psImageToGslMatrix(A, in);
     1121    matrixPStoGSL(A, in);
    10531122
    10541123    // Calculate SVD decomposition
    … …  
    10561125
    10571126    // Copy GSL matrix data to psImage data
    1058     gslMatrixToPsImage(evec, V);
    1059     gslVectorToPsVector(eval, S);
     1127    matrixGSLtoPS(evec, V);
     1128    vectorGSLtoPS(eval, S);
    10601129
    10611130    // Take the square root of eval

  • trunk/psLib/src/math/psMatrix.h

    r24084 r26001  
    6666 */
    6767psImage *psMatrixLUInvert(
    68     psImage *out,                  ///< place result here if not NULL
    69     const psImage* LU,             ///< LU-decomposed matrix.
    70     const psVector* perm           ///< Permutation vector resulting from psMatrixLUD function.
     68    psImage *out,                  ///< place result here if not NULL
     69    const psImage* LU,             ///< LU-decomposed matrix.
     70    const psVector* perm           ///< Permutation vector resulting from psMatrixLUD function.
    7171);
    7272
    … …  
    186186);
    187187
     188/// Solve a matrix equation using Singular Value Decomposition
     189///
     190/// Solves Ax = b for x
     191psVector *psMatrixSolveSVD(
     192    psVector *solution,                 ///< Solution to output, or NULL
     193    const psImage *matrix,              ///< Matrix to be solved
     194    const psVector *vector              ///< Vector of values
     195    );
     196
     197
    188198/// Single value decomposition, provided by Andy Becker
    189199psImage *psMatrixSVD(psImage* evec, psVector* eval, const psImage* in);
Note: See TracChangeset for help on using the changeset viewer.