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 <cpl.h>
00037 #include <math.h>
00038 #include <float.h>
00039
00040 #include "irplib_flat.h"
00041 #include "irplib_utils.h"
00042 #include "irplib_polynomial.h"
00043 #include "irplib_distortion.h"
00044
00045
00046
00047
00048
00049 #define IRPLIB_MAX(A,B) ((A) > (B) ? (A) : (B))
00050 #define IRPLIB_MIN(A,B) ((A) < (B) ? (A) : (B))
00051
00052 #define ARC_MINGOODPIX 100
00053 #define ARC_MINARCLENFACT 2.0
00054 #define ARC_MINNBARCS 4
00055 #define ARC_RANGE_FACT 3.0
00056 #define ARC_WINDOWSIZE 32
00057
00058 #define TRESH_MEDIAN_MIN 0.0
00059 #define TRESH_SIGMA_MAX 200.0
00060
00061
00065
00066
00067
00068
00069
00070
00071 static cpl_apertures * irplib_distortion_detect_arcs(cpl_image *,
00072 cpl_image **, int, int, double, int, int, int, int) ;
00073 static cpl_error_code irplib_distortion_fill_border(cpl_image *, int, int,
00074 int, int, double);
00075 static int irplib_distortion_threshold1d(cpl_image *, double, cpl_image *,
00076 double) ;
00077 static cpl_error_code irplib_distortion_purge_arcs(cpl_apertures **, cpl_image *,
00078 const cpl_image *, int, int,
00079 double);
00080 static cpl_error_code irplib_distortion_fill_arc_positions(cpl_bivector *,
00081 cpl_vector *,
00082 const cpl_image *,
00083 const cpl_image *,
00084 const cpl_apertures *);
00085
00086 static double irplib_distortion_get_row_centroid(const cpl_image *,
00087 const cpl_image *, int, int);
00088
00089 static int irplib_distortion_sub_hor_lowpass(cpl_image *, int) ;
00090 static cpl_image * irplib_distortion_remove_ramp(const cpl_image *) ;
00091
00092 static cpl_error_code irplib_polynomial_fit_2d(cpl_polynomial *,
00093 const cpl_bivector *,
00094 const cpl_vector *, int,
00095 double, double *);
00096
00097 static cpl_matrix * irplib_matrix_product_normal_create(const cpl_matrix *);
00098
00099
00100
00101
00102
00105
00134
00135 cpl_polynomial * irplib_distortion_estimate(
00136 const cpl_image * org,
00137 int xmin,
00138 int ymin,
00139 int xmax,
00140 int ymax,
00141 int auto_ramp_sub,
00142 int arc_sat,
00143 int max_arc_width,
00144 double kappa,
00145 int degree,
00146 cpl_apertures ** arcs)
00147 {
00148 cpl_image * local_im ;
00149 cpl_image * label_image ;
00150 double rightmost, leftmost ;
00151 cpl_bivector * grid ;
00152 cpl_vector * values_to_fit ;
00153 int n_arcs ;
00154 cpl_polynomial * poly2d ;
00155 double mse = 0.0;
00156 const int nx = cpl_image_get_size_x(org);
00157 const int ny = cpl_image_get_size_y(org);
00158 const int min_arc_range = (int)(nx / ARC_RANGE_FACT);
00159 int i;
00160
00161
00162 cpl_ensure(org != NULL, CPL_ERROR_NULL_INPUT, NULL);
00163 cpl_ensure(kappa >= 0.0, CPL_ERROR_ILLEGAL_INPUT, NULL);
00164
00165 local_im = auto_ramp_sub ? irplib_distortion_remove_ramp(org)
00166 : cpl_image_duplicate(org);
00167
00168 cpl_error_ensure(local_im != NULL, cpl_error_get_code(),
00169 return(NULL), "Cannot clean the image");
00170
00171
00172 *arcs = irplib_distortion_detect_arcs(local_im, &label_image, arc_sat,
00173 max_arc_width, kappa, xmin, ymin,
00174 xmax, ymax);
00175 if (*arcs == NULL) {
00176 cpl_image_delete(local_im);
00177 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00178 "Cannot detect the arcs");
00179 return NULL ;
00180 }
00181 n_arcs = cpl_apertures_get_size(*arcs) ;
00182 cpl_msg_info(cpl_func, "%d detected arcs", n_arcs) ;
00183
00184
00185 rightmost = leftmost = cpl_apertures_get_max_x(*arcs, 1) ;
00186 for (i=1 ; i<n_arcs ; i++) {
00187 if (cpl_apertures_get_max_x(*arcs, i+1) < leftmost)
00188 leftmost = cpl_apertures_get_max_x(*arcs, i+1) ;
00189 if (cpl_apertures_get_max_x(*arcs, i+1) > rightmost)
00190 rightmost = cpl_apertures_get_max_x(*arcs, i+1) ;
00191 }
00192 if ((int)(rightmost-leftmost) < min_arc_range) {
00193 #if defined CPL_HAVE_VA_ARGS && CPL_HAVE_VA_ARGS != 0
00194 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00195 "too narrow range (%g-%g)<%d",
00196 rightmost, leftmost, min_arc_range);
00197 #else
00198 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00199 "too narrow range");
00200 #endif
00201 cpl_apertures_delete(*arcs) ;
00202 cpl_image_delete(local_im) ;
00203 cpl_image_delete(label_image) ;
00204 *arcs = NULL;
00205 return NULL ;
00206 }
00207
00208
00209 cpl_msg_info(cpl_func, "Create deformation grid");
00210 grid = cpl_bivector_new(n_arcs * ny);
00211 values_to_fit = cpl_vector_new(n_arcs * ny);
00212
00213 if (irplib_distortion_fill_arc_positions(grid, values_to_fit, local_im,
00214 label_image, *arcs)){
00215 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00216 "cannot get arcs positions") ;
00217 cpl_apertures_delete(*arcs) ;
00218 cpl_image_delete(local_im);
00219 cpl_image_delete(label_image) ;
00220 *arcs = NULL;
00221 return NULL ;
00222 }
00223 cpl_image_delete(label_image);
00224 cpl_image_delete(local_im);
00225
00226
00227 poly2d = cpl_polynomial_new(2);
00228 if (irplib_polynomial_fit_2d(poly2d, grid, values_to_fit, degree,
00229 0.5*(ny+1), &mse)) {
00230 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00231 "cannot apply the 2d fit") ;
00232 cpl_bivector_delete(grid);
00233 cpl_vector_delete(values_to_fit);
00234 cpl_apertures_delete(*arcs);
00235 *arcs = NULL;
00236 return NULL;
00237 }
00238
00239 cpl_msg_info(cpl_func, "Fitted a %d. degree 2D-polynomial to %d points "
00240 "with mean-square error: %g", degree,
00241 cpl_vector_get_size(values_to_fit), mse);
00242
00243
00244 cpl_bivector_delete(grid) ;
00245 cpl_vector_delete(values_to_fit) ;
00246 return poly2d ;
00247 }
00248
00251
00267
00268 static cpl_apertures * irplib_distortion_detect_arcs(
00269 cpl_image * im,
00270 cpl_image ** label_im,
00271 int arc_sat,
00272 int max_arc_width,
00273 double kappa,
00274 int xmin,
00275 int ymin,
00276 int xmax,
00277 int ymax)
00278 {
00279 const int ny = cpl_image_get_size_y(im);
00280
00281 const int min_arclen = (int)(ny / ARC_MINARCLENFACT);
00282 cpl_image * filt_im ;
00283 cpl_matrix * filter ;
00284 cpl_image * collapsed ;
00285 cpl_mask * bin_im ;
00286 double threshold, fillval, median_val, sigma ;
00287 cpl_apertures * det ;
00288 int nobj ;
00289 int ngoodpix ;
00290
00291
00292 *label_im = NULL ;
00293
00294
00295 median_val = cpl_image_get_median_dev(im, &sigma) ;
00296 fillval = median_val-sigma/2.0 ;
00297 if (irplib_distortion_fill_border(im, xmin, ymin, xmax, ymax, fillval)) {
00298 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00299 "cannot fill bad zones") ;
00300 return NULL ;
00301 }
00302
00303
00304 filter = cpl_matrix_new(3, 1) ;
00305 cpl_matrix_fill(filter, 1.0) ;
00306
00307 filt_im = cpl_image_duplicate(im) ;
00308 cpl_matrix_delete(filter) ;
00309
00310
00311 if (irplib_distortion_sub_hor_lowpass(filt_im, ARC_WINDOWSIZE) == -1) {
00312 cpl_image_delete(filt_im) ;
00313 return NULL ;
00314 }
00315
00316
00317 median_val = cpl_image_get_median_dev(filt_im, &sigma) ;
00318
00319
00320 if (median_val < TRESH_MEDIAN_MIN) median_val = TRESH_MEDIAN_MIN ;
00321 if (sigma > TRESH_SIGMA_MAX) sigma = TRESH_SIGMA_MAX ;
00322
00323
00324 threshold = median_val + sigma * kappa ;
00325
00326
00327 collapsed = cpl_image_collapse_median_create(filt_im, 0, 0, 0) ;
00328
00329
00330 if (irplib_distortion_threshold1d(filt_im, median_val, collapsed, 0.0)==-1) {
00331 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00332 "cannot threshold the filtered image") ;
00333 cpl_image_delete(filt_im) ;
00334 cpl_image_delete(collapsed) ;
00335 return NULL ;
00336 }
00337 cpl_image_delete(collapsed) ;
00338
00339
00340 bin_im = cpl_mask_threshold_image_create(filt_im, threshold,
00341 DBL_MAX);
00342 cpl_image_delete(filt_im) ;
00343 if (bin_im == NULL) {
00344 cpl_error_set_message(cpl_func, CPL_ERROR_DATA_NOT_FOUND,
00345 "cannot binarise the image") ;
00346 return NULL ;
00347 }
00348
00349
00350 ngoodpix = cpl_mask_count(bin_im) ;
00351 if (ngoodpix < ARC_MINGOODPIX) {
00352 #if defined CPL_HAVE_VA_ARGS && CPL_HAVE_VA_ARGS != 0
00353 cpl_error_set_message(cpl_func, CPL_ERROR_DATA_NOT_FOUND,
00354 "Too few (%d) white pixels", ngoodpix);
00355 #else
00356 cpl_error_set_message(cpl_func, CPL_ERROR_DATA_NOT_FOUND,
00357 "Too few white pixels");
00358 #endif
00359 cpl_mask_delete(bin_im) ;
00360 return NULL ;
00361 }
00362
00363
00364 filter = cpl_matrix_new(3, 3) ;
00365 cpl_matrix_fill(filter, 1.0) ;
00366 cpl_mask_erosion(bin_im, filter) ;
00367 cpl_mask_dilation(bin_im, filter) ;
00368 cpl_matrix_delete(filter) ;
00369
00370
00371 *label_im = cpl_image_labelise_mask_create(bin_im, &nobj) ;
00372 cpl_mask_delete(bin_im) ;
00373
00374
00375 if ((det = cpl_apertures_new_from_image(im, *label_im)) == NULL) {
00376 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00377 "Cannot compute arcs stats") ;
00378 cpl_image_delete(*label_im) ;
00379 *label_im = NULL ;
00380 return NULL ;
00381 }
00382
00383
00384 if (irplib_distortion_purge_arcs(&det, *label_im, im, min_arclen,
00385 max_arc_width, arc_sat)) {
00386 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00387 "Cannot purge the arcs") ;
00388 cpl_image_delete(*label_im) ;
00389 *label_im = NULL ;
00390 cpl_apertures_delete(det) ;
00391 return NULL ;
00392 }
00393 if (cpl_apertures_get_size(det) < ARC_MINNBARCS) {
00394 #if defined CPL_HAVE_VA_ARGS && CPL_HAVE_VA_ARGS != 0
00395 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00396 "Not enough valid arcs (%d < %d)",
00397 cpl_apertures_get_size(det), ARC_MINNBARCS);
00398 #else
00399 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00400 "Not enough valid arcs, min="
00401 IRPLIB_STRINGIFY(ARC_MINNBARCS));
00402 #endif
00403 cpl_image_delete(*label_im) ;
00404 *label_im = NULL ;
00405 cpl_apertures_delete(det) ;
00406 return NULL ;
00407 }
00408
00409
00410 return det ;
00411 }
00412
00413
00423
00424 static cpl_error_code irplib_distortion_fill_border(cpl_image * self,
00425 int xmin,
00426 int ymin,
00427 int xmax,
00428 int ymax,
00429 double fillval)
00430 {
00431 const int nx = cpl_image_get_size_x(self);
00432 const int ny = cpl_image_get_size_y(self);
00433 float * pfi = cpl_image_get_data_float(self);
00434 const float fvalue = (float)fillval;
00435 int i, j;
00436
00437
00438 cpl_ensure_code(pfi != NULL, cpl_error_get_code());
00439
00440
00441 xmin = IRPLIB_MIN(xmin, nx+1);
00442 ymax = IRPLIB_MIN(ymax, ny);
00443
00444
00445 xmax = IRPLIB_MAX(xmax, xmin - 1);
00446 ymin = IRPLIB_MIN(ymin, ymax + 1);
00447
00448
00449
00450 for (j = 0; j < ymin-1; j++) {
00451 for (i = 0; i < nx; i++) {
00452 pfi[i+j*nx] = fvalue;
00453 }
00454 }
00455
00456
00457 for (; j < ymax; j++) {
00458 for (i = 0; i < xmin-1; i++) {
00459 pfi[i+j*nx] = fvalue;
00460 }
00461 for (i = xmax; i < nx; i++) {
00462 pfi[i+j*nx] = fvalue;
00463 }
00464 }
00465
00466
00467 for (; j < ny; j++) {
00468 for (i = 0; i < nx; i++) {
00469 pfi[i+j*nx] = fvalue;
00470 }
00471 }
00472
00473 return CPL_ERROR_NONE;
00474 }
00475
00476 static int irplib_distortion_threshold1d(
00477 cpl_image * im,
00478 double threshold,
00479 cpl_image * im1d,
00480 double newval)
00481 {
00482 float * pim ;
00483 float * pim1d ;
00484 int nx, ny ;
00485 int i, j ;
00486
00487
00488 if (im == NULL) return -1 ;
00489 if (im1d == NULL) return -1 ;
00490 if (cpl_image_get_type(im) != CPL_TYPE_FLOAT) return -1 ;
00491 if (cpl_image_get_type(im1d) != CPL_TYPE_FLOAT) return -1 ;
00492
00493
00494 pim = cpl_image_get_data_float(im) ;
00495 pim1d = cpl_image_get_data_float(im1d) ;
00496 nx = cpl_image_get_size_x(im) ;
00497 ny = cpl_image_get_size_y(im) ;
00498
00499
00500 for (i=0 ; i<nx ; i++)
00501 if (pim1d[i] < threshold) {
00502 for (j=0 ; j<ny ; j++) pim[i+j*nx] = (float)newval ;
00503 }
00504
00505
00506 return 0 ;
00507 }
00508
00509 static int irplib_distortion_sub_hor_lowpass(
00510 cpl_image * im,
00511 int filt_size)
00512 {
00513 cpl_vector * linehi ;
00514 cpl_vector * linelo ;
00515 cpl_vector * avglinehi ;
00516 cpl_vector * avglinelo ;
00517 double * pavglinehi ;
00518 float * pim ;
00519 int lopos, hipos, nx, ny ;
00520 int i, j ;
00521
00522
00523 if (im == NULL) return -1 ;
00524 if (filt_size <= 0) return -1 ;
00525
00526
00527 nx = cpl_image_get_size_x(im) ;
00528 ny = cpl_image_get_size_y(im) ;
00529 lopos = (int)(ny/4) ;
00530 hipos = (int)(3*ny/4) ;
00531
00532
00533 if ((linehi = cpl_vector_new_from_image_row(im, hipos)) == NULL) {
00534 return -1 ;
00535 }
00536 if ((linelo = cpl_vector_new_from_image_row(im, lopos)) == NULL) {
00537 cpl_vector_delete(linehi) ;
00538 return -1 ;
00539 }
00540
00541
00542 if ((avglinehi = cpl_vector_filter_median_create(linehi,
00543 filt_size)) == NULL) {
00544 cpl_vector_delete(linehi) ;
00545 cpl_vector_delete(linelo) ;
00546 return -1 ;
00547 }
00548 cpl_vector_delete(linehi) ;
00549
00550 if ((avglinelo = cpl_vector_filter_median_create(linelo,
00551 filt_size)) == NULL) {
00552 cpl_vector_delete(linelo) ;
00553 cpl_vector_delete(avglinehi) ;
00554 return -1 ;
00555 }
00556 cpl_vector_delete(linelo) ;
00557
00558
00559 cpl_vector_add(avglinehi, avglinelo) ;
00560 cpl_vector_delete(avglinelo) ;
00561 cpl_vector_divide_scalar(avglinehi, 2.0) ;
00562
00563
00564 pavglinehi = cpl_vector_get_data(avglinehi) ;
00565 pim = cpl_image_get_data_float(im) ;
00566 for (i=0 ; i<nx ; i++) {
00567 for (j=0 ; j<ny ; j++) {
00568 pim[i+j*nx] -= pavglinehi[i] ;
00569 }
00570 }
00571 cpl_vector_delete(avglinehi) ;
00572
00573 return 0 ;
00574 }
00575
00576
00587
00588 static
00589 cpl_error_code irplib_distortion_purge_arcs(cpl_apertures ** self,
00590 cpl_image * lab_im,
00591 const cpl_image * arc_im,
00592 int min_arclen,
00593 int max_arcwidth,
00594 double arc_sat)
00595 {
00596 const double ycenter = 0.5 * (1 + cpl_image_get_size_y(arc_im));
00597 int narcs;
00598 int nkeep = 0;
00599 int ifirst = 1;
00600 int * relabel;
00601 int i;
00602
00603
00604 cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
00605
00606
00607 narcs = cpl_apertures_get_size(*self);
00608
00609 cpl_ensure_code(narcs > 0, CPL_ERROR_DATA_NOT_FOUND);
00610 cpl_ensure_code(cpl_image_get_type(lab_im) == CPL_TYPE_INT,
00611 CPL_ERROR_ILLEGAL_INPUT);
00612
00613
00614 relabel = cpl_calloc(narcs, sizeof(int));
00615
00616
00617 for (i = 0; i < narcs; i++) {
00618
00619 const int arclen = 1
00620 + cpl_apertures_get_top(*self, i+1)
00621 - cpl_apertures_get_bottom(*self, i+1);
00622
00623 if (cpl_apertures_get_top(*self, i+1) < ycenter) continue;
00624 if (cpl_apertures_get_bottom(*self, i+1) > ycenter) continue;
00625
00626 if (arclen > min_arclen) {
00627 const int arcwidth = 1
00628 + cpl_apertures_get_right(*self, i+1)
00629 - cpl_apertures_get_left(*self, i+1);
00630 if (arcwidth < max_arcwidth) {
00631 const int edge = cpl_apertures_get_left_y(*self, i+1);
00632 if (edge > 0) {
00633 const double mean = cpl_apertures_get_mean(*self, i+1);
00634 if (mean < arc_sat) {
00635 relabel[i] = ++nkeep;
00636
00637 if (nkeep == i+1) ifirst = nkeep;
00638 }
00639 }
00640 }
00641 }
00642 }
00643
00644 if (nkeep < narcs) {
00645
00646 int * plabim = cpl_image_get_data_int(lab_im);
00647 const int npix = cpl_image_get_size_x(lab_im)
00648 * cpl_image_get_size_y(lab_im);
00649
00650 if (nkeep == 0) {
00651 cpl_free(relabel);
00652 #if defined CPL_HAVE_VA_ARGS && CPL_HAVE_VA_ARGS != 0
00653 return cpl_error_set_message(cpl_func, CPL_ERROR_DATA_NOT_FOUND,
00654 "All %d arc(s) are invalid", narcs);
00655 #else
00656 return cpl_error_set_message(cpl_func, CPL_ERROR_DATA_NOT_FOUND,
00657 "All arcs are invalid");
00658 #endif
00659 }
00660
00661 for (i = 0; i < npix; i++) {
00662 const int label = plabim[i];
00663
00664 if (label < 0 || label > narcs) break;
00665 if (label >= ifirst) plabim[i] = relabel[label-1];
00666 }
00667
00668 if (i < npix) {
00669
00670 cpl_free(relabel);
00671 return cpl_error_set(cpl_func, plabim[i] < 0
00672 ? CPL_ERROR_ILLEGAL_INPUT
00673 : CPL_ERROR_INCOMPATIBLE_INPUT);
00674 }
00675
00676
00677 cpl_apertures_delete(*self) ;
00678 *self = cpl_apertures_new_from_image(arc_im, lab_im);
00679
00680 }
00681
00682 cpl_free(relabel);
00683
00684 cpl_msg_info(cpl_func, "Purged %d of %d arcs (1st purged=%d)", narcs - nkeep,
00685 narcs, ifirst);
00686
00687
00688 cpl_ensure_code(*self != NULL, cpl_error_get_code());
00689
00690 return CPL_ERROR_NONE;
00691 }
00692
00693
00694
00708
00709 static cpl_error_code
00710 irplib_distortion_fill_arc_positions(cpl_bivector * grid,
00711 cpl_vector * fitvalues,
00712 const cpl_image * in,
00713 const cpl_image * label_im,
00714 const cpl_apertures * det)
00715 {
00716 const int narcs = cpl_apertures_get_size(det);
00717 int nfitvals = cpl_vector_get_size(fitvalues);
00718 const int nx = cpl_image_get_size_x(label_im);
00719 const int ny = cpl_image_get_size_y(label_im);
00720 cpl_image * filt_img;
00721 cpl_matrix * kernel;
00722 cpl_vector * gridx = cpl_bivector_get_x(grid);
00723 cpl_vector * gridy = cpl_bivector_get_y(grid);
00724 cpl_vector * dist1dx = NULL;
00725 cpl_vector * dist1dy = NULL;
00726 double * dgridx;
00727 double * dgridy;
00728 double * dfitv;
00729 int ndone = 0;
00730 int i, obj;
00731
00732 cpl_ensure_code(nfitvals > 0, CPL_ERROR_DATA_NOT_FOUND);
00733 cpl_ensure_code(narcs > 0, CPL_ERROR_DATA_NOT_FOUND);
00734 cpl_ensure_code(cpl_image_get_type(label_im) == CPL_TYPE_INT,
00735 CPL_ERROR_TYPE_MISMATCH);
00736
00737
00738 if (nfitvals < narcs * ny) {
00739 nfitvals = narcs * ny;
00740 cpl_vector_set_size(fitvalues, nfitvals);
00741 }
00742 if (cpl_vector_get_size(gridx) < nfitvals ||
00743 cpl_vector_get_size(gridy) < nfitvals) {
00744 cpl_vector_set_size(gridx, nfitvals);
00745 cpl_vector_set_size(gridy, nfitvals);
00746 }
00747
00748
00749 dgridx = cpl_vector_get_data(gridx);
00750 dgridy = cpl_vector_get_data(gridy);
00751 dfitv = cpl_vector_get_data(fitvalues);
00752
00753
00754 kernel = cpl_matrix_new(3, 3);
00755 cpl_matrix_fill(kernel, 1.0);
00756 filt_img = cpl_image_filter_median(in, kernel);
00757 cpl_matrix_delete(kernel);
00758
00759 for (obj = 0; obj < narcs; obj++) {
00760
00761 const int * plabel_im = cpl_image_get_data_int_const(label_im);
00762 const int ndist1d = cpl_apertures_get_top(det, obj+1)
00763 - cpl_apertures_get_bottom(det, obj+1) + 1;
00764 int j;
00765 int k = 0;
00766
00767 (void)cpl_vector_unwrap(dist1dx);
00768 (void)cpl_vector_unwrap(dist1dy);
00769 dist1dx = cpl_vector_wrap(ndist1d, dgridy + ndone);
00770 dist1dy = cpl_vector_wrap(ndist1d, dfitv + ndone);
00771
00772
00773
00774 for (j = cpl_apertures_get_bottom(det, obj+1)-1;
00775 j < cpl_apertures_get_top(det, obj+1); j++) {
00776
00777 for (i = 0; i < nx; i++) {
00778 if (plabel_im[i + j * nx] == obj + 1) break;
00779 }
00780 if (i < nx) {
00781
00782 cpl_errorstate prestate = cpl_errorstate_get();
00783
00784 const double x_finepos
00785 = irplib_distortion_get_row_centroid(filt_img, label_im,
00786 i, j);
00787 if (!cpl_errorstate_is_equal(prestate)) {
00788 irplib_error_recover(prestate, "Could not find X-position "
00789 "for line %d at y=%d (x=%d)",
00790 obj+1, j+1, i+1);
00791 } else if (x_finepos >= 0.0) {
00792 cpl_vector_set(dist1dx, k, 1.0 + j);
00793 cpl_vector_set(dist1dy, k, 1.0 + x_finepos);
00794 k++;
00795 }
00796 }
00797 }
00798 if (k > 0) {
00799 cpl_polynomial * dist1d;
00800 double ref_xpos, grad;
00801
00802
00803 (void)cpl_vector_unwrap(dist1dx);
00804 (void)cpl_vector_unwrap(dist1dy);
00805 dist1dx = cpl_vector_wrap(k, dgridy + ndone);
00806 dist1dy = cpl_vector_wrap(k, dfitv + ndone);
00807
00808 dist1d = cpl_polynomial_fit_1d_create(dist1dx, dist1dy, 2, NULL);
00809
00810 ref_xpos = cpl_polynomial_eval_1d(dist1d, 0.5 * (ny + 1), &grad);
00811
00812 for (j = cpl_apertures_get_bottom(det, obj+1)-1;
00813 j < cpl_apertures_get_top(det, obj+1); j++) {
00814 const double xpos = cpl_polynomial_eval_1d(dist1d, j+1.0, NULL);
00815
00816 dfitv [ndone] = xpos;
00817 dgridx[ndone] = ref_xpos;
00818
00819
00820
00821 ndone++;
00822 }
00823 cpl_msg_info(cpl_func, "Line %d has center gradient %g", obj+1,
00824 grad);
00825 cpl_polynomial_delete(dist1d);
00826 }
00827 }
00828
00829 cpl_image_delete(filt_img);
00830 (void)cpl_vector_unwrap(dist1dx);
00831 (void)cpl_vector_unwrap(dist1dy);
00832
00833 cpl_msg_info(cpl_func, "Found %d fitting points ("
00834 "expected up to %d points)", ndone, nfitvals);
00835
00836 cpl_ensure_code(ndone > 0, CPL_ERROR_DATA_NOT_FOUND);
00837
00838 cpl_vector_set_size(fitvalues, ndone);
00839 cpl_vector_set_size(gridx, ndone);
00840 cpl_vector_set_size(gridy, ndone);
00841
00842 return CPL_ERROR_NONE;
00843 }
00844
00845
00855
00856 static double irplib_distortion_get_row_centroid(const cpl_image * im,
00857 const cpl_image * label_im,
00858 int x,
00859 int y)
00860 {
00861 const int nx = cpl_image_get_size_x(im);
00862 const int ny = cpl_image_get_size_y(im);
00863 const int ynx = y * nx;
00864 const float * pim = cpl_image_get_data_float_const(im);
00865 const int * plabel_im = cpl_image_get_data_int_const(label_im);
00866 int firstpos = -1;
00867 int lastpos = -1;
00868 int maxpos = x;
00869 int objnum;
00870 double wsum = 0.0;
00871 double sum = 0.0;
00872 double max = 0.0;
00873
00874 cpl_ensure(pim != NULL, cpl_error_get_code(), -1.0);
00875 cpl_ensure(plabel_im != NULL, cpl_error_get_code(), -2.0);
00876 cpl_ensure(x >= 0, CPL_ERROR_ILLEGAL_INPUT, -3.0);
00877 cpl_ensure(y >= 0, CPL_ERROR_ILLEGAL_INPUT, -4.0);
00878 cpl_ensure(x < nx, CPL_ERROR_ILLEGAL_INPUT, -5.0);
00879 cpl_ensure(y < ny, CPL_ERROR_ILLEGAL_INPUT, -6.0);
00880
00881 max = (double)pim[x + ynx];
00882 objnum = plabel_im[x + ynx];
00883
00884
00885 do {
00886 const double val = (double)pim[x + ynx];
00887
00888 if (val > 0.0) {
00889 wsum += x * val;
00890 sum += val;
00891
00892 if (firstpos < 0) firstpos = x;
00893 lastpos = x;
00894
00895 if (val > max) {
00896 max = val;
00897 maxpos = x;
00898 }
00899 }
00900
00901
00902
00903 x++;
00904
00905 } while (x < nx && objnum == plabel_im[x + ynx]);
00906
00907 cpl_ensure(sum > 0.0, CPL_ERROR_DATA_NOT_FOUND, -7.0);
00908
00909
00910
00911
00912
00913
00914
00915 return (wsum < sum * firstpos || wsum > sum * lastpos)
00916 ? maxpos : wsum / sum;
00917 }
00918
00919
00925
00926 #define IS_NB_TESTPOINTS 8
00927 #define IS_MIN_SLOPE 0.01
00928 #define IS_MAX_SLOPE_DIF 0.075
00929 #define IS_MAX_FIT_EDGE_DIF 0.05
00930 #define IS_MIN_RAMP 10.0
00931 #define IS_MAX_MNERR 13.0
00932 #define IS_MAX_MNERR_DIF 8.0
00933 #define IS_MAX_INTER_DIF 20.0
00934 #define IS_SKIPZONE 2.5
00935 #define SQR(x) ((x)*(x))
00936 static cpl_image * irplib_distortion_remove_ramp(const cpl_image * in)
00937 {
00938 int ramp_present ;
00939 const int nx = cpl_image_get_size_x(in);
00940 const int ny = cpl_image_get_size_y(in);
00941 const int yhi = (int)(ny/2);
00942 const int ylo = yhi - 1;
00943 int y;
00944 cpl_vector * tmp_vector ;
00945 cpl_bivector * testpointlo ;
00946 double * testpointlo_x ;
00947 double * testpointlo_y ;
00948 cpl_bivector * testpointhi ;
00949 double * testpointhi_x ;
00950 double * testpointhi_y ;
00951 const int spacing = ny / (IS_SKIPZONE*IS_NB_TESTPOINTS);
00952 double rampdif, fitslope;
00953 double * pol_coefhi,
00954 * pol_coeflo ;
00955 cpl_vector * median ;
00956 double * median_data ;
00957 double medianerrlo, medianerrhi;
00958 double slope ;
00959 cpl_image * out ;
00960 float * pout ;
00961 float val ;
00962 int i, j ;
00963
00964 cpl_ensure(cpl_image_get_type(in) == CPL_TYPE_FLOAT,
00965 CPL_ERROR_UNSUPPORTED_MODE, NULL);
00966
00967 if (ny < IS_SKIPZONE * IS_NB_TESTPOINTS){
00968 #if defined CPL_HAVE_VA_ARGS && CPL_HAVE_VA_ARGS != 0
00969 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00970 "image has %d lines, min="
00971 IRPLIB_STRINGIFY(IS_SKIPZONE) "*"
00972 IRPLIB_STRINGIFY(IS_NB_TESTPOINTS), ny);
00973 #else
00974 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00975 "image has too few lines, min="
00976 IRPLIB_STRINGIFY(IS_SKIPZONE) "*"
00977 IRPLIB_STRINGIFY(IS_NB_TESTPOINTS));
00978 #endif
00979 return NULL ;
00980 }
00981
00982 slope=0.0 ;
00983
00984 testpointhi = cpl_bivector_new(IS_NB_TESTPOINTS) ;
00985 testpointhi_x = cpl_bivector_get_x_data(testpointhi) ;
00986 testpointhi_y = cpl_bivector_get_y_data(testpointhi) ;
00987 testpointlo = cpl_bivector_new(IS_NB_TESTPOINTS) ;
00988 testpointlo_x = cpl_bivector_get_x_data(testpointlo) ;
00989 testpointlo_y = cpl_bivector_get_y_data(testpointlo) ;
00990 for (i=0 ; i<IS_NB_TESTPOINTS ; i++) {
00991 y = yhi + i * spacing;
00992 tmp_vector = cpl_vector_new_from_image_row(in, y+1) ;
00993 testpointhi_x[i] = y - ny / 2;
00994 testpointhi_y[i] = cpl_vector_get_median_const(tmp_vector) ;
00995 cpl_vector_delete(tmp_vector) ;
00996 y = ylo - i * spacing;
00997 tmp_vector = cpl_vector_new_from_image_row(in, y+1) ;
00998 testpointlo_x[IS_NB_TESTPOINTS-i-1] = y ;
00999 testpointlo_y[IS_NB_TESTPOINTS-i-1]=cpl_vector_get_median_const(tmp_vector) ;
01000 cpl_vector_delete(tmp_vector) ;
01001 }
01002
01003
01004 pol_coefhi = irplib_flat_fit_slope_robust(testpointhi_x,
01005 testpointhi_y, IS_NB_TESTPOINTS) ;
01006 pol_coeflo = irplib_flat_fit_slope_robust(testpointlo_x,
01007 testpointlo_y, IS_NB_TESTPOINTS) ;
01008
01009
01010 median = cpl_vector_new(IS_NB_TESTPOINTS) ;
01011 median_data = cpl_vector_get_data(median) ;
01012 for (i=0 ; i<IS_NB_TESTPOINTS ; i++) {
01013 median_data[i]=SQR(testpointhi_y[i]
01014 - pol_coefhi[0] - pol_coefhi[1] * testpointhi_x[i]);
01015 }
01016 medianerrhi = cpl_vector_get_median(median) ;
01017 for (i=0; i<IS_NB_TESTPOINTS; i++) {
01018 median_data[i]=SQR(testpointlo_y[i]
01019 - pol_coeflo[0] - pol_coeflo[1] * testpointlo_x[i]);
01020 }
01021 medianerrlo = cpl_vector_get_median(median) ;
01022 cpl_vector_delete(median) ;
01023 rampdif = testpointlo_y[IS_NB_TESTPOINTS-1] - testpointhi_y[0];
01024 slope = rampdif / (ny/2.0) ;
01025 fitslope = (pol_coefhi[1] + pol_coeflo[1]) / 2.0 ;
01026
01027 cpl_bivector_delete(testpointlo);
01028 cpl_bivector_delete(testpointhi);
01029
01030
01031 if (fabs(rampdif)<IS_MIN_RAMP ||
01032 fabs(pol_coefhi[1]) < IS_MIN_SLOPE ||
01033 fabs(pol_coeflo[1]) < IS_MIN_SLOPE ||
01034 pol_coefhi[1]/pol_coeflo[1]<0.5 ||
01035 pol_coefhi[1]/pol_coeflo[1]>2.0 ||
01036 fabs(pol_coefhi[1]-pol_coeflo[1])>IS_MAX_SLOPE_DIF ||
01037 fabs(pol_coefhi[0]-pol_coeflo[0]) > IS_MAX_INTER_DIF ||
01038 medianerrlo> IS_MAX_MNERR ||
01039 medianerrhi> IS_MAX_MNERR ||
01040 fabs(medianerrlo-medianerrhi) >IS_MAX_MNERR_DIF ||
01041 fabs(slope-fitslope) > IS_MAX_FIT_EDGE_DIF ||
01042 slope/fitslope<0.5 ||
01043 slope/fitslope>2.0) ramp_present = 0 ;
01044 else ramp_present = 1 ;
01045
01046 cpl_free(pol_coeflo) ;
01047 cpl_free(pol_coefhi) ;
01048
01049
01050 out = cpl_image_duplicate(in) ;
01051 pout = cpl_image_get_data_float(out) ;
01052 if (ramp_present == 1) {
01053 for (j=0 ; j<ny/2 ; j++) {
01054 val = slope * (j-ny/2) ;
01055 for (i=0 ; i<nx ; i++)
01056 pout[i+j*nx] -= val ;
01057 }
01058 for (j=ny/2 ; j<ny ; j++) {
01059 val = slope * (j-ny) ;
01060 for (i=0 ; i<nx ; i++)
01061 pout[i+j*nx] -= val ;
01062 }
01063
01064 }
01065
01066 return out;
01067 }
01068
01094 static cpl_matrix * irplib_matrix_product_normal_create(const cpl_matrix * self)
01095 {
01096
01097 double sum;
01098 cpl_matrix * product;
01099 const double * ai = cpl_matrix_get_data_const(self);
01100 const double * aj;
01101 double * bwrite;
01102 const int m = cpl_matrix_get_nrow(self);
01103 const int n = cpl_matrix_get_ncol(self);
01104 int i, j, k;
01105
01106
01107 cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, NULL);
01108
01109 #if 0
01110
01111
01112
01113 product = cpl_matrix_new(m, m);
01114 bwrite = cpl_matrix_get_data(product);
01115 #else
01116 bwrite = (double *) cpl_malloc(m * m * sizeof(double));
01117 product = cpl_matrix_wrap(m, m, bwrite);
01118 #endif
01119
01120
01121 for (i = 0; i < m; i++, bwrite += m, ai += n) {
01122 aj = ai;
01123 for (j = i; j < m; j++, aj += n) {
01124 sum = 0.0;
01125 for (k = 0; k < n; k++) {
01126 sum += ai[k] * aj[k];
01127 }
01128 bwrite[j] = sum;
01129 }
01130 }
01131
01132 return product;
01133
01134 }
01135
01136
01148
01149 static cpl_error_code irplib_polynomial_fit_2d(cpl_polynomial * self,
01150 const cpl_bivector * xy_pos,
01151 const cpl_vector * values,
01152 int degree, double fixy,
01153 double * mse)
01154 {
01155
01156 const int np = cpl_bivector_get_size(xy_pos);
01157
01158 const int nc1 = 1+degree;
01159
01160
01161 const int nc = nc1 * (1 + nc1) / 2 - nc1;
01162 cpl_matrix * mv;
01163 cpl_matrix * mh;
01164 cpl_matrix * mb;
01165 cpl_matrix * mx;
01166 const double * coeffs1d;
01167 double * dmv;
01168 cpl_vector * xhat;
01169 cpl_vector * yhat;
01170 cpl_vector * zhat;
01171 int powers[2];
01172 int degx, degy;
01173 int i, j;
01174 cpl_error_code error;
01175
01176
01177 cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
01178 cpl_ensure_code(cpl_polynomial_get_dimension(self) == 2,
01179 CPL_ERROR_INVALID_TYPE);
01180 cpl_ensure_code(np > 0, cpl_error_get_code());
01181 cpl_ensure_code(values != NULL, CPL_ERROR_NULL_INPUT);
01182
01183 cpl_ensure_code(cpl_vector_get_size(values) == np,
01184 CPL_ERROR_INCOMPATIBLE_INPUT);
01185
01186 cpl_ensure_code(degree > 0, CPL_ERROR_ILLEGAL_INPUT);
01187 cpl_ensure_code(np >= nc, CPL_ERROR_DATA_NOT_FOUND);
01188
01189
01190 yhat = cpl_vector_duplicate(cpl_bivector_get_y_const(xy_pos));
01191 cpl_vector_subtract_scalar(yhat, fixy);
01192
01193
01194 xhat = cpl_vector_duplicate(cpl_bivector_get_x_const(xy_pos));
01195 zhat = cpl_vector_duplicate(values);
01196 cpl_vector_subtract(zhat, xhat);
01197
01198
01199
01200
01201 dmv = (double*)cpl_malloc(nc*np*sizeof(double));
01202 mv = cpl_matrix_wrap(nc, np, dmv);
01203
01204
01205 for (i=0 ; i < np ; i++) {
01206 const double x = cpl_vector_get(xhat, i);
01207 const double y = cpl_vector_get(yhat, i);
01208 double xvalue;
01209 double yvalue = y;
01210 j = 0;
01211 for (degy = 1; degy <= degree; degy++) {
01212 xvalue = 1;
01213 for (degx = 0; degx <= degree-degy; degx++, j++) {
01214 dmv[np * j + i] = xvalue * yvalue;
01215 xvalue *= x;
01216 }
01217 yvalue *= y;
01218 }
01219
01220 }
01221 cpl_vector_delete(xhat);
01222 cpl_vector_delete(yhat);
01223
01224
01225 mb = cpl_matrix_wrap(np, 1, cpl_vector_get_data(zhat));
01226
01227
01228 mx = cpl_matrix_product_create(mv, mb);
01229
01230 cpl_matrix_unwrap(mb);
01231 cpl_vector_delete(zhat);
01232
01233
01234 mh = irplib_matrix_product_normal_create(mv);
01235 cpl_matrix_delete(mv);
01236
01237
01238 error = cpl_matrix_decomp_chol(mh) || cpl_matrix_solve_chol(mh, mx);
01239
01240 cpl_matrix_delete(mh);
01241
01242 if (error) {
01243 cpl_matrix_delete(mx);
01244 cpl_ensure_code(0, error);
01245 }
01246
01247
01248
01249 coeffs1d = cpl_matrix_get_data(mx);
01250
01251 j = 0;
01252 for (degy = 1; degy <= degree; degy++) {
01253 powers[1] = degy;
01254 for (degx = 0; degx <= degree-degy; degx++, j++) {
01255 powers[0] = degx;
01256
01257 cpl_polynomial_set_coeff(self, powers, cpl_matrix_get(mx, j, 0));
01258 }
01259 }
01260
01261
01262 cpl_matrix_delete(mx);
01263
01264
01265 powers[0] = 1;
01266 powers[1] = 0;
01267 cpl_polynomial_set_coeff(self, powers, 1.0);
01268
01269
01270 irplib_polynomial_shift_1d(self, 1, -fixy);
01271
01272
01273 if (mse != NULL) {
01274 const cpl_vector * x_pos = cpl_bivector_get_x_const(xy_pos);
01275 const cpl_vector * y_pos = cpl_bivector_get_y_const(xy_pos);
01276 cpl_vector * x_val = cpl_vector_new(2);
01277 double residue;
01278
01279 *mse = 0;
01280 for (i=0 ; i<np ; i++) {
01281 cpl_vector_set(x_val, 0, cpl_vector_get(x_pos, i));
01282 cpl_vector_set(x_val, 1, cpl_vector_get(y_pos, i));
01283
01284 residue = cpl_vector_get(values, i)
01285 - cpl_polynomial_eval(self, x_val);
01286 *mse += residue * residue;
01287 }
01288 cpl_vector_delete(x_val);
01289
01290 *mse /= np;
01291 }
01292
01293 return CPL_ERROR_NONE;
01294 }
01295