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 "irplib_distortion.h"
00037
00038 #include "irplib_flat.h"
00039 #include "irplib_utils.h"
00040 #include "irplib_polynomial.h"
00041
00042 #include <math.h>
00043 #include <float.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_image_filter_background_line(cpl_image *,
00093 const cpl_image *, int, cpl_boolean) ;
00094
00095 static cpl_error_code irplib_polynomial_fit_2d(cpl_polynomial *,
00096 const cpl_bivector *,
00097 const cpl_vector *, int,
00098 double, double *);
00099
00100 static cpl_matrix * irplib_matrix_product_normal_create(const cpl_matrix *);
00101
00102
00103
00104
00105
00108
00137
00138 cpl_polynomial * irplib_distortion_estimate(
00139 const cpl_image * org,
00140 int xmin,
00141 int ymin,
00142 int xmax,
00143 int ymax,
00144 int auto_ramp_sub,
00145 int arc_sat,
00146 int max_arc_width,
00147 double kappa,
00148 int degree,
00149 cpl_apertures ** arcs)
00150 {
00151 cpl_image * local_im;
00152 cpl_image * filtered;
00153 cpl_image * label_image;
00154 double rightmost, leftmost;
00155 cpl_bivector * grid;
00156 cpl_vector * values_to_fit;
00157 int n_arcs;
00158 cpl_polynomial * poly2d;
00159 double mse = 0.0;
00160 const int nx = cpl_image_get_size_x(org);
00161 const int ny = cpl_image_get_size_y(org);
00162 const int min_arc_range = (int)(nx / ARC_RANGE_FACT);
00163 int i;
00164
00165
00166 cpl_ensure(org != NULL, CPL_ERROR_NULL_INPUT, NULL);
00167 cpl_ensure(kappa >= 0.0, CPL_ERROR_ILLEGAL_INPUT, NULL);
00168 cpl_ensure(max_arc_width > 0, CPL_ERROR_ILLEGAL_INPUT, NULL);
00169
00170
00171
00172
00173 filtered = cpl_image_new(nx, ny, cpl_image_get_type(org));
00174
00175 irplib_image_filter_background_line(filtered, org, max_arc_width, CPL_TRUE);
00176
00177 if (auto_ramp_sub) {
00178 local_im = irplib_distortion_remove_ramp(filtered);
00179 cpl_image_delete(filtered);
00180 } else {
00181 local_im = filtered;
00182 }
00183
00184 cpl_error_ensure(local_im != NULL, cpl_error_get_code(),
00185 return(NULL), "Cannot clean the image");
00186
00187
00188 *arcs = irplib_distortion_detect_arcs(local_im, &label_image, arc_sat,
00189 max_arc_width, kappa, xmin, ymin,
00190 xmax, ymax);
00191 if (*arcs == NULL) {
00192 cpl_image_delete(local_im);
00193 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00194 "Cannot detect the arcs");
00195 return NULL;
00196 }
00197 n_arcs = cpl_apertures_get_size(*arcs);
00198 cpl_msg_info(cpl_func, "%d detected arcs", n_arcs);
00199
00200
00201 rightmost = leftmost = cpl_apertures_get_pos_x(*arcs, 1);
00202 for (i=1; i<n_arcs; i++) {
00203 if (cpl_apertures_get_pos_x(*arcs, i+1) < leftmost)
00204 leftmost = cpl_apertures_get_pos_x(*arcs, i+1);
00205 if (cpl_apertures_get_pos_x(*arcs, i+1) > rightmost)
00206 rightmost = cpl_apertures_get_pos_x(*arcs, i+1);
00207 }
00208 if ((int)(rightmost-leftmost) < min_arc_range) {
00209 #if defined CPL_HAVE_VA_ARGS && CPL_HAVE_VA_ARGS != 0
00210 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00211 "too narrow range (%g-%g)<%d",
00212 rightmost, leftmost, min_arc_range);
00213 #else
00214 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00215 "too narrow range");
00216 #endif
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
00224
00225 cpl_msg_info(cpl_func, "Create deformation grid");
00226 grid = cpl_bivector_new(n_arcs * ny);
00227 values_to_fit = cpl_vector_new(n_arcs * ny);
00228
00229 if (irplib_distortion_fill_arc_positions(grid, values_to_fit, local_im,
00230 label_image, *arcs)){
00231 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00232 "cannot get arcs positions");
00233 cpl_apertures_delete(*arcs);
00234 cpl_image_delete(local_im);
00235 cpl_image_delete(label_image);
00236 *arcs = NULL;
00237 return NULL;
00238 }
00239 cpl_image_delete(label_image);
00240 cpl_image_delete(local_im);
00241
00242
00243 poly2d = cpl_polynomial_new(2);
00244 if (irplib_polynomial_fit_2d(poly2d, grid, values_to_fit, degree,
00245 0.5*(ny+1), &mse)) {
00246 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00247 "cannot apply the 2d fit");
00248 cpl_bivector_delete(grid);
00249 cpl_vector_delete(values_to_fit);
00250 cpl_apertures_delete(*arcs);
00251 *arcs = NULL;
00252 return NULL;
00253 }
00254
00255 cpl_msg_info(cpl_func,
00256 "Fitted a %d. degree 2D-polynomial to %"CPL_SIZE_FORMAT" points "
00257 "with mean-square error: %g", degree,
00258 cpl_vector_get_size(values_to_fit), mse);
00259
00260
00261 cpl_bivector_delete(grid);
00262 cpl_vector_delete(values_to_fit);
00263 return poly2d;
00264 }
00265
00268
00284
00285 static cpl_apertures * irplib_distortion_detect_arcs(
00286 cpl_image * im,
00287 cpl_image ** label_im,
00288 int arc_sat,
00289 int max_arc_width,
00290 double kappa,
00291 int xmin,
00292 int ymin,
00293 int xmax,
00294 int ymax)
00295 {
00296 const int ny = cpl_image_get_size_y(im);
00297
00298 const int min_arclen = (int)(ny / ARC_MINARCLENFACT);
00299 cpl_image * filt_im;
00300 cpl_mask * filter;
00301 cpl_image * collapsed;
00302 cpl_mask * bin_im;
00303 double threshold, fillval, median_val, sigma;
00304 cpl_apertures * det;
00305 cpl_size nobj;
00306 int ngoodpix;
00307
00308
00309 *label_im = NULL;
00310
00311
00312 median_val = cpl_image_get_median_dev(im, &sigma);
00313 fillval = median_val-sigma/2.0;
00314 if (irplib_distortion_fill_border(im, xmin, ymin, xmax, ymax, fillval)) {
00315 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00316 "cannot fill bad zones");
00317 return NULL;
00318 }
00319
00320
00321 filt_im = cpl_image_duplicate(im);
00322 if (irplib_distortion_sub_hor_lowpass(filt_im, ARC_WINDOWSIZE) == -1) {
00323 cpl_image_delete(filt_im);
00324 return NULL;
00325 }
00326
00327
00328 median_val = cpl_image_get_median_dev(filt_im, &sigma);
00329
00330
00331 if (median_val < TRESH_MEDIAN_MIN) median_val = TRESH_MEDIAN_MIN;
00332 if (sigma > TRESH_SIGMA_MAX) sigma = TRESH_SIGMA_MAX;
00333
00334
00335 threshold = median_val + sigma * kappa;
00336
00337
00338 collapsed = cpl_image_collapse_median_create(filt_im, 0, 0, 0);
00339
00340
00341 if (irplib_distortion_threshold1d(filt_im, median_val, collapsed, 0.0)==-1) {
00342 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00343 "cannot threshold the filtered image");
00344 cpl_image_delete(filt_im);
00345 cpl_image_delete(collapsed);
00346 return NULL;
00347 }
00348 cpl_image_delete(collapsed);
00349
00350
00351 bin_im = cpl_mask_threshold_image_create(filt_im, threshold,
00352 DBL_MAX);
00353 cpl_image_delete(filt_im);
00354 if (bin_im == NULL) {
00355 cpl_error_set_message(cpl_func, CPL_ERROR_DATA_NOT_FOUND,
00356 "cannot binarise the image");
00357 return NULL;
00358 }
00359
00360
00361 ngoodpix = cpl_mask_count(bin_im);
00362 if (ngoodpix < ARC_MINGOODPIX) {
00363 #if defined CPL_HAVE_VA_ARGS && CPL_HAVE_VA_ARGS != 0
00364 cpl_error_set_message(cpl_func, CPL_ERROR_DATA_NOT_FOUND,
00365 "Too few (%d) white pixels", ngoodpix);
00366 #else
00367 cpl_error_set_message(cpl_func, CPL_ERROR_DATA_NOT_FOUND,
00368 "Too few white pixels");
00369 #endif
00370 cpl_mask_delete(bin_im);
00371 return NULL;
00372 }
00373
00374
00375 filter = cpl_mask_new(3, 3);
00376 cpl_mask_not(filter);
00377 cpl_mask_filter(bin_im, bin_im, filter, CPL_FILTER_OPENING,
00378 CPL_BORDER_ZERO);
00379 cpl_mask_delete(filter);
00380
00381
00382 *label_im = cpl_image_labelise_mask_create(bin_im, &nobj);
00383 cpl_mask_delete(bin_im);
00384
00385
00386 if ((det = cpl_apertures_new_from_image(im, *label_im)) == NULL) {
00387 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00388 "Cannot compute arcs stats");
00389 cpl_image_delete(*label_im);
00390 *label_im = NULL;
00391 return NULL;
00392 }
00393
00394
00395 if (irplib_distortion_purge_arcs(&det, *label_im, im, min_arclen,
00396 max_arc_width, arc_sat)) {
00397 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00398 "Cannot purge the arcs");
00399 cpl_image_delete(*label_im);
00400 *label_im = NULL;
00401 cpl_apertures_delete(det);
00402 return NULL;
00403 }
00404 if (cpl_apertures_get_size(det) < ARC_MINNBARCS) {
00405 #if defined CPL_HAVE_VA_ARGS && CPL_HAVE_VA_ARGS != 0
00406 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00407 "Not enough valid arcs (%"CPL_SIZE_FORMAT" < %d)",
00408 cpl_apertures_get_size(det), ARC_MINNBARCS);
00409 #else
00410 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00411 "Not enough valid arcs, min="
00412 IRPLIB_STRINGIFY(ARC_MINNBARCS));
00413 #endif
00414 cpl_image_delete(*label_im);
00415 *label_im = NULL;
00416 cpl_apertures_delete(det);
00417 return NULL;
00418 }
00419
00420
00421 return det;
00422 }
00423
00424
00434
00435 static cpl_error_code irplib_distortion_fill_border(cpl_image * self,
00436 int xmin,
00437 int ymin,
00438 int xmax,
00439 int ymax,
00440 double fillval)
00441 {
00442 const int nx = cpl_image_get_size_x(self);
00443 const int ny = cpl_image_get_size_y(self);
00444 float * pfi = cpl_image_get_data_float(self);
00445 const float fvalue = (float)fillval;
00446 int i, j;
00447
00448
00449 cpl_ensure_code(pfi != NULL, cpl_error_get_code());
00450
00451
00452 xmin = IRPLIB_MIN(xmin, nx+1);
00453 ymax = IRPLIB_MIN(ymax, ny);
00454
00455
00456 xmax = IRPLIB_MAX(xmax, xmin - 1);
00457 ymin = IRPLIB_MIN(ymin, ymax + 1);
00458
00459
00460
00461 for (j = 0; j < ymin-1; j++) {
00462 for (i = 0; i < nx; i++) {
00463 pfi[i+j*nx] = fvalue;
00464 }
00465 }
00466
00467
00468 for (; j < ymax; j++) {
00469 for (i = 0; i < xmin-1; i++) {
00470 pfi[i+j*nx] = fvalue;
00471 }
00472 for (i = xmax; i < nx; i++) {
00473 pfi[i+j*nx] = fvalue;
00474 }
00475 }
00476
00477
00478 for (; j < ny; j++) {
00479 for (i = 0; i < nx; i++) {
00480 pfi[i+j*nx] = fvalue;
00481 }
00482 }
00483
00484 return CPL_ERROR_NONE;
00485 }
00486
00487 static int irplib_distortion_threshold1d(
00488 cpl_image * im,
00489 double threshold,
00490 cpl_image * im1d,
00491 double newval)
00492 {
00493 float * pim;
00494 float * pim1d;
00495 int nx, ny;
00496 int i, j;
00497
00498
00499 if (im == NULL) return -1;
00500 if (im1d == NULL) return -1;
00501 if (cpl_image_get_type(im) != CPL_TYPE_FLOAT) return -1;
00502 if (cpl_image_get_type(im1d) != CPL_TYPE_FLOAT) return -1;
00503
00504
00505 pim = cpl_image_get_data_float(im);
00506 pim1d = cpl_image_get_data_float(im1d);
00507 nx = cpl_image_get_size_x(im);
00508 ny = cpl_image_get_size_y(im);
00509
00510
00511 for (i=0; i<nx; i++)
00512 if (pim1d[i] < threshold) {
00513 for (j=0; j<ny; j++) pim[i+j*nx] = (float)newval;
00514 }
00515
00516
00517 return 0;
00518 }
00519
00520 static int irplib_distortion_sub_hor_lowpass(
00521 cpl_image * im,
00522 int filt_size)
00523 {
00524 cpl_vector * linehi;
00525 cpl_vector * linelo;
00526 cpl_vector * avglinehi;
00527 cpl_vector * avglinelo;
00528 double * pavglinehi;
00529 float * pim;
00530 int lopos, hipos, nx, ny;
00531 int i, j;
00532
00533
00534 if (im == NULL) return -1;
00535 if (filt_size <= 0) return -1;
00536
00537
00538 nx = cpl_image_get_size_x(im);
00539 ny = cpl_image_get_size_y(im);
00540 lopos = (int)(ny/4);
00541 hipos = (int)(3*ny/4);
00542
00543
00544 if ((linehi = cpl_vector_new_from_image_row(im, hipos)) == NULL) {
00545 return -1;
00546 }
00547 if ((linelo = cpl_vector_new_from_image_row(im, lopos)) == NULL) {
00548 cpl_vector_delete(linehi);
00549 return -1;
00550 }
00551
00552
00553 if ((avglinehi = cpl_vector_filter_median_create(linehi,
00554 filt_size)) == NULL) {
00555 cpl_vector_delete(linehi);
00556 cpl_vector_delete(linelo);
00557 return -1;
00558 }
00559 cpl_vector_delete(linehi);
00560
00561 if ((avglinelo = cpl_vector_filter_median_create(linelo,
00562 filt_size)) == NULL) {
00563 cpl_vector_delete(linelo);
00564 cpl_vector_delete(avglinehi);
00565 return -1;
00566 }
00567 cpl_vector_delete(linelo);
00568
00569
00570 cpl_vector_add(avglinehi, avglinelo);
00571 cpl_vector_delete(avglinelo);
00572 cpl_vector_divide_scalar(avglinehi, 2.0);
00573
00574
00575 pavglinehi = cpl_vector_get_data(avglinehi);
00576 pim = cpl_image_get_data_float(im);
00577 for (i=0; i<nx; i++) {
00578 for (j=0; j<ny; j++) {
00579 pim[i+j*nx] -= pavglinehi[i];
00580 }
00581 }
00582 cpl_vector_delete(avglinehi);
00583
00584 return 0;
00585 }
00586
00587
00598
00599 static
00600 cpl_error_code irplib_distortion_purge_arcs(cpl_apertures ** self,
00601 cpl_image * lab_im,
00602 const cpl_image * arc_im,
00603 int min_arclen,
00604 int max_arcwidth,
00605 double arc_sat)
00606 {
00607 const double ycenter = 0.5 * (1 + cpl_image_get_size_y(arc_im));
00608 int narcs;
00609 int nkeep = 0;
00610 int ifirst = 1;
00611 int * relabel;
00612 int i;
00613
00614
00615 cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
00616
00617
00618 narcs = cpl_apertures_get_size(*self);
00619
00620 cpl_ensure_code(narcs > 0, CPL_ERROR_DATA_NOT_FOUND);
00621 cpl_ensure_code(cpl_image_get_type(lab_im) == CPL_TYPE_INT,
00622 CPL_ERROR_ILLEGAL_INPUT);
00623
00624
00625 relabel = cpl_calloc(narcs, sizeof(int));
00626
00627
00628 for (i = 0; i < narcs; i++) {
00629
00630 const int arclen = 1
00631 + cpl_apertures_get_top(*self, i+1)
00632 - cpl_apertures_get_bottom(*self, i+1);
00633
00634 if (cpl_apertures_get_top(*self, i+1) < ycenter) continue;
00635 if (cpl_apertures_get_bottom(*self, i+1) > ycenter) continue;
00636
00637 if (arclen > min_arclen) {
00638 const int arcwidth = 1
00639 + cpl_apertures_get_right(*self, i+1)
00640 - cpl_apertures_get_left(*self, i+1);
00641 if (arcwidth < max_arcwidth) {
00642 const int edge = cpl_apertures_get_left_y(*self, i+1);
00643 if (edge > 0) {
00644 const double mean = cpl_apertures_get_mean(*self, i+1);
00645 if (mean < arc_sat) {
00646 relabel[i] = ++nkeep;
00647
00648 if (nkeep == i+1) ifirst = nkeep;
00649 }
00650 }
00651 }
00652 }
00653 }
00654
00655 if (nkeep < narcs) {
00656
00657 int * plabim = cpl_image_get_data_int(lab_im);
00658 const int npix = cpl_image_get_size_x(lab_im)
00659 * cpl_image_get_size_y(lab_im);
00660
00661 if (nkeep == 0) {
00662 cpl_free(relabel);
00663 #if defined CPL_HAVE_VA_ARGS && CPL_HAVE_VA_ARGS != 0
00664 return cpl_error_set_message(cpl_func, CPL_ERROR_DATA_NOT_FOUND,
00665 "All %d arc(s) are invalid", narcs);
00666 #else
00667 return cpl_error_set_message(cpl_func, CPL_ERROR_DATA_NOT_FOUND,
00668 "All arcs are invalid");
00669 #endif
00670 }
00671
00672 for (i = 0; i < npix; i++) {
00673 const int label = plabim[i];
00674
00675 if (label < 0 || label > narcs) break;
00676 if (label >= ifirst) plabim[i] = relabel[label-1];
00677 }
00678
00679 if (i < npix) {
00680
00681 cpl_free(relabel);
00682 return cpl_error_set(cpl_func, plabim[i] < 0
00683 ? CPL_ERROR_ILLEGAL_INPUT
00684 : CPL_ERROR_INCOMPATIBLE_INPUT);
00685 }
00686
00687
00688 cpl_apertures_delete(*self);
00689 *self = cpl_apertures_new_from_image(arc_im, lab_im);
00690
00691 }
00692
00693 cpl_free(relabel);
00694
00695 cpl_msg_info(cpl_func, "Purged %d of %d arcs (1st purged=%d)", narcs - nkeep,
00696 narcs, ifirst);
00697
00698
00699 cpl_ensure_code(*self != NULL, cpl_error_get_code());
00700
00701 return CPL_ERROR_NONE;
00702 }
00703
00704
00705
00719
00720 static cpl_error_code
00721 irplib_distortion_fill_arc_positions(cpl_bivector * grid,
00722 cpl_vector * fitvalues,
00723 const cpl_image * in,
00724 const cpl_image * label_im,
00725 const cpl_apertures * det)
00726 {
00727 const int narcs = cpl_apertures_get_size(det);
00728 int nfitvals = cpl_vector_get_size(fitvalues);
00729 const int nx = cpl_image_get_size_x(label_im);
00730 const int ny = cpl_image_get_size_y(label_im);
00731 cpl_image * filt_img;
00732 cpl_mask * kernel;
00733 cpl_vector * gridx = cpl_bivector_get_x(grid);
00734 cpl_vector * gridy = cpl_bivector_get_y(grid);
00735 cpl_polynomial* dist1d;
00736 cpl_matrix * dist1dx = NULL;
00737 cpl_vector * dist1dy = NULL;
00738 double * dgridx;
00739 double * dgridy;
00740 double * dfitv;
00741 int ndone = 0;
00742 int i, obj;
00743
00744 cpl_ensure_code(nfitvals > 0, CPL_ERROR_DATA_NOT_FOUND);
00745 cpl_ensure_code(narcs > 0, CPL_ERROR_DATA_NOT_FOUND);
00746 cpl_ensure_code(cpl_image_get_type(label_im) == CPL_TYPE_INT,
00747 CPL_ERROR_TYPE_MISMATCH);
00748
00749
00750 if (nfitvals < narcs * ny) {
00751 nfitvals = narcs * ny;
00752 cpl_vector_set_size(fitvalues, nfitvals);
00753 }
00754 if (cpl_vector_get_size(gridx) < nfitvals ||
00755 cpl_vector_get_size(gridy) < nfitvals) {
00756 cpl_vector_set_size(gridx, nfitvals);
00757 cpl_vector_set_size(gridy, nfitvals);
00758 }
00759
00760
00761 dgridx = cpl_vector_get_data(gridx);
00762 dgridy = cpl_vector_get_data(gridy);
00763 dfitv = cpl_vector_get_data(fitvalues);
00764
00765
00766 kernel = cpl_mask_new(3, 3);
00767 cpl_mask_not(kernel);
00768 filt_img = cpl_image_new(nx, ny, cpl_image_get_type(in));
00769 cpl_image_filter_mask(filt_img, in, kernel, CPL_FILTER_MEDIAN,
00770 CPL_BORDER_FILTER);
00771 cpl_mask_delete(kernel);
00772
00773 dist1d = cpl_polynomial_new(1);
00774
00775 for (obj = 0; obj < narcs; obj++) {
00776
00777 const int * plabel_im = cpl_image_get_data_int_const(label_im);
00778 const int ndist1d = cpl_apertures_get_top(det, obj+1)
00779 - cpl_apertures_get_bottom(det, obj+1) + 1;
00780 cpl_boolean sampsym = CPL_TRUE;
00781 int j, prevj = 0;
00782 int k = 0;
00783
00784 (void)cpl_matrix_unwrap(dist1dx);
00785 (void)cpl_vector_unwrap(dist1dy);
00786 dist1dx = cpl_matrix_wrap(1, ndist1d, dgridy + ndone);
00787 dist1dy = cpl_vector_wrap(ndist1d, dfitv + ndone);
00788
00789
00790
00791 for (j = cpl_apertures_get_bottom(det, obj+1)-1;
00792 j < cpl_apertures_get_top(det, obj+1); j++) {
00793
00794 for (i = 0; i < nx; i++) {
00795 if (plabel_im[i + j * nx] == obj + 1) break;
00796 }
00797 if (i < nx) {
00798
00799 cpl_errorstate prestate = cpl_errorstate_get();
00800
00801 const double x_finepos
00802 = irplib_distortion_get_row_centroid(filt_img, label_im,
00803 i, j);
00804 if (!cpl_errorstate_is_equal(prestate)) {
00805 irplib_error_recover(prestate, "Could not find X-position "
00806 "for line %d at y=%d (x=%d)",
00807 obj+1, j+1, i+1);
00808 } else if (x_finepos >= 0.0) {
00809 cpl_matrix_set(dist1dx, 0, k, 1.0 + j);
00810 cpl_vector_set(dist1dy, k, 1.0 + x_finepos);
00811 if (k > 0 && j != 1 + prevj) sampsym = CPL_FALSE;
00812 prevj = j;
00813 k++;
00814 }
00815 }
00816 }
00817 if (k > 0) {
00818 double ref_xpos, grad;
00819 cpl_error_code error;
00820 const cpl_boolean did_drop = k != ndist1d;
00821 const cpl_size mindeg = 0;
00822 const cpl_size maxdeg = 2;
00823
00824 if (did_drop) {
00825
00826 dist1dx = cpl_matrix_wrap(1, k, cpl_matrix_unwrap(dist1dx));
00827 dist1dy = cpl_vector_wrap(k, cpl_vector_unwrap(dist1dy));
00828 }
00829
00830 error = cpl_polynomial_fit(dist1d, dist1dx, &sampsym, dist1dy, NULL,
00831 CPL_FALSE, &mindeg, &maxdeg);
00832 if (error) {
00833 cpl_msg_error(cpl_func, "1D-fit failed");
00834 break;
00835 }
00836
00837 ref_xpos = cpl_polynomial_eval_1d(dist1d, 0.5 * (ny + 1), &grad);
00838
00839 for (j = cpl_apertures_get_bottom(det, obj+1)-1;
00840 j < cpl_apertures_get_top(det, obj+1); j++) {
00841 const double xpos = cpl_polynomial_eval_1d(dist1d, j+1.0, NULL);
00842
00843 dfitv [ndone] = xpos;
00844 dgridx[ndone] = ref_xpos;
00845
00846
00847 if (did_drop)
00848 dgridy[ndone] = 1.0 + j;
00849 ndone++;
00850 }
00851 cpl_msg_info(cpl_func, "Line %d has center gradient %g", obj+1,
00852 grad);
00853 }
00854 }
00855
00856 cpl_image_delete(filt_img);
00857 cpl_polynomial_delete(dist1d);
00858 (void)cpl_matrix_unwrap(dist1dx);
00859 (void)cpl_vector_unwrap(dist1dy);
00860
00861 cpl_msg_info(cpl_func, "Found %d fitting points ("
00862 "expected up to %d points)", ndone, nfitvals);
00863
00864 cpl_ensure_code(obj == narcs, cpl_error_get_code());
00865
00866 cpl_ensure_code(ndone > 0, CPL_ERROR_DATA_NOT_FOUND);
00867
00868 cpl_vector_set_size(fitvalues, ndone);
00869 cpl_vector_set_size(gridx, ndone);
00870 cpl_vector_set_size(gridy, ndone);
00871
00872 return CPL_ERROR_NONE;
00873 }
00874
00875
00885
00886 static double irplib_distortion_get_row_centroid(const cpl_image * im,
00887 const cpl_image * label_im,
00888 int x,
00889 int y)
00890 {
00891 const int nx = cpl_image_get_size_x(im);
00892 const int ny = cpl_image_get_size_y(im);
00893 const int ynx = y * nx;
00894 const float * pim = cpl_image_get_data_float_const(im);
00895 const int * plabel_im = cpl_image_get_data_int_const(label_im);
00896 int firstpos = -1;
00897 int lastpos = -1;
00898 int maxpos = x;
00899 int objnum;
00900 double wsum = 0.0;
00901 double sum = 0.0;
00902 double max = 0.0;
00903
00904 cpl_ensure(pim != NULL, cpl_error_get_code(), -1.0);
00905 cpl_ensure(plabel_im != NULL, cpl_error_get_code(), -2.0);
00906 cpl_ensure(x >= 0, CPL_ERROR_ILLEGAL_INPUT, -3.0);
00907 cpl_ensure(y >= 0, CPL_ERROR_ILLEGAL_INPUT, -4.0);
00908 cpl_ensure(x < nx, CPL_ERROR_ILLEGAL_INPUT, -5.0);
00909 cpl_ensure(y < ny, CPL_ERROR_ILLEGAL_INPUT, -6.0);
00910
00911 max = (double)pim[x + ynx];
00912 objnum = plabel_im[x + ynx];
00913
00914
00915 do {
00916 const double val = (double)pim[x + ynx];
00917
00918 if (val > 0.0) {
00919 wsum += x * val;
00920 sum += val;
00921
00922 if (firstpos < 0) firstpos = x;
00923 lastpos = x;
00924
00925 if (val > max) {
00926 max = val;
00927 maxpos = x;
00928 }
00929 }
00930
00931
00932
00933 x++;
00934
00935 } while (x < nx && objnum == plabel_im[x + ynx]);
00936
00937 cpl_ensure(sum > 0.0, CPL_ERROR_DATA_NOT_FOUND, -7.0);
00938
00939
00940
00941
00942
00943
00944
00945 return (wsum < sum * firstpos || wsum > sum * lastpos)
00946 ? maxpos : wsum / sum;
00947 }
00948
00949
00955
00956 #define IS_NB_TESTPOINTS 8
00957 #define IS_MIN_SLOPE 0.01
00958 #define IS_MAX_SLOPE_DIF 0.075
00959 #define IS_MAX_FIT_EDGE_DIF 0.05
00960 #define IS_MIN_RAMP 10.0
00961 #define IS_MAX_MNERR 13.0
00962 #define IS_MAX_MNERR_DIF 8.0
00963 #define IS_MAX_INTER_DIF 20.0
00964 #define IS_SKIPZONE 2.5
00965 #define SQR(x) ((x)*(x))
00966 static cpl_image * irplib_distortion_remove_ramp(const cpl_image * in)
00967 {
00968 int ramp_present;
00969 const int nx = cpl_image_get_size_x(in);
00970 const int ny = cpl_image_get_size_y(in);
00971 const int yhi = (int)(ny/2);
00972 const int ylo = yhi - 1;
00973 int y;
00974 cpl_vector * tmp_vector;
00975 cpl_bivector * testpointlo;
00976 double * testpointlo_x;
00977 double * testpointlo_y;
00978 cpl_bivector * testpointhi;
00979 double * testpointhi_x;
00980 double * testpointhi_y;
00981 const int spacing = ny / (IS_SKIPZONE*IS_NB_TESTPOINTS);
00982 double rampdif, fitslope;
00983 double * pol_coefhi,
00984 * pol_coeflo;
00985 cpl_vector * median;
00986 double * median_data;
00987 double medianerrlo, medianerrhi;
00988 double slope;
00989 cpl_image * out;
00990 float * pout;
00991 float val;
00992 int i, j;
00993
00994 cpl_ensure(cpl_image_get_type(in) == CPL_TYPE_FLOAT,
00995 CPL_ERROR_UNSUPPORTED_MODE, NULL);
00996
00997 if (ny < IS_SKIPZONE * IS_NB_TESTPOINTS){
00998 #if defined CPL_HAVE_VA_ARGS && CPL_HAVE_VA_ARGS != 0
00999 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
01000 "image has %d lines, min="
01001 IRPLIB_STRINGIFY(IS_SKIPZONE) "*"
01002 IRPLIB_STRINGIFY(IS_NB_TESTPOINTS), ny);
01003 #else
01004 cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
01005 "image has too few lines, min="
01006 IRPLIB_STRINGIFY(IS_SKIPZONE) "*"
01007 IRPLIB_STRINGIFY(IS_NB_TESTPOINTS));
01008 #endif
01009 return NULL;
01010 }
01011
01012
01013 testpointhi = cpl_bivector_new(IS_NB_TESTPOINTS);
01014 testpointhi_x = cpl_bivector_get_x_data(testpointhi);
01015 testpointhi_y = cpl_bivector_get_y_data(testpointhi);
01016 testpointlo = cpl_bivector_new(IS_NB_TESTPOINTS);
01017 testpointlo_x = cpl_bivector_get_x_data(testpointlo);
01018 testpointlo_y = cpl_bivector_get_y_data(testpointlo);
01019 for (i=0; i<IS_NB_TESTPOINTS; i++) {
01020 y = yhi + i * spacing;
01021 tmp_vector = cpl_vector_new_from_image_row(in, y+1);
01022 testpointhi_x[i] = y - ny / 2;
01023 testpointhi_y[i] = cpl_vector_get_median_const(tmp_vector);
01024 cpl_vector_delete(tmp_vector);
01025 y = ylo - i * spacing;
01026 tmp_vector = cpl_vector_new_from_image_row(in, y+1);
01027 testpointlo_x[IS_NB_TESTPOINTS-i-1] = y;
01028 testpointlo_y[IS_NB_TESTPOINTS-i-1]=cpl_vector_get_median_const(tmp_vector);
01029 cpl_vector_delete(tmp_vector);
01030 }
01031
01032
01033 pol_coefhi = irplib_flat_fit_slope_robust(testpointhi_x,
01034 testpointhi_y, IS_NB_TESTPOINTS);
01035 pol_coeflo = irplib_flat_fit_slope_robust(testpointlo_x,
01036 testpointlo_y, IS_NB_TESTPOINTS);
01037
01038
01039 median = cpl_vector_new(IS_NB_TESTPOINTS);
01040 median_data = cpl_vector_get_data(median);
01041 for (i=0; i<IS_NB_TESTPOINTS; i++) {
01042 median_data[i]=SQR(testpointhi_y[i]
01043 - pol_coefhi[0] - pol_coefhi[1] * testpointhi_x[i]);
01044 }
01045 medianerrhi = cpl_vector_get_median(median);
01046 for (i=0; i<IS_NB_TESTPOINTS; i++) {
01047 median_data[i]=SQR(testpointlo_y[i]
01048 - pol_coeflo[0] - pol_coeflo[1] * testpointlo_x[i]);
01049 }
01050 medianerrlo = cpl_vector_get_median(median);
01051 cpl_vector_delete(median);
01052 rampdif = testpointlo_y[IS_NB_TESTPOINTS-1] - testpointhi_y[0];
01053 slope = rampdif / (ny/2.0);
01054 fitslope = (pol_coefhi[1] + pol_coeflo[1]) / 2.0;
01055
01056 cpl_bivector_delete(testpointlo);
01057 cpl_bivector_delete(testpointhi);
01058
01059
01060 if (fabs(rampdif)<IS_MIN_RAMP ||
01061 fabs(pol_coefhi[1]) < IS_MIN_SLOPE ||
01062 fabs(pol_coeflo[1]) < IS_MIN_SLOPE ||
01063 pol_coefhi[1]/pol_coeflo[1]<0.5 ||
01064 pol_coefhi[1]/pol_coeflo[1]>2.0 ||
01065 fabs(pol_coefhi[1]-pol_coeflo[1])>IS_MAX_SLOPE_DIF ||
01066 fabs(pol_coefhi[0]-pol_coeflo[0]) > IS_MAX_INTER_DIF ||
01067 medianerrlo> IS_MAX_MNERR ||
01068 medianerrhi> IS_MAX_MNERR ||
01069 fabs(medianerrlo-medianerrhi) >IS_MAX_MNERR_DIF ||
01070 fabs(slope-fitslope) > IS_MAX_FIT_EDGE_DIF ||
01071 slope/fitslope<0.5 ||
01072 slope/fitslope>2.0) ramp_present = 0;
01073 else ramp_present = 1;
01074
01075 cpl_free(pol_coeflo);
01076 cpl_free(pol_coefhi);
01077
01078
01079 out = cpl_image_duplicate(in);
01080 pout = cpl_image_get_data_float(out);
01081 if (ramp_present == 1) {
01082 for (j=0; j<ny/2; j++) {
01083 val = slope * (j-ny/2);
01084 for (i=0; i<nx; i++)
01085 pout[i+j*nx] -= val;
01086 }
01087 for (j=ny/2; j<ny; j++) {
01088 val = slope * (j-ny);
01089 for (i=0; i<nx; i++)
01090 pout[i+j*nx] -= val;
01091 }
01092
01093 }
01094
01095 return out;
01096 }
01097
01098
01112
01113 static cpl_error_code irplib_image_filter_background_line(cpl_image * self,
01114 const cpl_image * other,
01115 int hsize,
01116 cpl_boolean vertical)
01117 {
01118 const int nx = cpl_image_get_size_x(self);
01119 const int ny = cpl_image_get_size_y(self);
01120 const int msize = 1 + 2 * hsize;
01121 cpl_mask * mask;
01122 cpl_image * background;
01123 cpl_error_code error = CPL_ERROR_NONE;
01124
01125 cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
01126 cpl_ensure_code(hsize >= 0, CPL_ERROR_ILLEGAL_INPUT);
01127
01128 if (other == NULL) other = self;
01129
01130 mask = vertical ? cpl_mask_new(msize, 1) : cpl_mask_new(1, msize);
01131
01132 error |= cpl_mask_not(mask);
01133
01134 background = cpl_image_new(nx, ny, cpl_image_get_type(other));
01135
01136 error |= cpl_image_filter_mask(background, other, mask, CPL_FILTER_MEDIAN,
01137 CPL_BORDER_FILTER);
01138 cpl_mask_delete(mask);
01139
01140 if (self != other) {
01141 error |= cpl_image_copy(self, other, 1, 1);
01142 }
01143
01144 error |= cpl_image_subtract(self, background);
01145 cpl_image_delete(background);
01146
01147 return error ? cpl_error_set_where(cpl_func) : CPL_ERROR_NONE;
01148 }
01149
01150
01151
01177 static cpl_matrix * irplib_matrix_product_normal_create(const cpl_matrix * self)
01178 {
01179
01180 double sum;
01181 cpl_matrix * product;
01182 const double * ai = cpl_matrix_get_data_const(self);
01183 const double * aj;
01184 double * bwrite;
01185 const int m = cpl_matrix_get_nrow(self);
01186 const int n = cpl_matrix_get_ncol(self);
01187 int i, j, k;
01188
01189
01190 cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, NULL);
01191
01192 #if 0
01193
01194
01195
01196 product = cpl_matrix_new(m, m);
01197 bwrite = cpl_matrix_get_data(product);
01198 #else
01199 bwrite = (double *) cpl_malloc(m * m * sizeof(double));
01200 product = cpl_matrix_wrap(m, m, bwrite);
01201 #endif
01202
01203
01204 for (i = 0; i < m; i++, bwrite += m, ai += n) {
01205 aj = ai;
01206 for (j = i; j < m; j++, aj += n) {
01207 sum = 0.0;
01208 for (k = 0; k < n; k++) {
01209 sum += ai[k] * aj[k];
01210 }
01211 bwrite[j] = sum;
01212 }
01213 }
01214
01215 return product;
01216
01217 }
01218
01219
01233
01234 static cpl_error_code irplib_polynomial_fit_2d(cpl_polynomial * self,
01235 const cpl_bivector * xy_pos,
01236 const cpl_vector * values,
01237 int degree, double fixy,
01238 double * mse)
01239 {
01240
01241 const int np = cpl_bivector_get_size(xy_pos);
01242
01243 const int nc1 = 1+degree;
01244
01245
01246 const int nc = nc1 * (1 + nc1) / 2 - nc1;
01247 cpl_matrix * mv;
01248 cpl_matrix * mh;
01249 cpl_matrix * mb;
01250 cpl_matrix * mx;
01251 #ifdef IRPLIB_DISTORTION_ASSERT
01252 const double * coeffs1d;
01253 #endif
01254 double * dmv;
01255 cpl_vector * xhat;
01256 cpl_vector * yhat;
01257 cpl_vector * zhat;
01258 cpl_size powers[2];
01259 int degx, degy;
01260 int i, j;
01261 cpl_error_code error;
01262
01263
01264 cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
01265 cpl_ensure_code(cpl_polynomial_get_dimension(self) == 2,
01266 CPL_ERROR_INVALID_TYPE);
01267 cpl_ensure_code(np > 0, cpl_error_get_code());
01268 cpl_ensure_code(values != NULL, CPL_ERROR_NULL_INPUT);
01269
01270 cpl_ensure_code(cpl_vector_get_size(values) == np,
01271 CPL_ERROR_INCOMPATIBLE_INPUT);
01272
01273 cpl_ensure_code(degree > 0, CPL_ERROR_ILLEGAL_INPUT);
01274 cpl_ensure_code(np >= nc, CPL_ERROR_DATA_NOT_FOUND);
01275
01276
01277 yhat = cpl_vector_duplicate(cpl_bivector_get_y_const(xy_pos));
01278 cpl_vector_subtract_scalar(yhat, fixy);
01279
01280
01281 xhat = cpl_vector_duplicate(cpl_bivector_get_x_const(xy_pos));
01282 zhat = cpl_vector_duplicate(values);
01283 cpl_vector_subtract(zhat, xhat);
01284
01285
01286
01287
01288 dmv = (double*)cpl_malloc(nc*np*sizeof(double));
01289 mv = cpl_matrix_wrap(nc, np, dmv);
01290
01291
01292 for (i=0; i < np; i++) {
01293 const double x = cpl_vector_get(xhat, i);
01294 const double y = cpl_vector_get(yhat, i);
01295 double xvalue;
01296 double yvalue = y;
01297 j = 0;
01298 for (degy = 1; degy <= degree; degy++) {
01299 xvalue = 1;
01300 for (degx = 0; degx <= degree-degy; degx++, j++) {
01301 dmv[np * j + i] = xvalue * yvalue;
01302 xvalue *= x;
01303 }
01304 yvalue *= y;
01305 }
01306
01307 }
01308 cpl_vector_delete(xhat);
01309 cpl_vector_delete(yhat);
01310
01311
01312 mb = cpl_matrix_wrap(np, 1, cpl_vector_get_data(zhat));
01313
01314
01315 mx = cpl_matrix_product_create(mv, mb);
01316
01317 cpl_matrix_unwrap(mb);
01318 cpl_vector_delete(zhat);
01319
01320
01321 mh = irplib_matrix_product_normal_create(mv);
01322 cpl_matrix_delete(mv);
01323
01324
01325 error = cpl_matrix_decomp_chol(mh) || cpl_matrix_solve_chol(mh, mx);
01326
01327 cpl_matrix_delete(mh);
01328
01329 if (error) {
01330 cpl_matrix_delete(mx);
01331 cpl_ensure_code(0, error);
01332 }
01333
01334
01335
01336 #ifdef IRPLIB_DISTORTION_ASSERT
01337 coeffs1d = cpl_matrix_get_data(mx);
01338 #endif
01339
01340 j = 0;
01341 for (degy = 1; degy <= degree; degy++) {
01342 powers[1] = degy;
01343 for (degx = 0; degx <= degree-degy; degx++, j++) {
01344 powers[0] = degx;
01345
01346 cpl_polynomial_set_coeff(self, powers, cpl_matrix_get(mx, j, 0));
01347 }
01348 }
01349
01350
01351 cpl_matrix_delete(mx);
01352
01353
01354 powers[0] = 1;
01355 powers[1] = 0;
01356 cpl_polynomial_set_coeff(self, powers, 1.0);
01357
01358
01359 cpl_polynomial_shift_1d(self, 1, -fixy);
01360
01361
01362 if (mse != NULL) {
01363 const cpl_vector * x_pos = cpl_bivector_get_x_const(xy_pos);
01364 const cpl_vector * y_pos = cpl_bivector_get_y_const(xy_pos);
01365 cpl_vector * x_val = cpl_vector_new(2);
01366 double residue;
01367
01368 *mse = 0;
01369 for (i=0; i<np; i++) {
01370 cpl_vector_set(x_val, 0, cpl_vector_get(x_pos, i));
01371 cpl_vector_set(x_val, 1, cpl_vector_get(y_pos, i));
01372
01373 residue = cpl_vector_get(values, i)
01374 - cpl_polynomial_eval(self, x_val);
01375 *mse += residue * residue;
01376 }
01377 cpl_vector_delete(x_val);
01378
01379 *mse /= np;
01380 }
01381
01382 return CPL_ERROR_NONE;
01383 }
01384