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