irplib_distortion.c

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

Generated on Wed Mar 9 15:46:16 2011 for NACO Pipeline Reference Manual by  doxygen 1.5.8