irplib_distortion.c

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

Generated on 3 Mar 2013 for DETMON Pipeline Reference Manual by  doxygen 1.6.1