irplib_distortion.c

00001 /* $Id: irplib_distortion.c,v 1.48 2011/12/22 12:34:24 cgarcia Exp $
00002  *
00003  * This file is part of the irplib package
00004  * Copyright (C) 2002,2003 European Southern Observatory
00005  *
00006  * This program is free software; you can redistribute it and/or modify
00007  * it under the terms of the GNU General Public License as published by
00008  * the Free Software Foundation; either version 2 of the License, or
00009  * (at your option) any later version.
00010  *
00011  * This program is distributed in the hope that it will be useful,
00012  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  * GNU General Public License for more details.
00015  *
00016  * You should have received a copy of the GNU General Public License
00017  * along with this program; if not, write to the Free Software
00018  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  */
00020 
00021 /*
00022  * $Author: cgarcia $
00023  * $Date: 2011/12/22 12:34:24 $
00024  * $Revision: 1.48 $
00025  * $Name: HEAD $
00026  */
00027 
00028 #ifdef HAVE_CONFIG_H
00029 #include <config.h>
00030 #endif
00031 
00032 /*-----------------------------------------------------------------------------
00033                                    Includes
00034  -----------------------------------------------------------------------------*/
00035 
00036 /* TEMPORARY SUPPORT OF CPL 5.x */
00037 #include <cpl.h>
00038 
00039 #ifndef CPL_SIZE_FORMAT
00040 #define CPL_SIZE_FORMAT "d"
00041 #define cpl_size int
00042 #endif
00043 /* END TEMPORARY SUPPORT OF CPL 5.x */
00044 
00045 #include "irplib_distortion.h"
00046 
00047 #include "irplib_flat.h"
00048 #include "irplib_utils.h"
00049 #include "irplib_polynomial.h"
00050 
00051 #include <math.h>
00052 #include <float.h>
00053 
00054 /*-----------------------------------------------------------------------------
00055                                    Define
00056  -----------------------------------------------------------------------------*/
00057 
00058 #define IRPLIB_MAX(A,B) ((A) > (B) ? (A) : (B))
00059 #define IRPLIB_MIN(A,B) ((A) < (B) ? (A) : (B))
00060 
00061 #define ARC_MINGOODPIX      100
00062 #define ARC_MINARCLENFACT   2.0
00063 #define ARC_MINNBARCS       4
00064 #define ARC_RANGE_FACT      3.0
00065 #define ARC_WINDOWSIZE      32
00066 
00067 #define TRESH_MEDIAN_MIN    0.0
00068 #define TRESH_SIGMA_MAX     200.0
00069 
00070 /*----------------------------------------------------------------------------*/
00074 /*----------------------------------------------------------------------------*/
00075 
00076 /*-----------------------------------------------------------------------------
00077                                 Functions prototypes
00078  -----------------------------------------------------------------------------*/
00079 
00080 static cpl_apertures * irplib_distortion_detect_arcs(cpl_image *,
00081         cpl_image **, int, int, double, int, int, int, int);
00082 static cpl_error_code irplib_distortion_fill_border(cpl_image *, int, int,
00083                                                     int, int, double);
00084 static int irplib_distortion_threshold1d(cpl_image *, double, cpl_image *, 
00085         double);
00086 static cpl_error_code irplib_distortion_purge_arcs(cpl_apertures **, cpl_image *,
00087                                                    const cpl_image *, int, int,
00088                                                    double);
00089 static cpl_error_code irplib_distortion_fill_arc_positions(cpl_bivector *,
00090                                                           cpl_vector *,
00091                                                           const cpl_image *,
00092                                                           const cpl_image *,
00093                                                           const cpl_apertures *);
00094 
00095 static double irplib_distortion_get_row_centroid(const cpl_image *,
00096                                                  const cpl_image *, int, int);
00097 
00098 static int irplib_distortion_sub_hor_lowpass(cpl_image *, int);
00099 static cpl_image * irplib_distortion_remove_ramp(const cpl_image *);
00100 
00101 static cpl_error_code irplib_image_filter_background_line(cpl_image *,
00102         const cpl_image *, int, cpl_boolean) ;
00103 
00104 static cpl_error_code irplib_polynomial_fit_2d(cpl_polynomial *,
00105                                                const cpl_bivector *,
00106                                                const cpl_vector *, int,
00107                                                double, double *);
00108 
00109 static cpl_matrix * irplib_matrix_product_normal_create(const cpl_matrix *);
00110 
00111 /*-----------------------------------------------------------------------------
00112                                 Functions code
00113  -----------------------------------------------------------------------------*/
00114 
00117 /*----------------------------------------------------------------------------*/
00146 /*----------------------------------------------------------------------------*/
00147 cpl_polynomial * irplib_distortion_estimate(
00148         const cpl_image *   org,
00149         int                 xmin,
00150         int                 ymin,
00151         int                 xmax,
00152         int                 ymax,
00153         int                 auto_ramp_sub,
00154         int                 arc_sat,
00155         int                 max_arc_width,
00156         double              kappa,
00157         int                 degree,
00158         cpl_apertures   **  arcs)
00159 {
00160     cpl_image      * local_im;
00161     cpl_image      * filtered;
00162     cpl_image      * label_image;
00163     double           rightmost, leftmost;
00164     cpl_bivector   * grid;
00165     cpl_vector     * values_to_fit;
00166     int              n_arcs;
00167     cpl_polynomial * poly2d;
00168     double           mse = 0.0;
00169     const int        nx = cpl_image_get_size_x(org);
00170     const int        ny = cpl_image_get_size_y(org);
00171     const int        min_arc_range = (int)(nx / ARC_RANGE_FACT);
00172     int              i;
00173 
00174     /* Check entries */
00175     cpl_ensure(org           != NULL, CPL_ERROR_NULL_INPUT,    NULL);
00176     cpl_ensure(kappa         >= 0.0,  CPL_ERROR_ILLEGAL_INPUT, NULL);
00177     cpl_ensure(max_arc_width > 0,     CPL_ERROR_ILLEGAL_INPUT, NULL);
00178 
00179     /* The background may vary strongly along the vertical line. */
00180     /* Detect and rm background with a 1+2*max_arc_width x 1 median filter */
00181 
00182     filtered = cpl_image_new(nx, ny, cpl_image_get_type(org));
00183 
00184     irplib_image_filter_background_line(filtered, org, max_arc_width, CPL_TRUE);
00185 
00186     if (auto_ramp_sub) {
00187         local_im = irplib_distortion_remove_ramp(filtered);
00188         cpl_image_delete(filtered);
00189     } else {
00190         local_im = filtered;
00191     }
00192 
00193     cpl_error_ensure(local_im != NULL, cpl_error_get_code(),
00194                      return(NULL), "Cannot clean the image");
00195 
00196     /* Detect the arcs in the input image */
00197     *arcs = irplib_distortion_detect_arcs(local_im, &label_image, arc_sat,
00198                                           max_arc_width, kappa, xmin, ymin,
00199                                           xmax, ymax);
00200     if (*arcs == NULL) {
00201         cpl_image_delete(local_im);
00202         cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00203                               "Cannot detect the arcs");
00204         return NULL;
00205     }
00206     n_arcs = cpl_apertures_get_size(*arcs);
00207     cpl_msg_info(cpl_func, "%d detected arcs", n_arcs);
00208 
00209     /* Check that the arcs are not concentrated in the same zone */
00210     rightmost = leftmost = cpl_apertures_get_pos_x(*arcs, 1);
00211     for (i=1; i<n_arcs; i++) {
00212         if (cpl_apertures_get_pos_x(*arcs, i+1) < leftmost)
00213             leftmost = cpl_apertures_get_pos_x(*arcs, i+1);
00214         if (cpl_apertures_get_pos_x(*arcs, i+1) > rightmost)
00215             rightmost = cpl_apertures_get_pos_x(*arcs, i+1);
00216     }
00217     if ((int)(rightmost-leftmost) < min_arc_range) {
00218 #if defined CPL_HAVE_VA_ARGS && CPL_HAVE_VA_ARGS != 0
00219         cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00220                               "too narrow range (%g-%g)<%d",
00221                               rightmost, leftmost, min_arc_range);
00222 #else
00223         cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00224                               "too narrow range");
00225 #endif
00226         cpl_apertures_delete(*arcs);
00227         cpl_image_delete(local_im);
00228         cpl_image_delete(label_image);
00229         *arcs = NULL;
00230         return NULL;
00231     }
00232 
00233     /* Create a 2-D deformation grid with detected arcs */
00234     cpl_msg_info(cpl_func, "Create deformation grid");
00235     grid = cpl_bivector_new(n_arcs * ny);
00236     values_to_fit = cpl_vector_new(n_arcs * ny);
00237 
00238     if (irplib_distortion_fill_arc_positions(grid, values_to_fit, local_im,
00239                                             label_image, *arcs)){
00240         cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00241                               "cannot get arcs positions");
00242         cpl_apertures_delete(*arcs);
00243         cpl_image_delete(local_im);
00244         cpl_image_delete(label_image);
00245         *arcs = NULL;
00246         return NULL;
00247     }
00248     cpl_image_delete(label_image);
00249     cpl_image_delete(local_im);
00250 
00251     /* Apply the fitting */
00252     poly2d = cpl_polynomial_new(2);
00253     if (irplib_polynomial_fit_2d(poly2d, grid, values_to_fit, degree,
00254                                  0.5*(ny+1), &mse)) {
00255         cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00256                               "cannot apply the 2d fit");
00257         cpl_bivector_delete(grid);
00258         cpl_vector_delete(values_to_fit);
00259         cpl_apertures_delete(*arcs);
00260         *arcs = NULL;
00261         return NULL;
00262     }
00263 
00264     cpl_msg_info(cpl_func, 
00265             "Fitted a %d. degree 2D-polynomial to %"CPL_SIZE_FORMAT" points "
00266             "with mean-square error: %g", degree,
00267             cpl_vector_get_size(values_to_fit), mse);
00268 
00269     /* Free and return */
00270     cpl_bivector_delete(grid);
00271     cpl_vector_delete(values_to_fit);
00272     return poly2d;
00273 }
00274 
00277 /*----------------------------------------------------------------------------*/
00293 /*----------------------------------------------------------------------------*/
00294 static cpl_apertures * irplib_distortion_detect_arcs(
00295         cpl_image *   im,
00296         cpl_image **  label_im,
00297         int             arc_sat,
00298         int             max_arc_width,
00299         double          kappa,
00300         int             xmin,
00301         int             ymin,
00302         int             xmax,
00303         int             ymax)
00304 {
00305     const int           ny = cpl_image_get_size_y(im);
00306     /* Set min_arclen */
00307     const int           min_arclen = (int)(ny / ARC_MINARCLENFACT);
00308     cpl_image       *   filt_im;
00309     cpl_mask        *   filter;
00310     cpl_image       *   collapsed;
00311     cpl_mask        *   bin_im;
00312     double              threshold, fillval, median_val, sigma;
00313     cpl_apertures   *   det;
00314     cpl_size            nobj;
00315     int                 ngoodpix;
00316     
00317     /* Default values for output parameters */
00318     *label_im = NULL;
00319 
00320     /* Clear zones to be ignored (to avoid false detections) */
00321     median_val = cpl_image_get_median_dev(im, &sigma);
00322     fillval = median_val-sigma/2.0;
00323     if (irplib_distortion_fill_border(im, xmin, ymin, xmax, ymax, fillval)) {
00324         cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00325                               "cannot fill bad zones");
00326         return NULL;
00327     }
00328 
00329     /* Subtract a low-pass */
00330     filt_im = cpl_image_duplicate(im);
00331     if (irplib_distortion_sub_hor_lowpass(filt_im, ARC_WINDOWSIZE) == -1) {
00332         cpl_image_delete(filt_im);
00333         return NULL;
00334     }
00335     
00336     /* Get relevant stats for thresholding */
00337     median_val = cpl_image_get_median_dev(filt_im, &sigma);
00338 
00339     /* Correct median_val and sigma if necessary */
00340     if (median_val < TRESH_MEDIAN_MIN) median_val = TRESH_MEDIAN_MIN;
00341     if (sigma > TRESH_SIGMA_MAX) sigma = TRESH_SIGMA_MAX;
00342 
00343     /* Set the threshold */
00344     threshold = median_val + sigma * kappa;
00345 
00346     /* Collapse the image */
00347     collapsed = cpl_image_collapse_median_create(filt_im, 0, 0, 0);
00348 
00349     /* Threshold to keep only the arcs - use of the collapsed image */
00350     if (irplib_distortion_threshold1d(filt_im, median_val, collapsed, 0.0)==-1) {
00351         cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00352                               "cannot threshold the filtered image");
00353         cpl_image_delete(filt_im);
00354         cpl_image_delete(collapsed);
00355         return NULL;
00356     }
00357     cpl_image_delete(collapsed);
00358 
00359     /* Binarize the image */
00360     bin_im = cpl_mask_threshold_image_create(filt_im, threshold, 
00361             DBL_MAX);
00362     cpl_image_delete(filt_im);
00363     if (bin_im == NULL) {
00364         cpl_error_set_message(cpl_func, CPL_ERROR_DATA_NOT_FOUND,
00365                               "cannot binarise the image");
00366         return NULL;
00367     }
00368 
00369     /* Test if there are enough good pixels */
00370     ngoodpix = cpl_mask_count(bin_im);
00371     if (ngoodpix < ARC_MINGOODPIX) {
00372 #if defined CPL_HAVE_VA_ARGS && CPL_HAVE_VA_ARGS != 0
00373         cpl_error_set_message(cpl_func, CPL_ERROR_DATA_NOT_FOUND,
00374                               "Too few (%d) white pixels", ngoodpix);
00375 #else
00376         cpl_error_set_message(cpl_func, CPL_ERROR_DATA_NOT_FOUND,
00377                               "Too few white pixels");
00378 #endif
00379         cpl_mask_delete(bin_im);
00380         return NULL;
00381     }
00382 
00383     /* Apply a morphological opening to clean the isolated pixels */
00384     filter = cpl_mask_new(3, 3);
00385     cpl_mask_not(filter);
00386     cpl_mask_filter(bin_im, bin_im, filter, CPL_FILTER_OPENING,
00387                     CPL_BORDER_ZERO);
00388     cpl_mask_delete(filter);
00389 
00390     /* Labelize pixel map to a label image */
00391     *label_im = cpl_image_labelise_mask_create(bin_im, &nobj);
00392     cpl_mask_delete(bin_im);
00393 
00394     /* Compute statistics on objects */
00395     if ((det = cpl_apertures_new_from_image(im, *label_im)) == NULL) {
00396         cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00397                               "Cannot compute arcs stats");
00398         cpl_image_delete(*label_im);
00399         *label_im = NULL;
00400         return NULL;
00401     }
00402 
00403     /* Purge non-relevant arcs */
00404     if (irplib_distortion_purge_arcs(&det, *label_im, im, min_arclen,
00405                                      max_arc_width, arc_sat)) {
00406         cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00407                               "Cannot purge the arcs");
00408         cpl_image_delete(*label_im);
00409         *label_im = NULL;
00410         cpl_apertures_delete(det);
00411         return NULL;
00412     }
00413     if (cpl_apertures_get_size(det) < ARC_MINNBARCS) {
00414 #if defined CPL_HAVE_VA_ARGS && CPL_HAVE_VA_ARGS != 0
00415         cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00416                 "Not enough valid arcs (%"CPL_SIZE_FORMAT" < %d)", 
00417                 cpl_apertures_get_size(det), ARC_MINNBARCS);
00418 #else
00419         cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
00420                               "Not enough valid arcs, min="
00421                               IRPLIB_STRINGIFY(ARC_MINNBARCS));
00422 #endif
00423         cpl_image_delete(*label_im);
00424         *label_im = NULL;
00425         cpl_apertures_delete(det);
00426         return NULL;
00427     }
00428 
00429     /* Return  */
00430     return det;
00431 }
00432 
00433 /*----------------------------------------------------------------------------*/
00443 /*----------------------------------------------------------------------------*/
00444 static cpl_error_code irplib_distortion_fill_border(cpl_image * self,
00445                                                     int         xmin,
00446                                                     int         ymin,
00447                                                     int         xmax,
00448                                                     int         ymax,
00449                                                     double      fillval)
00450 {
00451     const int   nx     = cpl_image_get_size_x(self);
00452     const int   ny     = cpl_image_get_size_y(self);
00453     float     * pfi    = cpl_image_get_data_float(self);
00454     const float fvalue = (float)fillval;
00455     int         i, j;
00456 
00457 
00458     cpl_ensure_code(pfi != NULL, cpl_error_get_code());
00459 
00460     /* Ensure validity of pixel buffer access */
00461     xmin = IRPLIB_MIN(xmin, nx+1);
00462     ymax = IRPLIB_MIN(ymax, ny);
00463 
00464     /* - and avoid double access */
00465     xmax = IRPLIB_MAX(xmax, xmin - 1);
00466     ymin = IRPLIB_MIN(ymin, ymax + 1);
00467 
00468     /* Fill the zone */
00469 
00470     for (j = 0; j < ymin-1; j++) {
00471         for (i = 0; i < nx; i++) {
00472             pfi[i+j*nx] = fvalue;
00473         }
00474     }
00475     /* assert( j == IRPLIB_MAX(0, ymin-1) ); */
00476 
00477     for (; j < ymax; j++) {
00478         for (i = 0; i < xmin-1; i++) {
00479             pfi[i+j*nx] = fvalue;
00480         }
00481         for (i = xmax; i < nx; i++) {
00482             pfi[i+j*nx] = fvalue;
00483         }
00484     }
00485     /* assert( j == IRPLIB_MAX(0, ymax) ); */
00486 
00487     for (; j < ny; j++) {
00488         for (i = 0; i < nx; i++) {
00489             pfi[i+j*nx] = fvalue;
00490         }
00491     }
00492 
00493     return CPL_ERROR_NONE;
00494 }
00495 
00496 static int irplib_distortion_threshold1d(
00497         cpl_image   *   im,
00498         double          threshold,
00499         cpl_image   *   im1d,
00500         double          newval)
00501 {
00502     float       *   pim;
00503     float       *   pim1d;
00504     int             nx, ny;
00505     int             i, j;
00506 
00507     /* Check entries */
00508     if (im == NULL) return -1;
00509     if (im1d == NULL) return -1;
00510     if (cpl_image_get_type(im) != CPL_TYPE_FLOAT) return -1;
00511     if (cpl_image_get_type(im1d) != CPL_TYPE_FLOAT) return -1;
00512 
00513     /* Get access to the im / im1d data */
00514     pim = cpl_image_get_data_float(im);
00515     pim1d = cpl_image_get_data_float(im1d);
00516     nx = cpl_image_get_size_x(im);
00517     ny = cpl_image_get_size_y(im);
00518 
00519     /* Apply the thresholding */
00520     for (i=0; i<nx; i++)
00521         if (pim1d[i] < threshold) {
00522             for (j=0; j<ny; j++) pim[i+j*nx] = (float)newval;
00523         }
00524 
00525     /* Return */
00526     return 0;
00527 }
00528 
00529 static int irplib_distortion_sub_hor_lowpass(
00530         cpl_image   *   im, 
00531         int             filt_size)
00532 {
00533     cpl_vector  *   linehi;
00534     cpl_vector  *   linelo;
00535     cpl_vector  *   avglinehi;
00536     cpl_vector  *   avglinelo;
00537     double      *   pavglinehi;
00538     float       *   pim;
00539     int             lopos, hipos, nx, ny;
00540     int             i, j;
00541 
00542     /* Test entries */
00543     if (im == NULL) return -1;
00544     if (filt_size <= 0) return -1;
00545     
00546     /* Initialise */
00547     nx = cpl_image_get_size_x(im);
00548     ny = cpl_image_get_size_y(im);
00549     lopos = (int)(ny/4);
00550     hipos = (int)(3*ny/4);
00551 
00552     /* Get the vectors out of the image */
00553     if ((linehi = cpl_vector_new_from_image_row(im, hipos)) == NULL) {
00554         return -1;
00555     }
00556     if ((linelo = cpl_vector_new_from_image_row(im, lopos)) == NULL) {
00557         cpl_vector_delete(linehi);
00558         return -1;
00559     }
00560     
00561     /* Filter the vectors */
00562     if ((avglinehi = cpl_vector_filter_median_create(linehi, 
00563                     filt_size)) == NULL) {
00564         cpl_vector_delete(linehi);
00565         cpl_vector_delete(linelo);
00566         return -1;
00567     }
00568     cpl_vector_delete(linehi);
00569     
00570     if ((avglinelo = cpl_vector_filter_median_create(linelo, 
00571                     filt_size)) == NULL) {
00572         cpl_vector_delete(linelo);
00573         cpl_vector_delete(avglinehi);
00574         return -1;
00575     }
00576     cpl_vector_delete(linelo);
00577 
00578     /* Average the filtered vectors to get the low freq signal */
00579     cpl_vector_add(avglinehi, avglinelo);
00580     cpl_vector_delete(avglinelo);
00581     cpl_vector_divide_scalar(avglinehi, 2.0);
00582 
00583     /* Subtract the low frequency signal */
00584     pavglinehi = cpl_vector_get_data(avglinehi);
00585     pim = cpl_image_get_data_float(im);
00586     for (i=0; i<nx; i++) {
00587         for (j=0; j<ny; j++) {
00588             pim[i+j*nx] -= pavglinehi[i];
00589         }
00590     }
00591     cpl_vector_delete(avglinehi);
00592 
00593     return 0;
00594 }
00595 
00596 /*----------------------------------------------------------------------------*/
00607 /*----------------------------------------------------------------------------*/
00608 static
00609 cpl_error_code irplib_distortion_purge_arcs(cpl_apertures  ** self,
00610                                             cpl_image       * lab_im,
00611                                             const cpl_image * arc_im,
00612                                             int               min_arclen,
00613                                             int               max_arcwidth,
00614                                             double            arc_sat)
00615 {
00616     const double ycenter = 0.5 * (1 + cpl_image_get_size_y(arc_im));
00617     int   narcs;
00618     int   nkeep  = 0;
00619     int   ifirst = 1;
00620     int * relabel;
00621     int   i;
00622 
00623     /* Check entries */
00624     cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
00625 
00626     /* Get number of arcs */
00627     narcs = cpl_apertures_get_size(*self);
00628 
00629     cpl_ensure_code(narcs  > 0,     CPL_ERROR_DATA_NOT_FOUND);
00630     cpl_ensure_code(cpl_image_get_type(lab_im) == CPL_TYPE_INT,
00631                     CPL_ERROR_ILLEGAL_INPUT);
00632 
00633     /* Allocate relabel array with default relabelling to zero */
00634     relabel = cpl_calloc(narcs, sizeof(int));
00635 
00636     /* Loop on the different arcs candidates */
00637     for (i = 0; i < narcs; i++) {
00638         /* Test if the current object is a valid arc */
00639         const int arclen = 1
00640             + cpl_apertures_get_top(*self, i+1)
00641             - cpl_apertures_get_bottom(*self, i+1);
00642 
00643         if (cpl_apertures_get_top(*self,    i+1) < ycenter) continue;
00644         if (cpl_apertures_get_bottom(*self, i+1) > ycenter) continue;
00645 
00646         if (arclen > min_arclen) {
00647             const int arcwidth = 1
00648                 + cpl_apertures_get_right(*self, i+1)
00649                 - cpl_apertures_get_left(*self, i+1);
00650             if (arcwidth < max_arcwidth) {
00651                 const int edge = cpl_apertures_get_left_y(*self, i+1);
00652                 if (edge > 0) {
00653                     const double mean = cpl_apertures_get_mean(*self, i+1);
00654                     if (mean < arc_sat) {
00655                         relabel[i] = ++nkeep;
00656                         /* Relabeling, if any, starts with ifirst */
00657                         if (nkeep == i+1) ifirst = nkeep;
00658                     }
00659                 }
00660             }
00661         }
00662     }
00663 
00664     if (nkeep < narcs) {
00665         /* Update the labelised image by erasing non valid arcs */
00666         int     * plabim = cpl_image_get_data_int(lab_im);
00667         const int npix   = cpl_image_get_size_x(lab_im)
00668             * cpl_image_get_size_y(lab_im);
00669 
00670         if (nkeep == 0) {
00671             cpl_free(relabel);
00672 #if defined CPL_HAVE_VA_ARGS && CPL_HAVE_VA_ARGS != 0
00673             return cpl_error_set_message(cpl_func, CPL_ERROR_DATA_NOT_FOUND,
00674                                          "All %d arc(s) are invalid", narcs);
00675 #else
00676             return cpl_error_set_message(cpl_func, CPL_ERROR_DATA_NOT_FOUND,
00677                                          "All arcs are invalid");
00678 #endif
00679         }
00680 
00681         for (i = 0; i < npix; i++) {
00682             const int label = plabim[i];
00683 
00684             if (label < 0 || label > narcs) break;
00685             if (label >= ifirst) plabim[i] = relabel[label-1];
00686         }
00687 
00688         if (i < npix) {
00689             /* lab_im is not a valid label image */
00690             cpl_free(relabel);
00691             return cpl_error_set(cpl_func, plabim[i] < 0
00692                                          ? CPL_ERROR_ILLEGAL_INPUT
00693                                          : CPL_ERROR_INCOMPATIBLE_INPUT);
00694         }
00695 
00696         /* Purge the bad arcs */
00697         cpl_apertures_delete(*self);
00698         *self = cpl_apertures_new_from_image(arc_im, lab_im);
00699 
00700     }
00701 
00702     cpl_free(relabel);
00703 
00704     cpl_msg_info(cpl_func, "Purged %d of %d arcs (1st purged=%d)", narcs - nkeep,
00705                  narcs, ifirst);
00706 
00707     /* arc_im may be invalid */
00708     cpl_ensure_code(*self != NULL, cpl_error_get_code());
00709 
00710     return CPL_ERROR_NONE;
00711 }
00712 
00713 
00714 /*----------------------------------------------------------------------------*/
00728 /*----------------------------------------------------------------------------*/
00729 static cpl_error_code
00730 irplib_distortion_fill_arc_positions(cpl_bivector        * grid,
00731                                      cpl_vector          * fitvalues,
00732                                      const cpl_image     * in,
00733                                      const cpl_image     * label_im,
00734                                      const cpl_apertures * det)
00735 {
00736     const int    narcs = cpl_apertures_get_size(det);
00737     int          nfitvals = cpl_vector_get_size(fitvalues);
00738     const int    nx = cpl_image_get_size_x(label_im);
00739     const int    ny = cpl_image_get_size_y(label_im);
00740     cpl_image  * filt_img;
00741     cpl_mask   * kernel;
00742     cpl_vector * gridx = cpl_bivector_get_x(grid);
00743     cpl_vector * gridy = cpl_bivector_get_y(grid);
00744     cpl_polynomial* dist1d;
00745     cpl_matrix * dist1dx = NULL;
00746     cpl_vector * dist1dy = NULL;
00747     double     * dgridx;
00748     double     * dgridy;
00749     double     * dfitv;
00750     int          ndone = 0;
00751     int          i, obj;
00752 
00753     cpl_ensure_code(nfitvals > 0,      CPL_ERROR_DATA_NOT_FOUND);
00754     cpl_ensure_code(narcs    > 0,      CPL_ERROR_DATA_NOT_FOUND);
00755     cpl_ensure_code(cpl_image_get_type(label_im) == CPL_TYPE_INT,
00756                     CPL_ERROR_TYPE_MISMATCH);
00757 
00758     /* Ensure space for output */
00759     if (nfitvals < narcs * ny) {
00760         nfitvals = narcs * ny;
00761         cpl_vector_set_size(fitvalues, nfitvals);
00762     }
00763     if (cpl_vector_get_size(gridx) < nfitvals ||
00764         cpl_vector_get_size(gridy) < nfitvals) {
00765         cpl_vector_set_size(gridx, nfitvals);
00766         cpl_vector_set_size(gridy, nfitvals);
00767     }
00768 
00769     /* Get data after resizing */
00770     dgridx = cpl_vector_get_data(gridx);
00771     dgridy = cpl_vector_get_data(gridy);
00772     dfitv  = cpl_vector_get_data(fitvalues);
00773 
00774     /* Median filter on input image */
00775     kernel = cpl_mask_new(3, 3);
00776     cpl_mask_not(kernel);
00777     filt_img = cpl_image_new(nx, ny, cpl_image_get_type(in));
00778     cpl_image_filter_mask(filt_img, in, kernel, CPL_FILTER_MEDIAN,
00779                                                 CPL_BORDER_FILTER);
00780     cpl_mask_delete(kernel);
00781 
00782     dist1d = cpl_polynomial_new(1);
00783 
00784     for (obj = 0; obj < narcs; obj++) {
00785         /* Find the reference X-coordinate for the arc */
00786         const int  * plabel_im = cpl_image_get_data_int_const(label_im);
00787         const int    ndist1d = cpl_apertures_get_top(det, obj+1)
00788             - cpl_apertures_get_bottom(det, obj+1) + 1;
00789         cpl_boolean sampsym = CPL_TRUE;
00790         int         j, prevj = 0;
00791         int         k = 0;
00792 
00793         (void)cpl_matrix_unwrap(dist1dx);
00794         (void)cpl_vector_unwrap(dist1dy);
00795         dist1dx = cpl_matrix_wrap(1, ndist1d, dgridy + ndone);
00796         dist1dy = cpl_vector_wrap(ndist1d, dfitv  + ndone);
00797 
00798         /* Find out the X coord. at all Y positions on the arc */
00799 
00800         for (j = cpl_apertures_get_bottom(det, obj+1)-1;
00801              j < cpl_apertures_get_top(det, obj+1); j++) {
00802 
00803             for (i = 0; i < nx; i++) {
00804                 if (plabel_im[i + j * nx] == obj + 1) break;
00805             }
00806             if (i < nx) {
00807                 /* Found 1st pixel of aperture obj+1 in row j+1 */
00808                 cpl_errorstate prestate = cpl_errorstate_get();
00809 
00810                 const double x_finepos
00811                     = irplib_distortion_get_row_centroid(filt_img, label_im,
00812                                                          i, j);
00813                 if (!cpl_errorstate_is_equal(prestate)) {
00814                     irplib_error_recover(prestate, "Could not find X-position "
00815                                          "for line %d at y=%d (x=%d)",
00816                                          obj+1, j+1, i+1);
00817                 } else if (x_finepos >= 0.0) {
00818                     cpl_matrix_set(dist1dx, 0, k, 1.0 + j);
00819                     cpl_vector_set(dist1dy, k, 1.0 + x_finepos);
00820                     if (k > 0 && j != 1 + prevj) sampsym = CPL_FALSE;
00821                     prevj = j;
00822                     k++;
00823                 }
00824             }
00825         }
00826         if (k > 0) {
00827             double ref_xpos, grad;
00828             cpl_error_code error;
00829             const cpl_boolean did_drop = k != ndist1d;
00830             const cpl_size mindeg = 0;
00831             const cpl_size maxdeg = 2;
00832 
00833             if (did_drop) {
00834                 /* Set correct size */
00835                 dist1dx = cpl_matrix_wrap(1, k, cpl_matrix_unwrap(dist1dx));
00836                 dist1dy = cpl_vector_wrap(k, cpl_vector_unwrap(dist1dy));
00837             }
00838 
00839             error = cpl_polynomial_fit(dist1d, dist1dx, &sampsym, dist1dy, NULL,
00840                              CPL_FALSE, &mindeg, &maxdeg);
00841             if (error) {
00842                 cpl_msg_error(cpl_func, "1D-fit failed");
00843                 break;
00844             }
00845 
00846             ref_xpos = cpl_polynomial_eval_1d(dist1d, 0.5 * (ny + 1), &grad);
00847 
00848             for (j = cpl_apertures_get_bottom(det, obj+1)-1;
00849                  j < cpl_apertures_get_top(det, obj+1); j++) {
00850                 const double xpos = cpl_polynomial_eval_1d(dist1d, j+1.0, NULL);
00851 
00852                 dfitv [ndone] = xpos;
00853                 dgridx[ndone] = ref_xpos;
00854                 /* Wrapping dist1dx does _not_ take care of dgridy,
00855                    in case of "Could not find X-position " */
00856                 if (did_drop)
00857                     dgridy[ndone] = 1.0 + j;
00858                 ndone++;
00859             }
00860             cpl_msg_info(cpl_func, "Line %d has center gradient %g", obj+1,
00861                          grad);
00862         }
00863     }
00864 
00865     cpl_image_delete(filt_img);
00866     cpl_polynomial_delete(dist1d);
00867     (void)cpl_matrix_unwrap(dist1dx);
00868     (void)cpl_vector_unwrap(dist1dy);
00869 
00870     cpl_msg_info(cpl_func, "Found %d fitting points ("
00871                  "expected up to %d points)", ndone, nfitvals);
00872 
00873     cpl_ensure_code(obj == narcs, cpl_error_get_code());
00874 
00875     cpl_ensure_code(ndone > 0, CPL_ERROR_DATA_NOT_FOUND);
00876 
00877     cpl_vector_set_size(fitvalues, ndone);
00878     cpl_vector_set_size(gridx, ndone);
00879     cpl_vector_set_size(gridy, ndone);
00880 
00881     return CPL_ERROR_NONE;
00882 }
00883 
00884 /*----------------------------------------------------------------------------*/
00894 /*----------------------------------------------------------------------------*/
00895 static double irplib_distortion_get_row_centroid(const cpl_image * im,
00896                                                  const cpl_image * label_im,
00897                                                  int               x,
00898                                                  int               y)
00899 {
00900     const int     nx        = cpl_image_get_size_x(im);
00901     const int     ny        = cpl_image_get_size_y(im);
00902     const int     ynx       = y * nx;
00903     const float * pim       = cpl_image_get_data_float_const(im);
00904     const int   * plabel_im = cpl_image_get_data_int_const(label_im);
00905     int           firstpos = -1;
00906     int           lastpos  = -1;
00907     int           maxpos   = x;
00908     int           objnum;
00909     double        wsum = 0.0;
00910     double        sum  = 0.0;
00911     double        max  = 0.0;
00912 
00913     cpl_ensure(pim       != NULL, cpl_error_get_code(),    -1.0);
00914     cpl_ensure(plabel_im != NULL, cpl_error_get_code(),    -2.0);
00915     cpl_ensure(x         >= 0,    CPL_ERROR_ILLEGAL_INPUT, -3.0);
00916     cpl_ensure(y         >= 0,    CPL_ERROR_ILLEGAL_INPUT, -4.0);
00917     cpl_ensure(x         <  nx,   CPL_ERROR_ILLEGAL_INPUT, -5.0);
00918     cpl_ensure(y         <  ny,   CPL_ERROR_ILLEGAL_INPUT, -6.0);
00919 
00920     max    = (double)pim[x + ynx];
00921     objnum = plabel_im[x + ynx];
00922 
00923     /* While we stay in the same object... */
00924     do {
00925         const double val = (double)pim[x + ynx];
00926 
00927         if (val > 0.0) { /* FIXME: Handle this exception better */
00928             wsum += x * val;
00929             sum += val;
00930 
00931             if (firstpos < 0) firstpos = x;
00932             lastpos = x;
00933 
00934             if (val > max) {
00935                 max = val;
00936                 maxpos = x;
00937             }
00938         }
00939 
00940 
00941         /* Next point */
00942         x++;
00943 
00944     } while (x < nx && objnum == plabel_im[x + ynx]);
00945 
00946     cpl_ensure(sum > 0.0, CPL_ERROR_DATA_NOT_FOUND, -7.0);
00947 
00948     /*
00949        assert( 0 <= maxpos && maxpos < nx );
00950        assert( objnum == plabel_im[maxpos + ynx] );
00951        assert( wsum >= 0.0 );
00952     */
00953 
00954     return (wsum < sum * firstpos || wsum > sum * lastpos)
00955         ? maxpos : wsum / sum;
00956 }
00957 
00958 /*----------------------------------------------------------------------------*/
00964 /*----------------------------------------------------------------------------*/
00965 #define IS_NB_TESTPOINTS    8
00966 #define IS_MIN_SLOPE        0.01
00967 #define IS_MAX_SLOPE_DIF    0.075
00968 #define IS_MAX_FIT_EDGE_DIF 0.05
00969 #define IS_MIN_RAMP         10.0
00970 #define IS_MAX_MNERR        13.0
00971 #define IS_MAX_MNERR_DIF    8.0
00972 #define IS_MAX_INTER_DIF    20.0
00973 #define IS_SKIPZONE         2.5
00974 #define SQR(x) ((x)*(x))
00975 static cpl_image * irplib_distortion_remove_ramp(const cpl_image * in) 
00976 {
00977     int                 ramp_present;
00978     const int           nx = cpl_image_get_size_x(in);
00979     const int           ny = cpl_image_get_size_y(in);
00980     const int           yhi = (int)(ny/2);
00981     const int           ylo = yhi - 1;
00982     int                 y;
00983     cpl_vector      *   tmp_vector;
00984     cpl_bivector    *   testpointlo;
00985     double          *   testpointlo_x;
00986     double          *   testpointlo_y;
00987     cpl_bivector    *   testpointhi;
00988     double          *   testpointhi_x;
00989     double          *   testpointhi_y;
00990     const int           spacing = ny / (IS_SKIPZONE*IS_NB_TESTPOINTS);
00991     double              rampdif, fitslope;
00992     double          *   pol_coefhi,
00993                     *   pol_coeflo;
00994     cpl_vector      *   median;
00995     double          *   median_data;
00996     double              medianerrlo, medianerrhi;
00997     double              slope;
00998     cpl_image       *   out;
00999     float           *   pout;
01000     float               val;
01001     int                 i, j;
01002 
01003     cpl_ensure(cpl_image_get_type(in) == CPL_TYPE_FLOAT,
01004                CPL_ERROR_UNSUPPORTED_MODE, NULL);
01005                     
01006     if (ny < IS_SKIPZONE * IS_NB_TESTPOINTS){
01007 #if defined CPL_HAVE_VA_ARGS && CPL_HAVE_VA_ARGS != 0
01008         cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
01009                               "image has %d lines, min="
01010                               IRPLIB_STRINGIFY(IS_SKIPZONE) "*"
01011                               IRPLIB_STRINGIFY(IS_NB_TESTPOINTS), ny);
01012 #else
01013         cpl_error_set_message(cpl_func, CPL_ERROR_ILLEGAL_INPUT,
01014                               "image has too few lines, min="
01015                               IRPLIB_STRINGIFY(IS_SKIPZONE) "*"
01016                               IRPLIB_STRINGIFY(IS_NB_TESTPOINTS));
01017 #endif
01018         return NULL;
01019     }
01020     
01021     slope=0.0;
01022     /* Fill the vectors */
01023     testpointhi = cpl_bivector_new(IS_NB_TESTPOINTS);
01024     testpointhi_x = cpl_bivector_get_x_data(testpointhi);
01025     testpointhi_y = cpl_bivector_get_y_data(testpointhi);
01026     testpointlo = cpl_bivector_new(IS_NB_TESTPOINTS);
01027     testpointlo_x = cpl_bivector_get_x_data(testpointlo);
01028     testpointlo_y = cpl_bivector_get_y_data(testpointlo);
01029     for (i=0; i<IS_NB_TESTPOINTS; i++) {
01030         y = yhi + i * spacing;
01031         tmp_vector = cpl_vector_new_from_image_row(in, y+1);
01032         testpointhi_x[i] = y - ny / 2;
01033         testpointhi_y[i] = cpl_vector_get_median_const(tmp_vector);
01034         cpl_vector_delete(tmp_vector);
01035         y = ylo - i * spacing;
01036         tmp_vector = cpl_vector_new_from_image_row(in, y+1);
01037         testpointlo_x[IS_NB_TESTPOINTS-i-1] = y;
01038         testpointlo_y[IS_NB_TESTPOINTS-i-1]=cpl_vector_get_median_const(tmp_vector);
01039         cpl_vector_delete(tmp_vector);
01040     }
01041 
01042     /* Apply the fit */
01043     pol_coefhi = irplib_flat_fit_slope_robust(testpointhi_x,
01044             testpointhi_y, IS_NB_TESTPOINTS);
01045     pol_coeflo = irplib_flat_fit_slope_robust(testpointlo_x, 
01046             testpointlo_y, IS_NB_TESTPOINTS);
01047 
01048     /* Compute the errors */
01049     median = cpl_vector_new(IS_NB_TESTPOINTS);
01050     median_data = cpl_vector_get_data(median);
01051     for (i=0; i<IS_NB_TESTPOINTS; i++) {
01052         median_data[i]=SQR(testpointhi_y[i]
01053                 - pol_coefhi[0] - pol_coefhi[1] * testpointhi_x[i]);
01054     }
01055     medianerrhi = cpl_vector_get_median(median);
01056     for (i=0; i<IS_NB_TESTPOINTS; i++) {
01057         median_data[i]=SQR(testpointlo_y[i]
01058                 - pol_coeflo[0] - pol_coeflo[1] * testpointlo_x[i]);
01059     }
01060     medianerrlo = cpl_vector_get_median(median);
01061     cpl_vector_delete(median);
01062     rampdif = testpointlo_y[IS_NB_TESTPOINTS-1] - testpointhi_y[0];
01063     slope = rampdif / (ny/2.0);
01064     fitslope = (pol_coefhi[1] + pol_coeflo[1]) / 2.0;
01065 
01066     cpl_bivector_delete(testpointlo);
01067     cpl_bivector_delete(testpointhi);
01068 
01069     /* Decide if there is a ramp or not  */
01070     if (fabs(rampdif)<IS_MIN_RAMP ||
01071             fabs(pol_coefhi[1]) < IS_MIN_SLOPE ||
01072             fabs(pol_coeflo[1]) < IS_MIN_SLOPE ||
01073             pol_coefhi[1]/pol_coeflo[1]<0.5 ||
01074             pol_coefhi[1]/pol_coeflo[1]>2.0 ||
01075             fabs(pol_coefhi[1]-pol_coeflo[1])>IS_MAX_SLOPE_DIF ||
01076             fabs(pol_coefhi[0]-pol_coeflo[0]) > IS_MAX_INTER_DIF ||
01077             medianerrlo> IS_MAX_MNERR ||
01078             medianerrhi> IS_MAX_MNERR ||
01079             fabs(medianerrlo-medianerrhi) >IS_MAX_MNERR_DIF ||
01080             fabs(slope-fitslope) > IS_MAX_FIT_EDGE_DIF ||
01081             slope/fitslope<0.5 ||
01082             slope/fitslope>2.0) ramp_present = 0;
01083     else ramp_present = 1;
01084 
01085     cpl_free(pol_coeflo);
01086     cpl_free(pol_coefhi);
01087 
01088     /* Correct the ramp if it is there */
01089     out = cpl_image_duplicate(in);
01090     pout = cpl_image_get_data_float(out);
01091     if (ramp_present == 1) {
01092         for (j=0; j<ny/2; j++) {
01093             val = slope * (j-ny/2);
01094             for (i=0; i<nx; i++)
01095                 pout[i+j*nx] -= val;
01096         }
01097         for (j=ny/2; j<ny; j++) {
01098             val = slope * (j-ny);
01099             for (i=0; i<nx; i++)
01100                 pout[i+j*nx] -= val;
01101         }
01102 
01103     }
01104 
01105     return out;
01106 }
01107 
01108 /*----------------------------------------------------------------------------*/
01122 /*----------------------------------------------------------------------------*/
01123 static cpl_error_code irplib_image_filter_background_line(cpl_image * self,
01124                                                    const cpl_image * other,
01125                                                    int hsize,
01126                                                    cpl_boolean vertical)
01127 {
01128     const int      nx = cpl_image_get_size_x(self);
01129     const int      ny = cpl_image_get_size_y(self);
01130     const int      msize = 1 + 2 * hsize;
01131     cpl_mask     * mask;
01132     cpl_image    * background;
01133     cpl_error_code error = CPL_ERROR_NONE;
01134 
01135     cpl_ensure_code(self  != NULL, CPL_ERROR_NULL_INPUT);
01136     cpl_ensure_code(hsize >= 0,    CPL_ERROR_ILLEGAL_INPUT);
01137 
01138     if (other == NULL) other = self;
01139 
01140     mask = vertical ? cpl_mask_new(msize, 1) : cpl_mask_new(1, msize);
01141 
01142     error |= cpl_mask_not(mask);
01143 
01144     background = cpl_image_new(nx, ny, cpl_image_get_type(other));
01145 
01146     error |= cpl_image_filter_mask(background, other, mask, CPL_FILTER_MEDIAN,
01147                                    CPL_BORDER_FILTER);
01148     cpl_mask_delete(mask);
01149 
01150     if (self != other) {
01151         error |= cpl_image_copy(self, other, 1, 1);
01152     }
01153 
01154     error |= cpl_image_subtract(self, background);
01155     cpl_image_delete(background);
01156 
01157     return error ? cpl_error_set_where(cpl_func) : CPL_ERROR_NONE;
01158 }
01159 
01160 
01161 
01187 static cpl_matrix * irplib_matrix_product_normal_create(const cpl_matrix * self)
01188 {
01189 
01190     double         sum;
01191     cpl_matrix   * product;
01192     const double * ai = cpl_matrix_get_data_const(self);
01193     const double * aj;
01194     double       * bwrite;
01195     const int      m = cpl_matrix_get_nrow(self);
01196     const int      n = cpl_matrix_get_ncol(self);
01197     int            i, j, k;
01198 
01199 
01200     cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, NULL);
01201 
01202 #if 0
01203     /* Initialize all values to zero.
01204        This is done to avoid access of uninitilized memory,  in case
01205        someone passes the matrix to for example cpl_matrix_dump(). */
01206     product = cpl_matrix_new(m, m);
01207     bwrite = cpl_matrix_get_data(product);
01208 #else
01209     bwrite = (double *) cpl_malloc(m * m * sizeof(double));
01210     product = cpl_matrix_wrap(m, m, bwrite);
01211 #endif
01212 
01213     /* The result at (i,j) is the dot-product of i'th and j'th row */
01214     for (i = 0; i < m; i++, bwrite += m, ai += n) {
01215         aj = ai; /* aj points to first entry in j'th row */
01216         for (j = i; j < m; j++, aj += n) {
01217             sum = 0.0;
01218             for (k = 0; k < n; k++) {
01219                 sum += ai[k] * aj[k];
01220             }
01221             bwrite[j] = sum;
01222         }
01223     }
01224 
01225     return product;
01226 
01227 }
01228 
01229 /*----------------------------------------------------------------------------*/
01243 /*----------------------------------------------------------------------------*/
01244 static cpl_error_code irplib_polynomial_fit_2d(cpl_polynomial * self,
01245                                                const cpl_bivector * xy_pos,
01246                                                const cpl_vector * values,
01247                                                int degree, double fixy,
01248                                                double * mse)
01249 {
01250 
01251     const int        np = cpl_bivector_get_size(xy_pos);
01252     /* Number of unknowns to determine in one dimension */
01253     const int        nc1 = 1+degree;
01254     /* Number of unknowns to determine */
01255     /* P_{i,0} = 0, except P_{1,0} = 1 */
01256     const int        nc = nc1 * (1 + nc1) / 2 - nc1;
01257     cpl_matrix     * mv;   /* The transpose of the Vandermonde matrix */
01258     cpl_matrix     * mh;   /* Block-Hankel matrix, V'*V */
01259     cpl_matrix     * mb;
01260     cpl_matrix     * mx;
01261 #ifdef IRPLIB_DISTORTION_ASSERT
01262     const double   * coeffs1d;
01263 #endif
01264     double         * dmv;
01265     cpl_vector     * xhat;
01266     cpl_vector     * yhat;
01267     cpl_vector     * zhat;
01268     cpl_size         powers[2];
01269     int              degx, degy;
01270     int              i, j;
01271     cpl_error_code   error;
01272    
01273 
01274     cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
01275     cpl_ensure_code(cpl_polynomial_get_dimension(self) == 2,
01276                     CPL_ERROR_INVALID_TYPE);
01277     cpl_ensure_code(np > 0,         cpl_error_get_code());
01278     cpl_ensure_code(values != NULL, CPL_ERROR_NULL_INPUT);
01279 
01280     cpl_ensure_code(cpl_vector_get_size(values) == np,
01281                     CPL_ERROR_INCOMPATIBLE_INPUT);
01282 
01283     cpl_ensure_code(degree > 0, CPL_ERROR_ILLEGAL_INPUT);
01284     cpl_ensure_code(np >= nc,   CPL_ERROR_DATA_NOT_FOUND);
01285 
01286     /* transform zero-point to fixy */
01287     yhat = cpl_vector_duplicate(cpl_bivector_get_y_const(xy_pos));
01288     cpl_vector_subtract_scalar(yhat, fixy);
01289 
01290     /* - and ensure P(y) = y on center line */
01291     xhat = cpl_vector_duplicate(cpl_bivector_get_x_const(xy_pos));
01292     zhat = cpl_vector_duplicate(values);
01293     cpl_vector_subtract(zhat, xhat);
01294 
01295     /* Initialize matrices */
01296     /* mv contains the polynomial terms in the order described */
01297     /* above in each row, for each input point. */
01298     dmv = (double*)cpl_malloc(nc*np*sizeof(double));
01299     mv = cpl_matrix_wrap(nc, np, dmv);
01300 
01301     /* Has redundant FLOPs, appears to improve accuracy */
01302     for (i=0; i < np; i++) {
01303         const double x = cpl_vector_get(xhat, i);
01304         const double y = cpl_vector_get(yhat, i);
01305         double xvalue;
01306         double yvalue = y;
01307         j = 0;
01308         for (degy = 1; degy <= degree; degy++) {
01309             xvalue = 1;
01310             for (degx = 0; degx <= degree-degy; degx++, j++) {
01311                 dmv[np * j + i] = xvalue * yvalue;
01312                 xvalue *= x;
01313             }
01314             yvalue *= y;
01315         }
01316         /* cx_assert( j == nc ); */
01317     }
01318     cpl_vector_delete(xhat);
01319     cpl_vector_delete(yhat);
01320 
01321     /* mb contains the values, it is not modified */
01322     mb = cpl_matrix_wrap(np, 1, cpl_vector_get_data(zhat));
01323 
01324     /* Form the right hand side of the normal equations */
01325     mx = cpl_matrix_product_create(mv, mb);
01326 
01327     cpl_matrix_unwrap(mb);
01328     cpl_vector_delete(zhat);
01329 
01330     /* Form the matrix of the normal equations */
01331     mh = irplib_matrix_product_normal_create(mv);
01332     cpl_matrix_delete(mv);
01333 
01334     /* Solve XA=B by a least-square solution (aka pseudo-inverse). */
01335     error = cpl_matrix_decomp_chol(mh) || cpl_matrix_solve_chol(mh, mx);
01336 
01337     cpl_matrix_delete(mh);
01338 
01339     if (error) {
01340         cpl_matrix_delete(mx);
01341         cpl_ensure_code(0, error);
01342     }
01343 
01344     /* Store coefficients for output */
01345 
01346 #ifdef IRPLIB_DISTORTION_ASSERT
01347     coeffs1d = cpl_matrix_get_data(mx);
01348 #endif
01349 
01350     j = 0;
01351     for (degy = 1; degy <= degree; degy++) {
01352         powers[1] = degy;
01353         for (degx = 0; degx <= degree-degy; degx++, j++) {
01354             powers[0] = degx;
01355             /* cx_assert( coeffs1d[j] == cpl_matrix_get(mx, j, 0) ); */
01356             cpl_polynomial_set_coeff(self, powers, cpl_matrix_get(mx, j, 0));
01357         }
01358     }
01359     /* cx_assert( j == nc ); */
01360 
01361     cpl_matrix_delete(mx);
01362 
01363     /* P_{1,0} = 1 */
01364     powers[0] = 1;
01365     powers[1] = 0;
01366     cpl_polynomial_set_coeff(self, powers, 1.0);
01367 
01368     /* Transform the polynomial back in Y */
01369     cpl_polynomial_shift_1d(self, 1, -fixy);
01370 
01371     /* If requested, compute mean squared error */
01372     if (mse != NULL) {
01373         const cpl_vector * x_pos = cpl_bivector_get_x_const(xy_pos);
01374         const cpl_vector * y_pos = cpl_bivector_get_y_const(xy_pos);
01375         cpl_vector * x_val = cpl_vector_new(2);
01376         double residue;
01377 
01378         *mse = 0;
01379         for (i=0; i<np; i++) {
01380             cpl_vector_set(x_val, 0, cpl_vector_get(x_pos, i));
01381             cpl_vector_set(x_val, 1, cpl_vector_get(y_pos, i));
01382             /* Subtract from the true value, square, accumulate */
01383             residue = cpl_vector_get(values, i)
01384                 - cpl_polynomial_eval(self, x_val);
01385             *mse += residue * residue;
01386         }
01387         cpl_vector_delete(x_val);
01388         /* Average the error term */
01389         *mse /= np;
01390     }
01391 
01392     return CPL_ERROR_NONE;
01393 }
01394 

Generated on 9 Jan 2012 for DETMON Pipeline Reference Manual by  doxygen 1.6.1