00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifdef HAVE_CONFIG_H
00029 #include <config.h>
00030 #endif
00031
00032
00033
00034
00035
00036 #include <math.h>
00037 #include <assert.h>
00038
00039 #include "xsh_fit.h"
00040
00041
00047
00050
00051
00052
00053
00054 static double irplib_tools_ipow(double, int);
00055
00056 static cpl_vector * irplib_vector_transform_mean(const cpl_vector *, double *);
00057
00058 static cpl_matrix * irplib_matrix_product_normal_create(const cpl_matrix *);
00059
00060 static cpl_error_code irplib_matrix_product_transpose(cpl_matrix *,
00061 const cpl_matrix *,
00062 const cpl_matrix *);
00063
00064 static cpl_error_code irplib_matrix_solve_chol_transpose(const cpl_matrix *,
00065 cpl_matrix *);
00066
00067 static void irplib_fit_imagelist_polynomial_double(cpl_imagelist *,
00068 const cpl_matrix *,
00069 const cpl_matrix *,
00070 const cpl_vector *,
00071 const cpl_imagelist *,
00072 const cpl_vector *,
00073 double, int, int,
00074 cpl_image *);
00075
00076 static void irplib_fit_imagelist_polynomial_float(cpl_imagelist *,
00077 const cpl_matrix *,
00078 const cpl_matrix *,
00079 const cpl_vector *,
00080 const cpl_imagelist *,
00081 const cpl_vector *,
00082 double, int, int,
00083 cpl_image *);
00084
00085 static void irplib_fit_imagelist_polynomial_int(cpl_imagelist *,
00086 const cpl_matrix *,
00087 const cpl_matrix *,
00088 const cpl_vector *,
00089 const cpl_imagelist *,
00090 const cpl_vector *,
00091 double, int, int,
00092 cpl_image *);
00093
00094 static void irplib_fit_imagelist_residual_double(cpl_image *, int,
00095 const cpl_vector *,
00096 const cpl_vector *,
00097 const cpl_matrix *,
00098 const cpl_matrix *);
00099
00100 static void irplib_fit_imagelist_residual_float(cpl_image *, int,
00101 const cpl_vector *,
00102 const cpl_vector *,
00103 const cpl_matrix *,
00104 const cpl_matrix *);
00105
00106 static void irplib_fit_imagelist_residual_int(cpl_image *, int,
00107 const cpl_vector *,
00108 const cpl_vector *,
00109 const cpl_matrix *,
00110 const cpl_matrix *);
00111
00112 static void irplib_polynomial_shift_double(double *, int, double);
00113
00114
00115
00170
00171 cpl_imagelist * xsh_fit_imagelist_polynomial(const cpl_vector * x_pos,
00172 const cpl_imagelist * values,
00173 int mindeg,
00174 int maxdeg,
00175 cpl_boolean is_eqdist,
00176 cpl_image * fiterror)
00177 {
00178
00179 cpl_imagelist * self = NULL;
00180 cpl_matrix * mv;
00181 cpl_matrix * mh;
00182
00183 cpl_vector * xhat;
00184
00185 const cpl_image * value = cpl_imagelist_get_const(values, 0);
00186 const double * dx;
00187 double xmean;
00188
00189 cpl_error_code error;
00190
00191
00192 const int nc = 1 + maxdeg - mindeg;
00193 const int np = cpl_vector_get_size(x_pos);
00194 const int nx = cpl_image_get_size_x(value);
00195 const int ny = cpl_image_get_size_y(value);
00196
00197 const cpl_boolean is_eqzero = is_eqdist && mindeg == 0;
00198
00199 int i, j, k;
00200
00201
00202 cpl_ensure(x_pos != NULL, CPL_ERROR_NULL_INPUT, NULL);
00203 cpl_ensure(values != NULL, CPL_ERROR_NULL_INPUT, NULL);
00204
00205 cpl_ensure(mindeg >= 0, CPL_ERROR_ILLEGAL_INPUT, NULL);
00206 cpl_ensure(maxdeg >= mindeg, CPL_ERROR_ILLEGAL_INPUT, NULL);
00207
00208 assert( nc > 0);
00209
00210 cpl_ensure(np == cpl_imagelist_get_size(values),
00211 CPL_ERROR_INCOMPATIBLE_INPUT, NULL);
00212
00213 cpl_ensure(cpl_imagelist_is_uniform(values)==0,
00214 CPL_ERROR_INCOMPATIBLE_INPUT, NULL);
00215
00216 if (fiterror != NULL) {
00217 cpl_ensure(cpl_image_get_size_x(fiterror) == nx &&
00218 cpl_image_get_size_y(fiterror) == ny,
00219 CPL_ERROR_INCOMPATIBLE_INPUT, NULL);
00220 }
00221
00222 if (mindeg == 0) {
00223
00224 xhat = irplib_vector_transform_mean(x_pos, &xmean);
00225 assert( xhat != NULL );
00226
00227
00228 if (is_eqdist && (np & 1)) cpl_vector_set(xhat, np>>1, 0.0);
00229
00230 } else {
00231 xhat = (cpl_vector*)x_pos;
00232 xmean = 0.0;
00233 }
00234
00235 dx = cpl_vector_get_data(xhat);
00236
00237
00238 mv = cpl_matrix_wrap(nc, np,
00239 cpl_malloc(nc * np * sizeof(double)));
00240
00241
00242 for (j=0; j < np; j++) {
00243 double f_prod = irplib_tools_ipow(dx[j], mindeg);
00244 cpl_matrix_set(mv, 0, j, f_prod);
00245 for (k=1; k < nc; k++) {
00246 f_prod *= dx[j];
00247 cpl_matrix_set(mv, k, j, f_prod);
00248 }
00249 }
00250
00251 if (xhat != x_pos) cpl_vector_delete(xhat);
00252
00253
00254
00255
00256
00257
00258
00259
00260 mh = irplib_matrix_product_normal_create(mv);
00261
00262 if (is_eqzero) {
00263
00264
00265
00266
00267 double * dmh = cpl_matrix_get_data(mh);
00268
00269 for (i = 0; i < nc; i++) {
00270 for (j = i + 1; j < nc; j += 2) {
00271 dmh[nc * i + j] = 0.0;
00272 }
00273 }
00274 }
00275
00276 error = cpl_matrix_decomp_chol(mh);
00277
00278 if (!error) {
00279
00280 cpl_vector * xpow = NULL;
00281
00282
00283
00284
00285 self = cpl_imagelist_new();
00286 for (i=0; i < nc; i++) {
00287 cpl_imagelist_set(self, cpl_image_new(nx, ny, CPL_TYPE_DOUBLE),
00288 i);
00289 }
00290
00291 if (mindeg > 0) {
00292 const double * d_pos = cpl_vector_get_data_const(x_pos);
00293 double * ppow = cpl_malloc(np * sizeof(double));
00294
00295 xpow = cpl_vector_wrap(np, ppow);
00296
00297 for (i = 0; i < np; i++) {
00298 ppow[i] = irplib_tools_ipow(d_pos[i], mindeg);
00299 }
00300 }
00301
00302 switch (cpl_image_get_type(value)) {
00303 case CPL_TYPE_DOUBLE:
00304 irplib_fit_imagelist_polynomial_double(self, mh, mv, x_pos, values,
00305 xpow, -xmean, np, nc,
00306 fiterror);
00307 break;
00308 case CPL_TYPE_FLOAT:
00309 irplib_fit_imagelist_polynomial_float(self, mh, mv, x_pos, values,
00310 xpow, -xmean, np, nc,
00311 fiterror);
00312 break;
00313 case CPL_TYPE_INT:
00314 irplib_fit_imagelist_polynomial_int(self, mh, mv, x_pos, values,
00315 xpow, -xmean, np, nc,
00316 fiterror);
00317 break;
00318 default:
00319
00320 assert( 0 );
00321 }
00322
00323 cpl_vector_delete(xpow);
00324
00325 }
00326
00327 cpl_matrix_delete(mh);
00328 cpl_matrix_delete(mv);
00329
00330
00331 cpl_ensure(!error, error, NULL);
00332
00333 return self;
00334
00335 }
00336
00337
00347
00348 static double irplib_tools_ipow(double x, int p)
00349 {
00350
00351 double result;
00352 double pow2 = x;
00353
00354
00355
00356
00357
00358
00359
00360 result = p & 1 ? x : 1.0;
00361
00362 while (p >>= 1) {
00363 pow2 *= pow2;
00364
00365 if (p & 1) result *= pow2;
00366 }
00367
00368 return result;
00369 }
00370
00371
00383
00384 static cpl_error_code irplib_matrix_product_transpose(cpl_matrix * self,
00385 const cpl_matrix * ma,
00386 const cpl_matrix * mb)
00387 {
00388
00389 double sum;
00390
00391 double * ds = cpl_matrix_get_data(self);
00392 const double * d1 = cpl_matrix_get_data_const( ma);
00393 const double * d2 = cpl_matrix_get_data_const( mb);
00394 const double * di;
00395
00396 const int nr = cpl_matrix_get_nrow(ma);
00397 const int nc = cpl_matrix_get_nrow(mb);
00398 const int nk = cpl_matrix_get_ncol(mb);
00399 int i, j, k;
00400
00401
00402 cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
00403 cpl_ensure_code(ma != NULL, CPL_ERROR_NULL_INPUT);
00404 cpl_ensure_code(mb != NULL, CPL_ERROR_NULL_INPUT);
00405
00406 cpl_ensure_code(cpl_matrix_get_ncol(ma) == nk,
00407 CPL_ERROR_INCOMPATIBLE_INPUT);
00408
00409 if (cpl_matrix_get_nrow(self) != nr || cpl_matrix_get_ncol(self) != nc)
00410 cpl_matrix_set_size(self, nr, nc);
00411
00412 for (i = 0; i < nr; i++, d1 += nk) {
00413
00414
00415
00416 di = d2;
00417 for (j = 0; j < nc; j++, di += nk) {
00418 sum = 0.0;
00419 for (k = 0; k < nk; k++) {
00420 sum += d1[k] * di[k];
00421 }
00422 ds[nc * i + j] = sum;
00423 }
00424 }
00425
00426 return CPL_ERROR_NONE;
00427
00428 }
00429
00430
00444
00445 static void irplib_polynomial_shift_double(double * coeffs, int n, double u)
00446 {
00447
00448 int i, j;
00449
00450
00451 assert( coeffs );
00452
00453 assert( n > 0 );
00454
00455 for (j = 0; j < n-1; j++)
00456 for (i = 1; i < n - j; i++ )
00457 coeffs[n-1-i] += coeffs[n-i] * u;
00458
00459 }
00460
00461
00473
00474 static cpl_vector * irplib_vector_transform_mean(const cpl_vector * x,
00475 double * pm)
00476 {
00477
00478 cpl_vector * xhat = cpl_vector_duplicate(x);
00479
00480
00481 assert( xhat != NULL );
00482 assert( pm != NULL );
00483
00484 *pm = cpl_vector_get_mean(xhat);
00485 cpl_vector_subtract_scalar(xhat, *pm);
00486
00487 return xhat;
00488
00489 }
00490
00491
00519 static cpl_matrix * irplib_matrix_product_normal_create(const cpl_matrix * self)
00520 {
00521
00522 cpl_matrix * product;
00523 const double * ai = cpl_matrix_get_data_const(self);
00524 const double * aj;
00525 double * bwrite;
00526 double sum;
00527 const int m = cpl_matrix_get_nrow(self);
00528 const int n = cpl_matrix_get_ncol(self);
00529 int i, j, k;
00530
00531
00532 cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, NULL);
00533
00534 bwrite = (double *) cpl_malloc(m * m * sizeof(double));
00535
00536 product = cpl_matrix_wrap(m, m, bwrite);
00537
00538
00539 for (i = 0; i < m; i++, ai += n, bwrite += m) {
00540 aj = ai;
00541 for (j = i; j < m; j++, aj += n) {
00542 sum = 0.0;
00543 for (k = 0; k < n; k++) {
00544 sum += ai[k] * aj[k];
00545 }
00546 bwrite[j] = sum;
00547 }
00548 }
00549
00550 return product;
00551
00552 }
00553
00554
00555
00556
00598
00599 static cpl_error_code
00600 irplib_matrix_solve_chol_transpose(const cpl_matrix * self,
00601 cpl_matrix * rhs)
00602 {
00603
00604 int n, i, j, k;
00605 int nrhs;
00606 const double * aread;
00607 const double * ai;
00608 double * bk;
00609 double sub;
00610
00611 cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
00612 cpl_ensure_code(rhs != NULL, CPL_ERROR_NULL_INPUT);
00613
00614 n = cpl_matrix_get_ncol(self);
00615
00616 cpl_ensure_code(cpl_matrix_get_nrow(self) == n, CPL_ERROR_ILLEGAL_INPUT);
00617 cpl_ensure_code(cpl_matrix_get_ncol(rhs) == n, CPL_ERROR_INCOMPATIBLE_INPUT);
00618
00619 nrhs = cpl_matrix_get_nrow(rhs);
00620
00621 aread = cpl_matrix_get_data_const(self);
00622
00623
00624 bk = cpl_matrix_get_data(rhs);
00625
00626 for (k=0; k < nrhs; k++, bk += n) {
00627
00628
00629
00630
00631
00632 ai = aread;
00633 for (i = 0; i < n; i++, ai += n) {
00634 sub = 0.0;
00635 for (j = 0; j < i; j++) {
00636 sub += ai[j] * bk[j];
00637 }
00638 cpl_ensure_code(k > 0 || ai[j] != 0.0, CPL_ERROR_DIVISION_BY_ZERO);
00639 bk[j] = (bk[j] - sub) / ai[j];
00640 }
00641
00642
00643
00644 for (i = n-1; i >= 0; i--) {
00645 sub = bk[i];
00646 for (j = i+1; j < n; j++) {
00647 sub -= aread[n * j + i] * bk[j];
00648 }
00649 bk[i] = sub/aread[n * i + i];
00650 }
00651 }
00652
00653 return CPL_ERROR_NONE;
00654
00655 }
00656
00657
00658
00694
00695 cpl_error_code
00696 xsh_image_find_barycenter(
00697 const cpl_image * im,
00698 int xpos,
00699 int ypos,
00700 int size,
00701 double * norm,
00702 double * xcen,
00703 double * ycen,
00704 double * sig_x,
00705 double * sig_y,
00706 double * fwhm_x,
00707 double * fwhm_y)
00708 {
00709 cpl_image * extracted ;
00710 int is_rejected;
00711 int llx, lly, urx, ury ;
00712 double u0, ux, uy, uxx, uyy ;
00713 double cenx, ceny;
00714 const double * pi ;
00715 int pos ;
00716 double max_val ;
00717 int i, j ;
00718 int nx=0;
00719 int ny=0;
00720 int enx=0;
00721 int eny=0;
00722
00723
00724 cpl_ensure_code(im, CPL_ERROR_NULL_INPUT) ;
00725
00726 nx=cpl_image_get_size_x(im);
00727 ny=cpl_image_get_size_y(im);
00728
00729 cpl_ensure_code(xpos>=1 && xpos<=nx, CPL_ERROR_ILLEGAL_INPUT);
00730 cpl_ensure_code(ypos>=1 && ypos<=ny, CPL_ERROR_ILLEGAL_INPUT);
00731
00732 cpl_ensure_code(size>1 && size<nx && size<ny,
00733 CPL_ERROR_ILLEGAL_INPUT) ;
00734
00735
00736 llx = xpos - (int)(size/2) ;
00737 lly = ypos - (int)(size/2) ;
00738 urx = xpos + (int)(size/2) ;
00739 ury = ypos + (int)(size/2) ;
00740 if (llx < 1) llx = 1 ;
00741 if (lly < 1) lly = 1 ;
00742 if (urx > nx) urx = nx ;
00743 if (ury > ny) ury = ny ;
00744
00745
00746 extracted = cpl_image_extract(im, llx, lly, urx, ury) ;
00747 cpl_ensure_code(extracted, CPL_ERROR_ILLEGAL_INPUT) ;
00748
00749
00750 if (5 * cpl_image_count_rejected(extracted) >
00751 cpl_image_get_size_x(extracted) * cpl_image_get_size_y(extracted)) {
00752 cpl_image_delete(extracted) ;
00753 cpl_ensure_code(0, CPL_ERROR_ILLEGAL_INPUT) ;
00754 }
00755
00756 if (cpl_image_get_type(extracted) != CPL_TYPE_DOUBLE) {
00757
00758 cpl_image * tmp = extracted;
00759 extracted = cpl_image_cast(tmp, CPL_TYPE_DOUBLE);
00760 cpl_image_delete(tmp);
00761 cpl_ensure_code(extracted, CPL_ERROR_TYPE_MISMATCH);
00762 }
00763 pi = cpl_image_get_data_double(extracted);
00764
00765
00766 enx=cpl_image_get_size_x(extracted);
00767 eny=cpl_image_get_size_y(extracted);
00768
00769 u0 = ux = uy = 0.0 ;
00770 for (j=0 ; j<eny ; j++) {
00771 for (i=0 ; i<enx ; i++) {
00772 if (!cpl_image_is_rejected(extracted, i+1, j+1)) {
00773 pos = i + j * enx ;
00774 u0 += pi[pos] ;
00775 ux += (i+1) * pi[pos] ;
00776 uy += (j+1) * pi[pos] ;
00777 }
00778 }
00779 }
00780
00781
00782
00783 if (u0 == 0 || u0 > ux || ux > u0*enx ||
00784 u0 > uy || uy > u0*eny) {
00785 cpl_image_delete(extracted) ;
00786 cpl_ensure_code(0, CPL_ERROR_ILLEGAL_INPUT) ;
00787 }
00788
00789 cenx = ux/u0;
00790 ceny = uy/u0;
00791
00792
00793 uxx = uyy = 0.0 ;
00794 for (j=0 ; j<eny ; j++) {
00795 for (i=0 ; i<enx ; i++) {
00796 if (!cpl_image_is_rejected(extracted, i+1, j+1)) {
00797 pos = i + j * enx ;
00798 uxx += ((i+1)-cenx) * ((i+1)-cenx) * pi[pos] ;
00799 uyy += ((j+1)-ceny) * ((j+1)-ceny) * pi[pos] ;
00800 }
00801 }
00802 }
00803 if (sig_x) *sig_x = sqrt(fabs(uxx/u0)) ;
00804 if (sig_y) *sig_y = sqrt(fabs(uyy/u0)) ;
00805 if (fwhm_x) *fwhm_x = 2 * sqrt(2 * log(2.0)) * sqrt(fabs(uxx/u0)) ;
00806 if (fwhm_y) *fwhm_y = 2 * sqrt(2 * log(2.0)) * sqrt(fabs(uyy/u0)) ;
00807
00808
00809 max_val = cpl_image_get(extracted, (int)cenx, (int)ceny,
00810 &is_rejected);
00811 cpl_ensure_code(is_rejected >= 0, cpl_error_get_code());
00812
00813 if (is_rejected) {
00814 cpl_errorstate pstate = cpl_errorstate_get();
00815 max_val = cpl_image_get_mean_window(extracted, (int)cenx,
00816 (int)ceny, (int)(cenx+1), (int)(ceny+1)) ;
00817 cpl_ensure_code(cpl_errorstate_is_equal(pstate), cpl_error_get_code());
00818 }
00819
00820 cpl_image_delete(extracted) ;
00821
00822 if (norm) *norm = max_val*2*CX_PI*sqrt(fabs(uxx/u0))*sqrt(fabs(uyy/u0)) ;
00823
00824
00825 if (xcen) *xcen = cenx + llx - 1 ;
00826 if (ycen) *ycen = ceny + lly - 1 ;
00827
00828
00829 return CPL_ERROR_NONE ;
00830 }
00831
00832
00833
00834
00835
00836
00837 #define CONCAT(a,b) a ## _ ## b
00838 #define CONCAT2X(a,b) CONCAT(a,b)
00839
00840 #define CPL_TYPE double
00841 #include "xsh_fit_body.h"
00842 #undef CPL_TYPE
00843
00844 #define CPL_TYPE float
00845 #include "xsh_fit_body.h"
00846 #undef CPL_TYPE
00847
00848 #define CPL_TYPE int
00849 #include "xsh_fit_body.h"
00850 #undef CPL_TYPE
00851